First version of the SOurce SDK 2013

1 files changed, 385 insertions, 0 deletions

diff --git a/sp/src/public/mathlib/matrixmath.h b/sp/src/public/mathlib/matrixmath.h
new file mode 100644
index 00000000..40de0c02
--- /dev/null
+++ b/sp/src/public/mathlib/matrixmath.h
@@ -0,0 +1,385 @@
+//========= 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