vsMatrix4x4 vsMatrix4x4::Inverse() const { /* NB. OpenGL Matrices are COLUMN major. */ /* Here's some shorthand converting standard (row,column) to index. */ if( m41 != 0.0f || m42 != 0.0f || m43 != 0.0f || m44 != 1.0f ) { return InverseGeneral(); } /* Inverse = adjoint / det. (See linear algebra texts.)*/ /* Allow out == in, but don't do an extra copy */ vsMatrix4x4 temp; temp.x.x= m22 * m33 - m23 * m32; temp.x.y= m23 * m31 - m21 * m33; temp.x.z= m21 * m32 - m22 * m31; /* Compute determinant as early as possible using these cofactors. */ register float det; det= m11 * temp.x.x + m12 * temp.x.y + m13 * temp.x.z; /* Run singularity test. */ if (det == 0.0) { vsLog("invert_matrix: Warning: Singular matrix."); return vsMatrix4x4::Identity; } float d12, d13, d23, d24, d34, d41; register float im11, im12, im13, im14; det= 1.0f / det; /* Compute rest of inverse. */ temp.x.x *= det; temp.x.y *= det; temp.x.z *= det; temp.x.w = 0.0f; im11= m11 * det; im12= m12 * det; im13= m13 * det; im14= m14 * det; temp.y.x = im13 * m32 - im12 * m33; temp.y.y = im11 * m33 - im13 * m31; temp.y.z = im12 * m31 - im11 * m32; temp.y.w = 0.0f; /* Pre-compute 2x2 dets for first two rows when computing */ /* cofactors of last two rows. */ d12 = im11*m22 - m21*im12; d13 = im11*m23 - m21*im13; d23 = im12*m23 - m22*im13; d24 = im12*m24 - m22*im14; d34 = im13*m24 - m23*im14; d41 = im14*m21 - m24*im11; temp.z.x = d23; temp.z.y = -d13; temp.z.z = d12; temp.z.w = 0.0f; temp.w.x = -(m32 * d34 - m33 * d24 + m34 * d23); temp.w.y = (m31 * d34 + m33 * d41 + m34 * d13); temp.w.z = -(m31 * d24 + m32 * d41 + m34 * d12); temp.w.w = 1.0f; #undef m11 #undef m12 #undef m13 #undef m14 #undef m21 #undef m22 #undef m23 #undef m24 #undef m31 #undef m32 #undef m33 #undef m34 #undef m41 #undef m42 #undef m43 #undef m44 return temp; }
inline VMatrix VMatrix::operator~() const { VMatrix mRet; InverseGeneral( mRet ); return mRet; }