1HEADmaster

1 files changed, 113 insertions, 0 deletions

diff --git a/vphysics/linear_solver.cpp b/vphysics/linear_solver.cpp
new file mode 100644
index 0000000..66e36ea
--- /dev/null
+++ b/vphysics/linear_solver.cpp
@@ -0,0 +1,113 @@
+//========= Copyright Valve Corporation, All rights reserved. ============//
+//
+// Purpose: 
+//
+// $NoKeywords: $
+//
+//=============================================================================//
+
+#include "tier0/platform.h"
+#include "linear_solver.h"
+#include <stdio.h>
+#include <string.h>
+#include <math.h>
+
+// BUGBUG: Remove/tune this number somehow!!!
+#define DET_EPSILON		1e-6f
+	
+
+// assumes square matrix!
+// ONLY SUPPORTS 2x2, 3x3, and 4x4
+float Det( float *matrix, int rows )
+{
+	if ( rows == 2 )
+	{
+		return matrix[0]*matrix[3] - matrix[1]*matrix[2];
+	}
+	if ( rows == 3 )
+	{
+		return matrix[0]*matrix[4]*matrix[8] - matrix[0]*matrix[7]*matrix[5] - matrix[1]*matrix[3]*matrix[8] +
+				matrix[1]*matrix[5]*matrix[6] + matrix[2]*matrix[3]*matrix[7] - matrix[2]*matrix[4]*matrix[6];
+	}
+
+	// ERROR no more than 4x4
+	if ( rows != 4 )
+		return 0;
+
+	// UNDONE: Generalize this to NxN
+	float tmp[9];
+	float det = 0.f;
+	// do 4 3x3 dets
+	for ( int i = 0; i < 4; i++ )
+	{
+		// develop on row 0
+		int out = 0;
+		for ( int j = 1; j < 4; j++ )
+		{
+			// iterate each column and 
+			for ( int k = 0; k < 4; k++ )
+			{
+				if ( k == i )
+					continue;
+				tmp[out] = matrix[(j*rows)+k];
+				out++;
+			}
+		}
+		if ( i & 1 )
+		{
+			det -= matrix[i]*Det(tmp,3);
+		}
+		else
+		{
+			det += matrix[i]*Det(tmp,3);
+		}
+	}
+
+	return det;
+}
+
+float *SolveCramer( const float *matrix, int rows, int columns )
+{
+	// max 4 equations, 4 unknowns (until determinant routine is more general)
+	float tmpMain[16*16], tmpSub[16*16];
+	static float solution[16];
+
+	int i, j;
+
+	if ( rows > 4 || columns > 5 )
+	{
+		return NULL;
+	}
+
+
+	int outCol = columns - 1;
+	// copy out the square matrix
+	for ( i = 0; i < rows; i++ )
+	{
+		memcpy( tmpMain + (i*outCol), matrix + i*columns, sizeof(float)*outCol );
+	}
+
+	float detMain = Det( tmpMain, rows );
+
+	// probably degenerate!
+	if ( fabs(detMain) < DET_EPSILON )
+	{
+		return NULL;
+	}
+
+	for ( i = 0; i < rows; i++ )
+	{
+		// copy the square matrix
+		memcpy( tmpSub, tmpMain, sizeof(float)*rows*rows );
+		
+		// copy the column of constants over the row
+		for ( j = 0; j < rows; j++ )
+		{
+			tmpSub[i+j*outCol] = matrix[j*columns+columns-1];
+		}
+		float det = Det( tmpSub, rows );
+		solution[i] = det / detMain;
+	}
+
+	return solution;
+}