void MatrixInverseEx(
MATRIX &mOut,
const MATRIX &mIn)
{
MATRIX mTmp;
float *ppfRows[4];
float pfRes[4];
float pfIn[20];
int i, j;
for(i = 0; i < 4; ++i)
ppfRows[i] = &pfIn[i * 5];
/* Solve 4 sets of 4 linear equations */
for(i = 0; i < 4; ++i)
{
for(j = 0; j < 4; ++j)
{
ppfRows[j][0] = c_mIdentity.f[i + 4 * j];
memcpy(&ppfRows[j][1], &mIn.f[j * 4], 4 * sizeof(float));
}
MatrixLinearEqSolve(pfRes, (float**)ppfRows, 4);
for(j = 0; j < 4; ++j)
{
mTmp.f[i + 4 * j] = pfRes[j];
}
}
mOut = mTmp;
}
void TransTransformBack(
VECTOR4 * const pOut,
const VECTOR4 * const pV,
const MATRIX * const pM)
{
VERTTYPE *ppfRows[4];
VERTTYPE pfIn[20];
int i;
const MATRIX *pMa;
#if defined(BUILD_OGL) || defined(BUILD_OGLES) || defined(BUILD_OGLES2)
MATRIX mT;
MatrixTranspose(mT, *pM);
pMa = &mT;
#else
pMa = pM;
#endif
for(i = 0; i < 4; ++i)
{
/*
Set up the array of pointers to matrix coefficients
*/
ppfRows[i] = &pfIn[i * 5];
/*
Copy the 4x4 matrix into RHS of the 5x4 matrix
*/
memcpy(&ppfRows[i][1], &pMa->f[i * 4], 4 * sizeof(float));
}
/*
Copy the "result" vector into the first column of the 5x4 matrix
*/
ppfRows[0][0] = pV->x;
ppfRows[1][0] = pV->y;
ppfRows[2][0] = pV->z;
ppfRows[3][0] = pV->w;
/*
Solve a set of 4 linear equations
*/
MatrixLinearEqSolve(&pOut->x, ppfRows, 4);
}
void MatrixLinearEqSolve(
float * const pRes,
float ** const pSrc, // 2D array of floats. 4 Eq linear problem is 5x4 matrix, constants in first column.
const int nCnt)
{
int i, j, k;
float f;
#if 0
/*
Show the matrix in debug output
*/
_RPT1(_CRT_WARN, "LinearEqSolve(%d)\n", nCnt);
for(i = 0; i < nCnt; ++i)
{
_RPT1(_CRT_WARN, "%.8f |", pSrc[i][0]);
for(j = 1; j <= nCnt; ++j)
_RPT1(_CRT_WARN, " %.8f", pSrc[i][j]);
_RPT0(_CRT_WARN, "\n");
}
#endif
if(nCnt == 1)
{
_ASSERT(pSrc[0][1] != 0);
pRes[0] = pSrc[0][0] / pSrc[0][1];
return;
}
// Loop backwards in an attempt avoid the need to swap rows
i = nCnt;
while(i)
{
--i;
if(pSrc[i][nCnt] != 0)
{
// Row i can be used to zero the other rows; let's move it to the bottom
if(i != (nCnt-1))
{
for(j = 0; j <= nCnt; ++j)
{
// Swap the two values
f = pSrc[nCnt-1][j];
pSrc[nCnt-1][j] = pSrc[i][j];
pSrc[i][j] = f;
}
}
// Now zero the last columns of the top rows
for(j = 0; j < (nCnt-1); ++j)
{
_ASSERT(pSrc[nCnt-1][nCnt] != 0);
f = pSrc[j][nCnt] / pSrc[nCnt-1][nCnt];
// No need to actually calculate a zero for the final column
for(k = 0; k < nCnt; ++k)
{
pSrc[j][k] -= f * pSrc[nCnt-1][k];
}
}
break;
}
}
// Solve the top-left sub matrix
MatrixLinearEqSolve(pRes, pSrc, nCnt - 1);
// Now calc the solution for the bottom row
f = pSrc[nCnt-1][0];
for(k = 1; k < nCnt; ++k)
{
f -= pSrc[nCnt-1][k] * pRes[k-1];
}
_ASSERT(pSrc[nCnt-1][nCnt] != 0);
f /= pSrc[nCnt-1][nCnt];
pRes[nCnt-1] = f;
#if 0
{
float fCnt;
/*
Verify that the result is correct
*/
fCnt = 0;
for(i = 1; i <= nCnt; ++i)
fCnt += pSrc[nCnt-1][i] * pRes[i-1];
_ASSERT(_ABS(fCnt - pSrc[nCnt-1][0]) < 1e-3);
}
#endif
}
