MathGem:Fast Matrix Inversion

From GPWiki

Rather than doing a general 4x4 matrix inversion, you can take advantage of some of the properties of matrices that involve only rotation and translation in order to invert them quickly.

First, let's simplify the notation. The matrix can be represented like this:

   R  T
   0  1

where R is the upper-left 3x3 part of the 4x4 matrix, T is the upper-right 3x1 part of the 4x4 matix, 0 is the 1x3 row of zeros on the bottom, and 1 is the 1 in the lower-right corner. Note that R and T conveniently correspond to the rotation and translation portions of the matrix. If you are using row vectors, then the matrices here are shown transposed. That doesn't affect the math, it just affects the notation.

The inverse of this matrix is:

   R-1 -R-1T
   
   0   1

In addition, since R is orthonormal, R^-1 = R^T and the result is

   RT -RTT
   
   0   1

and no inversion is actually necessary.

One more possible optimization... T' = -R^TT can also be computed like this:

   T'X = -(RX dot T)
   T'Y = -(RY dot T)
   T'Z = -(RZ dot T)

where R_X, R_Y, and R_Z are the first, second, and third columns of R.