qboolean MatrixInverse(matrix_t matrix)
{
float mdet = MatrixDet(matrix);
matrix3x3_t mtemp;
int i, j, sign;
matrix_t m4x4_temp;
#if 0
if ( fabs( mdet ) < 0.0000000001 )
return qtrue;
#endif
MatrixCopy(matrix, m4x4_temp);
for ( i = 0; i < 4; i++ )
for ( j = 0; j < 4; j++ )
{
sign = 1 - ( (i +j) % 2 ) * 2;
m4_submat( m4x4_temp, mtemp, i, j );
// FIXME: try using * inverse det and see if speed/accuracy are good enough
matrix[i+j*4] = ( m3_det( mtemp ) * sign ) / mdet;
}
return qfalse;
}
static inline double m4_det(double *mr)
{
double det, result = 0.0, i = 1.0, msub3[9];
int n;
for (n = 0; n < 4; n++, i *= -1.0)
{
m4_submat(mr, msub3, 0, n);
det = m3_det(msub3);
result += mr[n] * det * i;
}
return result;
}
float m4_det( m4x4_t mr )
{
float det, result = 0, i = 1;
m3x3_t msub3;
int n;
for ( n = 0; n < 4; n++, i *= -1 )
{
m4_submat( mr, msub3, 0, n );
det = m3_det( msub3 );
result += mr[n] * det * i;
}
return result;
}
static inline int m4_inverse(double *mr, double *ma)
{
double mtemp[9], mdet = m4_det(ma);
int i, j, sign;
if (fabs(mdet) == 0.0)
return 0;
for (i = 0; i < 4; i++)
{
for (j = 0; j < 4; j++)
{
sign = 1 - ((i +j) % 2) * 2;
m4_submat(ma, mtemp, i, j);
mr[i+j*4] = (m3_det(mtemp) * sign) / mdet;
}
}
return 1;
}
int m4x4_invert(m4x4_t matrix)
{
float mdet = m4_det( matrix );
m3x3_t mtemp;
int i, j, sign;
m4x4_t m4x4_temp;
if ( fabs( mdet ) < 0.0000000001 ) //% 0.0005
return 1;
memcpy(m4x4_temp, matrix, sizeof(m4x4_t));
for ( i = 0; i < 4; i++ )
for ( j = 0; j < 4; j++ )
{
sign = 1 - ( (i +j) % 2 ) * 2;
m4_submat( m4x4_temp, mtemp, i, j );
matrix[i+j*4] = ( m3_det( mtemp ) * sign ) / mdet;
}
return 0;
}
static int m3_inverse( matrix3x3_t mr, matrix3x3_t ma )
{
float det = m3_det( ma );
if (det == 0 )
{
return 1;
}
mr[0] = ma[4]*ma[8] - ma[5]*ma[7] / det;
mr[1] = -( ma[1]*ma[8] - ma[7]*ma[2] ) / det;
mr[2] = ma[1]*ma[5] - ma[4]*ma[2] / det;
mr[3] = -( ma[3]*ma[8] - ma[5]*ma[6] ) / det;
mr[4] = ma[0]*ma[8] - ma[6]*ma[2] / det;
mr[5] = -( ma[0]*ma[5] - ma[3]*ma[2] ) / det;
mr[6] = ma[3]*ma[7] - ma[6]*ma[4] / det;
mr[7] = -( ma[0]*ma[7] - ma[6]*ma[1] ) / det;
mr[8] = ma[0]*ma[4] - ma[1]*ma[3] / det;
return 0;
}