1HEADmaster

1 files changed, 332 insertions, 0 deletions

diff --git a/public/nmatrix.h b/public/nmatrix.h
new file mode 100644
index 0000000..2b8d206
--- /dev/null
+++ b/public/nmatrix.h
@@ -0,0 +1,332 @@
+//========= Copyright Valve Corporation, All rights reserved. ============//
+//
+// Purpose: 
+//
+// $NoKeywords: $
+//=============================================================================//
+
+#ifndef NMATRIX_H
+#define NMATRIX_H
+#ifdef _WIN32
+#pragma once
+#endif
+
+
+#include <assert.h>
+#include "nvector.h"
+
+
+#define NMatrixMN NMatrix<M,N>
+
+
+template<int M, int N>
+class NMatrix
+{
+public:
+
+						NMatrixMN() {}
+	
+	static NMatrixMN	SetupNMatrixNull();					// Return a matrix of all zeros.
+	static NMatrixMN	SetupNMatrixIdentity();				// Return an identity matrix.
+
+	NMatrixMN const&	operator=( NMatrixMN const &other );
+
+	NMatrixMN			operator+( NMatrixMN const &v ) const;
+	NMatrixMN const&	operator+=( NMatrixMN const &v );
+
+	NMatrixMN			operator-() const;
+	NMatrixMN			operator-( NMatrixMN const &v ) const;
+
+	// Multiplies the column vector on the right-hand side.
+	NVector<M>			operator*( NVector<N> const &v ) const;
+
+	// Can't get the compiler to work with a real MxN * NxR matrix multiply...
+	NMatrix<M,M>		operator*( NMatrix<N,M> const &b ) const;
+
+	NMatrixMN			operator*( float val ) const;
+	
+	bool				InverseGeneral( NMatrixMN &mInverse ) const;
+	NMatrix<N,M>		Transpose() const;
+
+
+public:
+
+	float				m[M][N];
+};
+
+
+
+// Return the matrix generated by multiplying a column vector 'a' by row vector 'b'.
+template<int N>
+inline NMatrix<N,N> OuterProduct( NVectorN const &a, NVectorN const &b )
+{
+	NMatrix<N,N> ret;
+
+	for( int i=0; i < N; i++ )
+		for( int j=0; j < N; j++ )
+			ret.m[i][j] = a.v[i] * b.v[j];
+
+	return ret;
+}
+
+
+// -------------------------------------------------------------------------------- //
+// NMatrix inlines.
+// -------------------------------------------------------------------------------- //
+
+template<int M, int N>
+inline NMatrixMN NMatrixMN::SetupNMatrixNull()
+{
+	NMatrix ret;
+	memset( ret.m, 0, sizeof(float)*M*N );
+	return ret;
+}
+
+
+template<int M, int N>
+inline NMatrixMN NMatrixMN::SetupNMatrixIdentity()
+{
+	assert( M == N );	// Identity matrices must be square.
+
+	NMatrix ret;
+	memset( ret.m, 0, sizeof(float)*M*N );
+	for( int i=0; i < N; i++ )
+		ret.m[i][i] = 1;
+	return ret;
+}
+
+
+template<int M, int N>
+inline NMatrixMN const &NMatrixMN::operator=( NMatrixMN const &v )
+{
+	memcpy( m, v.m, sizeof(float)*M*N );
+	return *this;
+}
+
+
+template<int M, int N>
+inline NMatrixMN NMatrixMN::operator+( NMatrixMN const &v ) const
+{
+	NMatrixMN ret;
+	for( int i=0; i < M; i++ )
+		for( int j=0; j < N; j++ )
+			ret.m[i][j] = m[i][j] + v.m[i][j];
+
+	return ret;
+}
+
+
+template<int M, int N>
+inline NMatrixMN const &NMatrixMN::operator+=( NMatrixMN const &v )
+{
+	for( int i=0; i < M; i++ )
+		for( int j=0; j < N; j++ )
+			m[i][j] += v.m[i][j];
+
+	return *this;
+}
+
+
+template<int M, int N>
+inline NMatrixMN NMatrixMN::operator-() const
+{
+	NMatrixMN ret;
+	
+	for( int i=0; i < M*N; i++ )
+		((float*)ret.m)[i] = -((float*)m)[i];
+	
+	return ret;
+}
+
+
+template<int M, int N>
+inline NMatrixMN NMatrixMN::operator-( NMatrixMN const &v ) const
+{
+	NMatrixMN ret;
+	for( int i=0; i < M; i++ )
+		for( int j=0; j < N; j++ )
+			ret.m[i][j] = m[i][j] - v.m[i][j];
+	return ret;
+}
+
+
+template<int M, int N>
+inline NVector<M> NMatrixMN::operator*( NVectorN const &v ) const
+{
+	NVectorN ret;
+
+	for( int i=0; i < M; i++ )
+	{
+		ret.v[i] = 0;
+
+		for( int j=0; j < N; j++ )
+			ret.v[i] += m[i][j] * v.v[j];
+	}
+	
+	return ret;
+}
+
+
+template<int M, int N>
+inline NMatrix<M,M> NMatrixMN::operator*( NMatrix<N,M> const &b ) const
+{
+	NMatrix<M,M> ret;
+
+	for( int myRow=0; myRow < M; myRow++ )
+	{
+		for( int otherCol=0; otherCol < M; otherCol++ )
+		{
+			ret[myRow][otherCol] = 0;
+			for( int i=0; i < N; i++ )
+				ret[myRow][otherCol] += a.m[myRow][i] * b.m[i][otherCol];
+		}
+	}
+
+	return ret;
+}
+
+
+template<int M, int N>
+inline NMatrixMN NMatrixMN::operator*( float val ) const
+{
+	NMatrixMN ret;
+
+	for( int i=0; i < N*M; i++ )
+		((float*)ret.m)[i] = ((float*)m)[i] * val;
+
+	return ret;
+}
+
+
+template<int M, int N>
+bool NMatrixMN::InverseGeneral( NMatrixMN &mInverse ) const
+{
+	int iRow, i, j, iTemp, iTest;
+	float mul, fTest, fLargest;
+	float mat[N][2*N];
+	int rowMap[N], iLargest;
+	float *pOut, *pRow, *pScaleRow;
+
+	
+	// Can only invert square matrices.
+	if( M != N )
+	{
+		assert( !"Tried to invert a non-square matrix" );
+		return false;
+	}
+
+
+	// How it's done.
+	// AX = I
+	// A = this
+	// X = the matrix we're looking for
+	// I = identity
+
+	// Setup AI
+	for(i=0; i < N; i++)
+	{
+		const float *pIn = m[i];
+		pOut = mat[i];
+
+		for(j=0; j < N; j++)
+		{
+			pOut[j] = pIn[j];
+		}
+
+		for(j=N; j < 2*N; j++)
+			pOut[j] = 0;
+		
+		pOut[i+N] = 1.0f;
+
+		rowMap[i] = i;
+	}
+
+	// Use row operations to get to reduced row-echelon form using these rules:
+	// 1. Multiply or divide a row by a nonzero number.
+	// 2. Add a multiple of one row to another.
+	// 3. Interchange two rows.
+
+	for(iRow=0; iRow < N; iRow++)
+	{
+		// Find the row with the largest element in this column.
+		fLargest = 0.001f;
+		iLargest = -1;
+		for(iTest=iRow; iTest < N; iTest++)
+		{
+			fTest = (float)fabs(mat[rowMap[iTest]][iRow]);
+			if(fTest > fLargest)
+			{
+				iLargest = iTest;
+				fLargest = fTest;
+			}
+		}
+
+		// They're all too small.. sorry.
+		if(iLargest == -1)
+		{
+			return false;
+		}
+
+		// Swap the rows.
+		iTemp = rowMap[iLargest];
+		rowMap[iLargest] = rowMap[iRow];
+		rowMap[iRow] = iTemp;
+
+		pRow = mat[rowMap[iRow]];
+
+		// Divide this row by the element.
+		mul = 1.0f / pRow[iRow];
+		for(j=0; j < 2*N; j++)
+			pRow[j] *= mul;
+
+		pRow[iRow] = 1.0f; // Preserve accuracy...
+		
+		// Eliminate this element from the other rows using operation 2.
+		for(i=0; i < N; i++)
+		{
+			if(i == iRow)
+				continue;
+
+			pScaleRow = mat[rowMap[i]];
+		
+			// Multiply this row by -(iRow*the element).
+			mul = -pScaleRow[iRow];
+			for(j=0; j < 2*N; j++)
+			{
+				pScaleRow[j] += pRow[j] * mul;
+			}
+
+			pScaleRow[iRow] = 0.0f; // Preserve accuracy...
+		}
+	}
+
+	// The inverse is on the right side of AX now (the identity is on the left).
+	for(i=0; i < N; i++)
+	{
+		const float *pIn = mat[rowMap[i]] + N;
+		pOut = mInverse.m[i];
+
+		for(j=0; j < N; j++)
+		{
+			pOut[j] = pIn[j];
+		}
+	}
+
+	return true;
+}
+
+
+template<int M, int N>
+inline NMatrix<N,M> NMatrixMN::Transpose() const
+{
+	NMatrix<N,M> ret;
+	
+	for( int i=0; i < M; i++ )
+		for( int j=0; j < N; j++ )
+			ret.m[j][i] = m[i][j];
+	
+	return ret;
+}
+
+#endif // NMATRIX_H
+