mirror of
https://github.com/id-Software/DOOM-3-BFG.git
synced 2024-12-03 09:22:45 +00:00
1780 lines
44 KiB
C++
1780 lines
44 KiB
C++
/*
|
|
===========================================================================
|
|
|
|
Doom 3 BFG Edition GPL Source Code
|
|
Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company.
|
|
|
|
This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").
|
|
|
|
Doom 3 BFG Edition Source Code is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
Doom 3 BFG Edition Source Code is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with Doom 3 BFG Edition Source Code. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
In addition, the Doom 3 BFG Edition Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 BFG Edition Source Code. If not, please request a copy in writing from id Software at the address below.
|
|
|
|
If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
|
|
|
|
===========================================================================
|
|
*/
|
|
|
|
#ifndef __MATH_MATX_H__
|
|
#define __MATH_MATX_H__
|
|
|
|
/*
|
|
===============================================================================
|
|
|
|
idMatX - arbitrary sized dense real matrix
|
|
|
|
The matrix lives on 16 byte aligned and 16 byte padded memory.
|
|
|
|
NOTE: due to the temporary memory pool idMatX cannot be used by multiple threads.
|
|
|
|
===============================================================================
|
|
*/
|
|
|
|
#define MATX_MAX_TEMP 1024
|
|
#define MATX_QUAD( x ) ( ( ( ( x ) + 3 ) & ~3 ) * sizeof( float ) )
|
|
#define MATX_CLEAREND() int s = numRows * numColumns; while( s < ( ( s + 3 ) & ~3 ) ) { mat[s++] = 0.0f; }
|
|
#define MATX_ALLOCA( n ) ( (float *) _alloca16( MATX_QUAD( n ) ) )
|
|
#define MATX_ALLOCA_CACHE_LINES( n ) ( (float *) _alloca128( ( ( n ) * sizeof( float ) + CACHE_LINE_SIZE - 1 ) & ~ ( CACHE_LINE_SIZE - 1 ) ) )
|
|
#define MATX_SIMD
|
|
|
|
class idMatX
|
|
{
|
|
public:
|
|
ID_INLINE idMatX();
|
|
ID_INLINE idMatX( const idMatX& other );
|
|
ID_INLINE explicit idMatX( int rows, int columns );
|
|
ID_INLINE explicit idMatX( int rows, int columns, float* src );
|
|
ID_INLINE ~idMatX();
|
|
|
|
ID_INLINE void Set( int rows, int columns, const float* src );
|
|
ID_INLINE void Set( const idMat3& m1, const idMat3& m2 );
|
|
ID_INLINE void Set( const idMat3& m1, const idMat3& m2, const idMat3& m3, const idMat3& m4 );
|
|
|
|
ID_INLINE const float* operator[]( int index ) const;
|
|
ID_INLINE float* operator[]( int index );
|
|
ID_INLINE idMatX& operator=( const idMatX& a );
|
|
ID_INLINE idMatX operator*( const float a ) const;
|
|
ID_INLINE idVecX operator*( const idVecX& vec ) const;
|
|
ID_INLINE idMatX operator*( const idMatX& a ) const;
|
|
ID_INLINE idMatX operator+( const idMatX& a ) const;
|
|
ID_INLINE idMatX operator-( const idMatX& a ) const;
|
|
ID_INLINE idMatX& operator*=( const float a );
|
|
ID_INLINE idMatX& operator*=( const idMatX& a );
|
|
ID_INLINE idMatX& operator+=( const idMatX& a );
|
|
ID_INLINE idMatX& operator-=( const idMatX& a );
|
|
|
|
friend ID_INLINE idMatX operator*( const float a, const idMatX& m );
|
|
friend ID_INLINE idVecX operator*( const idVecX& vec, const idMatX& m );
|
|
friend ID_INLINE idVecX& operator*=( idVecX& vec, const idMatX& m );
|
|
|
|
ID_INLINE bool Compare( const idMatX& a ) const; // exact compare, no epsilon
|
|
ID_INLINE bool Compare( const idMatX& a, const float epsilon ) const; // compare with epsilon
|
|
ID_INLINE bool operator==( const idMatX& a ) const; // exact compare, no epsilon
|
|
ID_INLINE bool operator!=( const idMatX& a ) const; // exact compare, no epsilon
|
|
|
|
ID_INLINE void SetSize( int rows, int columns ); // set the number of rows/columns
|
|
void ChangeSize( int rows, int columns, bool makeZero = false ); // change the size keeping data intact where possible
|
|
ID_INLINE void ChangeNumRows( int rows )
|
|
{
|
|
ChangeSize( rows, numColumns ); // set the number of rows/columns
|
|
}
|
|
int GetNumRows() const
|
|
{
|
|
return numRows; // get the number of rows
|
|
}
|
|
int GetNumColumns() const
|
|
{
|
|
return numColumns; // get the number of columns
|
|
}
|
|
ID_INLINE void SetData( int rows, int columns, float* data ); // set float array pointer
|
|
ID_INLINE void SetDataCacheLines( int rows, int columns, float* data, bool clear );// set float array pointer
|
|
ID_INLINE void Zero(); // clear matrix
|
|
ID_INLINE void Zero( int rows, int columns ); // set size and clear matrix
|
|
ID_INLINE void Identity(); // clear to identity matrix
|
|
ID_INLINE void Identity( int rows, int columns ); // set size and clear to identity matrix
|
|
ID_INLINE void Diag( const idVecX& v ); // create diagonal matrix from vector
|
|
ID_INLINE void Random( int seed, float l = 0.0f, float u = 1.0f ); // fill matrix with random values
|
|
ID_INLINE void Random( int rows, int columns, int seed, float l = 0.0f, float u = 1.0f );
|
|
ID_INLINE void Negate(); // (*this) = - (*this)
|
|
ID_INLINE void Clamp( float min, float max ); // clamp all values
|
|
ID_INLINE idMatX& SwapRows( int r1, int r2 ); // swap rows
|
|
ID_INLINE idMatX& SwapColumns( int r1, int r2 ); // swap columns
|
|
ID_INLINE idMatX& SwapRowsColumns( int r1, int r2 ); // swap rows and columns
|
|
idMatX& RemoveRow( int r ); // remove a row
|
|
idMatX& RemoveColumn( int r ); // remove a column
|
|
idMatX& RemoveRowColumn( int r ); // remove a row and column
|
|
ID_INLINE void ClearUpperTriangle(); // clear the upper triangle
|
|
ID_INLINE void ClearLowerTriangle(); // clear the lower triangle
|
|
void CopyLowerToUpperTriangle(); // copy the lower triangle to the upper triangle
|
|
ID_INLINE void SquareSubMatrix( const idMatX& m, int size ); // get square sub-matrix from 0,0 to size,size
|
|
ID_INLINE float MaxDifference( const idMatX& m ) const; // return maximum element difference between this and m
|
|
|
|
ID_INLINE bool IsSquare() const
|
|
{
|
|
return ( numRows == numColumns );
|
|
}
|
|
ID_INLINE bool IsZero( const float epsilon = MATRIX_EPSILON ) const;
|
|
ID_INLINE bool IsIdentity( const float epsilon = MATRIX_EPSILON ) const;
|
|
ID_INLINE bool IsDiagonal( const float epsilon = MATRIX_EPSILON ) const;
|
|
ID_INLINE bool IsTriDiagonal( const float epsilon = MATRIX_EPSILON ) const;
|
|
ID_INLINE bool IsSymmetric( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsOrthogonal( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsOrthonormal( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsPMatrix( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsZMatrix( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsPositiveDefinite( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsSymmetricPositiveDefinite( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsPositiveSemiDefinite( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsSymmetricPositiveSemiDefinite( const float epsilon = MATRIX_EPSILON ) const;
|
|
|
|
ID_INLINE float Trace() const; // returns product of diagonal elements
|
|
ID_INLINE float Determinant() const; // returns determinant of matrix
|
|
ID_INLINE idMatX Transpose() const; // returns transpose
|
|
ID_INLINE idMatX& TransposeSelf(); // transposes the matrix itself
|
|
ID_INLINE void Transpose( idMatX& dst ) const; // stores transpose in 'dst'
|
|
ID_INLINE idMatX Inverse() const; // returns the inverse ( m * m.Inverse() = identity )
|
|
ID_INLINE bool InverseSelf(); // returns false if determinant is zero
|
|
ID_INLINE idMatX InverseFast() const; // returns the inverse ( m * m.Inverse() = identity )
|
|
ID_INLINE bool InverseFastSelf(); // returns false if determinant is zero
|
|
ID_INLINE void Inverse( idMatX& dst ) const; // stores the inverse in 'dst' ( m * m.Inverse() = identity )
|
|
|
|
bool LowerTriangularInverse(); // in-place inversion, returns false if determinant is zero
|
|
bool UpperTriangularInverse(); // in-place inversion, returns false if determinant is zero
|
|
|
|
ID_INLINE void Subtract( const idMatX& a ); // (*this) -= a;
|
|
|
|
ID_INLINE idVecX Multiply( const idVecX& vec ) const; // (*this) * vec
|
|
ID_INLINE idVecX TransposeMultiply( const idVecX& vec ) const; // this->Transpose() * vec
|
|
|
|
ID_INLINE idMatX Multiply( const idMatX& a ) const; // (*this) * a
|
|
ID_INLINE idMatX TransposeMultiply( const idMatX& a ) const; // this->Transpose() * a
|
|
|
|
ID_INLINE void Multiply( idVecX& dst, const idVecX& vec ) const; // dst = (*this) * vec
|
|
ID_INLINE void MultiplyAdd( idVecX& dst, const idVecX& vec ) const; // dst += (*this) * vec
|
|
ID_INLINE void MultiplySub( idVecX& dst, const idVecX& vec ) const; // dst -= (*this) * vec
|
|
ID_INLINE void TransposeMultiply( idVecX& dst, const idVecX& vec ) const; // dst = this->Transpose() * vec
|
|
ID_INLINE void TransposeMultiplyAdd( idVecX& dst, const idVecX& vec ) const; // dst += this->Transpose() * vec
|
|
ID_INLINE void TransposeMultiplySub( idVecX& dst, const idVecX& vec ) const; // dst -= this->Transpose() * vec
|
|
|
|
ID_INLINE void Multiply( idMatX& dst, const idMatX& a ) const; // dst = (*this) * a
|
|
ID_INLINE void TransposeMultiply( idMatX& dst, const idMatX& a ) const; // dst = this->Transpose() * a
|
|
|
|
ID_INLINE int GetDimension() const; // returns total number of values in matrix
|
|
|
|
ID_INLINE const idVec6& SubVec6( int row ) const; // interpret beginning of row as a const idVec6
|
|
ID_INLINE idVec6& SubVec6( int row ); // interpret beginning of row as an idVec6
|
|
ID_INLINE const idVecX SubVecX( int row ) const; // interpret complete row as a const idVecX
|
|
ID_INLINE idVecX SubVecX( int row ); // interpret complete row as an idVecX
|
|
ID_INLINE const float* ToFloatPtr() const; // pointer to const matrix float array
|
|
ID_INLINE float* ToFloatPtr(); // pointer to matrix float array
|
|
const char* ToString( int precision = 2 ) const;
|
|
|
|
void Update_RankOne( const idVecX& v, const idVecX& w, float alpha );
|
|
void Update_RankOneSymmetric( const idVecX& v, float alpha );
|
|
void Update_RowColumn( const idVecX& v, const idVecX& w, int r );
|
|
void Update_RowColumnSymmetric( const idVecX& v, int r );
|
|
void Update_Increment( const idVecX& v, const idVecX& w );
|
|
void Update_IncrementSymmetric( const idVecX& v );
|
|
void Update_Decrement( int r );
|
|
|
|
bool Inverse_GaussJordan(); // invert in-place with Gauss-Jordan elimination
|
|
bool Inverse_UpdateRankOne( const idVecX& v, const idVecX& w, float alpha );
|
|
bool Inverse_UpdateRowColumn( const idVecX& v, const idVecX& w, int r );
|
|
bool Inverse_UpdateIncrement( const idVecX& v, const idVecX& w );
|
|
bool Inverse_UpdateDecrement( const idVecX& v, const idVecX& w, int r );
|
|
void Inverse_Solve( idVecX& x, const idVecX& b ) const;
|
|
|
|
bool LU_Factor( int* index, float* det = NULL ); // factor in-place: L * U
|
|
bool LU_UpdateRankOne( const idVecX& v, const idVecX& w, float alpha, int* index );
|
|
bool LU_UpdateRowColumn( const idVecX& v, const idVecX& w, int r, int* index );
|
|
bool LU_UpdateIncrement( const idVecX& v, const idVecX& w, int* index );
|
|
bool LU_UpdateDecrement( const idVecX& v, const idVecX& w, const idVecX& u, int r, int* index );
|
|
void LU_Solve( idVecX& x, const idVecX& b, const int* index ) const;
|
|
void LU_Inverse( idMatX& inv, const int* index ) const;
|
|
void LU_UnpackFactors( idMatX& L, idMatX& U ) const;
|
|
void LU_MultiplyFactors( idMatX& m, const int* index ) const;
|
|
|
|
bool QR_Factor( idVecX& c, idVecX& d ); // factor in-place: Q * R
|
|
bool QR_UpdateRankOne( idMatX& R, const idVecX& v, const idVecX& w, float alpha );
|
|
bool QR_UpdateRowColumn( idMatX& R, const idVecX& v, const idVecX& w, int r );
|
|
bool QR_UpdateIncrement( idMatX& R, const idVecX& v, const idVecX& w );
|
|
bool QR_UpdateDecrement( idMatX& R, const idVecX& v, const idVecX& w, int r );
|
|
void QR_Solve( idVecX& x, const idVecX& b, const idVecX& c, const idVecX& d ) const;
|
|
void QR_Solve( idVecX& x, const idVecX& b, const idMatX& R ) const;
|
|
void QR_Inverse( idMatX& inv, const idVecX& c, const idVecX& d ) const;
|
|
void QR_UnpackFactors( idMatX& Q, idMatX& R, const idVecX& c, const idVecX& d ) const;
|
|
void QR_MultiplyFactors( idMatX& m, const idVecX& c, const idVecX& d ) const;
|
|
|
|
bool SVD_Factor( idVecX& w, idMatX& V ); // factor in-place: U * Diag(w) * V.Transpose()
|
|
void SVD_Solve( idVecX& x, const idVecX& b, const idVecX& w, const idMatX& V ) const;
|
|
void SVD_Inverse( idMatX& inv, const idVecX& w, const idMatX& V ) const;
|
|
void SVD_MultiplyFactors( idMatX& m, const idVecX& w, const idMatX& V ) const;
|
|
|
|
bool Cholesky_Factor(); // factor in-place: L * L.Transpose()
|
|
bool Cholesky_UpdateRankOne( const idVecX& v, float alpha, int offset = 0 );
|
|
bool Cholesky_UpdateRowColumn( const idVecX& v, int r );
|
|
bool Cholesky_UpdateIncrement( const idVecX& v );
|
|
bool Cholesky_UpdateDecrement( const idVecX& v, int r );
|
|
void Cholesky_Solve( idVecX& x, const idVecX& b ) const;
|
|
void Cholesky_Inverse( idMatX& inv ) const;
|
|
void Cholesky_MultiplyFactors( idMatX& m ) const;
|
|
|
|
bool LDLT_Factor(); // factor in-place: L * D * L.Transpose()
|
|
bool LDLT_UpdateRankOne( const idVecX& v, float alpha, int offset = 0 );
|
|
bool LDLT_UpdateRowColumn( const idVecX& v, int r );
|
|
bool LDLT_UpdateIncrement( const idVecX& v );
|
|
bool LDLT_UpdateDecrement( const idVecX& v, int r );
|
|
void LDLT_Solve( idVecX& x, const idVecX& b ) const;
|
|
void LDLT_Inverse( idMatX& inv ) const;
|
|
void LDLT_UnpackFactors( idMatX& L, idMatX& D ) const;
|
|
void LDLT_MultiplyFactors( idMatX& m ) const;
|
|
|
|
void TriDiagonal_ClearTriangles();
|
|
bool TriDiagonal_Solve( idVecX& x, const idVecX& b ) const;
|
|
void TriDiagonal_Inverse( idMatX& inv ) const;
|
|
|
|
bool Eigen_SolveSymmetricTriDiagonal( idVecX& eigenValues );
|
|
bool Eigen_SolveSymmetric( idVecX& eigenValues );
|
|
bool Eigen_Solve( idVecX& realEigenValues, idVecX& imaginaryEigenValues );
|
|
void Eigen_SortIncreasing( idVecX& eigenValues );
|
|
void Eigen_SortDecreasing( idVecX& eigenValues );
|
|
|
|
static void Test();
|
|
|
|
private:
|
|
int numRows; // number of rows
|
|
int numColumns; // number of columns
|
|
int alloced; // floats allocated, if -1 then mat points to data set with SetData
|
|
float* mat; // memory the matrix is stored
|
|
|
|
static float temp[MATX_MAX_TEMP + 4]; // used to store intermediate results
|
|
static float* tempPtr; // pointer to 16 byte aligned temporary memory
|
|
static int tempIndex; // index into memory pool, wraps around
|
|
|
|
private:
|
|
void SetTempSize( int rows, int columns );
|
|
float DeterminantGeneric() const;
|
|
bool InverseSelfGeneric();
|
|
void QR_Rotate( idMatX& R, int i, float a, float b );
|
|
float Pythag( float a, float b ) const;
|
|
void SVD_BiDiag( idVecX& w, idVecX& rv1, float& anorm );
|
|
void SVD_InitialWV( idVecX& w, idMatX& V, idVecX& rv1 );
|
|
void HouseholderReduction( idVecX& diag, idVecX& subd );
|
|
bool QL( idVecX& diag, idVecX& subd );
|
|
void HessenbergReduction( idMatX& H );
|
|
void ComplexDivision( float xr, float xi, float yr, float yi, float& cdivr, float& cdivi );
|
|
bool HessenbergToRealSchur( idMatX& H, idVecX& realEigenValues, idVecX& imaginaryEigenValues );
|
|
};
|
|
|
|
/*
|
|
========================
|
|
idMatX::idMatX
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX::idMatX()
|
|
{
|
|
numRows = numColumns = alloced = 0;
|
|
mat = NULL;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::~idMatX
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX::~idMatX()
|
|
{
|
|
// if not temp memory
|
|
if( mat != NULL && ( mat < idMatX::tempPtr || mat > idMatX::tempPtr + MATX_MAX_TEMP ) && alloced != -1 )
|
|
{
|
|
Mem_Free16( mat );
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::idMatX
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX::idMatX( int rows, int columns )
|
|
{
|
|
numRows = numColumns = alloced = 0;
|
|
mat = NULL;
|
|
SetSize( rows, columns );
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::idMatX
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX::idMatX( const idMatX& other )
|
|
{
|
|
numRows = numColumns = alloced = 0;
|
|
mat = NULL;
|
|
Set( other.GetNumRows(), other.GetNumColumns(), other.ToFloatPtr() );
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::idMatX
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX::idMatX( int rows, int columns, float* src )
|
|
{
|
|
numRows = numColumns = alloced = 0;
|
|
mat = NULL;
|
|
SetData( rows, columns, src );
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Set
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::Set( int rows, int columns, const float* src )
|
|
{
|
|
SetSize( rows, columns );
|
|
memcpy( this->mat, src, rows * columns * sizeof( float ) );
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Set
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::Set( const idMat3& m1, const idMat3& m2 )
|
|
{
|
|
SetSize( 3, 6 );
|
|
for( int i = 0; i < 3; i++ )
|
|
{
|
|
for( int j = 0; j < 3; j++ )
|
|
{
|
|
mat[( i + 0 ) * numColumns + ( j + 0 )] = m1[i][j];
|
|
mat[( i + 0 ) * numColumns + ( j + 3 )] = m2[i][j];
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Set
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::Set( const idMat3& m1, const idMat3& m2, const idMat3& m3, const idMat3& m4 )
|
|
{
|
|
SetSize( 6, 6 );
|
|
for( int i = 0; i < 3; i++ )
|
|
{
|
|
for( int j = 0; j < 3; j++ )
|
|
{
|
|
mat[( i + 0 ) * numColumns + ( j + 0 )] = m1[i][j];
|
|
mat[( i + 0 ) * numColumns + ( j + 3 )] = m2[i][j];
|
|
mat[( i + 3 ) * numColumns + ( j + 0 )] = m3[i][j];
|
|
mat[( i + 3 ) * numColumns + ( j + 3 )] = m4[i][j];
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::operator[]
|
|
========================
|
|
*/
|
|
ID_INLINE const float* idMatX::operator[]( int index ) const
|
|
{
|
|
assert( ( index >= 0 ) && ( index < numRows ) );
|
|
return mat + index * numColumns;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::operator[]
|
|
========================
|
|
*/
|
|
ID_INLINE float* idMatX::operator[]( int index )
|
|
{
|
|
assert( ( index >= 0 ) && ( index < numRows ) );
|
|
return mat + index * numColumns;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::operator=
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX& idMatX::operator=( const idMatX& a )
|
|
{
|
|
SetSize( a.numRows, a.numColumns );
|
|
int s = a.numRows * a.numColumns;
|
|
#ifdef MATX_SIMD
|
|
for( int i = 0; i < s; i += 4 )
|
|
{
|
|
_mm_store_ps( mat + i, _mm_load_ps( a.mat + i ) );
|
|
}
|
|
#else
|
|
memcpy( mat, a.mat, s * sizeof( float ) );
|
|
#endif
|
|
idMatX::tempIndex = 0;
|
|
return *this;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::operator*
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX idMatX::operator*( const float a ) const
|
|
{
|
|
idMatX m;
|
|
|
|
m.SetTempSize( numRows, numColumns );
|
|
int s = numRows * numColumns;
|
|
#ifdef MATX_SIMD
|
|
__m128 va = _mm_load1_ps( & a );
|
|
for( int i = 0; i < s; i += 4 )
|
|
{
|
|
_mm_store_ps( m.mat + i, _mm_mul_ps( _mm_load_ps( mat + i ), va ) );
|
|
}
|
|
#else
|
|
for( int i = 0; i < s; i++ )
|
|
{
|
|
m.mat[i] = mat[i] * a;
|
|
}
|
|
#endif
|
|
return m;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::operator*
|
|
========================
|
|
*/
|
|
ID_INLINE idVecX idMatX::operator*( const idVecX& vec ) const
|
|
{
|
|
assert( numColumns == vec.GetSize() );
|
|
|
|
idVecX dst;
|
|
dst.SetTempSize( numRows );
|
|
Multiply( dst, vec );
|
|
return dst;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::operator*
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX idMatX::operator*( const idMatX& a ) const
|
|
{
|
|
assert( numColumns == a.numRows );
|
|
|
|
idMatX dst;
|
|
dst.SetTempSize( numRows, a.numColumns );
|
|
Multiply( dst, a );
|
|
return dst;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::operator+
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX idMatX::operator+( const idMatX& a ) const
|
|
{
|
|
idMatX m;
|
|
|
|
assert( numRows == a.numRows && numColumns == a.numColumns );
|
|
m.SetTempSize( numRows, numColumns );
|
|
int s = numRows * numColumns;
|
|
#ifdef MATX_SIMD
|
|
for( int i = 0; i < s; i += 4 )
|
|
{
|
|
_mm_store_ps( m.mat + i, _mm_add_ps( _mm_load_ps( mat + i ), _mm_load_ps( a.mat + i ) ) );
|
|
}
|
|
#else
|
|
for( int i = 0; i < s; i++ )
|
|
{
|
|
m.mat[i] = mat[i] + a.mat[i];
|
|
}
|
|
#endif
|
|
return m;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::operator-
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX idMatX::operator-( const idMatX& a ) const
|
|
{
|
|
idMatX m;
|
|
|
|
assert( numRows == a.numRows && numColumns == a.numColumns );
|
|
m.SetTempSize( numRows, numColumns );
|
|
int s = numRows * numColumns;
|
|
#ifdef MATX_SIMD
|
|
for( int i = 0; i < s; i += 4 )
|
|
{
|
|
_mm_store_ps( m.mat + i, _mm_sub_ps( _mm_load_ps( mat + i ), _mm_load_ps( a.mat + i ) ) );
|
|
}
|
|
#else
|
|
for( int i = 0; i < s; i++ )
|
|
{
|
|
m.mat[i] = mat[i] - a.mat[i];
|
|
}
|
|
#endif
|
|
return m;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::operator*=
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX& idMatX::operator*=( const float a )
|
|
{
|
|
int s = numRows * numColumns;
|
|
#ifdef MATX_SIMD
|
|
__m128 va = _mm_load1_ps( & a );
|
|
for( int i = 0; i < s; i += 4 )
|
|
{
|
|
_mm_store_ps( mat + i, _mm_mul_ps( _mm_load_ps( mat + i ), va ) );
|
|
}
|
|
#else
|
|
for( int i = 0; i < s; i++ )
|
|
{
|
|
mat[i] *= a;
|
|
}
|
|
#endif
|
|
idMatX::tempIndex = 0;
|
|
return *this;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::operator*=
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX& idMatX::operator*=( const idMatX& a )
|
|
{
|
|
*this = *this * a;
|
|
idMatX::tempIndex = 0;
|
|
return *this;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::operator+=
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX& idMatX::operator+=( const idMatX& a )
|
|
{
|
|
assert( numRows == a.numRows && numColumns == a.numColumns );
|
|
int s = numRows * numColumns;
|
|
#ifdef MATX_SIMD
|
|
for( int i = 0; i < s; i += 4 )
|
|
{
|
|
_mm_store_ps( mat + i, _mm_add_ps( _mm_load_ps( mat + i ), _mm_load_ps( a.mat + i ) ) );
|
|
}
|
|
#else
|
|
for( int i = 0; i < s; i++ )
|
|
{
|
|
mat[i] += a.mat[i];
|
|
}
|
|
#endif
|
|
idMatX::tempIndex = 0;
|
|
return *this;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::operator-=
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX& idMatX::operator-=( const idMatX& a )
|
|
{
|
|
assert( numRows == a.numRows && numColumns == a.numColumns );
|
|
int s = numRows * numColumns;
|
|
#ifdef MATX_SIMD
|
|
for( int i = 0; i < s; i += 4 )
|
|
{
|
|
_mm_store_ps( mat + i, _mm_sub_ps( _mm_load_ps( mat + i ), _mm_load_ps( a.mat + i ) ) );
|
|
}
|
|
#else
|
|
for( int i = 0; i < s; i++ )
|
|
{
|
|
mat[i] -= a.mat[i];
|
|
}
|
|
#endif
|
|
idMatX::tempIndex = 0;
|
|
return *this;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
operator*
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX operator*( const float a, idMatX const& m )
|
|
{
|
|
return m * a;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
operator*
|
|
========================
|
|
*/
|
|
ID_INLINE idVecX operator*( const idVecX& vec, const idMatX& m )
|
|
{
|
|
return m * vec;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
operator*=
|
|
========================
|
|
*/
|
|
ID_INLINE idVecX& operator*=( idVecX& vec, const idMatX& m )
|
|
{
|
|
vec = m * vec;
|
|
return vec;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Compare
|
|
========================
|
|
*/
|
|
ID_INLINE bool idMatX::Compare( const idMatX& a ) const
|
|
{
|
|
assert( numRows == a.numRows && numColumns == a.numColumns );
|
|
|
|
int s = numRows * numColumns;
|
|
for( int i = 0; i < s; i++ )
|
|
{
|
|
if( mat[i] != a.mat[i] )
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Compare
|
|
========================
|
|
*/
|
|
ID_INLINE bool idMatX::Compare( const idMatX& a, const float epsilon ) const
|
|
{
|
|
assert( numRows == a.numRows && numColumns == a.numColumns );
|
|
|
|
int s = numRows * numColumns;
|
|
for( int i = 0; i < s; i++ )
|
|
{
|
|
if( idMath::Fabs( mat[i] - a.mat[i] ) > epsilon )
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::operator==
|
|
========================
|
|
*/
|
|
ID_INLINE bool idMatX::operator==( const idMatX& a ) const
|
|
{
|
|
return Compare( a );
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::operator!=
|
|
========================
|
|
*/
|
|
ID_INLINE bool idMatX::operator!=( const idMatX& a ) const
|
|
{
|
|
return !Compare( a );
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::SetSize
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::SetSize( int rows, int columns )
|
|
{
|
|
if( rows != numRows || columns != numColumns || mat == NULL )
|
|
{
|
|
assert( mat < idMatX::tempPtr || mat > idMatX::tempPtr + MATX_MAX_TEMP );
|
|
int alloc = ( rows * columns + 3 ) & ~3;
|
|
if( alloc > alloced && alloced != -1 )
|
|
{
|
|
if( mat != NULL )
|
|
{
|
|
Mem_Free16( mat );
|
|
}
|
|
mat = ( float* ) Mem_Alloc16( alloc * sizeof( float ), TAG_MATH );
|
|
alloced = alloc;
|
|
}
|
|
numRows = rows;
|
|
numColumns = columns;
|
|
MATX_CLEAREND();
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::SetTempSize
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::SetTempSize( int rows, int columns )
|
|
{
|
|
int newSize;
|
|
|
|
newSize = ( rows * columns + 3 ) & ~3;
|
|
assert( newSize < MATX_MAX_TEMP );
|
|
if( idMatX::tempIndex + newSize > MATX_MAX_TEMP )
|
|
{
|
|
idMatX::tempIndex = 0;
|
|
}
|
|
mat = idMatX::tempPtr + idMatX::tempIndex;
|
|
idMatX::tempIndex += newSize;
|
|
alloced = newSize;
|
|
numRows = rows;
|
|
numColumns = columns;
|
|
MATX_CLEAREND();
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::SetData
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::SetData( int rows, int columns, float* data )
|
|
{
|
|
assert( mat < idMatX::tempPtr || mat > idMatX::tempPtr + MATX_MAX_TEMP );
|
|
if( mat != NULL && alloced != -1 )
|
|
{
|
|
Mem_Free16( mat );
|
|
}
|
|
// RB: changed UINT_PTR to uintptr_t
|
|
assert( ( ( ( uintptr_t ) data ) & 15 ) == 0 ); // data must be 16 byte aligned
|
|
// RB end
|
|
mat = data;
|
|
alloced = -1;
|
|
numRows = rows;
|
|
numColumns = columns;
|
|
MATX_CLEAREND();
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::SetDataCacheLines
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::SetDataCacheLines( int rows, int columns, float* data, bool clear )
|
|
{
|
|
if( mat != NULL && alloced != -1 )
|
|
{
|
|
Mem_Free( mat );
|
|
}
|
|
|
|
// RB: changed UINT_PTR to uintptr_t
|
|
assert( ( ( ( uintptr_t ) data ) & 127 ) == 0 ); // data must be 128 byte aligned
|
|
// RB end
|
|
|
|
mat = data;
|
|
alloced = -1;
|
|
numRows = rows;
|
|
numColumns = columns;
|
|
|
|
if( clear )
|
|
{
|
|
int size = numRows * numColumns * sizeof( float );
|
|
for( int i = 0; i < size; i += CACHE_LINE_SIZE )
|
|
{
|
|
ZeroCacheLine( mat, i );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
MATX_CLEAREND();
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Zero
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::Zero()
|
|
{
|
|
int s = numRows * numColumns;
|
|
#ifdef MATX_SIMD
|
|
for( int i = 0; i < s; i += 4 )
|
|
{
|
|
_mm_store_ps( mat + i, _mm_setzero_ps() );
|
|
}
|
|
#else
|
|
s;
|
|
memset( mat, 0, numRows * numColumns * sizeof( float ) );
|
|
#endif
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Zero
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::Zero( int rows, int columns )
|
|
{
|
|
SetSize( rows, columns );
|
|
Zero();
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Identity
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::Identity()
|
|
{
|
|
assert( numRows == numColumns );
|
|
Zero();
|
|
for( int i = 0; i < numRows; i++ )
|
|
{
|
|
mat[i * numColumns + i] = 1.0f;
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Identity
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::Identity( int rows, int columns )
|
|
{
|
|
assert( rows == columns );
|
|
SetSize( rows, columns );
|
|
idMatX::Identity();
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Diag
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::Diag( const idVecX& v )
|
|
{
|
|
Zero( v.GetSize(), v.GetSize() );
|
|
for( int i = 0; i < v.GetSize(); i++ )
|
|
{
|
|
mat[i * numColumns + i] = v[i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Random
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::Random( int seed, float l, float u )
|
|
{
|
|
idRandom rnd( seed );
|
|
|
|
float c = u - l;
|
|
int s = numRows * numColumns;
|
|
for( int i = 0; i < s; i++ )
|
|
{
|
|
mat[i] = l + rnd.RandomFloat() * c;
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Random
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::Random( int rows, int columns, int seed, float l, float u )
|
|
{
|
|
idRandom rnd( seed );
|
|
|
|
SetSize( rows, columns );
|
|
float c = u - l;
|
|
int s = numRows * numColumns;
|
|
for( int i = 0; i < s; i++ )
|
|
{
|
|
mat[i] = l + rnd.RandomFloat() * c;
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Negate
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::Negate()
|
|
{
|
|
int s = numRows * numColumns;
|
|
#ifdef MATX_SIMD
|
|
ALIGN16( const unsigned int signBit[4] ) = { IEEE_FLT_SIGN_MASK, IEEE_FLT_SIGN_MASK, IEEE_FLT_SIGN_MASK, IEEE_FLT_SIGN_MASK };
|
|
for( int i = 0; i < s; i += 4 )
|
|
{
|
|
_mm_store_ps( mat + i, _mm_xor_ps( _mm_load_ps( mat + i ), ( __m128& ) signBit[0] ) );
|
|
}
|
|
#else
|
|
for( int i = 0; i < s; i++ )
|
|
{
|
|
mat[i] = -mat[i];
|
|
}
|
|
#endif
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Clamp
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::Clamp( float min, float max )
|
|
{
|
|
int s = numRows * numColumns;
|
|
for( int i = 0; i < s; i++ )
|
|
{
|
|
if( mat[i] < min )
|
|
{
|
|
mat[i] = min;
|
|
}
|
|
else if( mat[i] > max )
|
|
{
|
|
mat[i] = max;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::SwapRows
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX& idMatX::SwapRows( int r1, int r2 )
|
|
{
|
|
float* ptr1 = mat + r1 * numColumns;
|
|
float* ptr2 = mat + r2 * numColumns;
|
|
for( int i = 0; i < numColumns; i++ )
|
|
{
|
|
SwapValues( ptr1[i], ptr2[i] );
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::SwapColumns
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX& idMatX::SwapColumns( int r1, int r2 )
|
|
{
|
|
float* ptr = mat;
|
|
for( int i = 0; i < numRows; i++, ptr += numColumns )
|
|
{
|
|
SwapValues( ptr[r1], ptr[r2] );
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::SwapRowsColumns
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX& idMatX::SwapRowsColumns( int r1, int r2 )
|
|
{
|
|
SwapRows( r1, r2 );
|
|
SwapColumns( r1, r2 );
|
|
return *this;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::ClearUpperTriangle
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::ClearUpperTriangle()
|
|
{
|
|
assert( numRows == numColumns );
|
|
for( int i = numRows - 2; i >= 0; i-- )
|
|
{
|
|
memset( mat + i * numColumns + i + 1, 0, ( numColumns - 1 - i ) * sizeof( float ) );
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::ClearLowerTriangle
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::ClearLowerTriangle()
|
|
{
|
|
assert( numRows == numColumns );
|
|
for( int i = 1; i < numRows; i++ )
|
|
{
|
|
memset( mat + i * numColumns, 0, i * sizeof( float ) );
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::SquareSubMatrix
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::SquareSubMatrix( const idMatX& m, int size )
|
|
{
|
|
assert( size <= m.numRows && size <= m.numColumns );
|
|
SetSize( size, size );
|
|
for( int i = 0; i < size; i++ )
|
|
{
|
|
memcpy( mat + i * numColumns, m.mat + i * m.numColumns, size * sizeof( float ) );
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::MaxDifference
|
|
========================
|
|
*/
|
|
ID_INLINE float idMatX::MaxDifference( const idMatX& m ) const
|
|
{
|
|
assert( numRows == m.numRows && numColumns == m.numColumns );
|
|
|
|
float maxDiff = -1.0f;
|
|
for( int i = 0; i < numRows; i++ )
|
|
{
|
|
for( int j = 0; j < numColumns; j++ )
|
|
{
|
|
float diff = idMath::Fabs( mat[ i * numColumns + j ] - m[i][j] );
|
|
if( maxDiff < 0.0f || diff > maxDiff )
|
|
{
|
|
maxDiff = diff;
|
|
}
|
|
}
|
|
}
|
|
return maxDiff;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::IsZero
|
|
========================
|
|
*/
|
|
ID_INLINE bool idMatX::IsZero( const float epsilon ) const
|
|
{
|
|
// returns true if (*this) == Zero
|
|
for( int i = 0; i < numRows; i++ )
|
|
{
|
|
for( int j = 0; j < numColumns; j++ )
|
|
{
|
|
if( idMath::Fabs( mat[i * numColumns + j] ) > epsilon )
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::IsIdentity
|
|
========================
|
|
*/
|
|
ID_INLINE bool idMatX::IsIdentity( const float epsilon ) const
|
|
{
|
|
// returns true if (*this) == Identity
|
|
assert( numRows == numColumns );
|
|
for( int i = 0; i < numRows; i++ )
|
|
{
|
|
for( int j = 0; j < numColumns; j++ )
|
|
{
|
|
if( idMath::Fabs( mat[i * numColumns + j] - ( float )( i == j ) ) > epsilon )
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::IsDiagonal
|
|
========================
|
|
*/
|
|
ID_INLINE bool idMatX::IsDiagonal( const float epsilon ) const
|
|
{
|
|
// returns true if all elements are zero except for the elements on the diagonal
|
|
assert( numRows == numColumns );
|
|
for( int i = 0; i < numRows; i++ )
|
|
{
|
|
for( int j = 0; j < numColumns; j++ )
|
|
{
|
|
if( i != j && idMath::Fabs( mat[i * numColumns + j] ) > epsilon )
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::IsTriDiagonal
|
|
========================
|
|
*/
|
|
ID_INLINE bool idMatX::IsTriDiagonal( const float epsilon ) const
|
|
{
|
|
// returns true if all elements are zero except for the elements on the diagonal plus or minus one column
|
|
|
|
if( numRows != numColumns )
|
|
{
|
|
return false;
|
|
}
|
|
for( int i = 0; i < numRows - 2; i++ )
|
|
{
|
|
for( int j = i + 2; j < numColumns; j++ )
|
|
{
|
|
if( idMath::Fabs( ( *this )[i][j] ) > epsilon )
|
|
{
|
|
return false;
|
|
}
|
|
if( idMath::Fabs( ( *this )[j][i] ) > epsilon )
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::IsSymmetric
|
|
========================
|
|
*/
|
|
ID_INLINE bool idMatX::IsSymmetric( const float epsilon ) const
|
|
{
|
|
// (*this)[i][j] == (*this)[j][i]
|
|
if( numRows != numColumns )
|
|
{
|
|
return false;
|
|
}
|
|
for( int i = 0; i < numRows; i++ )
|
|
{
|
|
for( int j = 0; j < numColumns; j++ )
|
|
{
|
|
if( idMath::Fabs( mat[ i * numColumns + j ] - mat[ j * numColumns + i ] ) > epsilon )
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Trace
|
|
========================
|
|
*/
|
|
ID_INLINE float idMatX::Trace() const
|
|
{
|
|
float trace = 0.0f;
|
|
|
|
assert( numRows == numColumns );
|
|
|
|
// sum of elements on the diagonal
|
|
for( int i = 0; i < numRows; i++ )
|
|
{
|
|
trace += mat[i * numRows + i];
|
|
}
|
|
return trace;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Determinant
|
|
========================
|
|
*/
|
|
ID_INLINE float idMatX::Determinant() const
|
|
{
|
|
|
|
assert( numRows == numColumns );
|
|
|
|
switch( numRows )
|
|
{
|
|
case 1:
|
|
return mat[0];
|
|
case 2:
|
|
return reinterpret_cast<const idMat2*>( mat )->Determinant();
|
|
case 3:
|
|
return reinterpret_cast<const idMat3*>( mat )->Determinant();
|
|
case 4:
|
|
return reinterpret_cast<const idMat4*>( mat )->Determinant();
|
|
case 5:
|
|
return reinterpret_cast<const idMat5*>( mat )->Determinant();
|
|
case 6:
|
|
return reinterpret_cast<const idMat6*>( mat )->Determinant();
|
|
default:
|
|
return DeterminantGeneric();
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Transpose
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX idMatX::Transpose() const
|
|
{
|
|
idMatX transpose;
|
|
|
|
transpose.SetTempSize( numColumns, numRows );
|
|
|
|
for( int i = 0; i < numRows; i++ )
|
|
{
|
|
for( int j = 0; j < numColumns; j++ )
|
|
{
|
|
transpose.mat[j * transpose.numColumns + i] = mat[i * numColumns + j];
|
|
}
|
|
}
|
|
|
|
return transpose;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::TransposeSelf
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX& idMatX::TransposeSelf()
|
|
{
|
|
*this = Transpose();
|
|
return *this;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Transpose
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::Transpose( idMatX& dst ) const
|
|
{
|
|
dst = Transpose();
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Inverse
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX idMatX::Inverse() const
|
|
{
|
|
idMatX invMat;
|
|
|
|
invMat.SetTempSize( numRows, numColumns );
|
|
memcpy( invMat.mat, mat, numRows * numColumns * sizeof( float ) );
|
|
verify( invMat.InverseSelf() );
|
|
return invMat;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::InverseSelf
|
|
========================
|
|
*/
|
|
ID_INLINE bool idMatX::InverseSelf()
|
|
{
|
|
|
|
assert( numRows == numColumns );
|
|
|
|
switch( numRows )
|
|
{
|
|
case 1:
|
|
if( idMath::Fabs( mat[0] ) < MATRIX_INVERSE_EPSILON )
|
|
{
|
|
return false;
|
|
}
|
|
mat[0] = 1.0f / mat[0];
|
|
return true;
|
|
case 2:
|
|
return reinterpret_cast<idMat2*>( mat )->InverseSelf();
|
|
case 3:
|
|
return reinterpret_cast<idMat3*>( mat )->InverseSelf();
|
|
case 4:
|
|
return reinterpret_cast<idMat4*>( mat )->InverseSelf();
|
|
case 5:
|
|
return reinterpret_cast<idMat5*>( mat )->InverseSelf();
|
|
case 6:
|
|
return reinterpret_cast<idMat6*>( mat )->InverseSelf();
|
|
default:
|
|
return InverseSelfGeneric();
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::InverseFast
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX idMatX::InverseFast() const
|
|
{
|
|
idMatX invMat;
|
|
|
|
invMat.SetTempSize( numRows, numColumns );
|
|
memcpy( invMat.mat, mat, numRows * numColumns * sizeof( float ) );
|
|
verify( invMat.InverseFastSelf() );
|
|
return invMat;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::InverseFastSelf
|
|
========================
|
|
*/
|
|
ID_INLINE bool idMatX::InverseFastSelf()
|
|
{
|
|
|
|
assert( numRows == numColumns );
|
|
|
|
switch( numRows )
|
|
{
|
|
case 1:
|
|
if( idMath::Fabs( mat[0] ) < MATRIX_INVERSE_EPSILON )
|
|
{
|
|
return false;
|
|
}
|
|
mat[0] = 1.0f / mat[0];
|
|
return true;
|
|
case 2:
|
|
return reinterpret_cast<idMat2*>( mat )->InverseFastSelf();
|
|
case 3:
|
|
return reinterpret_cast<idMat3*>( mat )->InverseFastSelf();
|
|
case 4:
|
|
return reinterpret_cast<idMat4*>( mat )->InverseFastSelf();
|
|
case 5:
|
|
return reinterpret_cast<idMat5*>( mat )->InverseFastSelf();
|
|
case 6:
|
|
return reinterpret_cast<idMat6*>( mat )->InverseFastSelf();
|
|
default:
|
|
return InverseSelfGeneric();
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Inverse
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::Inverse( idMatX& dst ) const
|
|
{
|
|
dst = InverseFast();
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Subtract
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::Subtract( const idMatX& a )
|
|
{
|
|
( *this ) -= a;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Multiply
|
|
========================
|
|
*/
|
|
ID_INLINE idVecX idMatX::Multiply( const idVecX& vec ) const
|
|
{
|
|
assert( numColumns == vec.GetSize() );
|
|
|
|
idVecX dst;
|
|
dst.SetTempSize( numRows );
|
|
Multiply( dst, vec );
|
|
return dst;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Multiply
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX idMatX::Multiply( const idMatX& a ) const
|
|
{
|
|
assert( numColumns == a.numRows );
|
|
|
|
idMatX dst;
|
|
dst.SetTempSize( numRows, a.numColumns );
|
|
Multiply( dst, a );
|
|
return dst;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::TransposeMultiply
|
|
========================
|
|
*/
|
|
ID_INLINE idVecX idMatX::TransposeMultiply( const idVecX& vec ) const
|
|
{
|
|
assert( numRows == vec.GetSize() );
|
|
|
|
idVecX dst;
|
|
dst.SetTempSize( numColumns );
|
|
TransposeMultiply( dst, vec );
|
|
return dst;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::TransposeMultiply
|
|
========================
|
|
*/
|
|
ID_INLINE idMatX idMatX::TransposeMultiply( const idMatX& a ) const
|
|
{
|
|
assert( numRows == a.numRows );
|
|
|
|
idMatX dst;
|
|
dst.SetTempSize( numColumns, a.numColumns );
|
|
TransposeMultiply( dst, a );
|
|
return dst;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Multiply
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::Multiply( idVecX& dst, const idVecX& vec ) const
|
|
{
|
|
dst.SetSize( numRows );
|
|
const float* mPtr = mat;
|
|
const float* vPtr = vec.ToFloatPtr();
|
|
float* dstPtr = dst.ToFloatPtr();
|
|
float* temp = ( float* )_alloca16( numRows * sizeof( float ) );
|
|
for( int i = 0; i < numRows; i++ )
|
|
{
|
|
float sum = mPtr[0] * vPtr[0];
|
|
for( int j = 1; j < numColumns; j++ )
|
|
{
|
|
sum += mPtr[j] * vPtr[j];
|
|
}
|
|
temp[i] = sum;
|
|
mPtr += numColumns;
|
|
}
|
|
for( int i = 0; i < numRows; i++ )
|
|
{
|
|
dstPtr[i] = temp[i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::MultiplyAdd
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::MultiplyAdd( idVecX& dst, const idVecX& vec ) const
|
|
{
|
|
assert( dst.GetSize() == numRows );
|
|
const float* mPtr = mat;
|
|
const float* vPtr = vec.ToFloatPtr();
|
|
float* dstPtr = dst.ToFloatPtr();
|
|
float* temp = ( float* )_alloca16( numRows * sizeof( float ) );
|
|
for( int i = 0; i < numRows; i++ )
|
|
{
|
|
float sum = mPtr[0] * vPtr[0];
|
|
for( int j = 1; j < numColumns; j++ )
|
|
{
|
|
sum += mPtr[j] * vPtr[j];
|
|
}
|
|
temp[i] = dstPtr[i] + sum;
|
|
mPtr += numColumns;
|
|
}
|
|
for( int i = 0; i < numRows; i++ )
|
|
{
|
|
dstPtr[i] = temp[i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::MultiplySub
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::MultiplySub( idVecX& dst, const idVecX& vec ) const
|
|
{
|
|
assert( dst.GetSize() == numRows );
|
|
const float* mPtr = mat;
|
|
const float* vPtr = vec.ToFloatPtr();
|
|
float* dstPtr = dst.ToFloatPtr();
|
|
float* temp = ( float* )_alloca16( numRows * sizeof( float ) );
|
|
for( int i = 0; i < numRows; i++ )
|
|
{
|
|
float sum = mPtr[0] * vPtr[0];
|
|
for( int j = 1; j < numColumns; j++ )
|
|
{
|
|
sum += mPtr[j] * vPtr[j];
|
|
}
|
|
temp[i] = dstPtr[i] - sum;
|
|
mPtr += numColumns;
|
|
}
|
|
for( int i = 0; i < numRows; i++ )
|
|
{
|
|
dstPtr[i] = temp[i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::TransposeMultiply
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::TransposeMultiply( idVecX& dst, const idVecX& vec ) const
|
|
{
|
|
dst.SetSize( numColumns );
|
|
const float* vPtr = vec.ToFloatPtr();
|
|
float* dstPtr = dst.ToFloatPtr();
|
|
float* temp = ( float* )_alloca16( numColumns * sizeof( float ) );
|
|
for( int i = 0; i < numColumns; i++ )
|
|
{
|
|
const float* mPtr = mat + i;
|
|
float sum = mPtr[0] * vPtr[0];
|
|
for( int j = 1; j < numRows; j++ )
|
|
{
|
|
mPtr += numColumns;
|
|
sum += mPtr[0] * vPtr[j];
|
|
}
|
|
temp[i] = sum;
|
|
}
|
|
for( int i = 0; i < numColumns; i++ )
|
|
{
|
|
dstPtr[i] = temp[i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::TransposeMultiplyAdd
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::TransposeMultiplyAdd( idVecX& dst, const idVecX& vec ) const
|
|
{
|
|
assert( dst.GetSize() == numColumns );
|
|
const float* vPtr = vec.ToFloatPtr();
|
|
float* dstPtr = dst.ToFloatPtr();
|
|
float* temp = ( float* )_alloca16( numColumns * sizeof( float ) );
|
|
for( int i = 0; i < numColumns; i++ )
|
|
{
|
|
const float* mPtr = mat + i;
|
|
float sum = mPtr[0] * vPtr[0];
|
|
for( int j = 1; j < numRows; j++ )
|
|
{
|
|
mPtr += numColumns;
|
|
sum += mPtr[0] * vPtr[j];
|
|
}
|
|
temp[i] = dstPtr[i] + sum;
|
|
}
|
|
for( int i = 0; i < numColumns; i++ )
|
|
{
|
|
dstPtr[i] = temp[i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::TransposeMultiplySub
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::TransposeMultiplySub( idVecX& dst, const idVecX& vec ) const
|
|
{
|
|
assert( dst.GetSize() == numColumns );
|
|
const float* vPtr = vec.ToFloatPtr();
|
|
float* dstPtr = dst.ToFloatPtr();
|
|
float* temp = ( float* )_alloca16( numColumns * sizeof( float ) );
|
|
for( int i = 0; i < numColumns; i++ )
|
|
{
|
|
const float* mPtr = mat + i;
|
|
float sum = mPtr[0] * vPtr[0];
|
|
for( int j = 1; j < numRows; j++ )
|
|
{
|
|
mPtr += numColumns;
|
|
sum += mPtr[0] * vPtr[j];
|
|
}
|
|
temp[i] = dstPtr[i] - sum;
|
|
}
|
|
for( int i = 0; i < numColumns; i++ )
|
|
{
|
|
dstPtr[i] = temp[i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::Multiply
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::Multiply( idMatX& dst, const idMatX& a ) const
|
|
{
|
|
assert( numColumns == a.numRows );
|
|
assert( &dst != &a && &dst != this );
|
|
|
|
dst.SetSize( numRows, a.numColumns );
|
|
float* dstPtr = dst.ToFloatPtr();
|
|
const float* m1Ptr = ToFloatPtr();
|
|
int k = numRows;
|
|
int l = a.GetNumColumns();
|
|
for( int i = 0; i < k; i++ )
|
|
{
|
|
for( int j = 0; j < l; j++ )
|
|
{
|
|
const float* m2Ptr = a.ToFloatPtr() + j;
|
|
float sum = m1Ptr[0] * m2Ptr[0];
|
|
for( int n = 1; n < numColumns; n++ )
|
|
{
|
|
m2Ptr += l;
|
|
sum += m1Ptr[n] * m2Ptr[0];
|
|
}
|
|
*dstPtr++ = sum;
|
|
}
|
|
m1Ptr += numColumns;
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::TransposeMultiply
|
|
========================
|
|
*/
|
|
ID_INLINE void idMatX::TransposeMultiply( idMatX& dst, const idMatX& a ) const
|
|
{
|
|
assert( numRows == a.numRows );
|
|
assert( &dst != &a && &dst != this );
|
|
|
|
dst.SetSize( numColumns, a.numColumns );
|
|
float* dstPtr = dst.ToFloatPtr();
|
|
int k = numColumns;
|
|
int l = a.numColumns;
|
|
for( int i = 0; i < k; i++ )
|
|
{
|
|
for( int j = 0; j < l; j++ )
|
|
{
|
|
const float* m1Ptr = ToFloatPtr() + i;
|
|
const float* m2Ptr = a.ToFloatPtr() + j;
|
|
float sum = m1Ptr[0] * m2Ptr[0];
|
|
for( int n = 1; n < numRows; n++ )
|
|
{
|
|
m1Ptr += numColumns;
|
|
m2Ptr += a.numColumns;
|
|
sum += m1Ptr[0] * m2Ptr[0];
|
|
}
|
|
*dstPtr++ = sum;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::GetDimension
|
|
========================
|
|
*/
|
|
ID_INLINE int idMatX::GetDimension() const
|
|
{
|
|
return numRows * numColumns;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::SubVec6
|
|
========================
|
|
*/
|
|
ID_INLINE const idVec6& idMatX::SubVec6( int row ) const
|
|
{
|
|
assert( numColumns >= 6 && row >= 0 && row < numRows );
|
|
return *reinterpret_cast<const idVec6*>( mat + row * numColumns );
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::SubVec6
|
|
========================
|
|
*/
|
|
ID_INLINE idVec6& idMatX::SubVec6( int row )
|
|
{
|
|
assert( numColumns >= 6 && row >= 0 && row < numRows );
|
|
return *reinterpret_cast<idVec6*>( mat + row * numColumns );
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::SubVecX
|
|
========================
|
|
*/
|
|
ID_INLINE const idVecX idMatX::SubVecX( int row ) const
|
|
{
|
|
idVecX v;
|
|
assert( row >= 0 && row < numRows );
|
|
v.SetData( numColumns, mat + row * numColumns );
|
|
return v;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::SubVecX
|
|
========================
|
|
*/
|
|
ID_INLINE idVecX idMatX::SubVecX( int row )
|
|
{
|
|
idVecX v;
|
|
assert( row >= 0 && row < numRows );
|
|
v.SetData( numColumns, mat + row * numColumns );
|
|
return v;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::ToFloatPtr
|
|
========================
|
|
*/
|
|
ID_INLINE const float* idMatX::ToFloatPtr() const
|
|
{
|
|
return mat;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::ToFloatPtr
|
|
========================
|
|
*/
|
|
ID_INLINE float* idMatX::ToFloatPtr()
|
|
{
|
|
return mat;
|
|
}
|
|
|
|
#endif // !__MATH_MATRIXX_H__
|