hl2sdk/public/mathlib/matrixmath.h

//========= 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