386 lines
12 KiB
C++
386 lines
12 KiB
C++
//========= 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
|