/*
* inverse( original_matrix, inverse_matrix )
*
* calculate the inverse of a 4x4 matrix
*
* -1
* A = ___1__ adjoint A
* det A
*/
bool inverse( Matrix4 *in, Matrix4 *out )
{
int i, j;
double det;
/* calculate the adjoint matrix */
adjoint( in, out );
/* calculate the 4x4 determinant
* if the determinant is zero,
* then the inverse matrix is not unique.
*/
det = det4x4( in );
if (fabs( det ) < SMALL_NUMBER)
return false;
/* scale the adjoint matrix to get the inverse */
for (i=0; i<4; i++)
for(j=0; j<4; j++)
out->element[i][j] = out->element[i][j] / det;
return true;
}
int MatrixInvert(Matrix4By4 in,Matrix4By4 out)
{ float det;
det=det4x4(in);
if (fabs(det)<PRECISION_LIMIT) /* NO INVERSE */
return(0);
Adjoint(in,out,det);
return(1);
}
int Matrix::Inverse(Matrix &m, float epsilon) const {
m = *this;
float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
a1 = m.data[0][0]; b1 = m.data[0][1]; c1 = m.data[0][2]; d1 = m.data[0][3];
a2 = m.data[1][0]; b2 = m.data[1][1]; c2 = m.data[1][2]; d2 = m.data[1][3];
a3 = m.data[2][0]; b3 = m.data[2][1]; c3 = m.data[2][2]; d3 = m.data[2][3];
a4 = m.data[3][0]; b4 = m.data[3][1]; c4 = m.data[3][2]; d4 = m.data[3][3];
float det = det4x4(a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,c3,c4,d1,d2,d3,d4);
if (fabs(det) < epsilon) {
printf ("Matrix::Inverse --- singular matrix, can't invert!\n");
assert(0);
return 0;
}
m.data[0][0] = det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4);
m.data[1][0] = - det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4);
m.data[2][0] = det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4);
m.data[3][0] = - det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
m.data[0][1] = - det3x3( b1, b3, b4, c1, c3, c4, d1, d3, d4);
m.data[1][1] = det3x3( a1, a3, a4, c1, c3, c4, d1, d3, d4);
m.data[2][1] = - det3x3( a1, a3, a4, b1, b3, b4, d1, d3, d4);
m.data[3][1] = det3x3( a1, a3, a4, b1, b3, b4, c1, c3, c4);
m.data[0][2] = det3x3( b1, b2, b4, c1, c2, c4, d1, d2, d4);
m.data[1][2] = - det3x3( a1, a2, a4, c1, c2, c4, d1, d2, d4);
m.data[2][2] = det3x3( a1, a2, a4, b1, b2, b4, d1, d2, d4);
m.data[3][2] = - det3x3( a1, a2, a4, b1, b2, b4, c1, c2, c4);
m.data[0][3] = - det3x3( b1, b2, b3, c1, c2, c3, d1, d2, d3);
m.data[1][3] = det3x3( a1, a2, a3, c1, c2, c3, d1, d2, d3);
m.data[2][3] = - det3x3( a1, a2, a3, b1, b2, b3, d1, d2, d3);
m.data[3][3] = det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3);
m *= 1/det;
return 1;
}
/* mat1 <-- mat2^-1 */
float mat_inv (Matrix mat1, Matrix mat2)
{
int i, j;
float det;
if (mat1 != mat2) {
for (i = 0; i < 4; i++)
for (j = 0; j < 4; j++)
mat1[i][j] = mat2[i][j];
}
det = det4x4 (mat1);
if (fabs (det) < EPSILON)
return 0.0;
adjoint (mat1);
for (i = 0; i < 4; i++)
for(j = 0; j < 4; j++)
mat1[i][j] = mat1[i][j] / det;
return det;
}
