void MatrixInverse(
MATRIX &mOut,
const MATRIX &mIn)
{
MATRIX mDummyMatrix;
double det_1;
double pos, neg, temp;
/* Calculate the determinant of submatrix A and determine if the
the matrix is singular as limited by the double precision
floating-point data representation. */
pos = neg = 0.0;
temp = mIn.f[ 0] * mIn.f[ 5] * mIn.f[10];
if (temp >= 0.0) pos += temp; else neg += temp;
temp = mIn.f[ 4] * mIn.f[ 9] * mIn.f[ 2];
if (temp >= 0.0) pos += temp; else neg += temp;
temp = mIn.f[ 8] * mIn.f[ 1] * mIn.f[ 6];
if (temp >= 0.0) pos += temp; else neg += temp;
temp = -mIn.f[ 8] * mIn.f[ 5] * mIn.f[ 2];
if (temp >= 0.0) pos += temp; else neg += temp;
temp = -mIn.f[ 4] * mIn.f[ 1] * mIn.f[10];
if (temp >= 0.0) pos += temp; else neg += temp;
temp = -mIn.f[ 0] * mIn.f[ 9] * mIn.f[ 6];
if (temp >= 0.0) pos += temp; else neg += temp;
det_1 = pos + neg;
/* Is the submatrix A singular? */
if ((det_1 == 0.0) || (_ABS(det_1 / (pos - neg)) < 1.0e-15))
{
/* Matrix M has no inverse */
printf("Matrix has no inverse : singular matrix\n");
return;
}
else
{
/* Calculate inverse(A) = adj(A) / det(A) */
det_1 = 1.0 / det_1;
mDummyMatrix.f[ 0] = ( mIn.f[ 5] * mIn.f[10] - mIn.f[ 9] * mIn.f[ 6] ) * (float)det_1;
mDummyMatrix.f[ 1] = - ( mIn.f[ 1] * mIn.f[10] - mIn.f[ 9] * mIn.f[ 2] ) * (float)det_1;
mDummyMatrix.f[ 2] = ( mIn.f[ 1] * mIn.f[ 6] - mIn.f[ 5] * mIn.f[ 2] ) * (float)det_1;
mDummyMatrix.f[ 4] = - ( mIn.f[ 4] * mIn.f[10] - mIn.f[ 8] * mIn.f[ 6] ) * (float)det_1;
mDummyMatrix.f[ 5] = ( mIn.f[ 0] * mIn.f[10] - mIn.f[ 8] * mIn.f[ 2] ) * (float)det_1;
mDummyMatrix.f[ 6] = - ( mIn.f[ 0] * mIn.f[ 6] - mIn.f[ 4] * mIn.f[ 2] ) * (float)det_1;
mDummyMatrix.f[ 8] = ( mIn.f[ 4] * mIn.f[ 9] - mIn.f[ 8] * mIn.f[ 5] ) * (float)det_1;
mDummyMatrix.f[ 9] = - ( mIn.f[ 0] * mIn.f[ 9] - mIn.f[ 8] * mIn.f[ 1] ) * (float)det_1;
mDummyMatrix.f[10] = ( mIn.f[ 0] * mIn.f[ 5] - mIn.f[ 4] * mIn.f[ 1] ) * (float)det_1;
/* Calculate -C * inverse(A) */
mDummyMatrix.f[12] = - ( mIn.f[12] * mDummyMatrix.f[ 0] + mIn.f[13] * mDummyMatrix.f[ 4] + mIn.f[14] * mDummyMatrix.f[ 8] );
mDummyMatrix.f[13] = - ( mIn.f[12] * mDummyMatrix.f[ 1] + mIn.f[13] * mDummyMatrix.f[ 5] + mIn.f[14] * mDummyMatrix.f[ 9] );
mDummyMatrix.f[14] = - ( mIn.f[12] * mDummyMatrix.f[ 2] + mIn.f[13] * mDummyMatrix.f[ 6] + mIn.f[14] * mDummyMatrix.f[10] );
/* Fill in last row */
mDummyMatrix.f[ 3] = 0.0f;
mDummyMatrix.f[ 7] = 0.0f;
mDummyMatrix.f[11] = 0.0f;
mDummyMatrix.f[15] = 1.0f;
}
/* Copy contents of dummy matrix in pfMatrix */
mOut = mDummyMatrix;
}