Modified source engine (2017) developed by valve and leaked in 2020. Not for commercial purporses
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

114 lines
2.2 KiB

5 years ago
//========= 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;
}