You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
385 lines
12 KiB
385 lines
12 KiB
//========= Copyright Valve Corporation, All rights reserved. ============// |
|
// |
|
// Purpose: |
|
// |
|
// A set of generic, template-based matrix functions. |
|
//===========================================================================// |
|
|
|
#ifndef MATRIXMATH_H |
|
#define MATRIXMATH_H |
|
|
|
#include <stdarg.h> |
|
|
|
// The operations in this file can perform basic matrix operations on matrices represented |
|
// using any class that supports the necessary operations: |
|
// |
|
// .Element( row, col ) - return the element at a given matrox position |
|
// .SetElement( row, col, val ) - modify an element |
|
// .Width(), .Height() - get dimensions |
|
// .SetDimensions( nrows, ncols) - set a matrix to be un-initted and the appropriate size |
|
// |
|
// Generally, vectors can be used with these functions by using N x 1 matrices to represent them. |
|
// Matrices are addressed as row, column, and indices are 0-based |
|
// |
|
// |
|
// Note that the template versions of these routines are defined for generality - it is expected |
|
// that template specialization is used for common high performance cases. |
|
|
|
namespace MatrixMath |
|
{ |
|
/// M *= flScaleValue |
|
template<class MATRIXCLASS> |
|
void ScaleMatrix( MATRIXCLASS &matrix, float flScaleValue ) |
|
{ |
|
for( int i = 0; i < matrix.Height(); i++ ) |
|
{ |
|
for( int j = 0; j < matrix.Width(); j++ ) |
|
{ |
|
matrix.SetElement( i, j, flScaleValue * matrix.Element( i, j ) ); |
|
} |
|
} |
|
} |
|
|
|
/// AppendElementToMatrix - same as setting the element, except only works when all calls |
|
/// happen in top to bottom left to right order, end you have to call FinishedAppending when |
|
/// done. For normal matrix classes this is not different then SetElement, but for |
|
/// CSparseMatrix, it is an accelerated way to fill a matrix from scratch. |
|
template<class MATRIXCLASS> |
|
FORCEINLINE void AppendElement( MATRIXCLASS &matrix, int nRow, int nCol, float flValue ) |
|
{ |
|
matrix.SetElement( nRow, nCol, flValue ); // default implementation |
|
} |
|
|
|
template<class MATRIXCLASS> |
|
FORCEINLINE void FinishedAppending( MATRIXCLASS &matrix ) {} // default implementation |
|
|
|
/// M += fl |
|
template<class MATRIXCLASS> |
|
void AddToMatrix( MATRIXCLASS &matrix, float flAddend ) |
|
{ |
|
for( int i = 0; i < matrix.Height(); i++ ) |
|
{ |
|
for( int j = 0; j < matrix.Width(); j++ ) |
|
{ |
|
matrix.SetElement( i, j, flAddend + matrix.Element( i, j ) ); |
|
} |
|
} |
|
} |
|
|
|
/// transpose |
|
template<class MATRIXCLASSIN, class MATRIXCLASSOUT> |
|
void TransposeMatrix( MATRIXCLASSIN const &matrixIn, MATRIXCLASSOUT *pMatrixOut ) |
|
{ |
|
pMatrixOut->SetDimensions( matrixIn.Width(), matrixIn.Height() ); |
|
for( int i = 0; i < pMatrixOut->Height(); i++ ) |
|
{ |
|
for( int j = 0; j < pMatrixOut->Width(); j++ ) |
|
{ |
|
AppendElement( *pMatrixOut, i, j, matrixIn.Element( j, i ) ); |
|
} |
|
} |
|
FinishedAppending( *pMatrixOut ); |
|
} |
|
|
|
/// copy |
|
template<class MATRIXCLASSIN, class MATRIXCLASSOUT> |
|
void CopyMatrix( MATRIXCLASSIN const &matrixIn, MATRIXCLASSOUT *pMatrixOut ) |
|
{ |
|
pMatrixOut->SetDimensions( matrixIn.Height(), matrixIn.Width() ); |
|
for( int i = 0; i < matrixIn.Height(); i++ ) |
|
{ |
|
for( int j = 0; j < matrixIn.Width(); j++ ) |
|
{ |
|
AppendElement( *pMatrixOut, i, j, matrixIn.Element( i, j ) ); |
|
} |
|
} |
|
FinishedAppending( *pMatrixOut ); |
|
} |
|
|
|
|
|
|
|
/// M+=M |
|
template<class MATRIXCLASSIN, class MATRIXCLASSOUT> |
|
void AddMatrixToMatrix( MATRIXCLASSIN const &matrixIn, MATRIXCLASSOUT *pMatrixOut ) |
|
{ |
|
for( int i = 0; i < matrixIn.Height(); i++ ) |
|
{ |
|
for( int j = 0; j < matrixIn.Width(); j++ ) |
|
{ |
|
pMatrixOut->SetElement( i, j, pMatrixOut->Element( i, j ) + matrixIn.Element( i, j ) ); |
|
} |
|
} |
|
} |
|
|
|
// M += scale * M |
|
template<class MATRIXCLASSIN, class MATRIXCLASSOUT> |
|
void AddScaledMatrixToMatrix( float flScale, MATRIXCLASSIN const &matrixIn, MATRIXCLASSOUT *pMatrixOut ) |
|
{ |
|
for( int i = 0; i < matrixIn.Height(); i++ ) |
|
{ |
|
for( int j = 0; j < matrixIn.Width(); j++ ) |
|
{ |
|
pMatrixOut->SetElement( i, j, pMatrixOut->Element( i, j ) + flScale * matrixIn.Element( i, j ) ); |
|
} |
|
} |
|
} |
|
|
|
|
|
// simple way to initialize a matrix with constants from code. |
|
template<class MATRIXCLASSOUT> |
|
void SetMatrixToIdentity( MATRIXCLASSOUT *pMatrixOut, float flDiagonalValue = 1.0 ) |
|
{ |
|
for( int i = 0; i < pMatrixOut->Height(); i++ ) |
|
{ |
|
for( int j = 0; j < pMatrixOut->Width(); j++ ) |
|
{ |
|
AppendElement( *pMatrixOut, i, j, ( i == j ) ? flDiagonalValue : 0 ); |
|
} |
|
} |
|
FinishedAppending( *pMatrixOut ); |
|
} |
|
|
|
//// simple way to initialize a matrix with constants from code |
|
template<class MATRIXCLASSOUT> |
|
void SetMatrixValues( MATRIXCLASSOUT *pMatrix, int nRows, int nCols, ... ) |
|
{ |
|
va_list argPtr; |
|
va_start( argPtr, nCols ); |
|
|
|
pMatrix->SetDimensions( nRows, nCols ); |
|
for( int nRow = 0; nRow < nRows; nRow++ ) |
|
{ |
|
for( int nCol = 0; nCol < nCols; nCol++ ) |
|
{ |
|
double flNewValue = va_arg( argPtr, double ); |
|
pMatrix->SetElement( nRow, nCol, flNewValue ); |
|
} |
|
} |
|
va_end( argPtr ); |
|
} |
|
|
|
|
|
/// row and colum accessors. treat a row or a column as a column vector |
|
template<class MATRIXTYPE> class MatrixRowAccessor |
|
{ |
|
public: |
|
FORCEINLINE MatrixRowAccessor( MATRIXTYPE const &matrix, int nRow ) |
|
{ |
|
m_pMatrix = &matrix; |
|
m_nRow = nRow; |
|
} |
|
|
|
FORCEINLINE float Element( int nRow, int nCol ) const |
|
{ |
|
Assert( nCol == 0 ); |
|
return m_pMatrix->Element( m_nRow, nRow ); |
|
} |
|
|
|
FORCEINLINE int Width( void ) const { return 1; }; |
|
FORCEINLINE int Height( void ) const { return m_pMatrix->Width(); } |
|
|
|
private: |
|
MATRIXTYPE const *m_pMatrix; |
|
int m_nRow; |
|
}; |
|
|
|
template<class MATRIXTYPE> class MatrixColumnAccessor |
|
{ |
|
public: |
|
FORCEINLINE MatrixColumnAccessor( MATRIXTYPE const &matrix, int nColumn ) |
|
{ |
|
m_pMatrix = &matrix; |
|
m_nColumn = nColumn; |
|
} |
|
|
|
FORCEINLINE float Element( int nRow, int nColumn ) const |
|
{ |
|
Assert( nColumn == 0 ); |
|
return m_pMatrix->Element( nRow, m_nColumn ); |
|
} |
|
|
|
FORCEINLINE int Width( void ) const { return 1; } |
|
FORCEINLINE int Height( void ) const { return m_pMatrix->Height(); } |
|
private: |
|
MATRIXTYPE const *m_pMatrix; |
|
int m_nColumn; |
|
}; |
|
|
|
/// this translator acts as a proxy for the transposed matrix |
|
template<class MATRIXTYPE> class MatrixTransposeAccessor |
|
{ |
|
public: |
|
FORCEINLINE MatrixTransposeAccessor( MATRIXTYPE const & matrix ) |
|
{ |
|
m_pMatrix = &matrix; |
|
} |
|
|
|
FORCEINLINE float Element( int nRow, int nColumn ) const |
|
{ |
|
return m_pMatrix->Element( nColumn, nRow ); |
|
} |
|
|
|
FORCEINLINE int Width( void ) const { return m_pMatrix->Height(); } |
|
FORCEINLINE int Height( void ) const { return m_pMatrix->Width(); } |
|
private: |
|
MATRIXTYPE const *m_pMatrix; |
|
}; |
|
|
|
/// this tranpose returns a wrapper around it's argument, allowing things like AddMatrixToMatrix( Transpose( matA ), &matB ) without an extra copy |
|
template<class MATRIXCLASSIN> |
|
MatrixTransposeAccessor<MATRIXCLASSIN> TransposeMatrix( MATRIXCLASSIN const &matrixIn ) |
|
{ |
|
return MatrixTransposeAccessor<MATRIXCLASSIN>( matrixIn ); |
|
} |
|
|
|
|
|
/// retrieve rows and columns |
|
template<class MATRIXTYPE> |
|
FORCEINLINE MatrixColumnAccessor<MATRIXTYPE> MatrixColumn( MATRIXTYPE const &matrix, int nColumn ) |
|
{ |
|
return MatrixColumnAccessor<MATRIXTYPE>( matrix, nColumn ); |
|
} |
|
|
|
template<class MATRIXTYPE> |
|
FORCEINLINE MatrixRowAccessor<MATRIXTYPE> MatrixRow( MATRIXTYPE const &matrix, int nRow ) |
|
{ |
|
return MatrixRowAccessor<MATRIXTYPE>( matrix, nRow ); |
|
} |
|
|
|
//// dot product between vectors (or rows and/or columns via accessors) |
|
template<class MATRIXACCESSORATYPE, class MATRIXACCESSORBTYPE > |
|
float InnerProduct( MATRIXACCESSORATYPE const &vecA, MATRIXACCESSORBTYPE const &vecB ) |
|
{ |
|
Assert( vecA.Width() == 1 ); |
|
Assert( vecB.Width() == 1 ); |
|
Assert( vecA.Height() == vecB.Height() ); |
|
double flResult = 0; |
|
for( int i = 0; i < vecA.Height(); i++ ) |
|
{ |
|
flResult += vecA.Element( i, 0 ) * vecB.Element( i, 0 ); |
|
} |
|
return flResult; |
|
} |
|
|
|
|
|
|
|
/// matrix x matrix multiplication |
|
template<class MATRIXATYPE, class MATRIXBTYPE, class MATRIXOUTTYPE> |
|
void MatrixMultiply( MATRIXATYPE const &matA, MATRIXBTYPE const &matB, MATRIXOUTTYPE *pMatrixOut ) |
|
{ |
|
Assert( matA.Width() == matB.Height() ); |
|
pMatrixOut->SetDimensions( matA.Height(), matB.Width() ); |
|
for( int i = 0; i < matA.Height(); i++ ) |
|
{ |
|
for( int j = 0; j < matB.Width(); j++ ) |
|
{ |
|
pMatrixOut->SetElement( i, j, InnerProduct( MatrixRow( matA, i ), MatrixColumn( matB, j ) ) ); |
|
} |
|
} |
|
} |
|
|
|
/// solve Ax=B via the conjugate graident method. Code and naming conventions based on the |
|
/// wikipedia article. |
|
template<class ATYPE, class XTYPE, class BTYPE> |
|
void ConjugateGradient( ATYPE const &matA, BTYPE const &vecB, XTYPE &vecX, float flTolerance = 1.0e-20 ) |
|
{ |
|
XTYPE vecR; |
|
vecR.SetDimensions( vecX.Height(), 1 ); |
|
MatrixMultiply( matA, vecX, &vecR ); |
|
ScaleMatrix( vecR, -1 ); |
|
AddMatrixToMatrix( vecB, &vecR ); |
|
XTYPE vecP; |
|
CopyMatrix( vecR, &vecP ); |
|
float flRsOld = InnerProduct( vecR, vecR ); |
|
for( int nIter = 0; nIter < 100; nIter++ ) |
|
{ |
|
XTYPE vecAp; |
|
MatrixMultiply( matA, vecP, &vecAp ); |
|
float flDivisor = InnerProduct( vecAp, vecP ); |
|
float flAlpha = flRsOld / flDivisor; |
|
AddScaledMatrixToMatrix( flAlpha, vecP, &vecX ); |
|
AddScaledMatrixToMatrix( -flAlpha, vecAp, &vecR ); |
|
float flRsNew = InnerProduct( vecR, vecR ); |
|
if ( flRsNew < flTolerance ) |
|
{ |
|
break; |
|
} |
|
ScaleMatrix( vecP, flRsNew / flRsOld ); |
|
AddMatrixToMatrix( vecR, &vecP ); |
|
flRsOld = flRsNew; |
|
} |
|
} |
|
|
|
/// solve (A'*A) x=B via the conjugate gradient method. Code and naming conventions based on |
|
/// the wikipedia article. Same as Conjugate gradient but allows passing in two matrices whose |
|
/// product is used as the A matrix (in order to preserve sparsity) |
|
template<class ATYPE, class APRIMETYPE, class XTYPE, class BTYPE> |
|
void ConjugateGradient( ATYPE const &matA, APRIMETYPE const &matAPrime, BTYPE const &vecB, XTYPE &vecX, float flTolerance = 1.0e-20 ) |
|
{ |
|
XTYPE vecR1; |
|
vecR1.SetDimensions( vecX.Height(), 1 ); |
|
MatrixMultiply( matA, vecX, &vecR1 ); |
|
XTYPE vecR; |
|
vecR.SetDimensions( vecR1.Height(), 1 ); |
|
MatrixMultiply( matAPrime, vecR1, &vecR ); |
|
ScaleMatrix( vecR, -1 ); |
|
AddMatrixToMatrix( vecB, &vecR ); |
|
XTYPE vecP; |
|
CopyMatrix( vecR, &vecP ); |
|
float flRsOld = InnerProduct( vecR, vecR ); |
|
for( int nIter = 0; nIter < 100; nIter++ ) |
|
{ |
|
XTYPE vecAp1; |
|
MatrixMultiply( matA, vecP, &vecAp1 ); |
|
XTYPE vecAp; |
|
MatrixMultiply( matAPrime, vecAp1, &vecAp ); |
|
float flDivisor = InnerProduct( vecAp, vecP ); |
|
float flAlpha = flRsOld / flDivisor; |
|
AddScaledMatrixToMatrix( flAlpha, vecP, &vecX ); |
|
AddScaledMatrixToMatrix( -flAlpha, vecAp, &vecR ); |
|
float flRsNew = InnerProduct( vecR, vecR ); |
|
if ( flRsNew < flTolerance ) |
|
{ |
|
break; |
|
} |
|
ScaleMatrix( vecP, flRsNew / flRsOld ); |
|
AddMatrixToMatrix( vecR, &vecP ); |
|
flRsOld = flRsNew; |
|
} |
|
} |
|
|
|
|
|
template<class ATYPE, class XTYPE, class BTYPE> |
|
void LeastSquaresFit( ATYPE const &matA, BTYPE const &vecB, XTYPE &vecX ) |
|
{ |
|
// now, generate the normal equations |
|
BTYPE vecBeta; |
|
MatrixMath::MatrixMultiply( MatrixMath::TransposeMatrix( matA ), vecB, &vecBeta ); |
|
|
|
vecX.SetDimensions( matA.Width(), 1 ); |
|
MatrixMath::SetMatrixToIdentity( &vecX ); |
|
|
|
ATYPE matATransposed; |
|
TransposeMatrix( matA, &matATransposed ); |
|
ConjugateGradient( matA, matATransposed, vecBeta, vecX, 1.0e-20 ); |
|
} |
|
|
|
}; |
|
|
|
/// a simple fixed-size matrix class |
|
template<int NUMROWS, int NUMCOLS> class CFixedMatrix |
|
{ |
|
public: |
|
FORCEINLINE int Width( void ) const { return NUMCOLS; } |
|
FORCEINLINE int Height( void ) const { return NUMROWS; } |
|
FORCEINLINE float Element( int nRow, int nCol ) const { return m_flValues[nRow][nCol]; } |
|
FORCEINLINE void SetElement( int nRow, int nCol, float flValue ) { m_flValues[nRow][nCol] = flValue; } |
|
FORCEINLINE void SetDimensions( int nNumRows, int nNumCols ) { Assert( ( nNumRows == NUMROWS ) && ( nNumCols == NUMCOLS ) ); } |
|
|
|
private: |
|
float m_flValues[NUMROWS][NUMCOLS]; |
|
}; |
|
|
|
|
|
|
|
#endif //matrixmath_h
|
|
|