#include "GraphicsGems.h" #include /**** * * affine_matrix4_inverse * * Computes the inverse of a 3D affine matrix; i.e. a matrix with a dimen- * sionality of 4 where the right column has the entries (0, 0, 0, 1). * * This procedure treats the 4 by 4 matrix as a block matrix and * calculates the inverse of one submatrix for a significant perform- * ance improvement over a general procedure that can invert any non- * singular matrix: * -- -- -- -- * | | -1 | -1 | * | A 0 | | A 0 | * -1 | | | | * M = | | = | -1 | * | C 1 | | -C A 1 | * | | | | * -- -- -- -- * * where M is a 4 by 4 matrix, * A is the 3 by 3 upper left submatrix of M, * C is the 1 by 3 lower left submatrix of M. * * Input: * in - 3D affine matrix * * Output: * out - inverse of 3D affine matrix * * Returned value: * TRUE if input matrix is nonsingular * FALSE otherwise * ***/ boolean affine_matrix4_inverse (in, out) register Matrix4 *in; register Matrix4 *out; { register double det_1; double pos, neg, temp; #define ACCUMULATE \ if (temp >= 0.0) \ pos += temp; \ else \ neg += temp; #define PRECISION_LIMIT (1.0e-15) /* * 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 = in->element[0][0] * in->element[1][1] * in->element[2][2]; ACCUMULATE temp = in->element[0][1] * in->element[1][2] * in->element[2][0]; ACCUMULATE temp = in->element[0][2] * in->element[1][0] * in->element[2][1]; ACCUMULATE temp = -in->element[0][2] * in->element[1][1] * in->element[2][0]; ACCUMULATE temp = -in->element[0][1] * in->element[1][0] * in->element[2][2]; ACCUMULATE temp = -in->element[0][0] * in->element[1][2] * in->element[2][1]; ACCUMULATE det_1 = pos + neg; /* Is the submatrix A singular? */ if ((det_1 == 0.0) || (ABS(det_1 / (pos - neg)) < PRECISION_LIMIT)) { /* Matrix M has no inverse */ fprintf (stderr, "affine_matrix4_inverse: singular matrix\n"); return FALSE; } else { /* Calculate inverse(A) = adj(A) / det(A) */ det_1 = 1.0 / det_1; out->element[0][0] = ( in->element[1][1] * in->element[2][2] - in->element[1][2] * in->element[2][1] ) * det_1; out->element[1][0] = - ( in->element[1][0] * in->element[2][2] - in->element[1][2] * in->element[2][0] ) * det_1; out->element[2][0] = ( in->element[1][0] * in->element[2][1] - in->element[1][1] * in->element[2][0] ) * det_1; out->element[0][1] = - ( in->element[0][1] * in->element[2][2] - in->element[0][2] * in->element[2][1] ) * det_1; out->element[1][1] = ( in->element[0][0] * in->element[2][2] - in->element[0][2] * in->element[2][0] ) * det_1; out->element[2][1] = - ( in->element[0][0] * in->element[2][1] - in->element[0][1] * in->element[2][0] ) * det_1; out->element[0][2] = ( in->element[0][1] * in->element[1][2] - in->element[0][2] * in->element[1][1] ) * det_1; out->element[1][2] = - ( in->element[0][0] * in->element[1][2] - in->element[0][2] * in->element[1][0] ) * det_1; out->element[2][2] = ( in->element[0][0] * in->element[1][1] - in->element[0][1] * in->element[1][0] ) * det_1; /* Calculate -C * inverse(A) */ out->element[3][0] = - ( in->element[3][0] * out->element[0][0] + in->element[3][1] * out->element[1][0] + in->element[3][2] * out->element[2][0] ); out->element[3][1] = - ( in->element[3][0] * out->element[0][1] + in->element[3][1] * out->element[1][1] + in->element[3][2] * out->element[2][1] ); out->element[3][2] = - ( in->element[3][0] * out->element[0][2] + in->element[3][1] * out->element[1][2] + in->element[3][2] * out->element[2][2] ); /* Fill in last column */ out->element[0][3] = out->element[1][3] = out->element[2][3] = 0.0; out->element[3][3] = 1.0; return TRUE; } }