Parallax Corrected Cubemaps/matrixinvert.h

From Jason Yu-Tseh Chi's "Calculation of matrix inverse in C/C++"

Modified to work with Valve's matrix_3x4_t

```//========= Copyright Jason Yu-Tseh Chi, All rights reserved. ============//
//
// Purpose:
//
//=============================================================================//

#include "mathlib/mathlib.h"

//-----------------------------------------------------------------------------
// Purpose: Calculate the cofactor of element (row, col)
//-----------------------------------------------------------------------------
int GetMatrixMinor( float **src, float **dest, int row, int col, int order )
{
// Indicate which col and row is being copied to dest
int colCount = 0, rowCount = 0;

for ( int i = 0; i < order; i++ )
{
if ( i != row )
{
colCount = 0;
for ( int j = 0; j < order; j++ )
{
// When j is not the element
if ( j != col )
{
dest[rowCount][colCount] = src[i][j];
colCount++;
}
}
rowCount++;
}
}

return 1;
}

//-----------------------------------------------------------------------------
// Purpose: Calculate the determinant recursively
//-----------------------------------------------------------------------------
double CalcMatrixDeterminant( float **mat, int order )
{
// Order must be >= 0
// Stop the recursion when matrix is a single element
if ( order == 1 )
return mat[0][0];

// The determinant value
float det = 0;

// Allocate the cofactor matrix
float **minor;
minor = new float *[order - 1];
for ( int i = 0; i < order - 1; i++ )
minor[i] = new float[order - 1];

for ( int i = 0; i < order; i++ )
{
// Get minor of element (0, i)
GetMatrixMinor( mat, minor, 0, i, order );
// The recusion is here!

det += (i % 2 == 1 ? -1.0 : 1.0) * mat[0][i] * CalcMatrixDeterminant( minor, order - 1 );
//det += pow( -1.0, i ) * mat[0][i] * CalcMatrixDeterminant( minor,order - 1 );
}

// Release memory
for ( int i = 0; i < order - 1; i++ )
delete[] minor[i];
delete[] minor;

return det;
}

//-----------------------------------------------------------------------------
// Purpose: Matrix inversion
//-----------------------------------------------------------------------------
void MatrixInversion( matrix3x4_t &in, matrix3x4_t &out )
{
float **A;
A = new float *[4];
for ( int i = 0; i < 4; i++ )
A[i] = new float[4];
int order = 4;

for ( int i = 0; i < 3; i++ )
{
for ( int j = 0; j < 4; j++ )
{
A[i][j] = in[i][j];
}
}
A[3][0] = A[3][1] = A[3][2] = 0;
A[3][3] = 1;

// Get the determinant of a
double det = 1.0 / CalcMatrixDeterminant( (float **)A, order );

// Memory allocation
float *temp = new float[(order - 1) * (order - 1)];
float **minor = new float *[order - 1];
for ( int i = 0; i < order - 1; i++ )
minor[i] = temp + (i * (order - 1));

for ( int j = 0; j < order; j++ )
{
for ( int i = 0; i<order; i++ )
{
// Get the co-factor (matrix) of A(j, i)
GetMatrixMinor( (float **)A, minor, j, i, order );
out[i][j] = det * CalcMatrixDeterminant( minor, order - 1 );
if ( (i + j) % 2 == 1 )
out[i][j] = -out[i][j];
}
}

// Release memory
for ( int i = 0; i < 4; i++ )
delete A[i];
delete A;
//delete[] minor[0];
delete[] temp;
delete[] minor;
}
```