MathGem:Fast Matrix Inversion
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 = RT and the result is
RT -RTT 0 1
and no inversion is actually necessary.
One more possible optimization... T' = -RTT can also be computed like this:
T'X = -(RX dot T) T'Y = -(RY dot T) T'Z = -(RZ dot T)
where RX, RY, and RZ are the first, second, and third columns of R.