home *** CD-ROM | disk | FTP | other *** search
/ GameStar 2006 March / Gamestar_82_2006-03_dvd.iso / DVDStar / Editace / quake4_sdkv10.exe / source / idlib / math / Matrix.cpp < prev    next >
C/C++ Source or Header  |  2005-11-14  |  252KB  |  8,230 lines
  1.  
  2. #include "../precompiled.h"
  3. #pragma hdrstop
  4.  
  5.  
  6. //===============================================================
  7. //
  8. //    idMat2
  9. //
  10. //===============================================================
  11.  
  12. idMat2 mat2_zero( idVec2( 0, 0 ), idVec2( 0, 0 ) );
  13. idMat2 mat2_identity( idVec2( 1, 0 ), idVec2( 0, 1 ) );
  14.  
  15. /*
  16. ============
  17. idMat2::InverseSelf
  18. ============
  19. */
  20. bool idMat2::InverseSelf( void ) {
  21.     // 2+4 = 6 multiplications
  22.     //         1 division
  23.     double det, invDet, a;
  24.  
  25.     det = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
  26.  
  27.     if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  28.         return false;
  29.     }
  30.  
  31.     invDet = 1.0f / det;
  32.  
  33.     a = mat[0][0];
  34.     mat[0][0] =   mat[1][1] * invDet;
  35.     mat[0][1] = - mat[0][1] * invDet;
  36.     mat[1][0] = - mat[1][0] * invDet;
  37.     mat[1][1] =   a * invDet;
  38.  
  39.     return true;
  40. }
  41.  
  42. /*
  43. ============
  44. idMat2::InverseFastSelf
  45. ============
  46. */
  47. bool idMat2::InverseFastSelf( void ) {
  48. #if 1
  49.     // 2+4 = 6 multiplications
  50.     //         1 division
  51.     double det, invDet, a;
  52.  
  53.     det = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
  54.  
  55.     if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  56.         return false;
  57.     }
  58.  
  59.     invDet = 1.0f / det;
  60.  
  61.     a = mat[0][0];
  62.     mat[0][0] =   mat[1][1] * invDet;
  63.     mat[0][1] = - mat[0][1] * invDet;
  64.     mat[1][0] = - mat[1][0] * invDet;
  65.     mat[1][1] =   a * invDet;
  66.  
  67.     return true;
  68. #else
  69.     // 2*4 = 8 multiplications
  70.     //         2 division
  71.     float *mat = reinterpret_cast<float *>(this);
  72.     double d, di;
  73.     float s;
  74.  
  75.     di = mat[0];
  76.     s = di;
  77.     mat[0*2+0] = d = 1.0f / di;
  78.     mat[0*2+1] *= d;
  79.     d = -d;
  80.     mat[1*2+0] *= d;
  81.     d = mat[1*2+0] * di;
  82.     mat[1*2+1] += mat[0*2+1] * d;
  83.     di = mat[1*2+1];
  84.     s *= di;
  85.     mat[1*2+1] = d = 1.0f / di;
  86.     mat[1*2+0] *= d;
  87.     d = -d;
  88.     mat[0*2+1] *= d;
  89.     d = mat[0*2+1] * di;
  90.     mat[0*2+0] += mat[1*2+0] * d;
  91.  
  92.     return ( s != 0.0f && !FLOAT_IS_NAN( s ) );
  93. #endif
  94. }
  95.  
  96. /*
  97. =============
  98. idMat2::ToString
  99. =============
  100. */
  101. const char *idMat2::ToString( int precision ) const {
  102.     return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
  103. }
  104.  
  105.  
  106. //===============================================================
  107. //
  108. //    idMat3
  109. //
  110. //===============================================================
  111.  
  112. idMat3 mat3_zero( idVec3( 0, 0, 0 ), idVec3( 0, 0, 0 ), idVec3( 0, 0, 0 ) );
  113. idMat3 mat3_identity( idVec3( 1, 0, 0 ), idVec3( 0, 1, 0 ), idVec3( 0, 0, 1 ) );
  114.  
  115. /*
  116. ============
  117. idMat3::ToAngles
  118. ============
  119. */
  120. idAngles idMat3::ToAngles( void ) const {
  121.     idAngles    angles;
  122.     double        theta;
  123.     double        cp;
  124.     float        sp;
  125.  
  126.     sp = mat[ 0 ][ 2 ];
  127.  
  128.     // cap off our sin value so that we don't get any NANs
  129.     if ( sp > 1.0f ) {
  130.         sp = 1.0f;
  131.     } else if ( sp < -1.0f ) {
  132.         sp = -1.0f;
  133.     }
  134.  
  135.     theta = -asin( sp );
  136.     cp = idMath::Cos( theta );
  137.  
  138. // RAVEN BEGIN
  139. // jscott: renamed to prevent name clash
  140.     if ( cp > 8192.0f * idMath::FLOAT_EPSILON ) {
  141. // RAVEN END
  142.         angles.pitch    = RAD2DEG( theta );
  143.         angles.yaw        = RAD2DEG( idMath::ATan( mat[ 0 ][ 1 ], mat[ 0 ][ 0 ] ) );
  144.         angles.roll        = RAD2DEG( idMath::ATan( mat[ 1 ][ 2 ], mat[ 2 ][ 2 ] ) );
  145.     } else {
  146.         angles.pitch    = RAD2DEG( theta );
  147.         angles.yaw        = RAD2DEG( -idMath::ATan( mat[ 1 ][ 0 ], mat[ 1 ][ 1 ] ) );
  148.         angles.roll        = 0;
  149.     }
  150.     return angles;
  151. }
  152.  
  153. // RAVEN BEGIN
  154.  
  155. // idMat3::RotateAbsolute -- rotates around world x, y, or z
  156. void idMat3::RotateAbsolute(int whichAxis, float howManyDegrees)
  157. {
  158.     idMat3    rotation;
  159.  
  160. // RAVEN BEGIN
  161.     float    sinVal, cosVal;
  162.  
  163.     idMath::SinCos( DEG2RAD( howManyDegrees), sinVal, cosVal );
  164. // RAVEN END
  165.  
  166.     rotation.Identity();
  167.     switch(whichAxis)
  168.     {
  169.     case 0:    // x-axis
  170.         rotation[1][1] = cosVal;
  171.         rotation[1][2] = sinVal;
  172.         rotation[2][1] = -sinVal;
  173.         rotation[2][2] = cosVal;
  174.         break;
  175.     case 1:    // y-axis
  176.         rotation[0][0] = cosVal;
  177.         rotation[0][2] = -sinVal;
  178.         rotation[2][0] = sinVal;
  179.         rotation[2][2] = cosVal;
  180.         break;
  181.     case 2:    // z-axis
  182.         rotation[0][0] = cosVal;
  183.         rotation[0][1] = sinVal;
  184.         rotation[1][0] = -sinVal;
  185.         rotation[1][1] = cosVal;
  186.         break;
  187.     default:
  188.         assert(!"idMat3::RotateAbsolute -- valid axis numbers are 0, 1, and 2");
  189.         return;
  190.     }
  191.     *this *= rotation;
  192. }
  193.  
  194. // idMat3::RotateRelative -- rotates around the fwd, left, or up vector of the matrix
  195. void idMat3::RotateRelative(int whichAxis, float howManyDegrees)
  196. {
  197.     idMat3    rotation;
  198.  
  199. // RAVEN BEGIN
  200.     float    sinVal, cosVal;
  201.  
  202.     idMath::SinCos( DEG2RAD( howManyDegrees), sinVal, cosVal );
  203. // RAVEN END
  204.  
  205.     rotation.Identity();
  206.     switch(whichAxis)
  207.     {
  208.     case 0:    // x-axis
  209.         rotation[1][1] = cosVal;
  210.         rotation[1][2] = sinVal;
  211.         rotation[2][1] = -sinVal;
  212.         rotation[2][2] = cosVal;
  213.         break;
  214.     case 1:    // y-axis
  215.         rotation[0][0] = cosVal;
  216.         rotation[0][2] = -sinVal;
  217.         rotation[2][0] = sinVal;
  218.         rotation[2][2] = cosVal;
  219.         break;
  220.     case 2:    // z-axis
  221.         rotation[0][0] = cosVal;
  222.         rotation[0][1] = sinVal;
  223.         rotation[1][0] = -sinVal;
  224.         rotation[1][1] = cosVal;
  225.         break;
  226.     default:
  227.         assert(!"idMat3::RotateRelative -- valid axis numbers are 0, 1, and 2");
  228.         return;
  229.     }
  230.     *this = rotation * (*this);
  231. }
  232.  
  233. // idMat3::RotateArbitrary -- rotates around the given unit vector
  234. void idMat3::RotateArbitrary(const idVec3 &rotAxis, float howManyDegrees)
  235. {
  236. // RAVEN BEGIN
  237.     float    sinVal, cosVal;
  238.  
  239.     idMath::SinCos( DEG2RAD( howManyDegrees), sinVal, cosVal );
  240. // RAVEN END
  241.  
  242.     float    d = idMath::Sqrt(rotAxis[1]*rotAxis[1] + rotAxis[2]*rotAxis[2]);
  243.  
  244.     // if the rotation axis turns out to be one of the world axes, just do a RotateAbsolute on it
  245.     if (rotAxis.Compare(idVec3(1, 0, 0)))
  246.     {
  247.         RotateAbsolute(0, howManyDegrees);
  248.         return;
  249.     }
  250.     if (rotAxis.Compare(idVec3(-1, 0, 0)))
  251.     {
  252.         RotateAbsolute(0, -howManyDegrees);
  253.         return;
  254.     }
  255.     if (rotAxis.Compare(idVec3(0, 1, 0)))
  256.     {
  257.         RotateAbsolute(1, howManyDegrees);
  258.         return;
  259.     }
  260.     if (rotAxis.Compare(idVec3(0, -1, 0)))
  261.     {
  262.         RotateAbsolute(1, -howManyDegrees);
  263.         return;
  264.     }
  265.     if (rotAxis.Compare(idVec3(0, 0, 1)))
  266.     {
  267.         RotateAbsolute(2, howManyDegrees);
  268.         return;
  269.     }
  270.     if (rotAxis.Compare(idVec3(0, 0, -1)))
  271.     {
  272.         RotateAbsolute(2, -howManyDegrees);
  273.         return;
  274.     }
  275.  
  276.     
  277.     idMat3    rotationX(1, 0, 0, 0, rotAxis[2]/d, rotAxis[1]/d, 0, -rotAxis[1]/d, rotAxis[2]/d);
  278.     idMat3    rotationY(d, 0, rotAxis[0], 0, 1, 0, -rotAxis[0], 0, d);
  279.     idMat3    rotationZ(cosVal, sinVal, 0, -sinVal, cosVal, 0, 0, 0, 1);
  280.     idMat3    rotationXinv = rotationX.Inverse();
  281.     idMat3    rotationYinv = rotationY.Inverse();
  282.     idMat3    tempMat;
  283.  
  284.     tempMat = rotationYinv * rotationXinv;
  285.     tempMat = rotationZ * tempMat;
  286.     tempMat = rotationY * tempMat;
  287.     tempMat = rotationX * tempMat;
  288.     *this = *this * tempMat;
  289. }
  290.  
  291. // RAVEN END
  292.  
  293. /*
  294. ============
  295. idMat3::ToQuat
  296. ============
  297. */
  298. idQuat idMat3::ToQuat( void ) const {
  299.     idQuat        q;
  300.     float        trace;
  301.     float        s;
  302.     float        t;
  303.     int         i;
  304.     int            j;
  305.     int            k;
  306.  
  307.     static int     next[ 3 ] = { 1, 2, 0 };
  308.  
  309.     trace = mat[ 0 ][ 0 ] + mat[ 1 ][ 1 ] + mat[ 2 ][ 2 ];
  310.  
  311.     if ( trace > 0.0f ) {
  312.  
  313.         t = trace + 1.0f;
  314.         s = idMath::InvSqrt( t ) * 0.5f;
  315.  
  316.         q[3] = s * t;
  317.         q[0] = ( mat[ 2 ][ 1 ] - mat[ 1 ][ 2 ] ) * s;
  318.         q[1] = ( mat[ 0 ][ 2 ] - mat[ 2 ][ 0 ] ) * s;
  319.         q[2] = ( mat[ 1 ][ 0 ] - mat[ 0 ][ 1 ] ) * s;
  320.  
  321.     } else {
  322.  
  323.         i = 0;
  324.         if ( mat[ 1 ][ 1 ] > mat[ 0 ][ 0 ] ) {
  325.             i = 1;
  326.         }
  327.         if ( mat[ 2 ][ 2 ] > mat[ i ][ i ] ) {
  328.             i = 2;
  329.         }
  330.         j = next[ i ];
  331.         k = next[ j ];
  332.  
  333.         t = ( mat[ i ][ i ] - ( mat[ j ][ j ] + mat[ k ][ k ] ) ) + 1.0f;
  334.         s = idMath::InvSqrt( t ) * 0.5f;
  335.  
  336.         q[i] = s * t;
  337.         q[3] = ( mat[ k ][ j ] - mat[ j ][ k ] ) * s;
  338.         q[j] = ( mat[ j ][ i ] + mat[ i ][ j ] ) * s;
  339.         q[k] = ( mat[ k ][ i ] + mat[ i ][ k ] ) * s;
  340.     }
  341.     return q;
  342. }
  343.  
  344. /*
  345. ============
  346. idMat3::ToCQuat
  347. ============
  348. */
  349. idCQuat idMat3::ToCQuat( void ) const {
  350.     idQuat q = ToQuat();
  351.     if ( q.w < 0.0f ) {
  352.         return idCQuat( -q.x, -q.y, -q.z );
  353.     }
  354.     return idCQuat( q.x, q.y, q.z );
  355. }
  356.  
  357. /*
  358. ============
  359. idMat3::ToRotation
  360. ============
  361. */
  362. idRotation idMat3::ToRotation( void ) const {
  363.     idRotation    r;
  364.     float        trace;
  365.     float        s;
  366.     float        t;
  367.     int         i;
  368.     int            j;
  369.     int            k;
  370.     static int     next[ 3 ] = { 1, 2, 0 };
  371.  
  372.     trace = mat[ 0 ][ 0 ] + mat[ 1 ][ 1 ] + mat[ 2 ][ 2 ];
  373.     if ( trace > 0.0f ) {
  374.  
  375.         t = trace + 1.0f;
  376.         s = idMath::InvSqrt( t ) * 0.5f;
  377.     
  378.         r.angle = s * t;
  379.         r.vec[0] = ( mat[ 2 ][ 1 ] - mat[ 1 ][ 2 ] ) * s;
  380.         r.vec[1] = ( mat[ 0 ][ 2 ] - mat[ 2 ][ 0 ] ) * s;
  381.         r.vec[2] = ( mat[ 1 ][ 0 ] - mat[ 0 ][ 1 ] ) * s;
  382.  
  383.     } else {
  384.  
  385.         i = 0;
  386.         if ( mat[ 1 ][ 1 ] > mat[ 0 ][ 0 ] ) {
  387.             i = 1;
  388.         }
  389.         if ( mat[ 2 ][ 2 ] > mat[ i ][ i ] ) {
  390.             i = 2;
  391.         }
  392.         j = next[ i ];  
  393.         k = next[ j ];
  394.     
  395.         t = ( mat[ i ][ i ] - ( mat[ j ][ j ] + mat[ k ][ k ] ) ) + 1.0f;
  396.         s = idMath::InvSqrt( t ) * 0.5f;
  397.     
  398.         r.vec[i]    = s * t;
  399.         r.angle        = ( mat[ k ][ j ] - mat[ j ][ k ] ) * s;
  400.         r.vec[j]    = ( mat[ j ][ i ] + mat[ i ][ j ] ) * s;
  401.         r.vec[k]    = ( mat[ k ][ i ] + mat[ i ][ k ] ) * s;
  402.     }
  403.     r.angle = idMath::ACos( r.angle );
  404.     if ( idMath::Fabs( r.angle ) < 1e-10f ) {
  405.         r.vec.Set( 0.0f, 0.0f, 1.0f );
  406.         r.angle = 0.0f;
  407.     } else {
  408.         //vec *= (1.0f / sin( angle ));
  409.         r.vec.Normalize();
  410.         r.vec.FixDegenerateNormal();
  411.         r.angle *= 2.0f * idMath::M_RAD2DEG;
  412.     }
  413.  
  414.     r.origin.Zero();
  415.     r.axis = *this;
  416.     r.axisValid = true;
  417.     return r;
  418. }
  419.  
  420. /*
  421. =================
  422. idMat3::ToAngularVelocity
  423. =================
  424. */
  425. idVec3 idMat3::ToAngularVelocity( void ) const {
  426.     idRotation rotation = ToRotation();
  427.     return rotation.GetVec() * DEG2RAD( rotation.GetAngle() );
  428. }
  429.  
  430. /*
  431. ============
  432. idMat3::Determinant
  433. ============
  434. */
  435. float idMat3::Determinant( void ) const {
  436.  
  437.     float det2_12_01 = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];
  438.     float det2_12_02 = mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0];
  439.     float det2_12_12 = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1];
  440.  
  441.     return mat[0][0] * det2_12_12 - mat[0][1] * det2_12_02 + mat[0][2] * det2_12_01;
  442. }
  443.  
  444. /*
  445. ============
  446. idMat3::InverseSelf
  447. ============
  448. */
  449. bool idMat3::InverseSelf( void ) {
  450.     // 18+3+9 = 30 multiplications
  451.     //             1 division
  452.     idMat3 inverse;
  453.     double det, invDet;
  454.  
  455.     inverse[0][0] = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1];
  456.     inverse[1][0] = mat[1][2] * mat[2][0] - mat[1][0] * mat[2][2];
  457.     inverse[2][0] = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];
  458.  
  459.     det = mat[0][0] * inverse[0][0] + mat[0][1] * inverse[1][0] + mat[0][2] * inverse[2][0];
  460.  
  461.     if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  462.         return false;
  463.     }
  464.  
  465.     invDet = 1.0f / det;
  466.  
  467.     inverse[0][1] = mat[0][2] * mat[2][1] - mat[0][1] * mat[2][2];
  468.     inverse[0][2] = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
  469.     inverse[1][1] = mat[0][0] * mat[2][2] - mat[0][2] * mat[2][0];
  470.     inverse[1][2] = mat[0][2] * mat[1][0] - mat[0][0] * mat[1][2];
  471.     inverse[2][1] = mat[0][1] * mat[2][0] - mat[0][0] * mat[2][1];
  472.     inverse[2][2] = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
  473.  
  474.     mat[0][0] = inverse[0][0] * invDet;
  475.     mat[0][1] = inverse[0][1] * invDet;
  476.     mat[0][2] = inverse[0][2] * invDet;
  477.  
  478.     mat[1][0] = inverse[1][0] * invDet;
  479.     mat[1][1] = inverse[1][1] * invDet;
  480.     mat[1][2] = inverse[1][2] * invDet;
  481.  
  482.     mat[2][0] = inverse[2][0] * invDet;
  483.     mat[2][1] = inverse[2][1] * invDet;
  484.     mat[2][2] = inverse[2][2] * invDet;
  485.  
  486.     return true;
  487. }
  488.  
  489. /*
  490. ============
  491. idMat3::InverseFastSelf
  492. ============
  493. */
  494. bool idMat3::InverseFastSelf( void ) {
  495. #if 1
  496.     // 18+3+9 = 30 multiplications
  497.     //             1 division
  498.     idMat3 inverse;
  499.     double det, invDet;
  500.  
  501.     inverse[0][0] = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1];
  502.     inverse[1][0] = mat[1][2] * mat[2][0] - mat[1][0] * mat[2][2];
  503.     inverse[2][0] = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];
  504.  
  505.     det = mat[0][0] * inverse[0][0] + mat[0][1] * inverse[1][0] + mat[0][2] * inverse[2][0];
  506.  
  507.     if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  508.         return false;
  509.     }
  510.  
  511.     invDet = 1.0f / det;
  512.  
  513.     inverse[0][1] = mat[0][2] * mat[2][1] - mat[0][1] * mat[2][2];
  514.     inverse[0][2] = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
  515.     inverse[1][1] = mat[0][0] * mat[2][2] - mat[0][2] * mat[2][0];
  516.     inverse[1][2] = mat[0][2] * mat[1][0] - mat[0][0] * mat[1][2];
  517.     inverse[2][1] = mat[0][1] * mat[2][0] - mat[0][0] * mat[2][1];
  518.     inverse[2][2] = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
  519.  
  520.     mat[0][0] = inverse[0][0] * invDet;
  521.     mat[0][1] = inverse[0][1] * invDet;
  522.     mat[0][2] = inverse[0][2] * invDet;
  523.  
  524.     mat[1][0] = inverse[1][0] * invDet;
  525.     mat[1][1] = inverse[1][1] * invDet;
  526.     mat[1][2] = inverse[1][2] * invDet;
  527.  
  528.     mat[2][0] = inverse[2][0] * invDet;
  529.     mat[2][1] = inverse[2][1] * invDet;
  530.     mat[2][2] = inverse[2][2] * invDet;
  531.  
  532.     return true;
  533. #elif 0
  534.     // 3*10 = 30 multiplications
  535.     //           3 divisions
  536.     float *mat = reinterpret_cast<float *>(this);
  537.     float s;
  538.     double d, di;
  539.  
  540.     di = mat[0];
  541.     s = di;
  542.     mat[0] = d = 1.0f / di;
  543.     mat[1] *= d;
  544.     mat[2] *= d;
  545.     d = -d;
  546.     mat[3] *= d;
  547.     mat[6] *= d;
  548.     d = mat[3] * di;
  549.     mat[4] += mat[1] * d;
  550.     mat[5] += mat[2] * d;
  551.     d = mat[6] * di;
  552.     mat[7] += mat[1] * d;
  553.     mat[8] += mat[2] * d;
  554.     di = mat[4];
  555.     s *= di;
  556.     mat[4] = d = 1.0f / di;
  557.     mat[3] *= d;
  558.     mat[5] *= d;
  559.     d = -d;
  560.     mat[1] *= d;
  561.     mat[7] *= d;
  562.     d = mat[1] * di;
  563.     mat[0] += mat[3] * d;
  564.     mat[2] += mat[5] * d;
  565.     d = mat[7] * di;
  566.     mat[6] += mat[3] * d;
  567.     mat[8] += mat[5] * d;
  568.     di = mat[8];
  569.     s *= di;
  570.     mat[8] = d = 1.0f / di;
  571.     mat[6] *= d;
  572.     mat[7] *= d;
  573.     d = -d;
  574.     mat[2] *= d;
  575.     mat[5] *= d;
  576.     d = mat[2] * di;
  577.     mat[0] += mat[6] * d;
  578.     mat[1] += mat[7] * d;
  579.     d = mat[5] * di;
  580.     mat[3] += mat[6] * d;
  581.     mat[4] += mat[7] * d;
  582.  
  583.     return ( s != 0.0f && !FLOAT_IS_NAN( s ) );
  584. #else
  585.     //    4*2+4*4 = 24 multiplications
  586.     //        2*1 =  2 divisions
  587.     idMat2 r0;
  588.     float r1[2], r2[2], r3;
  589.     float det, invDet;
  590.     float *mat = reinterpret_cast<float *>(this);
  591.  
  592.     // r0 = m0.Inverse();    // 2x2
  593.     det = mat[0*3+0] * mat[1*3+1] - mat[0*3+1] * mat[1*3+0];
  594.  
  595.     if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  596.         return false;
  597.     }
  598.  
  599.     invDet = 1.0f / det;
  600.  
  601.     r0[0][0] =   mat[1*3+1] * invDet;
  602.     r0[0][1] = - mat[0*3+1] * invDet;
  603.     r0[1][0] = - mat[1*3+0] * invDet;
  604.     r0[1][1] =   mat[0*3+0] * invDet;
  605.  
  606.     // r1 = r0 * m1;        // 2x1 = 2x2 * 2x1
  607.     r1[0] = r0[0][0] * mat[0*3+2] + r0[0][1] * mat[1*3+2];
  608.     r1[1] = r0[1][0] * mat[0*3+2] + r0[1][1] * mat[1*3+2];
  609.  
  610.     // r2 = m2 * r1;        // 1x1 = 1x2 * 2x1
  611.     r2[0] = mat[2*3+0] * r1[0] + mat[2*3+1] * r1[1];
  612.  
  613.     // r3 = r2 - m3;        // 1x1 = 1x1 - 1x1
  614.     r3 = r2[0] - mat[2*3+2];
  615.  
  616.     // r3.InverseSelf();
  617.     if ( idMath::Fabs( r3 ) < MATRIX_INVERSE_EPSILON ) {
  618.         return false;
  619.     }
  620.  
  621.     r3 = 1.0f / r3;
  622.  
  623.     // r2 = m2 * r0;        // 1x2 = 1x2 * 2x2
  624.     r2[0] = mat[2*3+0] * r0[0][0] + mat[2*3+1] * r0[1][0];
  625.     r2[1] = mat[2*3+0] * r0[0][1] + mat[2*3+1] * r0[1][1];
  626.  
  627.     // m2 = r3 * r2;        // 1x2 = 1x1 * 1x2
  628.     mat[2*3+0] = r3 * r2[0];
  629.     mat[2*3+1] = r3 * r2[1];
  630.  
  631.     // m0 = r0 - r1 * m2;    // 2x2 - 2x1 * 1x2
  632.     mat[0*3+0] = r0[0][0] - r1[0] * mat[2*3+0];
  633.     mat[0*3+1] = r0[0][1] - r1[0] * mat[2*3+1];
  634.     mat[1*3+0] = r0[1][0] - r1[1] * mat[2*3+0];
  635.     mat[1*3+1] = r0[1][1] - r1[1] * mat[2*3+1];
  636.  
  637.     // m1 = r1 * r3;        // 2x1 = 2x1 * 1x1
  638.     mat[0*3+2] = r1[0] * r3;
  639.     mat[1*3+2] = r1[1] * r3;
  640.  
  641.     // m3 = -r3;
  642.     mat[2*3+2] = -r3;
  643.  
  644.     return true;
  645. #endif
  646. }
  647.  
  648. /*
  649. ============
  650. idMat3::InertiaTranslate
  651. ============
  652. */
  653. idMat3 idMat3::InertiaTranslate( const float mass, const idVec3 ¢erOfMass, const idVec3 &translation ) const {
  654.     idMat3 m;
  655.     idVec3 newCenter;
  656.  
  657.     newCenter = centerOfMass + translation;
  658.  
  659.     m[0][0] = mass * ( ( centerOfMass[1] * centerOfMass[1] + centerOfMass[2] * centerOfMass[2] )
  660.                 - ( newCenter[1] * newCenter[1] + newCenter[2] * newCenter[2] ) );
  661.     m[1][1] = mass * ( ( centerOfMass[0] * centerOfMass[0] + centerOfMass[2] * centerOfMass[2] )
  662.                 - ( newCenter[0] * newCenter[0] + newCenter[2] * newCenter[2] ) );
  663.     m[2][2] = mass * ( ( centerOfMass[0] * centerOfMass[0] + centerOfMass[1] * centerOfMass[1] )
  664.                 - ( newCenter[0] * newCenter[0] + newCenter[1] * newCenter[1] ) );
  665.  
  666.     m[0][1] = m[1][0] = mass * ( newCenter[0] * newCenter[1] - centerOfMass[0] * centerOfMass[1] );
  667.     m[1][2] = m[2][1] = mass * ( newCenter[1] * newCenter[2] - centerOfMass[1] * centerOfMass[2] );
  668.     m[0][2] = m[2][0] = mass * ( newCenter[0] * newCenter[2] - centerOfMass[0] * centerOfMass[2] );
  669.  
  670.     return (*this) + m;
  671. }
  672.  
  673. /*
  674. ============
  675. idMat3::InertiaTranslateSelf
  676. ============
  677. */
  678. idMat3 &idMat3::InertiaTranslateSelf( const float mass, const idVec3 ¢erOfMass, const idVec3 &translation ) {
  679.     idMat3 m;
  680.     idVec3 newCenter;
  681.  
  682.     newCenter = centerOfMass + translation;
  683.  
  684.     m[0][0] = mass * ( ( centerOfMass[1] * centerOfMass[1] + centerOfMass[2] * centerOfMass[2] )
  685.                 - ( newCenter[1] * newCenter[1] + newCenter[2] * newCenter[2] ) );
  686.     m[1][1] = mass * ( ( centerOfMass[0] * centerOfMass[0] + centerOfMass[2] * centerOfMass[2] )
  687.                 - ( newCenter[0] * newCenter[0] + newCenter[2] * newCenter[2] ) );
  688.     m[2][2] = mass * ( ( centerOfMass[0] * centerOfMass[0] + centerOfMass[1] * centerOfMass[1] )
  689.                 - ( newCenter[0] * newCenter[0] + newCenter[1] * newCenter[1] ) );
  690.  
  691.     m[0][1] = m[1][0] = mass * ( newCenter[0] * newCenter[1] - centerOfMass[0] * centerOfMass[1] );
  692.     m[1][2] = m[2][1] = mass * ( newCenter[1] * newCenter[2] - centerOfMass[1] * centerOfMass[2] );
  693.     m[0][2] = m[2][0] = mass * ( newCenter[0] * newCenter[2] - centerOfMass[0] * centerOfMass[2] );
  694.  
  695.     (*this) += m;
  696.  
  697.     return (*this);
  698. }
  699.  
  700. /*
  701. ============
  702. idMat3::InertiaRotate
  703. ============
  704. */
  705. idMat3 idMat3::InertiaRotate( const idMat3 &rotation ) const {
  706.     // NOTE: the rotation matrix is stored column-major
  707.     return rotation.Transpose() * (*this) * rotation;
  708. }
  709.  
  710. /*
  711. ============
  712. idMat3::InertiaRotateSelf
  713. ============
  714. */
  715. idMat3 &idMat3::InertiaRotateSelf( const idMat3 &rotation ) {
  716.     // NOTE: the rotation matrix is stored column-major
  717.     *this = rotation.Transpose() * (*this) * rotation;
  718.     return *this;
  719. }
  720.  
  721. /*
  722. =============
  723. idMat3::ToString
  724. =============
  725. */
  726. const char *idMat3::ToString( int precision ) const {
  727.     return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
  728. }
  729.  
  730.  
  731. //===============================================================
  732. //
  733. //    idMat4
  734. //
  735. //===============================================================
  736.  
  737. idMat4 mat4_zero( idVec4( 0, 0, 0, 0 ), idVec4( 0, 0, 0, 0 ), idVec4( 0, 0, 0, 0 ), idVec4( 0, 0, 0, 0 ) );
  738. idMat4 mat4_identity( idVec4( 1, 0, 0, 0 ), idVec4( 0, 1, 0, 0 ), idVec4( 0, 0, 1, 0 ), idVec4( 0, 0, 0, 1 ) );
  739.  
  740. /*
  741. ============
  742. idMat4::Transpose
  743. ============
  744. */
  745. idMat4 idMat4::Transpose( void ) const {
  746.     idMat4    transpose;
  747.     int        i, j;
  748.    
  749.     for( i = 0; i < 4; i++ ) {
  750.         for( j = 0; j < 4; j++ ) {
  751.             transpose[ i ][ j ] = mat[ j ][ i ];
  752.         }
  753.     }
  754.     return transpose;
  755. }
  756.  
  757. /*
  758. ============
  759. idMat4::TransposeSelf
  760. ============
  761. */
  762. idMat4 &idMat4::TransposeSelf( void ) {
  763.     float    temp;
  764.     int        i, j;
  765.    
  766.     for( i = 0; i < 4; i++ ) {
  767.         for( j = i + 1; j < 4; j++ ) {
  768.             temp = mat[ i ][ j ];
  769.             mat[ i ][ j ] = mat[ j ][ i ];
  770.             mat[ j ][ i ] = temp;
  771.         }
  772.     }
  773.     return *this;
  774. }
  775.  
  776. /*
  777. ============
  778. idMat4::Determinant
  779. ============
  780. */
  781. float idMat4::Determinant( void ) const {
  782.  
  783.     // 2x2 sub-determinants
  784.     float det2_01_01 = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
  785.     float det2_01_02 = mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0];
  786.     float det2_01_03 = mat[0][0] * mat[1][3] - mat[0][3] * mat[1][0];
  787.     float det2_01_12 = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
  788.     float det2_01_13 = mat[0][1] * mat[1][3] - mat[0][3] * mat[1][1];
  789.     float det2_01_23 = mat[0][2] * mat[1][3] - mat[0][3] * mat[1][2];
  790.  
  791.     // 3x3 sub-determinants
  792.     float det3_201_012 = mat[2][0] * det2_01_12 - mat[2][1] * det2_01_02 + mat[2][2] * det2_01_01;
  793.     float det3_201_013 = mat[2][0] * det2_01_13 - mat[2][1] * det2_01_03 + mat[2][3] * det2_01_01;
  794.     float det3_201_023 = mat[2][0] * det2_01_23 - mat[2][2] * det2_01_03 + mat[2][3] * det2_01_02;
  795.     float det3_201_123 = mat[2][1] * det2_01_23 - mat[2][2] * det2_01_13 + mat[2][3] * det2_01_12;
  796.  
  797.     return ( - det3_201_123 * mat[3][0] + det3_201_023 * mat[3][1] - det3_201_013 * mat[3][2] + det3_201_012 * mat[3][3] );
  798. }
  799.  
  800. /*
  801. ============
  802. idMat4::InverseSelf
  803. ============
  804. */
  805. bool idMat4::InverseSelf( void ) {
  806.     // 84+4+16 = 104 multiplications
  807.     //               1 division
  808.     double det, invDet;
  809.  
  810.     // 2x2 sub-determinants required to calculate 4x4 determinant
  811.     float det2_01_01 = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
  812.     float det2_01_02 = mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0];
  813.     float det2_01_03 = mat[0][0] * mat[1][3] - mat[0][3] * mat[1][0];
  814.     float det2_01_12 = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
  815.     float det2_01_13 = mat[0][1] * mat[1][3] - mat[0][3] * mat[1][1];
  816.     float det2_01_23 = mat[0][2] * mat[1][3] - mat[0][3] * mat[1][2];
  817.  
  818.     // 3x3 sub-determinants required to calculate 4x4 determinant
  819.     float det3_201_012 = mat[2][0] * det2_01_12 - mat[2][1] * det2_01_02 + mat[2][2] * det2_01_01;
  820.     float det3_201_013 = mat[2][0] * det2_01_13 - mat[2][1] * det2_01_03 + mat[2][3] * det2_01_01;
  821.     float det3_201_023 = mat[2][0] * det2_01_23 - mat[2][2] * det2_01_03 + mat[2][3] * det2_01_02;
  822.     float det3_201_123 = mat[2][1] * det2_01_23 - mat[2][2] * det2_01_13 + mat[2][3] * det2_01_12;
  823.  
  824.     det = ( - det3_201_123 * mat[3][0] + det3_201_023 * mat[3][1] - det3_201_013 * mat[3][2] + det3_201_012 * mat[3][3] );
  825.  
  826.     if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  827.         return false;
  828.     }
  829.  
  830.     invDet = 1.0f / det;
  831.  
  832.     // remaining 2x2 sub-determinants
  833.     float det2_03_01 = mat[0][0] * mat[3][1] - mat[0][1] * mat[3][0];
  834.     float det2_03_02 = mat[0][0] * mat[3][2] - mat[0][2] * mat[3][0];
  835.     float det2_03_03 = mat[0][0] * mat[3][3] - mat[0][3] * mat[3][0];
  836.     float det2_03_12 = mat[0][1] * mat[3][2] - mat[0][2] * mat[3][1];
  837.     float det2_03_13 = mat[0][1] * mat[3][3] - mat[0][3] * mat[3][1];
  838.     float det2_03_23 = mat[0][2] * mat[3][3] - mat[0][3] * mat[3][2];
  839.  
  840.     float det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0];
  841.     float det2_13_02 = mat[1][0] * mat[3][2] - mat[1][2] * mat[3][0];
  842.     float det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0];
  843.     float det2_13_12 = mat[1][1] * mat[3][2] - mat[1][2] * mat[3][1];
  844.     float det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1];
  845.     float det2_13_23 = mat[1][2] * mat[3][3] - mat[1][3] * mat[3][2];
  846.  
  847.     // remaining 3x3 sub-determinants
  848.     float det3_203_012 = mat[2][0] * det2_03_12 - mat[2][1] * det2_03_02 + mat[2][2] * det2_03_01;
  849.     float det3_203_013 = mat[2][0] * det2_03_13 - mat[2][1] * det2_03_03 + mat[2][3] * det2_03_01;
  850.     float det3_203_023 = mat[2][0] * det2_03_23 - mat[2][2] * det2_03_03 + mat[2][3] * det2_03_02;
  851.     float det3_203_123 = mat[2][1] * det2_03_23 - mat[2][2] * det2_03_13 + mat[2][3] * det2_03_12;
  852.  
  853.     float det3_213_012 = mat[2][0] * det2_13_12 - mat[2][1] * det2_13_02 + mat[2][2] * det2_13_01;
  854.     float det3_213_013 = mat[2][0] * det2_13_13 - mat[2][1] * det2_13_03 + mat[2][3] * det2_13_01;
  855.     float det3_213_023 = mat[2][0] * det2_13_23 - mat[2][2] * det2_13_03 + mat[2][3] * det2_13_02;
  856.     float det3_213_123 = mat[2][1] * det2_13_23 - mat[2][2] * det2_13_13 + mat[2][3] * det2_13_12;
  857.  
  858.     float det3_301_012 = mat[3][0] * det2_01_12 - mat[3][1] * det2_01_02 + mat[3][2] * det2_01_01;
  859.     float det3_301_013 = mat[3][0] * det2_01_13 - mat[3][1] * det2_01_03 + mat[3][3] * det2_01_01;
  860.     float det3_301_023 = mat[3][0] * det2_01_23 - mat[3][2] * det2_01_03 + mat[3][3] * det2_01_02;
  861.     float det3_301_123 = mat[3][1] * det2_01_23 - mat[3][2] * det2_01_13 + mat[3][3] * det2_01_12;
  862.  
  863.     mat[0][0] =    - det3_213_123 * invDet;
  864.     mat[1][0] = + det3_213_023 * invDet;
  865.     mat[2][0] = - det3_213_013 * invDet;
  866.     mat[3][0] = + det3_213_012 * invDet;
  867.  
  868.     mat[0][1] = + det3_203_123 * invDet;
  869.     mat[1][1] = - det3_203_023 * invDet;
  870.     mat[2][1] = + det3_203_013 * invDet;
  871.     mat[3][1] = - det3_203_012 * invDet;
  872.  
  873.     mat[0][2] = + det3_301_123 * invDet;
  874.     mat[1][2] = - det3_301_023 * invDet;
  875.     mat[2][2] = + det3_301_013 * invDet;
  876.     mat[3][2] = - det3_301_012 * invDet;
  877.  
  878.     mat[0][3] = - det3_201_123 * invDet;
  879.     mat[1][3] = + det3_201_023 * invDet;
  880.     mat[2][3] = - det3_201_013 * invDet;
  881.     mat[3][3] = + det3_201_012 * invDet;
  882.  
  883.     return true;
  884. }
  885.  
  886. /*
  887. ============
  888. idMat4::InverseFastSelf
  889. ============
  890. */
  891. bool idMat4::InverseFastSelf( void ) {
  892. #if 0
  893.     // 84+4+16 = 104 multiplications
  894.     //               1 division
  895.     double det, invDet;
  896.  
  897.     // 2x2 sub-determinants required to calculate 4x4 determinant
  898.     float det2_01_01 = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
  899.     float det2_01_02 = mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0];
  900.     float det2_01_03 = mat[0][0] * mat[1][3] - mat[0][3] * mat[1][0];
  901.     float det2_01_12 = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
  902.     float det2_01_13 = mat[0][1] * mat[1][3] - mat[0][3] * mat[1][1];
  903.     float det2_01_23 = mat[0][2] * mat[1][3] - mat[0][3] * mat[1][2];
  904.  
  905.     // 3x3 sub-determinants required to calculate 4x4 determinant
  906.     float det3_201_012 = mat[2][0] * det2_01_12 - mat[2][1] * det2_01_02 + mat[2][2] * det2_01_01;
  907.     float det3_201_013 = mat[2][0] * det2_01_13 - mat[2][1] * det2_01_03 + mat[2][3] * det2_01_01;
  908.     float det3_201_023 = mat[2][0] * det2_01_23 - mat[2][2] * det2_01_03 + mat[2][3] * det2_01_02;
  909.     float det3_201_123 = mat[2][1] * det2_01_23 - mat[2][2] * det2_01_13 + mat[2][3] * det2_01_12;
  910.  
  911.     det = ( - det3_201_123 * mat[3][0] + det3_201_023 * mat[3][1] - det3_201_013 * mat[3][2] + det3_201_012 * mat[3][3] );
  912.  
  913.     if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  914.         return false;
  915.     }
  916.  
  917.     invDet = 1.0f / det;
  918.  
  919.     // remaining 2x2 sub-determinants
  920.     float det2_03_01 = mat[0][0] * mat[3][1] - mat[0][1] * mat[3][0];
  921.     float det2_03_02 = mat[0][0] * mat[3][2] - mat[0][2] * mat[3][0];
  922.     float det2_03_03 = mat[0][0] * mat[3][3] - mat[0][3] * mat[3][0];
  923.     float det2_03_12 = mat[0][1] * mat[3][2] - mat[0][2] * mat[3][1];
  924.     float det2_03_13 = mat[0][1] * mat[3][3] - mat[0][3] * mat[3][1];
  925.     float det2_03_23 = mat[0][2] * mat[3][3] - mat[0][3] * mat[3][2];
  926.  
  927.     float det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0];
  928.     float det2_13_02 = mat[1][0] * mat[3][2] - mat[1][2] * mat[3][0];
  929.     float det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0];
  930.     float det2_13_12 = mat[1][1] * mat[3][2] - mat[1][2] * mat[3][1];
  931.     float det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1];
  932.     float det2_13_23 = mat[1][2] * mat[3][3] - mat[1][3] * mat[3][2];
  933.  
  934.     // remaining 3x3 sub-determinants
  935.     float det3_203_012 = mat[2][0] * det2_03_12 - mat[2][1] * det2_03_02 + mat[2][2] * det2_03_01;
  936.     float det3_203_013 = mat[2][0] * det2_03_13 - mat[2][1] * det2_03_03 + mat[2][3] * det2_03_01;
  937.     float det3_203_023 = mat[2][0] * det2_03_23 - mat[2][2] * det2_03_03 + mat[2][3] * det2_03_02;
  938.     float det3_203_123 = mat[2][1] * det2_03_23 - mat[2][2] * det2_03_13 + mat[2][3] * det2_03_12;
  939.  
  940.     float det3_213_012 = mat[2][0] * det2_13_12 - mat[2][1] * det2_13_02 + mat[2][2] * det2_13_01;
  941.     float det3_213_013 = mat[2][0] * det2_13_13 - mat[2][1] * det2_13_03 + mat[2][3] * det2_13_01;
  942.     float det3_213_023 = mat[2][0] * det2_13_23 - mat[2][2] * det2_13_03 + mat[2][3] * det2_13_02;
  943.     float det3_213_123 = mat[2][1] * det2_13_23 - mat[2][2] * det2_13_13 + mat[2][3] * det2_13_12;
  944.  
  945.     float det3_301_012 = mat[3][0] * det2_01_12 - mat[3][1] * det2_01_02 + mat[3][2] * det2_01_01;
  946.     float det3_301_013 = mat[3][0] * det2_01_13 - mat[3][1] * det2_01_03 + mat[3][3] * det2_01_01;
  947.     float det3_301_023 = mat[3][0] * det2_01_23 - mat[3][2] * det2_01_03 + mat[3][3] * det2_01_02;
  948.     float det3_301_123 = mat[3][1] * det2_01_23 - mat[3][2] * det2_01_13 + mat[3][3] * det2_01_12;
  949.  
  950.     mat[0][0] =    - det3_213_123 * invDet;
  951.     mat[1][0] = + det3_213_023 * invDet;
  952.     mat[2][0] = - det3_213_013 * invDet;
  953.     mat[3][0] = + det3_213_012 * invDet;
  954.  
  955.     mat[0][1] = + det3_203_123 * invDet;
  956.     mat[1][1] = - det3_203_023 * invDet;
  957.     mat[2][1] = + det3_203_013 * invDet;
  958.     mat[3][1] = - det3_203_012 * invDet;
  959.  
  960.     mat[0][2] = + det3_301_123 * invDet;
  961.     mat[1][2] = - det3_301_023 * invDet;
  962.     mat[2][2] = + det3_301_013 * invDet;
  963.     mat[3][2] = - det3_301_012 * invDet;
  964.  
  965.     mat[0][3] = - det3_201_123 * invDet;
  966.     mat[1][3] = + det3_201_023 * invDet;
  967.     mat[2][3] = - det3_201_013 * invDet;
  968.     mat[3][3] = + det3_201_012 * invDet;
  969.  
  970.     return true;
  971. #elif 0
  972.     // 4*18 = 72 multiplications
  973.     //           4 divisions
  974.     float *mat = reinterpret_cast<float *>(this);
  975.     float s;
  976.     double d, di;
  977.  
  978.     di = mat[0];
  979.     s = di;
  980.     mat[0] = d = 1.0f / di;
  981.     mat[1] *= d;
  982.     mat[2] *= d;
  983.     mat[3] *= d;
  984.     d = -d;
  985.     mat[4] *= d;
  986.     mat[8] *= d;
  987.     mat[12] *= d;
  988.     d = mat[4] * di;
  989.     mat[5] += mat[1] * d;
  990.     mat[6] += mat[2] * d;
  991.     mat[7] += mat[3] * d;
  992.     d = mat[8] * di;
  993.     mat[9] += mat[1] * d;
  994.     mat[10] += mat[2] * d;
  995.     mat[11] += mat[3] * d;
  996.     d = mat[12] * di;
  997.     mat[13] += mat[1] * d;
  998.     mat[14] += mat[2] * d;
  999.     mat[15] += mat[3] * d;
  1000.     di = mat[5];
  1001.     s *= di;
  1002.     mat[5] = d = 1.0f / di;
  1003.     mat[4] *= d;
  1004.     mat[6] *= d;
  1005.     mat[7] *= d;
  1006.     d = -d;
  1007.     mat[1] *= d;
  1008.     mat[9] *= d;
  1009.     mat[13] *= d;
  1010.     d = mat[1] * di;
  1011.     mat[0] += mat[4] * d;
  1012.     mat[2] += mat[6] * d;
  1013.     mat[3] += mat[7] * d;
  1014.     d = mat[9] * di;
  1015.     mat[8] += mat[4] * d;
  1016.     mat[10] += mat[6] * d;
  1017.     mat[11] += mat[7] * d;
  1018.     d = mat[13] * di;
  1019.     mat[12] += mat[4] * d;
  1020.     mat[14] += mat[6] * d;
  1021.     mat[15] += mat[7] * d;
  1022.     di = mat[10];
  1023.     s *= di;
  1024.     mat[10] = d = 1.0f / di;
  1025.     mat[8] *= d;
  1026.     mat[9] *= d;
  1027.     mat[11] *= d;
  1028.     d = -d;
  1029.     mat[2] *= d;
  1030.     mat[6] *= d;
  1031.     mat[14] *= d;
  1032.     d = mat[2] * di;
  1033.     mat[0] += mat[8] * d;
  1034.     mat[1] += mat[9] * d;
  1035.     mat[3] += mat[11] * d;
  1036.     d = mat[6] * di;
  1037.     mat[4] += mat[8] * d;
  1038.     mat[5] += mat[9] * d;
  1039.     mat[7] += mat[11] * d;
  1040.     d = mat[14] * di;
  1041.     mat[12] += mat[8] * d;
  1042.     mat[13] += mat[9] * d;
  1043.     mat[15] += mat[11] * d;
  1044.     di = mat[15];
  1045.     s *= di;
  1046.     mat[15] = d = 1.0f / di;
  1047.     mat[12] *= d;
  1048.     mat[13] *= d;
  1049.     mat[14] *= d;
  1050.     d = -d;
  1051.     mat[3] *= d;
  1052.     mat[7] *= d;
  1053.     mat[11] *= d;
  1054.     d = mat[3] * di;
  1055.     mat[0] += mat[12] * d;
  1056.     mat[1] += mat[13] * d;
  1057.     mat[2] += mat[14] * d;
  1058.     d = mat[7] * di;
  1059.     mat[4] += mat[12] * d;
  1060.     mat[5] += mat[13] * d;
  1061.     mat[6] += mat[14] * d;
  1062.     d = mat[11] * di;
  1063.     mat[8] += mat[12] * d;
  1064.     mat[9] += mat[13] * d;
  1065.     mat[10] += mat[14] * d;
  1066.  
  1067.     return ( s != 0.0f && !FLOAT_IS_NAN( s ) );
  1068. #else
  1069.     //    6*8+2*6 = 60 multiplications
  1070.     //        2*1 =  2 divisions
  1071.     idMat2 r0, r1, r2, r3;
  1072.     float a, det, invDet;
  1073.     float *mat = reinterpret_cast<float *>(this);
  1074.  
  1075.     // r0 = m0.Inverse();
  1076.     det = mat[0*4+0] * mat[1*4+1] - mat[0*4+1] * mat[1*4+0];
  1077.  
  1078.     if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  1079.         return false;
  1080.     }
  1081.  
  1082.     invDet = 1.0f / det;
  1083.  
  1084.     r0[0][0] =   mat[1*4+1] * invDet;
  1085.     r0[0][1] = - mat[0*4+1] * invDet;
  1086.     r0[1][0] = - mat[1*4+0] * invDet;
  1087.     r0[1][1] =   mat[0*4+0] * invDet;
  1088.  
  1089.     // r1 = r0 * m1;
  1090.     r1[0][0] = r0[0][0] * mat[0*4+2] + r0[0][1] * mat[1*4+2];
  1091.     r1[0][1] = r0[0][0] * mat[0*4+3] + r0[0][1] * mat[1*4+3];
  1092.     r1[1][0] = r0[1][0] * mat[0*4+2] + r0[1][1] * mat[1*4+2];
  1093.     r1[1][1] = r0[1][0] * mat[0*4+3] + r0[1][1] * mat[1*4+3];
  1094.  
  1095.     // r2 = m2 * r1;
  1096.     r2[0][0] = mat[2*4+0] * r1[0][0] + mat[2*4+1] * r1[1][0];
  1097.     r2[0][1] = mat[2*4+0] * r1[0][1] + mat[2*4+1] * r1[1][1];
  1098.     r2[1][0] = mat[3*4+0] * r1[0][0] + mat[3*4+1] * r1[1][0];
  1099.     r2[1][1] = mat[3*4+0] * r1[0][1] + mat[3*4+1] * r1[1][1];
  1100.  
  1101.     // r3 = r2 - m3;
  1102.     r3[0][0] = r2[0][0] - mat[2*4+2];
  1103.     r3[0][1] = r2[0][1] - mat[2*4+3];
  1104.     r3[1][0] = r2[1][0] - mat[3*4+2];
  1105.     r3[1][1] = r2[1][1] - mat[3*4+3];
  1106.  
  1107.     // r3.InverseSelf();
  1108.     det = r3[0][0] * r3[1][1] - r3[0][1] * r3[1][0];
  1109.  
  1110.     if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  1111.         return false;
  1112.     }
  1113.  
  1114.     invDet = 1.0f / det;
  1115.  
  1116.     a = r3[0][0];
  1117.     r3[0][0] =   r3[1][1] * invDet;
  1118.     r3[0][1] = - r3[0][1] * invDet;
  1119.     r3[1][0] = - r3[1][0] * invDet;
  1120.     r3[1][1] =   a * invDet;
  1121.  
  1122.     // r2 = m2 * r0;
  1123.     r2[0][0] = mat[2*4+0] * r0[0][0] + mat[2*4+1] * r0[1][0];
  1124.     r2[0][1] = mat[2*4+0] * r0[0][1] + mat[2*4+1] * r0[1][1];
  1125.     r2[1][0] = mat[3*4+0] * r0[0][0] + mat[3*4+1] * r0[1][0];
  1126.     r2[1][1] = mat[3*4+0] * r0[0][1] + mat[3*4+1] * r0[1][1];
  1127.  
  1128.     // m2 = r3 * r2;
  1129.     mat[2*4+0] = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0];
  1130.     mat[2*4+1] = r3[0][0] * r2[0][1] + r3[0][1] * r2[1][1];
  1131.     mat[3*4+0] = r3[1][0] * r2[0][0] + r3[1][1] * r2[1][0];
  1132.     mat[3*4+1] = r3[1][0] * r2[0][1] + r3[1][1] * r2[1][1];
  1133.  
  1134.     // m0 = r0 - r1 * m2;
  1135.     mat[0*4+0] = r0[0][0] - r1[0][0] * mat[2*4+0] - r1[0][1] * mat[3*4+0];
  1136.     mat[0*4+1] = r0[0][1] - r1[0][0] * mat[2*4+1] - r1[0][1] * mat[3*4+1];
  1137.     mat[1*4+0] = r0[1][0] - r1[1][0] * mat[2*4+0] - r1[1][1] * mat[3*4+0];
  1138.     mat[1*4+1] = r0[1][1] - r1[1][0] * mat[2*4+1] - r1[1][1] * mat[3*4+1];
  1139.  
  1140.     // m1 = r1 * r3;
  1141.     mat[0*4+2] = r1[0][0] * r3[0][0] + r1[0][1] * r3[1][0];
  1142.     mat[0*4+3] = r1[0][0] * r3[0][1] + r1[0][1] * r3[1][1];
  1143.     mat[1*4+2] = r1[1][0] * r3[0][0] + r1[1][1] * r3[1][0];
  1144.     mat[1*4+3] = r1[1][0] * r3[0][1] + r1[1][1] * r3[1][1];
  1145.  
  1146.     // m3 = -r3;
  1147.     mat[2*4+2] = -r3[0][0];
  1148.     mat[2*4+3] = -r3[0][1];
  1149.     mat[3*4+2] = -r3[1][0];
  1150.     mat[3*4+3] = -r3[1][1];
  1151.  
  1152.     return true;
  1153. #endif
  1154. }
  1155.  
  1156. /*
  1157. =============
  1158. idMat4::ToString
  1159. =============
  1160. */
  1161. const char *idMat4::ToString( int precision ) const {
  1162.     return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
  1163. }
  1164.  
  1165.  
  1166. //===============================================================
  1167. //
  1168. //    idMat5
  1169. //
  1170. //===============================================================
  1171.  
  1172. idMat5 mat5_zero( idVec5( 0, 0, 0, 0, 0 ), idVec5( 0, 0, 0, 0, 0 ), idVec5( 0, 0, 0, 0, 0 ), idVec5( 0, 0, 0, 0, 0 ), idVec5( 0, 0, 0, 0, 0 ) );
  1173. idMat5 mat5_identity( idVec5( 1, 0, 0, 0, 0 ), idVec5( 0, 1, 0, 0, 0 ), idVec5( 0, 0, 1, 0, 0 ), idVec5( 0, 0, 0, 1, 0 ), idVec5( 0, 0, 0, 0, 1 ) );
  1174.  
  1175. /*
  1176. ============
  1177. idMat5::Transpose
  1178. ============
  1179. */
  1180. idMat5 idMat5::Transpose( void ) const {
  1181.     idMat5    transpose;
  1182.     int        i, j;
  1183.    
  1184.     for( i = 0; i < 5; i++ ) {
  1185.         for( j = 0; j < 5; j++ ) {
  1186.             transpose[ i ][ j ] = mat[ j ][ i ];
  1187.         }
  1188.     }
  1189.     return transpose;
  1190. }
  1191.  
  1192. /*
  1193. ============
  1194. idMat5::TransposeSelf
  1195. ============
  1196. */
  1197. idMat5 &idMat5::TransposeSelf( void ) {
  1198.     float    temp;
  1199.     int        i, j;
  1200.    
  1201.     for( i = 0; i < 5; i++ ) {
  1202.         for( j = i + 1; j < 5; j++ ) {
  1203.             temp = mat[ i ][ j ];
  1204.             mat[ i ][ j ] = mat[ j ][ i ];
  1205.             mat[ j ][ i ] = temp;
  1206.         }
  1207.     }
  1208.     return *this;
  1209. }
  1210.  
  1211. /*
  1212. ============
  1213. idMat5::Determinant
  1214. ============
  1215. */
  1216. float idMat5::Determinant( void ) const {
  1217.  
  1218.     // 2x2 sub-determinants required to calculate 5x5 determinant
  1219.     float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0];
  1220.     float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0];
  1221.     float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0];
  1222.     float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0];
  1223.     float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1];
  1224.     float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1];
  1225.     float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1];
  1226.     float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2];
  1227.     float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2];
  1228.     float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3];
  1229.  
  1230.     // 3x3 sub-determinants required to calculate 5x5 determinant
  1231.     float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01;
  1232.     float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01;
  1233.     float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01;
  1234.     float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02;
  1235.     float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02;
  1236.     float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03;
  1237.     float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12;
  1238.     float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12;
  1239.     float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13;
  1240.     float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23;
  1241.  
  1242.     // 4x4 sub-determinants required to calculate 5x5 determinant
  1243.     float det4_1234_0123 = mat[1][0] * det3_234_123 - mat[1][1] * det3_234_023 + mat[1][2] * det3_234_013 - mat[1][3] * det3_234_012;
  1244.     float det4_1234_0124 = mat[1][0] * det3_234_124 - mat[1][1] * det3_234_024 + mat[1][2] * det3_234_014 - mat[1][4] * det3_234_012;
  1245.     float det4_1234_0134 = mat[1][0] * det3_234_134 - mat[1][1] * det3_234_034 + mat[1][3] * det3_234_014 - mat[1][4] * det3_234_013;
  1246.     float det4_1234_0234 = mat[1][0] * det3_234_234 - mat[1][2] * det3_234_034 + mat[1][3] * det3_234_024 - mat[1][4] * det3_234_023;
  1247.     float det4_1234_1234 = mat[1][1] * det3_234_234 - mat[1][2] * det3_234_134 + mat[1][3] * det3_234_124 - mat[1][4] * det3_234_123;
  1248.  
  1249.     // determinant of 5x5 matrix
  1250.     return mat[0][0] * det4_1234_1234 - mat[0][1] * det4_1234_0234 + mat[0][2] * det4_1234_0134 - mat[0][3] * det4_1234_0124 + mat[0][4] * det4_1234_0123;
  1251. }
  1252.  
  1253. /*
  1254. ============
  1255. idMat5::InverseSelf
  1256. ============
  1257. */
  1258. bool idMat5::InverseSelf( void ) {
  1259.     // 280+5+25 = 310 multiplications
  1260.     //                1 division
  1261.     double det, invDet;
  1262.  
  1263.     // 2x2 sub-determinants required to calculate 5x5 determinant
  1264.     float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0];
  1265.     float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0];
  1266.     float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0];
  1267.     float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0];
  1268.     float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1];
  1269.     float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1];
  1270.     float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1];
  1271.     float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2];
  1272.     float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2];
  1273.     float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3];
  1274.  
  1275.     // 3x3 sub-determinants required to calculate 5x5 determinant
  1276.     float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01;
  1277.     float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01;
  1278.     float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01;
  1279.     float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02;
  1280.     float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02;
  1281.     float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03;
  1282.     float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12;
  1283.     float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12;
  1284.     float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13;
  1285.     float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23;
  1286.  
  1287.     // 4x4 sub-determinants required to calculate 5x5 determinant
  1288.     float det4_1234_0123 = mat[1][0] * det3_234_123 - mat[1][1] * det3_234_023 + mat[1][2] * det3_234_013 - mat[1][3] * det3_234_012;
  1289.     float det4_1234_0124 = mat[1][0] * det3_234_124 - mat[1][1] * det3_234_024 + mat[1][2] * det3_234_014 - mat[1][4] * det3_234_012;
  1290.     float det4_1234_0134 = mat[1][0] * det3_234_134 - mat[1][1] * det3_234_034 + mat[1][3] * det3_234_014 - mat[1][4] * det3_234_013;
  1291.     float det4_1234_0234 = mat[1][0] * det3_234_234 - mat[1][2] * det3_234_034 + mat[1][3] * det3_234_024 - mat[1][4] * det3_234_023;
  1292.     float det4_1234_1234 = mat[1][1] * det3_234_234 - mat[1][2] * det3_234_134 + mat[1][3] * det3_234_124 - mat[1][4] * det3_234_123;
  1293.  
  1294.     // determinant of 5x5 matrix
  1295.     det = mat[0][0] * det4_1234_1234 - mat[0][1] * det4_1234_0234 + mat[0][2] * det4_1234_0134 - mat[0][3] * det4_1234_0124 + mat[0][4] * det4_1234_0123;
  1296.  
  1297.     if( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {  
  1298.         return false;
  1299.     }
  1300.  
  1301.     invDet = 1.0f / det;
  1302.  
  1303.     // remaining 2x2 sub-determinants
  1304.     float det2_23_01 = mat[2][0] * mat[3][1] - mat[2][1] * mat[3][0];
  1305.     float det2_23_02 = mat[2][0] * mat[3][2] - mat[2][2] * mat[3][0];
  1306.     float det2_23_03 = mat[2][0] * mat[3][3] - mat[2][3] * mat[3][0];
  1307.     float det2_23_04 = mat[2][0] * mat[3][4] - mat[2][4] * mat[3][0];
  1308.     float det2_23_12 = mat[2][1] * mat[3][2] - mat[2][2] * mat[3][1];
  1309.     float det2_23_13 = mat[2][1] * mat[3][3] - mat[2][3] * mat[3][1];
  1310.     float det2_23_14 = mat[2][1] * mat[3][4] - mat[2][4] * mat[3][1];
  1311.     float det2_23_23 = mat[2][2] * mat[3][3] - mat[2][3] * mat[3][2];
  1312.     float det2_23_24 = mat[2][2] * mat[3][4] - mat[2][4] * mat[3][2];
  1313.     float det2_23_34 = mat[2][3] * mat[3][4] - mat[2][4] * mat[3][3];
  1314.     float det2_24_01 = mat[2][0] * mat[4][1] - mat[2][1] * mat[4][0];
  1315.     float det2_24_02 = mat[2][0] * mat[4][2] - mat[2][2] * mat[4][0];
  1316.     float det2_24_03 = mat[2][0] * mat[4][3] - mat[2][3] * mat[4][0];
  1317.     float det2_24_04 = mat[2][0] * mat[4][4] - mat[2][4] * mat[4][0];
  1318.     float det2_24_12 = mat[2][1] * mat[4][2] - mat[2][2] * mat[4][1];
  1319.     float det2_24_13 = mat[2][1] * mat[4][3] - mat[2][3] * mat[4][1];
  1320.     float det2_24_14 = mat[2][1] * mat[4][4] - mat[2][4] * mat[4][1];
  1321.     float det2_24_23 = mat[2][2] * mat[4][3] - mat[2][3] * mat[4][2];
  1322.     float det2_24_24 = mat[2][2] * mat[4][4] - mat[2][4] * mat[4][2];
  1323.     float det2_24_34 = mat[2][3] * mat[4][4] - mat[2][4] * mat[4][3];
  1324.  
  1325.     // remaining 3x3 sub-determinants
  1326.     float det3_123_012 = mat[1][0] * det2_23_12 - mat[1][1] * det2_23_02 + mat[1][2] * det2_23_01;
  1327.     float det3_123_013 = mat[1][0] * det2_23_13 - mat[1][1] * det2_23_03 + mat[1][3] * det2_23_01;
  1328.     float det3_123_014 = mat[1][0] * det2_23_14 - mat[1][1] * det2_23_04 + mat[1][4] * det2_23_01;
  1329.     float det3_123_023 = mat[1][0] * det2_23_23 - mat[1][2] * det2_23_03 + mat[1][3] * det2_23_02;
  1330.     float det3_123_024 = mat[1][0] * det2_23_24 - mat[1][2] * det2_23_04 + mat[1][4] * det2_23_02;
  1331.     float det3_123_034 = mat[1][0] * det2_23_34 - mat[1][3] * det2_23_04 + mat[1][4] * det2_23_03;
  1332.     float det3_123_123 = mat[1][1] * det2_23_23 - mat[1][2] * det2_23_13 + mat[1][3] * det2_23_12;
  1333.     float det3_123_124 = mat[1][1] * det2_23_24 - mat[1][2] * det2_23_14 + mat[1][4] * det2_23_12;
  1334.     float det3_123_134 = mat[1][1] * det2_23_34 - mat[1][3] * det2_23_14 + mat[1][4] * det2_23_13;
  1335.     float det3_123_234 = mat[1][2] * det2_23_34 - mat[1][3] * det2_23_24 + mat[1][4] * det2_23_23;
  1336.     float det3_124_012 = mat[1][0] * det2_24_12 - mat[1][1] * det2_24_02 + mat[1][2] * det2_24_01;
  1337.     float det3_124_013 = mat[1][0] * det2_24_13 - mat[1][1] * det2_24_03 + mat[1][3] * det2_24_01;
  1338.     float det3_124_014 = mat[1][0] * det2_24_14 - mat[1][1] * det2_24_04 + mat[1][4] * det2_24_01;
  1339.     float det3_124_023 = mat[1][0] * det2_24_23 - mat[1][2] * det2_24_03 + mat[1][3] * det2_24_02;
  1340.     float det3_124_024 = mat[1][0] * det2_24_24 - mat[1][2] * det2_24_04 + mat[1][4] * det2_24_02;
  1341.     float det3_124_034 = mat[1][0] * det2_24_34 - mat[1][3] * det2_24_04 + mat[1][4] * det2_24_03;
  1342.     float det3_124_123 = mat[1][1] * det2_24_23 - mat[1][2] * det2_24_13 + mat[1][3] * det2_24_12;
  1343.     float det3_124_124 = mat[1][1] * det2_24_24 - mat[1][2] * det2_24_14 + mat[1][4] * det2_24_12;
  1344.     float det3_124_134 = mat[1][1] * det2_24_34 - mat[1][3] * det2_24_14 + mat[1][4] * det2_24_13;
  1345.     float det3_124_234 = mat[1][2] * det2_24_34 - mat[1][3] * det2_24_24 + mat[1][4] * det2_24_23;
  1346.     float det3_134_012 = mat[1][0] * det2_34_12 - mat[1][1] * det2_34_02 + mat[1][2] * det2_34_01;
  1347.     float det3_134_013 = mat[1][0] * det2_34_13 - mat[1][1] * det2_34_03 + mat[1][3] * det2_34_01;
  1348.     float det3_134_014 = mat[1][0] * det2_34_14 - mat[1][1] * det2_34_04 + mat[1][4] * det2_34_01;
  1349.     float det3_134_023 = mat[1][0] * det2_34_23 - mat[1][2] * det2_34_03 + mat[1][3] * det2_34_02;
  1350.     float det3_134_024 = mat[1][0] * det2_34_24 - mat[1][2] * det2_34_04 + mat[1][4] * det2_34_02;
  1351.     float det3_134_034 = mat[1][0] * det2_34_34 - mat[1][3] * det2_34_04 + mat[1][4] * det2_34_03;
  1352.     float det3_134_123 = mat[1][1] * det2_34_23 - mat[1][2] * det2_34_13 + mat[1][3] * det2_34_12;
  1353.     float det3_134_124 = mat[1][1] * det2_34_24 - mat[1][2] * det2_34_14 + mat[1][4] * det2_34_12;
  1354.     float det3_134_134 = mat[1][1] * det2_34_34 - mat[1][3] * det2_34_14 + mat[1][4] * det2_34_13;
  1355.     float det3_134_234 = mat[1][2] * det2_34_34 - mat[1][3] * det2_34_24 + mat[1][4] * det2_34_23;
  1356.  
  1357.     // remaining 4x4 sub-determinants
  1358.     float det4_0123_0123 = mat[0][0] * det3_123_123 - mat[0][1] * det3_123_023 + mat[0][2] * det3_123_013 - mat[0][3] * det3_123_012;
  1359.     float det4_0123_0124 = mat[0][0] * det3_123_124 - mat[0][1] * det3_123_024 + mat[0][2] * det3_123_014 - mat[0][4] * det3_123_012;
  1360.     float det4_0123_0134 = mat[0][0] * det3_123_134 - mat[0][1] * det3_123_034 + mat[0][3] * det3_123_014 - mat[0][4] * det3_123_013;
  1361.     float det4_0123_0234 = mat[0][0] * det3_123_234 - mat[0][2] * det3_123_034 + mat[0][3] * det3_123_024 - mat[0][4] * det3_123_023;
  1362.     float det4_0123_1234 = mat[0][1] * det3_123_234 - mat[0][2] * det3_123_134 + mat[0][3] * det3_123_124 - mat[0][4] * det3_123_123;
  1363.     float det4_0124_0123 = mat[0][0] * det3_124_123 - mat[0][1] * det3_124_023 + mat[0][2] * det3_124_013 - mat[0][3] * det3_124_012;
  1364.     float det4_0124_0124 = mat[0][0] * det3_124_124 - mat[0][1] * det3_124_024 + mat[0][2] * det3_124_014 - mat[0][4] * det3_124_012;
  1365.     float det4_0124_0134 = mat[0][0] * det3_124_134 - mat[0][1] * det3_124_034 + mat[0][3] * det3_124_014 - mat[0][4] * det3_124_013;
  1366.     float det4_0124_0234 = mat[0][0] * det3_124_234 - mat[0][2] * det3_124_034 + mat[0][3] * det3_124_024 - mat[0][4] * det3_124_023;
  1367.     float det4_0124_1234 = mat[0][1] * det3_124_234 - mat[0][2] * det3_124_134 + mat[0][3] * det3_124_124 - mat[0][4] * det3_124_123;
  1368.     float det4_0134_0123 = mat[0][0] * det3_134_123 - mat[0][1] * det3_134_023 + mat[0][2] * det3_134_013 - mat[0][3] * det3_134_012;
  1369.     float det4_0134_0124 = mat[0][0] * det3_134_124 - mat[0][1] * det3_134_024 + mat[0][2] * det3_134_014 - mat[0][4] * det3_134_012;
  1370.     float det4_0134_0134 = mat[0][0] * det3_134_134 - mat[0][1] * det3_134_034 + mat[0][3] * det3_134_014 - mat[0][4] * det3_134_013;
  1371.     float det4_0134_0234 = mat[0][0] * det3_134_234 - mat[0][2] * det3_134_034 + mat[0][3] * det3_134_024 - mat[0][4] * det3_134_023;
  1372.     float det4_0134_1234 = mat[0][1] * det3_134_234 - mat[0][2] * det3_134_134 + mat[0][3] * det3_134_124 - mat[0][4] * det3_134_123;
  1373.     float det4_0234_0123 = mat[0][0] * det3_234_123 - mat[0][1] * det3_234_023 + mat[0][2] * det3_234_013 - mat[0][3] * det3_234_012;
  1374.     float det4_0234_0124 = mat[0][0] * det3_234_124 - mat[0][1] * det3_234_024 + mat[0][2] * det3_234_014 - mat[0][4] * det3_234_012;
  1375.     float det4_0234_0134 = mat[0][0] * det3_234_134 - mat[0][1] * det3_234_034 + mat[0][3] * det3_234_014 - mat[0][4] * det3_234_013;
  1376.     float det4_0234_0234 = mat[0][0] * det3_234_234 - mat[0][2] * det3_234_034 + mat[0][3] * det3_234_024 - mat[0][4] * det3_234_023;
  1377.     float det4_0234_1234 = mat[0][1] * det3_234_234 - mat[0][2] * det3_234_134 + mat[0][3] * det3_234_124 - mat[0][4] * det3_234_123;
  1378.  
  1379.     mat[0][0] =  det4_1234_1234 * invDet;
  1380.     mat[0][1] = -det4_0234_1234 * invDet;
  1381.     mat[0][2] =  det4_0134_1234 * invDet;
  1382.     mat[0][3] = -det4_0124_1234 * invDet;
  1383.     mat[0][4] =  det4_0123_1234 * invDet;
  1384.  
  1385.     mat[1][0] = -det4_1234_0234 * invDet;
  1386.     mat[1][1] =  det4_0234_0234 * invDet;
  1387.     mat[1][2] = -det4_0134_0234 * invDet;
  1388.     mat[1][3] =  det4_0124_0234 * invDet;
  1389.     mat[1][4] = -det4_0123_0234 * invDet;
  1390.  
  1391.     mat[2][0] =  det4_1234_0134 * invDet;
  1392.     mat[2][1] = -det4_0234_0134 * invDet;
  1393.     mat[2][2] =  det4_0134_0134 * invDet;
  1394.     mat[2][3] = -det4_0124_0134 * invDet;
  1395.     mat[2][4] =  det4_0123_0134 * invDet;
  1396.  
  1397.     mat[3][0] = -det4_1234_0124 * invDet;
  1398.     mat[3][1] =  det4_0234_0124 * invDet;
  1399.     mat[3][2] = -det4_0134_0124 * invDet;
  1400.     mat[3][3] =  det4_0124_0124 * invDet;
  1401.     mat[3][4] = -det4_0123_0124 * invDet;
  1402.  
  1403.     mat[4][0] =  det4_1234_0123 * invDet;
  1404.     mat[4][1] = -det4_0234_0123 * invDet;
  1405.     mat[4][2] =  det4_0134_0123 * invDet;
  1406.     mat[4][3] = -det4_0124_0123 * invDet;
  1407.     mat[4][4] =  det4_0123_0123 * invDet;
  1408.  
  1409.     return true;
  1410. }
  1411.  
  1412. /*
  1413. ============
  1414. idMat5::InverseFastSelf
  1415. ============
  1416. */
  1417. bool idMat5::InverseFastSelf( void ) {
  1418. #if 0
  1419.     // 280+5+25 = 310 multiplications
  1420.     //                1 division
  1421.     double det, invDet;
  1422.  
  1423.     // 2x2 sub-determinants required to calculate 5x5 determinant
  1424.     float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0];
  1425.     float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0];
  1426.     float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0];
  1427.     float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0];
  1428.     float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1];
  1429.     float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1];
  1430.     float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1];
  1431.     float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2];
  1432.     float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2];
  1433.     float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3];
  1434.  
  1435.     // 3x3 sub-determinants required to calculate 5x5 determinant
  1436.     float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01;
  1437.     float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01;
  1438.     float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01;
  1439.     float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02;
  1440.     float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02;
  1441.     float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03;
  1442.     float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12;
  1443.     float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12;
  1444.     float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13;
  1445.     float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23;
  1446.  
  1447.     // 4x4 sub-determinants required to calculate 5x5 determinant
  1448.     float det4_1234_0123 = mat[1][0] * det3_234_123 - mat[1][1] * det3_234_023 + mat[1][2] * det3_234_013 - mat[1][3] * det3_234_012;
  1449.     float det4_1234_0124 = mat[1][0] * det3_234_124 - mat[1][1] * det3_234_024 + mat[1][2] * det3_234_014 - mat[1][4] * det3_234_012;
  1450.     float det4_1234_0134 = mat[1][0] * det3_234_134 - mat[1][1] * det3_234_034 + mat[1][3] * det3_234_014 - mat[1][4] * det3_234_013;
  1451.     float det4_1234_0234 = mat[1][0] * det3_234_234 - mat[1][2] * det3_234_034 + mat[1][3] * det3_234_024 - mat[1][4] * det3_234_023;
  1452.     float det4_1234_1234 = mat[1][1] * det3_234_234 - mat[1][2] * det3_234_134 + mat[1][3] * det3_234_124 - mat[1][4] * det3_234_123;
  1453.  
  1454.     // determinant of 5x5 matrix
  1455.     det = mat[0][0] * det4_1234_1234 - mat[0][1] * det4_1234_0234 + mat[0][2] * det4_1234_0134 - mat[0][3] * det4_1234_0124 + mat[0][4] * det4_1234_0123;
  1456.  
  1457.     if( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {  
  1458.         return false;
  1459.     }
  1460.  
  1461.     invDet = 1.0f / det;
  1462.  
  1463.     // remaining 2x2 sub-determinants
  1464.     float det2_23_01 = mat[2][0] * mat[3][1] - mat[2][1] * mat[3][0];
  1465.     float det2_23_02 = mat[2][0] * mat[3][2] - mat[2][2] * mat[3][0];
  1466.     float det2_23_03 = mat[2][0] * mat[3][3] - mat[2][3] * mat[3][0];
  1467.     float det2_23_04 = mat[2][0] * mat[3][4] - mat[2][4] * mat[3][0];
  1468.     float det2_23_12 = mat[2][1] * mat[3][2] - mat[2][2] * mat[3][1];
  1469.     float det2_23_13 = mat[2][1] * mat[3][3] - mat[2][3] * mat[3][1];
  1470.     float det2_23_14 = mat[2][1] * mat[3][4] - mat[2][4] * mat[3][1];
  1471.     float det2_23_23 = mat[2][2] * mat[3][3] - mat[2][3] * mat[3][2];
  1472.     float det2_23_24 = mat[2][2] * mat[3][4] - mat[2][4] * mat[3][2];
  1473.     float det2_23_34 = mat[2][3] * mat[3][4] - mat[2][4] * mat[3][3];
  1474.     float det2_24_01 = mat[2][0] * mat[4][1] - mat[2][1] * mat[4][0];
  1475.     float det2_24_02 = mat[2][0] * mat[4][2] - mat[2][2] * mat[4][0];
  1476.     float det2_24_03 = mat[2][0] * mat[4][3] - mat[2][3] * mat[4][0];
  1477.     float det2_24_04 = mat[2][0] * mat[4][4] - mat[2][4] * mat[4][0];
  1478.     float det2_24_12 = mat[2][1] * mat[4][2] - mat[2][2] * mat[4][1];
  1479.     float det2_24_13 = mat[2][1] * mat[4][3] - mat[2][3] * mat[4][1];
  1480.     float det2_24_14 = mat[2][1] * mat[4][4] - mat[2][4] * mat[4][1];
  1481.     float det2_24_23 = mat[2][2] * mat[4][3] - mat[2][3] * mat[4][2];
  1482.     float det2_24_24 = mat[2][2] * mat[4][4] - mat[2][4] * mat[4][2];
  1483.     float det2_24_34 = mat[2][3] * mat[4][4] - mat[2][4] * mat[4][3];
  1484.  
  1485.     // remaining 3x3 sub-determinants
  1486.     float det3_123_012 = mat[1][0] * det2_23_12 - mat[1][1] * det2_23_02 + mat[1][2] * det2_23_01;
  1487.     float det3_123_013 = mat[1][0] * det2_23_13 - mat[1][1] * det2_23_03 + mat[1][3] * det2_23_01;
  1488.     float det3_123_014 = mat[1][0] * det2_23_14 - mat[1][1] * det2_23_04 + mat[1][4] * det2_23_01;
  1489.     float det3_123_023 = mat[1][0] * det2_23_23 - mat[1][2] * det2_23_03 + mat[1][3] * det2_23_02;
  1490.     float det3_123_024 = mat[1][0] * det2_23_24 - mat[1][2] * det2_23_04 + mat[1][4] * det2_23_02;
  1491.     float det3_123_034 = mat[1][0] * det2_23_34 - mat[1][3] * det2_23_04 + mat[1][4] * det2_23_03;
  1492.     float det3_123_123 = mat[1][1] * det2_23_23 - mat[1][2] * det2_23_13 + mat[1][3] * det2_23_12;
  1493.     float det3_123_124 = mat[1][1] * det2_23_24 - mat[1][2] * det2_23_14 + mat[1][4] * det2_23_12;
  1494.     float det3_123_134 = mat[1][1] * det2_23_34 - mat[1][3] * det2_23_14 + mat[1][4] * det2_23_13;
  1495.     float det3_123_234 = mat[1][2] * det2_23_34 - mat[1][3] * det2_23_24 + mat[1][4] * det2_23_23;
  1496.     float det3_124_012 = mat[1][0] * det2_24_12 - mat[1][1] * det2_24_02 + mat[1][2] * det2_24_01;
  1497.     float det3_124_013 = mat[1][0] * det2_24_13 - mat[1][1] * det2_24_03 + mat[1][3] * det2_24_01;
  1498.     float det3_124_014 = mat[1][0] * det2_24_14 - mat[1][1] * det2_24_04 + mat[1][4] * det2_24_01;
  1499.     float det3_124_023 = mat[1][0] * det2_24_23 - mat[1][2] * det2_24_03 + mat[1][3] * det2_24_02;
  1500.     float det3_124_024 = mat[1][0] * det2_24_24 - mat[1][2] * det2_24_04 + mat[1][4] * det2_24_02;
  1501.     float det3_124_034 = mat[1][0] * det2_24_34 - mat[1][3] * det2_24_04 + mat[1][4] * det2_24_03;
  1502.     float det3_124_123 = mat[1][1] * det2_24_23 - mat[1][2] * det2_24_13 + mat[1][3] * det2_24_12;
  1503.     float det3_124_124 = mat[1][1] * det2_24_24 - mat[1][2] * det2_24_14 + mat[1][4] * det2_24_12;
  1504.     float det3_124_134 = mat[1][1] * det2_24_34 - mat[1][3] * det2_24_14 + mat[1][4] * det2_24_13;
  1505.     float det3_124_234 = mat[1][2] * det2_24_34 - mat[1][3] * det2_24_24 + mat[1][4] * det2_24_23;
  1506.     float det3_134_012 = mat[1][0] * det2_34_12 - mat[1][1] * det2_34_02 + mat[1][2] * det2_34_01;
  1507.     float det3_134_013 = mat[1][0] * det2_34_13 - mat[1][1] * det2_34_03 + mat[1][3] * det2_34_01;
  1508.     float det3_134_014 = mat[1][0] * det2_34_14 - mat[1][1] * det2_34_04 + mat[1][4] * det2_34_01;
  1509.     float det3_134_023 = mat[1][0] * det2_34_23 - mat[1][2] * det2_34_03 + mat[1][3] * det2_34_02;
  1510.     float det3_134_024 = mat[1][0] * det2_34_24 - mat[1][2] * det2_34_04 + mat[1][4] * det2_34_02;
  1511.     float det3_134_034 = mat[1][0] * det2_34_34 - mat[1][3] * det2_34_04 + mat[1][4] * det2_34_03;
  1512.     float det3_134_123 = mat[1][1] * det2_34_23 - mat[1][2] * det2_34_13 + mat[1][3] * det2_34_12;
  1513.     float det3_134_124 = mat[1][1] * det2_34_24 - mat[1][2] * det2_34_14 + mat[1][4] * det2_34_12;
  1514.     float det3_134_134 = mat[1][1] * det2_34_34 - mat[1][3] * det2_34_14 + mat[1][4] * det2_34_13;
  1515.     float det3_134_234 = mat[1][2] * det2_34_34 - mat[1][3] * det2_34_24 + mat[1][4] * det2_34_23;
  1516.  
  1517.     // remaining 4x4 sub-determinants
  1518.     float det4_0123_0123 = mat[0][0] * det3_123_123 - mat[0][1] * det3_123_023 + mat[0][2] * det3_123_013 - mat[0][3] * det3_123_012;
  1519.     float det4_0123_0124 = mat[0][0] * det3_123_124 - mat[0][1] * det3_123_024 + mat[0][2] * det3_123_014 - mat[0][4] * det3_123_012;
  1520.     float det4_0123_0134 = mat[0][0] * det3_123_134 - mat[0][1] * det3_123_034 + mat[0][3] * det3_123_014 - mat[0][4] * det3_123_013;
  1521.     float det4_0123_0234 = mat[0][0] * det3_123_234 - mat[0][2] * det3_123_034 + mat[0][3] * det3_123_024 - mat[0][4] * det3_123_023;
  1522.     float det4_0123_1234 = mat[0][1] * det3_123_234 - mat[0][2] * det3_123_134 + mat[0][3] * det3_123_124 - mat[0][4] * det3_123_123;
  1523.     float det4_0124_0123 = mat[0][0] * det3_124_123 - mat[0][1] * det3_124_023 + mat[0][2] * det3_124_013 - mat[0][3] * det3_124_012;
  1524.     float det4_0124_0124 = mat[0][0] * det3_124_124 - mat[0][1] * det3_124_024 + mat[0][2] * det3_124_014 - mat[0][4] * det3_124_012;
  1525.     float det4_0124_0134 = mat[0][0] * det3_124_134 - mat[0][1] * det3_124_034 + mat[0][3] * det3_124_014 - mat[0][4] * det3_124_013;
  1526.     float det4_0124_0234 = mat[0][0] * det3_124_234 - mat[0][2] * det3_124_034 + mat[0][3] * det3_124_024 - mat[0][4] * det3_124_023;
  1527.     float det4_0124_1234 = mat[0][1] * det3_124_234 - mat[0][2] * det3_124_134 + mat[0][3] * det3_124_124 - mat[0][4] * det3_124_123;
  1528.     float det4_0134_0123 = mat[0][0] * det3_134_123 - mat[0][1] * det3_134_023 + mat[0][2] * det3_134_013 - mat[0][3] * det3_134_012;
  1529.     float det4_0134_0124 = mat[0][0] * det3_134_124 - mat[0][1] * det3_134_024 + mat[0][2] * det3_134_014 - mat[0][4] * det3_134_012;
  1530.     float det4_0134_0134 = mat[0][0] * det3_134_134 - mat[0][1] * det3_134_034 + mat[0][3] * det3_134_014 - mat[0][4] * det3_134_013;
  1531.     float det4_0134_0234 = mat[0][0] * det3_134_234 - mat[0][2] * det3_134_034 + mat[0][3] * det3_134_024 - mat[0][4] * det3_134_023;
  1532.     float det4_0134_1234 = mat[0][1] * det3_134_234 - mat[0][2] * det3_134_134 + mat[0][3] * det3_134_124 - mat[0][4] * det3_134_123;
  1533.     float det4_0234_0123 = mat[0][0] * det3_234_123 - mat[0][1] * det3_234_023 + mat[0][2] * det3_234_013 - mat[0][3] * det3_234_012;
  1534.     float det4_0234_0124 = mat[0][0] * det3_234_124 - mat[0][1] * det3_234_024 + mat[0][2] * det3_234_014 - mat[0][4] * det3_234_012;
  1535.     float det4_0234_0134 = mat[0][0] * det3_234_134 - mat[0][1] * det3_234_034 + mat[0][3] * det3_234_014 - mat[0][4] * det3_234_013;
  1536.     float det4_0234_0234 = mat[0][0] * det3_234_234 - mat[0][2] * det3_234_034 + mat[0][3] * det3_234_024 - mat[0][4] * det3_234_023;
  1537.     float det4_0234_1234 = mat[0][1] * det3_234_234 - mat[0][2] * det3_234_134 + mat[0][3] * det3_234_124 - mat[0][4] * det3_234_123;
  1538.  
  1539.     mat[0][0] =  det4_1234_1234 * invDet;
  1540.     mat[0][1] = -det4_0234_1234 * invDet;
  1541.     mat[0][2] =  det4_0134_1234 * invDet;
  1542.     mat[0][3] = -det4_0124_1234 * invDet;
  1543.     mat[0][4] =  det4_0123_1234 * invDet;
  1544.  
  1545.     mat[1][0] = -det4_1234_0234 * invDet;
  1546.     mat[1][1] =  det4_0234_0234 * invDet;
  1547.     mat[1][2] = -det4_0134_0234 * invDet;
  1548.     mat[1][3] =  det4_0124_0234 * invDet;
  1549.     mat[1][4] = -det4_0123_0234 * invDet;
  1550.  
  1551.     mat[2][0] =  det4_1234_0134 * invDet;
  1552.     mat[2][1] = -det4_0234_0134 * invDet;
  1553.     mat[2][2] =  det4_0134_0134 * invDet;
  1554.     mat[2][3] = -det4_0124_0134 * invDet;
  1555.     mat[2][4] =  det4_0123_0134 * invDet;
  1556.  
  1557.     mat[3][0] = -det4_1234_0124 * invDet;
  1558.     mat[3][1] =  det4_0234_0124 * invDet;
  1559.     mat[3][2] = -det4_0134_0124 * invDet;
  1560.     mat[3][3] =  det4_0124_0124 * invDet;
  1561.     mat[3][4] = -det4_0123_0124 * invDet;
  1562.  
  1563.     mat[4][0] =  det4_1234_0123 * invDet;
  1564.     mat[4][1] = -det4_0234_0123 * invDet;
  1565.     mat[4][2] =  det4_0134_0123 * invDet;
  1566.     mat[4][3] = -det4_0124_0123 * invDet;
  1567.     mat[4][4] =  det4_0123_0123 * invDet;
  1568.  
  1569.     return true;
  1570. #elif 0
  1571.     // 5*28 = 140 multiplications
  1572.     //            5 divisions
  1573.     float *mat = reinterpret_cast<float *>(this);
  1574.     float s;
  1575.     double d, di;
  1576.  
  1577.     di = mat[0];
  1578.     s = di;
  1579.     mat[0] = d = 1.0f / di;
  1580.     mat[1] *= d;
  1581.     mat[2] *= d;
  1582.     mat[3] *= d;
  1583.     mat[4] *= d;
  1584.     d = -d;
  1585.     mat[5] *= d;
  1586.     mat[10] *= d;
  1587.     mat[15] *= d;
  1588.     mat[20] *= d;
  1589.     d = mat[5] * di;
  1590.     mat[6] += mat[1] * d;
  1591.     mat[7] += mat[2] * d;
  1592.     mat[8] += mat[3] * d;
  1593.     mat[9] += mat[4] * d;
  1594.     d = mat[10] * di;
  1595.     mat[11] += mat[1] * d;
  1596.     mat[12] += mat[2] * d;
  1597.     mat[13] += mat[3] * d;
  1598.     mat[14] += mat[4] * d;
  1599.     d = mat[15] * di;
  1600.     mat[16] += mat[1] * d;
  1601.     mat[17] += mat[2] * d;
  1602.     mat[18] += mat[3] * d;
  1603.     mat[19] += mat[4] * d;
  1604.     d = mat[20] * di;
  1605.     mat[21] += mat[1] * d;
  1606.     mat[22] += mat[2] * d;
  1607.     mat[23] += mat[3] * d;
  1608.     mat[24] += mat[4] * d;
  1609.     di = mat[6];
  1610.     s *= di;
  1611.     mat[6] = d = 1.0f / di;
  1612.     mat[5] *= d;
  1613.     mat[7] *= d;
  1614.     mat[8] *= d;
  1615.     mat[9] *= d;
  1616.     d = -d;
  1617.     mat[1] *= d;
  1618.     mat[11] *= d;
  1619.     mat[16] *= d;
  1620.     mat[21] *= d;
  1621.     d = mat[1] * di;
  1622.     mat[0] += mat[5] * d;
  1623.     mat[2] += mat[7] * d;
  1624.     mat[3] += mat[8] * d;
  1625.     mat[4] += mat[9] * d;
  1626.     d = mat[11] * di;
  1627.     mat[10] += mat[5] * d;
  1628.     mat[12] += mat[7] * d;
  1629.     mat[13] += mat[8] * d;
  1630.     mat[14] += mat[9] * d;
  1631.     d = mat[16] * di;
  1632.     mat[15] += mat[5] * d;
  1633.     mat[17] += mat[7] * d;
  1634.     mat[18] += mat[8] * d;
  1635.     mat[19] += mat[9] * d;
  1636.     d = mat[21] * di;
  1637.     mat[20] += mat[5] * d;
  1638.     mat[22] += mat[7] * d;
  1639.     mat[23] += mat[8] * d;
  1640.     mat[24] += mat[9] * d;
  1641.     di = mat[12];
  1642.     s *= di;
  1643.     mat[12] = d = 1.0f / di;
  1644.     mat[10] *= d;
  1645.     mat[11] *= d;
  1646.     mat[13] *= d;
  1647.     mat[14] *= d;
  1648.     d = -d;
  1649.     mat[2] *= d;
  1650.     mat[7] *= d;
  1651.     mat[17] *= d;
  1652.     mat[22] *= d;
  1653.     d = mat[2] * di;
  1654.     mat[0] += mat[10] * d;
  1655.     mat[1] += mat[11] * d;
  1656.     mat[3] += mat[13] * d;
  1657.     mat[4] += mat[14] * d;
  1658.     d = mat[7] * di;
  1659.     mat[5] += mat[10] * d;
  1660.     mat[6] += mat[11] * d;
  1661.     mat[8] += mat[13] * d;
  1662.     mat[9] += mat[14] * d;
  1663.     d = mat[17] * di;
  1664.     mat[15] += mat[10] * d;
  1665.     mat[16] += mat[11] * d;
  1666.     mat[18] += mat[13] * d;
  1667.     mat[19] += mat[14] * d;
  1668.     d = mat[22] * di;
  1669.     mat[20] += mat[10] * d;
  1670.     mat[21] += mat[11] * d;
  1671.     mat[23] += mat[13] * d;
  1672.     mat[24] += mat[14] * d;
  1673.     di = mat[18];
  1674.     s *= di;
  1675.     mat[18] = d = 1.0f / di;
  1676.     mat[15] *= d;
  1677.     mat[16] *= d;
  1678.     mat[17] *= d;
  1679.     mat[19] *= d;
  1680.     d = -d;
  1681.     mat[3] *= d;
  1682.     mat[8] *= d;
  1683.     mat[13] *= d;
  1684.     mat[23] *= d;
  1685.     d = mat[3] * di;
  1686.     mat[0] += mat[15] * d;
  1687.     mat[1] += mat[16] * d;
  1688.     mat[2] += mat[17] * d;
  1689.     mat[4] += mat[19] * d;
  1690.     d = mat[8] * di;
  1691.     mat[5] += mat[15] * d;
  1692.     mat[6] += mat[16] * d;
  1693.     mat[7] += mat[17] * d;
  1694.     mat[9] += mat[19] * d;
  1695.     d = mat[13] * di;
  1696.     mat[10] += mat[15] * d;
  1697.     mat[11] += mat[16] * d;
  1698.     mat[12] += mat[17] * d;
  1699.     mat[14] += mat[19] * d;
  1700.     d = mat[23] * di;
  1701.     mat[20] += mat[15] * d;
  1702.     mat[21] += mat[16] * d;
  1703.     mat[22] += mat[17] * d;
  1704.     mat[24] += mat[19] * d;
  1705.     di = mat[24];
  1706.     s *= di;
  1707.     mat[24] = d = 1.0f / di;
  1708.     mat[20] *= d;
  1709.     mat[21] *= d;
  1710.     mat[22] *= d;
  1711.     mat[23] *= d;
  1712.     d = -d;
  1713.     mat[4] *= d;
  1714.     mat[9] *= d;
  1715.     mat[14] *= d;
  1716.     mat[19] *= d;
  1717.     d = mat[4] * di;
  1718.     mat[0] += mat[20] * d;
  1719.     mat[1] += mat[21] * d;
  1720.     mat[2] += mat[22] * d;
  1721.     mat[3] += mat[23] * d;
  1722.     d = mat[9] * di;
  1723.     mat[5] += mat[20] * d;
  1724.     mat[6] += mat[21] * d;
  1725.     mat[7] += mat[22] * d;
  1726.     mat[8] += mat[23] * d;
  1727.     d = mat[14] * di;
  1728.     mat[10] += mat[20] * d;
  1729.     mat[11] += mat[21] * d;
  1730.     mat[12] += mat[22] * d;
  1731.     mat[13] += mat[23] * d;
  1732.     d = mat[19] * di;
  1733.     mat[15] += mat[20] * d;
  1734.     mat[16] += mat[21] * d;
  1735.     mat[17] += mat[22] * d;
  1736.     mat[18] += mat[23] * d;
  1737.  
  1738.     return ( s != 0.0f && !FLOAT_IS_NAN( s ) );
  1739. #else
  1740.     // 86+30+6 = 122 multiplications
  1741.     //      2*1  =   2 divisions
  1742.     idMat3 r0, r1, r2, r3;
  1743.     float c0, c1, c2, det, invDet;
  1744.     float *mat = reinterpret_cast<float *>(this);
  1745.  
  1746.     // r0 = m0.Inverse();    // 3x3
  1747.     c0 = mat[1*5+1] * mat[2*5+2] - mat[1*5+2] * mat[2*5+1];
  1748.     c1 = mat[1*5+2] * mat[2*5+0] - mat[1*5+0] * mat[2*5+2];
  1749.     c2 = mat[1*5+0] * mat[2*5+1] - mat[1*5+1] * mat[2*5+0];
  1750.  
  1751.     det = mat[0*5+0] * c0 + mat[0*5+1] * c1 + mat[0*5+2] * c2;
  1752.  
  1753.     if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  1754.         return false;
  1755.     }
  1756.  
  1757.     invDet = 1.0f / det;
  1758.  
  1759.     r0[0][0] = c0 * invDet;
  1760.     r0[0][1] = ( mat[0*5+2] * mat[2*5+1] - mat[0*5+1] * mat[2*5+2] ) * invDet;
  1761.     r0[0][2] = ( mat[0*5+1] * mat[1*5+2] - mat[0*5+2] * mat[1*5+1] ) * invDet;
  1762.     r0[1][0] = c1 * invDet;
  1763.     r0[1][1] = ( mat[0*5+0] * mat[2*5+2] - mat[0*5+2] * mat[2*5+0] ) * invDet;
  1764.     r0[1][2] = ( mat[0*5+2] * mat[1*5+0] - mat[0*5+0] * mat[1*5+2] ) * invDet;
  1765.     r0[2][0] = c2 * invDet;
  1766.     r0[2][1] = ( mat[0*5+1] * mat[2*5+0] - mat[0*5+0] * mat[2*5+1] ) * invDet;
  1767.     r0[2][2] = ( mat[0*5+0] * mat[1*5+1] - mat[0*5+1] * mat[1*5+0] ) * invDet;
  1768.  
  1769.     // r1 = r0 * m1;        // 3x2 = 3x3 * 3x2
  1770.     r1[0][0] = r0[0][0] * mat[0*5+3] + r0[0][1] * mat[1*5+3] + r0[0][2] * mat[2*5+3];
  1771.     r1[0][1] = r0[0][0] * mat[0*5+4] + r0[0][1] * mat[1*5+4] + r0[0][2] * mat[2*5+4];
  1772.     r1[1][0] = r0[1][0] * mat[0*5+3] + r0[1][1] * mat[1*5+3] + r0[1][2] * mat[2*5+3];
  1773.     r1[1][1] = r0[1][0] * mat[0*5+4] + r0[1][1] * mat[1*5+4] + r0[1][2] * mat[2*5+4];
  1774.     r1[2][0] = r0[2][0] * mat[0*5+3] + r0[2][1] * mat[1*5+3] + r0[2][2] * mat[2*5+3];
  1775.     r1[2][1] = r0[2][0] * mat[0*5+4] + r0[2][1] * mat[1*5+4] + r0[2][2] * mat[2*5+4];
  1776.  
  1777.     // r2 = m2 * r1;        // 2x2 = 2x3 * 3x2
  1778.     r2[0][0] = mat[3*5+0] * r1[0][0] + mat[3*5+1] * r1[1][0] + mat[3*5+2] * r1[2][0];
  1779.     r2[0][1] = mat[3*5+0] * r1[0][1] + mat[3*5+1] * r1[1][1] + mat[3*5+2] * r1[2][1];
  1780.     r2[1][0] = mat[4*5+0] * r1[0][0] + mat[4*5+1] * r1[1][0] + mat[4*5+2] * r1[2][0];
  1781.     r2[1][1] = mat[4*5+0] * r1[0][1] + mat[4*5+1] * r1[1][1] + mat[4*5+2] * r1[2][1];
  1782.  
  1783.     // r3 = r2 - m3;        // 2x2 = 2x2 - 2x2
  1784.     r3[0][0] = r2[0][0] - mat[3*5+3];
  1785.     r3[0][1] = r2[0][1] - mat[3*5+4];
  1786.     r3[1][0] = r2[1][0] - mat[4*5+3];
  1787.     r3[1][1] = r2[1][1] - mat[4*5+4];
  1788.  
  1789.     // r3.InverseSelf();    // 2x2
  1790.     det = r3[0][0] * r3[1][1] - r3[0][1] * r3[1][0];
  1791.  
  1792.     if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  1793.         return false;
  1794.     }
  1795.  
  1796.     invDet = 1.0f / det;
  1797.  
  1798.     c0 = r3[0][0];
  1799.     r3[0][0] =   r3[1][1] * invDet;
  1800.     r3[0][1] = - r3[0][1] * invDet;
  1801.     r3[1][0] = - r3[1][0] * invDet;
  1802.     r3[1][1] =   c0 * invDet;
  1803.  
  1804.     // r2 = m2 * r0;        // 2x3 = 2x3 * 3x3
  1805.     r2[0][0] = mat[3*5+0] * r0[0][0] + mat[3*5+1] * r0[1][0] + mat[3*5+2] * r0[2][0];
  1806.     r2[0][1] = mat[3*5+0] * r0[0][1] + mat[3*5+1] * r0[1][1] + mat[3*5+2] * r0[2][1];
  1807.     r2[0][2] = mat[3*5+0] * r0[0][2] + mat[3*5+1] * r0[1][2] + mat[3*5+2] * r0[2][2];
  1808.     r2[1][0] = mat[4*5+0] * r0[0][0] + mat[4*5+1] * r0[1][0] + mat[4*5+2] * r0[2][0];
  1809.     r2[1][1] = mat[4*5+0] * r0[0][1] + mat[4*5+1] * r0[1][1] + mat[4*5+2] * r0[2][1];
  1810.     r2[1][2] = mat[4*5+0] * r0[0][2] + mat[4*5+1] * r0[1][2] + mat[4*5+2] * r0[2][2];
  1811.  
  1812.     // m2 = r3 * r2;        // 2x3 = 2x2 * 2x3
  1813.     mat[3*5+0] = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0];
  1814.     mat[3*5+1] = r3[0][0] * r2[0][1] + r3[0][1] * r2[1][1];
  1815.     mat[3*5+2] = r3[0][0] * r2[0][2] + r3[0][1] * r2[1][2];
  1816.     mat[4*5+0] = r3[1][0] * r2[0][0] + r3[1][1] * r2[1][0];
  1817.     mat[4*5+1] = r3[1][0] * r2[0][1] + r3[1][1] * r2[1][1];
  1818.     mat[4*5+2] = r3[1][0] * r2[0][2] + r3[1][1] * r2[1][2];
  1819.  
  1820.     // m0 = r0 - r1 * m2;    // 3x3 = 3x3 - 3x2 * 2x3
  1821.     mat[0*5+0] = r0[0][0] - r1[0][0] * mat[3*5+0] - r1[0][1] * mat[4*5+0];
  1822.     mat[0*5+1] = r0[0][1] - r1[0][0] * mat[3*5+1] - r1[0][1] * mat[4*5+1];
  1823.     mat[0*5+2] = r0[0][2] - r1[0][0] * mat[3*5+2] - r1[0][1] * mat[4*5+2];
  1824.     mat[1*5+0] = r0[1][0] - r1[1][0] * mat[3*5+0] - r1[1][1] * mat[4*5+0];
  1825.     mat[1*5+1] = r0[1][1] - r1[1][0] * mat[3*5+1] - r1[1][1] * mat[4*5+1];
  1826.     mat[1*5+2] = r0[1][2] - r1[1][0] * mat[3*5+2] - r1[1][1] * mat[4*5+2];
  1827.     mat[2*5+0] = r0[2][0] - r1[2][0] * mat[3*5+0] - r1[2][1] * mat[4*5+0];
  1828.     mat[2*5+1] = r0[2][1] - r1[2][0] * mat[3*5+1] - r1[2][1] * mat[4*5+1];
  1829.     mat[2*5+2] = r0[2][2] - r1[2][0] * mat[3*5+2] - r1[2][1] * mat[4*5+2];
  1830.  
  1831.     // m1 = r1 * r3;        // 3x2 = 3x2 * 2x2
  1832.     mat[0*5+3] = r1[0][0] * r3[0][0] + r1[0][1] * r3[1][0];
  1833.     mat[0*5+4] = r1[0][0] * r3[0][1] + r1[0][1] * r3[1][1];
  1834.     mat[1*5+3] = r1[1][0] * r3[0][0] + r1[1][1] * r3[1][0];
  1835.     mat[1*5+4] = r1[1][0] * r3[0][1] + r1[1][1] * r3[1][1];
  1836.     mat[2*5+3] = r1[2][0] * r3[0][0] + r1[2][1] * r3[1][0];
  1837.     mat[2*5+4] = r1[2][0] * r3[0][1] + r1[2][1] * r3[1][1];
  1838.  
  1839.     // m3 = -r3;            // 2x2 = - 2x2
  1840.     mat[3*5+3] = -r3[0][0];
  1841.     mat[3*5+4] = -r3[0][1];
  1842.     mat[4*5+3] = -r3[1][0];
  1843.     mat[4*5+4] = -r3[1][1];
  1844.  
  1845.     return true;
  1846. #endif
  1847. }
  1848.  
  1849. /*
  1850. =============
  1851. idMat5::ToString
  1852. =============
  1853. */
  1854. const char *idMat5::ToString( int precision ) const {
  1855.     return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
  1856. }
  1857.  
  1858.  
  1859. //===============================================================
  1860. //
  1861. //    idMat6
  1862. //
  1863. //===============================================================
  1864.  
  1865. idMat6 mat6_zero( idVec6( 0, 0, 0, 0, 0, 0 ), idVec6( 0, 0, 0, 0, 0, 0 ), idVec6( 0, 0, 0, 0, 0, 0 ), idVec6( 0, 0, 0, 0, 0, 0 ), idVec6( 0, 0, 0, 0, 0, 0 ), idVec6( 0, 0, 0, 0, 0, 0 ) );
  1866. idMat6 mat6_identity( idVec6( 1, 0, 0, 0, 0, 0 ), idVec6( 0, 1, 0, 0, 0, 0 ), idVec6( 0, 0, 1, 0, 0, 0 ), idVec6( 0, 0, 0, 1, 0, 0 ), idVec6( 0, 0, 0, 0, 1, 0 ), idVec6( 0, 0, 0, 0, 0, 1 ) );
  1867.  
  1868. /*
  1869. ============
  1870. idMat6::Transpose
  1871. ============
  1872. */
  1873. idMat6 idMat6::Transpose( void ) const {
  1874.     idMat6    transpose;
  1875.     int        i, j;
  1876.    
  1877.     for( i = 0; i < 6; i++ ) {
  1878.         for( j = 0; j < 6; j++ ) {
  1879.             transpose[ i ][ j ] = mat[ j ][ i ];
  1880.         }
  1881.     }
  1882.     return transpose;
  1883. }
  1884.  
  1885. /*
  1886. ============
  1887. idMat6::TransposeSelf
  1888. ============
  1889. */
  1890. idMat6 &idMat6::TransposeSelf( void ) {
  1891.     float    temp;
  1892.     int        i, j;
  1893.    
  1894.     for( i = 0; i < 6; i++ ) {
  1895.         for( j = i + 1; j < 6; j++ ) {
  1896.             temp = mat[ i ][ j ];
  1897.             mat[ i ][ j ] = mat[ j ][ i ];
  1898.             mat[ j ][ i ] = temp;
  1899.         }
  1900.     }
  1901.     return *this;
  1902. }
  1903.  
  1904. /*
  1905. ============
  1906. idMat6::Determinant
  1907. ============
  1908. */
  1909. float idMat6::Determinant( void ) const {
  1910.  
  1911.     // 2x2 sub-determinants required to calculate 6x6 determinant
  1912.     float det2_45_01 = mat[4][0] * mat[5][1] - mat[4][1] * mat[5][0];
  1913.     float det2_45_02 = mat[4][0] * mat[5][2] - mat[4][2] * mat[5][0];
  1914.     float det2_45_03 = mat[4][0] * mat[5][3] - mat[4][3] * mat[5][0];
  1915.     float det2_45_04 = mat[4][0] * mat[5][4] - mat[4][4] * mat[5][0];
  1916.     float det2_45_05 = mat[4][0] * mat[5][5] - mat[4][5] * mat[5][0];
  1917.     float det2_45_12 = mat[4][1] * mat[5][2] - mat[4][2] * mat[5][1];
  1918.     float det2_45_13 = mat[4][1] * mat[5][3] - mat[4][3] * mat[5][1];
  1919.     float det2_45_14 = mat[4][1] * mat[5][4] - mat[4][4] * mat[5][1];
  1920.     float det2_45_15 = mat[4][1] * mat[5][5] - mat[4][5] * mat[5][1];
  1921.     float det2_45_23 = mat[4][2] * mat[5][3] - mat[4][3] * mat[5][2];
  1922.     float det2_45_24 = mat[4][2] * mat[5][4] - mat[4][4] * mat[5][2];
  1923.     float det2_45_25 = mat[4][2] * mat[5][5] - mat[4][5] * mat[5][2];
  1924.     float det2_45_34 = mat[4][3] * mat[5][4] - mat[4][4] * mat[5][3];
  1925.     float det2_45_35 = mat[4][3] * mat[5][5] - mat[4][5] * mat[5][3];
  1926.     float det2_45_45 = mat[4][4] * mat[5][5] - mat[4][5] * mat[5][4];
  1927.  
  1928.     // 3x3 sub-determinants required to calculate 6x6 determinant
  1929.     float det3_345_012 = mat[3][0] * det2_45_12 - mat[3][1] * det2_45_02 + mat[3][2] * det2_45_01;
  1930.     float det3_345_013 = mat[3][0] * det2_45_13 - mat[3][1] * det2_45_03 + mat[3][3] * det2_45_01;
  1931.     float det3_345_014 = mat[3][0] * det2_45_14 - mat[3][1] * det2_45_04 + mat[3][4] * det2_45_01;
  1932.     float det3_345_015 = mat[3][0] * det2_45_15 - mat[3][1] * det2_45_05 + mat[3][5] * det2_45_01;
  1933.     float det3_345_023 = mat[3][0] * det2_45_23 - mat[3][2] * det2_45_03 + mat[3][3] * det2_45_02;
  1934.     float det3_345_024 = mat[3][0] * det2_45_24 - mat[3][2] * det2_45_04 + mat[3][4] * det2_45_02;
  1935.     float det3_345_025 = mat[3][0] * det2_45_25 - mat[3][2] * det2_45_05 + mat[3][5] * det2_45_02;
  1936.     float det3_345_034 = mat[3][0] * det2_45_34 - mat[3][3] * det2_45_04 + mat[3][4] * det2_45_03;
  1937.     float det3_345_035 = mat[3][0] * det2_45_35 - mat[3][3] * det2_45_05 + mat[3][5] * det2_45_03;
  1938.     float det3_345_045 = mat[3][0] * det2_45_45 - mat[3][4] * det2_45_05 + mat[3][5] * det2_45_04;
  1939.     float det3_345_123 = mat[3][1] * det2_45_23 - mat[3][2] * det2_45_13 + mat[3][3] * det2_45_12;
  1940.     float det3_345_124 = mat[3][1] * det2_45_24 - mat[3][2] * det2_45_14 + mat[3][4] * det2_45_12;
  1941.     float det3_345_125 = mat[3][1] * det2_45_25 - mat[3][2] * det2_45_15 + mat[3][5] * det2_45_12;
  1942.     float det3_345_134 = mat[3][1] * det2_45_34 - mat[3][3] * det2_45_14 + mat[3][4] * det2_45_13;
  1943.     float det3_345_135 = mat[3][1] * det2_45_35 - mat[3][3] * det2_45_15 + mat[3][5] * det2_45_13;
  1944.     float det3_345_145 = mat[3][1] * det2_45_45 - mat[3][4] * det2_45_15 + mat[3][5] * det2_45_14;
  1945.     float det3_345_234 = mat[3][2] * det2_45_34 - mat[3][3] * det2_45_24 + mat[3][4] * det2_45_23;
  1946.     float det3_345_235 = mat[3][2] * det2_45_35 - mat[3][3] * det2_45_25 + mat[3][5] * det2_45_23;
  1947.     float det3_345_245 = mat[3][2] * det2_45_45 - mat[3][4] * det2_45_25 + mat[3][5] * det2_45_24;
  1948.     float det3_345_345 = mat[3][3] * det2_45_45 - mat[3][4] * det2_45_35 + mat[3][5] * det2_45_34;
  1949.  
  1950.     // 4x4 sub-determinants required to calculate 6x6 determinant
  1951.     float det4_2345_0123 = mat[2][0] * det3_345_123 - mat[2][1] * det3_345_023 + mat[2][2] * det3_345_013 - mat[2][3] * det3_345_012;
  1952.     float det4_2345_0124 = mat[2][0] * det3_345_124 - mat[2][1] * det3_345_024 + mat[2][2] * det3_345_014 - mat[2][4] * det3_345_012;
  1953.     float det4_2345_0125 = mat[2][0] * det3_345_125 - mat[2][1] * det3_345_025 + mat[2][2] * det3_345_015 - mat[2][5] * det3_345_012;
  1954.     float det4_2345_0134 = mat[2][0] * det3_345_134 - mat[2][1] * det3_345_034 + mat[2][3] * det3_345_014 - mat[2][4] * det3_345_013;
  1955.     float det4_2345_0135 = mat[2][0] * det3_345_135 - mat[2][1] * det3_345_035 + mat[2][3] * det3_345_015 - mat[2][5] * det3_345_013;
  1956.     float det4_2345_0145 = mat[2][0] * det3_345_145 - mat[2][1] * det3_345_045 + mat[2][4] * det3_345_015 - mat[2][5] * det3_345_014;
  1957.     float det4_2345_0234 = mat[2][0] * det3_345_234 - mat[2][2] * det3_345_034 + mat[2][3] * det3_345_024 - mat[2][4] * det3_345_023;
  1958.     float det4_2345_0235 = mat[2][0] * det3_345_235 - mat[2][2] * det3_345_035 + mat[2][3] * det3_345_025 - mat[2][5] * det3_345_023;
  1959.     float det4_2345_0245 = mat[2][0] * det3_345_245 - mat[2][2] * det3_345_045 + mat[2][4] * det3_345_025 - mat[2][5] * det3_345_024;
  1960.     float det4_2345_0345 = mat[2][0] * det3_345_345 - mat[2][3] * det3_345_045 + mat[2][4] * det3_345_035 - mat[2][5] * det3_345_034;
  1961.     float det4_2345_1234 = mat[2][1] * det3_345_234 - mat[2][2] * det3_345_134 + mat[2][3] * det3_345_124 - mat[2][4] * det3_345_123;
  1962.     float det4_2345_1235 = mat[2][1] * det3_345_235 - mat[2][2] * det3_345_135 + mat[2][3] * det3_345_125 - mat[2][5] * det3_345_123;
  1963.     float det4_2345_1245 = mat[2][1] * det3_345_245 - mat[2][2] * det3_345_145 + mat[2][4] * det3_345_125 - mat[2][5] * det3_345_124;
  1964.     float det4_2345_1345 = mat[2][1] * det3_345_345 - mat[2][3] * det3_345_145 + mat[2][4] * det3_345_135 - mat[2][5] * det3_345_134;
  1965.     float det4_2345_2345 = mat[2][2] * det3_345_345 - mat[2][3] * det3_345_245 + mat[2][4] * det3_345_235 - mat[2][5] * det3_345_234;
  1966.  
  1967.     // 5x5 sub-determinants required to calculate 6x6 determinant
  1968.     float det5_12345_01234 = mat[1][0] * det4_2345_1234 - mat[1][1] * det4_2345_0234 + mat[1][2] * det4_2345_0134 - mat[1][3] * det4_2345_0124 + mat[1][4] * det4_2345_0123;
  1969.     float det5_12345_01235 = mat[1][0] * det4_2345_1235 - mat[1][1] * det4_2345_0235 + mat[1][2] * det4_2345_0135 - mat[1][3] * det4_2345_0125 + mat[1][5] * det4_2345_0123;
  1970.     float det5_12345_01245 = mat[1][0] * det4_2345_1245 - mat[1][1] * det4_2345_0245 + mat[1][2] * det4_2345_0145 - mat[1][4] * det4_2345_0125 + mat[1][5] * det4_2345_0124;
  1971.     float det5_12345_01345 = mat[1][0] * det4_2345_1345 - mat[1][1] * det4_2345_0345 + mat[1][3] * det4_2345_0145 - mat[1][4] * det4_2345_0135 + mat[1][5] * det4_2345_0134;
  1972.     float det5_12345_02345 = mat[1][0] * det4_2345_2345 - mat[1][2] * det4_2345_0345 + mat[1][3] * det4_2345_0245 - mat[1][4] * det4_2345_0235 + mat[1][5] * det4_2345_0234;
  1973.     float det5_12345_12345 = mat[1][1] * det4_2345_2345 - mat[1][2] * det4_2345_1345 + mat[1][3] * det4_2345_1245 - mat[1][4] * det4_2345_1235 + mat[1][5] * det4_2345_1234;
  1974.  
  1975.     // determinant of 6x6 matrix
  1976.     return    mat[0][0] * det5_12345_12345 - mat[0][1] * det5_12345_02345 + mat[0][2] * det5_12345_01345 -
  1977.             mat[0][3] * det5_12345_01245 + mat[0][4] * det5_12345_01235 - mat[0][5] * det5_12345_01234;
  1978. }
  1979.  
  1980. /*
  1981. ============
  1982. idMat6::InverseSelf
  1983. ============
  1984. */
  1985. bool idMat6::InverseSelf( void ) {
  1986.     // 810+6+36 = 852 multiplications
  1987.     //                1 division
  1988.     double det, invDet;
  1989.  
  1990.     // 2x2 sub-determinants required to calculate 6x6 determinant
  1991.     float det2_45_01 = mat[4][0] * mat[5][1] - mat[4][1] * mat[5][0];
  1992.     float det2_45_02 = mat[4][0] * mat[5][2] - mat[4][2] * mat[5][0];
  1993.     float det2_45_03 = mat[4][0] * mat[5][3] - mat[4][3] * mat[5][0];
  1994.     float det2_45_04 = mat[4][0] * mat[5][4] - mat[4][4] * mat[5][0];
  1995.     float det2_45_05 = mat[4][0] * mat[5][5] - mat[4][5] * mat[5][0];
  1996.     float det2_45_12 = mat[4][1] * mat[5][2] - mat[4][2] * mat[5][1];
  1997.     float det2_45_13 = mat[4][1] * mat[5][3] - mat[4][3] * mat[5][1];
  1998.     float det2_45_14 = mat[4][1] * mat[5][4] - mat[4][4] * mat[5][1];
  1999.     float det2_45_15 = mat[4][1] * mat[5][5] - mat[4][5] * mat[5][1];
  2000.     float det2_45_23 = mat[4][2] * mat[5][3] - mat[4][3] * mat[5][2];
  2001.     float det2_45_24 = mat[4][2] * mat[5][4] - mat[4][4] * mat[5][2];
  2002.     float det2_45_25 = mat[4][2] * mat[5][5] - mat[4][5] * mat[5][2];
  2003.     float det2_45_34 = mat[4][3] * mat[5][4] - mat[4][4] * mat[5][3];
  2004.     float det2_45_35 = mat[4][3] * mat[5][5] - mat[4][5] * mat[5][3];
  2005.     float det2_45_45 = mat[4][4] * mat[5][5] - mat[4][5] * mat[5][4];
  2006.  
  2007.     // 3x3 sub-determinants required to calculate 6x6 determinant
  2008.     float det3_345_012 = mat[3][0] * det2_45_12 - mat[3][1] * det2_45_02 + mat[3][2] * det2_45_01;
  2009.     float det3_345_013 = mat[3][0] * det2_45_13 - mat[3][1] * det2_45_03 + mat[3][3] * det2_45_01;
  2010.     float det3_345_014 = mat[3][0] * det2_45_14 - mat[3][1] * det2_45_04 + mat[3][4] * det2_45_01;
  2011.     float det3_345_015 = mat[3][0] * det2_45_15 - mat[3][1] * det2_45_05 + mat[3][5] * det2_45_01;
  2012.     float det3_345_023 = mat[3][0] * det2_45_23 - mat[3][2] * det2_45_03 + mat[3][3] * det2_45_02;
  2013.     float det3_345_024 = mat[3][0] * det2_45_24 - mat[3][2] * det2_45_04 + mat[3][4] * det2_45_02;
  2014.     float det3_345_025 = mat[3][0] * det2_45_25 - mat[3][2] * det2_45_05 + mat[3][5] * det2_45_02;
  2015.     float det3_345_034 = mat[3][0] * det2_45_34 - mat[3][3] * det2_45_04 + mat[3][4] * det2_45_03;
  2016.     float det3_345_035 = mat[3][0] * det2_45_35 - mat[3][3] * det2_45_05 + mat[3][5] * det2_45_03;
  2017.     float det3_345_045 = mat[3][0] * det2_45_45 - mat[3][4] * det2_45_05 + mat[3][5] * det2_45_04;
  2018.     float det3_345_123 = mat[3][1] * det2_45_23 - mat[3][2] * det2_45_13 + mat[3][3] * det2_45_12;
  2019.     float det3_345_124 = mat[3][1] * det2_45_24 - mat[3][2] * det2_45_14 + mat[3][4] * det2_45_12;
  2020.     float det3_345_125 = mat[3][1] * det2_45_25 - mat[3][2] * det2_45_15 + mat[3][5] * det2_45_12;
  2021.     float det3_345_134 = mat[3][1] * det2_45_34 - mat[3][3] * det2_45_14 + mat[3][4] * det2_45_13;
  2022.     float det3_345_135 = mat[3][1] * det2_45_35 - mat[3][3] * det2_45_15 + mat[3][5] * det2_45_13;
  2023.     float det3_345_145 = mat[3][1] * det2_45_45 - mat[3][4] * det2_45_15 + mat[3][5] * det2_45_14;
  2024.     float det3_345_234 = mat[3][2] * det2_45_34 - mat[3][3] * det2_45_24 + mat[3][4] * det2_45_23;
  2025.     float det3_345_235 = mat[3][2] * det2_45_35 - mat[3][3] * det2_45_25 + mat[3][5] * det2_45_23;
  2026.     float det3_345_245 = mat[3][2] * det2_45_45 - mat[3][4] * det2_45_25 + mat[3][5] * det2_45_24;
  2027.     float det3_345_345 = mat[3][3] * det2_45_45 - mat[3][4] * det2_45_35 + mat[3][5] * det2_45_34;
  2028.  
  2029.     // 4x4 sub-determinants required to calculate 6x6 determinant
  2030.     float det4_2345_0123 = mat[2][0] * det3_345_123 - mat[2][1] * det3_345_023 + mat[2][2] * det3_345_013 - mat[2][3] * det3_345_012;
  2031.     float det4_2345_0124 = mat[2][0] * det3_345_124 - mat[2][1] * det3_345_024 + mat[2][2] * det3_345_014 - mat[2][4] * det3_345_012;
  2032.     float det4_2345_0125 = mat[2][0] * det3_345_125 - mat[2][1] * det3_345_025 + mat[2][2] * det3_345_015 - mat[2][5] * det3_345_012;
  2033.     float det4_2345_0134 = mat[2][0] * det3_345_134 - mat[2][1] * det3_345_034 + mat[2][3] * det3_345_014 - mat[2][4] * det3_345_013;
  2034.     float det4_2345_0135 = mat[2][0] * det3_345_135 - mat[2][1] * det3_345_035 + mat[2][3] * det3_345_015 - mat[2][5] * det3_345_013;
  2035.     float det4_2345_0145 = mat[2][0] * det3_345_145 - mat[2][1] * det3_345_045 + mat[2][4] * det3_345_015 - mat[2][5] * det3_345_014;
  2036.     float det4_2345_0234 = mat[2][0] * det3_345_234 - mat[2][2] * det3_345_034 + mat[2][3] * det3_345_024 - mat[2][4] * det3_345_023;
  2037.     float det4_2345_0235 = mat[2][0] * det3_345_235 - mat[2][2] * det3_345_035 + mat[2][3] * det3_345_025 - mat[2][5] * det3_345_023;
  2038.     float det4_2345_0245 = mat[2][0] * det3_345_245 - mat[2][2] * det3_345_045 + mat[2][4] * det3_345_025 - mat[2][5] * det3_345_024;
  2039.     float det4_2345_0345 = mat[2][0] * det3_345_345 - mat[2][3] * det3_345_045 + mat[2][4] * det3_345_035 - mat[2][5] * det3_345_034;
  2040.     float det4_2345_1234 = mat[2][1] * det3_345_234 - mat[2][2] * det3_345_134 + mat[2][3] * det3_345_124 - mat[2][4] * det3_345_123;
  2041.     float det4_2345_1235 = mat[2][1] * det3_345_235 - mat[2][2] * det3_345_135 + mat[2][3] * det3_345_125 - mat[2][5] * det3_345_123;
  2042.     float det4_2345_1245 = mat[2][1] * det3_345_245 - mat[2][2] * det3_345_145 + mat[2][4] * det3_345_125 - mat[2][5] * det3_345_124;
  2043.     float det4_2345_1345 = mat[2][1] * det3_345_345 - mat[2][3] * det3_345_145 + mat[2][4] * det3_345_135 - mat[2][5] * det3_345_134;
  2044.     float det4_2345_2345 = mat[2][2] * det3_345_345 - mat[2][3] * det3_345_245 + mat[2][4] * det3_345_235 - mat[2][5] * det3_345_234;
  2045.  
  2046.     // 5x5 sub-determinants required to calculate 6x6 determinant
  2047.     float det5_12345_01234 = mat[1][0] * det4_2345_1234 - mat[1][1] * det4_2345_0234 + mat[1][2] * det4_2345_0134 - mat[1][3] * det4_2345_0124 + mat[1][4] * det4_2345_0123;
  2048.     float det5_12345_01235 = mat[1][0] * det4_2345_1235 - mat[1][1] * det4_2345_0235 + mat[1][2] * det4_2345_0135 - mat[1][3] * det4_2345_0125 + mat[1][5] * det4_2345_0123;
  2049.     float det5_12345_01245 = mat[1][0] * det4_2345_1245 - mat[1][1] * det4_2345_0245 + mat[1][2] * det4_2345_0145 - mat[1][4] * det4_2345_0125 + mat[1][5] * det4_2345_0124;
  2050.     float det5_12345_01345 = mat[1][0] * det4_2345_1345 - mat[1][1] * det4_2345_0345 + mat[1][3] * det4_2345_0145 - mat[1][4] * det4_2345_0135 + mat[1][5] * det4_2345_0134;
  2051.     float det5_12345_02345 = mat[1][0] * det4_2345_2345 - mat[1][2] * det4_2345_0345 + mat[1][3] * det4_2345_0245 - mat[1][4] * det4_2345_0235 + mat[1][5] * det4_2345_0234;
  2052.     float det5_12345_12345 = mat[1][1] * det4_2345_2345 - mat[1][2] * det4_2345_1345 + mat[1][3] * det4_2345_1245 - mat[1][4] * det4_2345_1235 + mat[1][5] * det4_2345_1234;
  2053.  
  2054.     // determinant of 6x6 matrix
  2055.     det = mat[0][0] * det5_12345_12345 - mat[0][1] * det5_12345_02345 + mat[0][2] * det5_12345_01345 -
  2056.                 mat[0][3] * det5_12345_01245 + mat[0][4] * det5_12345_01235 - mat[0][5] * det5_12345_01234;
  2057.  
  2058.     if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  2059.         return false;
  2060.     }
  2061.  
  2062.     invDet = 1.0f / det;
  2063.  
  2064.     // remaining 2x2 sub-determinants
  2065.     float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0];
  2066.     float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0];
  2067.     float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0];
  2068.     float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0];
  2069.     float det2_34_05 = mat[3][0] * mat[4][5] - mat[3][5] * mat[4][0];
  2070.     float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1];
  2071.     float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1];
  2072.     float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1];
  2073.     float det2_34_15 = mat[3][1] * mat[4][5] - mat[3][5] * mat[4][1];
  2074.     float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2];
  2075.     float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2];
  2076.     float det2_34_25 = mat[3][2] * mat[4][5] - mat[3][5] * mat[4][2];
  2077.     float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3];
  2078.     float det2_34_35 = mat[3][3] * mat[4][5] - mat[3][5] * mat[4][3];
  2079.     float det2_34_45 = mat[3][4] * mat[4][5] - mat[3][5] * mat[4][4];
  2080.     float det2_35_01 = mat[3][0] * mat[5][1] - mat[3][1] * mat[5][0];
  2081.     float det2_35_02 = mat[3][0] * mat[5][2] - mat[3][2] * mat[5][0];
  2082.     float det2_35_03 = mat[3][0] * mat[5][3] - mat[3][3] * mat[5][0];
  2083.     float det2_35_04 = mat[3][0] * mat[5][4] - mat[3][4] * mat[5][0];
  2084.     float det2_35_05 = mat[3][0] * mat[5][5] - mat[3][5] * mat[5][0];
  2085.     float det2_35_12 = mat[3][1] * mat[5][2] - mat[3][2] * mat[5][1];
  2086.     float det2_35_13 = mat[3][1] * mat[5][3] - mat[3][3] * mat[5][1];
  2087.     float det2_35_14 = mat[3][1] * mat[5][4] - mat[3][4] * mat[5][1];
  2088.     float det2_35_15 = mat[3][1] * mat[5][5] - mat[3][5] * mat[5][1];
  2089.     float det2_35_23 = mat[3][2] * mat[5][3] - mat[3][3] * mat[5][2];
  2090.     float det2_35_24 = mat[3][2] * mat[5][4] - mat[3][4] * mat[5][2];
  2091.     float det2_35_25 = mat[3][2] * mat[5][5] - mat[3][5] * mat[5][2];
  2092.     float det2_35_34 = mat[3][3] * mat[5][4] - mat[3][4] * mat[5][3];
  2093.     float det2_35_35 = mat[3][3] * mat[5][5] - mat[3][5] * mat[5][3];
  2094.     float det2_35_45 = mat[3][4] * mat[5][5] - mat[3][5] * mat[5][4];
  2095.  
  2096.     // remaining 3x3 sub-determinants
  2097.     float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01;
  2098.     float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01;
  2099.     float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01;
  2100.     float det3_234_015 = mat[2][0] * det2_34_15 - mat[2][1] * det2_34_05 + mat[2][5] * det2_34_01;
  2101.     float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02;
  2102.     float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02;
  2103.     float det3_234_025 = mat[2][0] * det2_34_25 - mat[2][2] * det2_34_05 + mat[2][5] * det2_34_02;
  2104.     float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03;
  2105.     float det3_234_035 = mat[2][0] * det2_34_35 - mat[2][3] * det2_34_05 + mat[2][5] * det2_34_03;
  2106.     float det3_234_045 = mat[2][0] * det2_34_45 - mat[2][4] * det2_34_05 + mat[2][5] * det2_34_04;
  2107.     float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12;
  2108.     float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12;
  2109.     float det3_234_125 = mat[2][1] * det2_34_25 - mat[2][2] * det2_34_15 + mat[2][5] * det2_34_12;
  2110.     float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13;
  2111.     float det3_234_135 = mat[2][1] * det2_34_35 - mat[2][3] * det2_34_15 + mat[2][5] * det2_34_13;
  2112.     float det3_234_145 = mat[2][1] * det2_34_45 - mat[2][4] * det2_34_15 + mat[2][5] * det2_34_14;
  2113.     float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23;
  2114.     float det3_234_235 = mat[2][2] * det2_34_35 - mat[2][3] * det2_34_25 + mat[2][5] * det2_34_23;
  2115.     float det3_234_245 = mat[2][2] * det2_34_45 - mat[2][4] * det2_34_25 + mat[2][5] * det2_34_24;
  2116.     float det3_234_345 = mat[2][3] * det2_34_45 - mat[2][4] * det2_34_35 + mat[2][5] * det2_34_34;
  2117.     float det3_235_012 = mat[2][0] * det2_35_12 - mat[2][1] * det2_35_02 + mat[2][2] * det2_35_01;
  2118.     float det3_235_013 = mat[2][0] * det2_35_13 - mat[2][1] * det2_35_03 + mat[2][3] * det2_35_01;
  2119.     float det3_235_014 = mat[2][0] * det2_35_14 - mat[2][1] * det2_35_04 + mat[2][4] * det2_35_01;
  2120.     float det3_235_015 = mat[2][0] * det2_35_15 - mat[2][1] * det2_35_05 + mat[2][5] * det2_35_01;
  2121.     float det3_235_023 = mat[2][0] * det2_35_23 - mat[2][2] * det2_35_03 + mat[2][3] * det2_35_02;
  2122.     float det3_235_024 = mat[2][0] * det2_35_24 - mat[2][2] * det2_35_04 + mat[2][4] * det2_35_02;
  2123.     float det3_235_025 = mat[2][0] * det2_35_25 - mat[2][2] * det2_35_05 + mat[2][5] * det2_35_02;
  2124.     float det3_235_034 = mat[2][0] * det2_35_34 - mat[2][3] * det2_35_04 + mat[2][4] * det2_35_03;
  2125.     float det3_235_035 = mat[2][0] * det2_35_35 - mat[2][3] * det2_35_05 + mat[2][5] * det2_35_03;
  2126.     float det3_235_045 = mat[2][0] * det2_35_45 - mat[2][4] * det2_35_05 + mat[2][5] * det2_35_04;
  2127.     float det3_235_123 = mat[2][1] * det2_35_23 - mat[2][2] * det2_35_13 + mat[2][3] * det2_35_12;
  2128.     float det3_235_124 = mat[2][1] * det2_35_24 - mat[2][2] * det2_35_14 + mat[2][4] * det2_35_12;
  2129.     float det3_235_125 = mat[2][1] * det2_35_25 - mat[2][2] * det2_35_15 + mat[2][5] * det2_35_12;
  2130.     float det3_235_134 = mat[2][1] * det2_35_34 - mat[2][3] * det2_35_14 + mat[2][4] * det2_35_13;
  2131.     float det3_235_135 = mat[2][1] * det2_35_35 - mat[2][3] * det2_35_15 + mat[2][5] * det2_35_13;
  2132.     float det3_235_145 = mat[2][1] * det2_35_45 - mat[2][4] * det2_35_15 + mat[2][5] * det2_35_14;
  2133.     float det3_235_234 = mat[2][2] * det2_35_34 - mat[2][3] * det2_35_24 + mat[2][4] * det2_35_23;
  2134.     float det3_235_235 = mat[2][2] * det2_35_35 - mat[2][3] * det2_35_25 + mat[2][5] * det2_35_23;
  2135.     float det3_235_245 = mat[2][2] * det2_35_45 - mat[2][4] * det2_35_25 + mat[2][5] * det2_35_24;
  2136.     float det3_235_345 = mat[2][3] * det2_35_45 - mat[2][4] * det2_35_35 + mat[2][5] * det2_35_34;
  2137.     float det3_245_012 = mat[2][0] * det2_45_12 - mat[2][1] * det2_45_02 + mat[2][2] * det2_45_01;
  2138.     float det3_245_013 = mat[2][0] * det2_45_13 - mat[2][1] * det2_45_03 + mat[2][3] * det2_45_01;
  2139.     float det3_245_014 = mat[2][0] * det2_45_14 - mat[2][1] * det2_45_04 + mat[2][4] * det2_45_01;
  2140.     float det3_245_015 = mat[2][0] * det2_45_15 - mat[2][1] * det2_45_05 + mat[2][5] * det2_45_01;
  2141.     float det3_245_023 = mat[2][0] * det2_45_23 - mat[2][2] * det2_45_03 + mat[2][3] * det2_45_02;
  2142.     float det3_245_024 = mat[2][0] * det2_45_24 - mat[2][2] * det2_45_04 + mat[2][4] * det2_45_02;
  2143.     float det3_245_025 = mat[2][0] * det2_45_25 - mat[2][2] * det2_45_05 + mat[2][5] * det2_45_02;
  2144.     float det3_245_034 = mat[2][0] * det2_45_34 - mat[2][3] * det2_45_04 + mat[2][4] * det2_45_03;
  2145.     float det3_245_035 = mat[2][0] * det2_45_35 - mat[2][3] * det2_45_05 + mat[2][5] * det2_45_03;
  2146.     float det3_245_045 = mat[2][0] * det2_45_45 - mat[2][4] * det2_45_05 + mat[2][5] * det2_45_04;
  2147.     float det3_245_123 = mat[2][1] * det2_45_23 - mat[2][2] * det2_45_13 + mat[2][3] * det2_45_12;
  2148.     float det3_245_124 = mat[2][1] * det2_45_24 - mat[2][2] * det2_45_14 + mat[2][4] * det2_45_12;
  2149.     float det3_245_125 = mat[2][1] * det2_45_25 - mat[2][2] * det2_45_15 + mat[2][5] * det2_45_12;
  2150.     float det3_245_134 = mat[2][1] * det2_45_34 - mat[2][3] * det2_45_14 + mat[2][4] * det2_45_13;
  2151.     float det3_245_135 = mat[2][1] * det2_45_35 - mat[2][3] * det2_45_15 + mat[2][5] * det2_45_13;
  2152.     float det3_245_145 = mat[2][1] * det2_45_45 - mat[2][4] * det2_45_15 + mat[2][5] * det2_45_14;
  2153.     float det3_245_234 = mat[2][2] * det2_45_34 - mat[2][3] * det2_45_24 + mat[2][4] * det2_45_23;
  2154.     float det3_245_235 = mat[2][2] * det2_45_35 - mat[2][3] * det2_45_25 + mat[2][5] * det2_45_23;
  2155.     float det3_245_245 = mat[2][2] * det2_45_45 - mat[2][4] * det2_45_25 + mat[2][5] * det2_45_24;
  2156.     float det3_245_345 = mat[2][3] * det2_45_45 - mat[2][4] * det2_45_35 + mat[2][5] * det2_45_34;
  2157.  
  2158.     // remaining 4x4 sub-determinants
  2159.     float det4_1234_0123 = mat[1][0] * det3_234_123 - mat[1][1] * det3_234_023 + mat[1][2] * det3_234_013 - mat[1][3] * det3_234_012;
  2160.     float det4_1234_0124 = mat[1][0] * det3_234_124 - mat[1][1] * det3_234_024 + mat[1][2] * det3_234_014 - mat[1][4] * det3_234_012;
  2161.     float det4_1234_0125 = mat[1][0] * det3_234_125 - mat[1][1] * det3_234_025 + mat[1][2] * det3_234_015 - mat[1][5] * det3_234_012;
  2162.     float det4_1234_0134 = mat[1][0] * det3_234_134 - mat[1][1] * det3_234_034 + mat[1][3] * det3_234_014 - mat[1][4] * det3_234_013;
  2163.     float det4_1234_0135 = mat[1][0] * det3_234_135 - mat[1][1] * det3_234_035 + mat[1][3] * det3_234_015 - mat[1][5] * det3_234_013;
  2164.     float det4_1234_0145 = mat[1][0] * det3_234_145 - mat[1][1] * det3_234_045 + mat[1][4] * det3_234_015 - mat[1][5] * det3_234_014;
  2165.     float det4_1234_0234 = mat[1][0] * det3_234_234 - mat[1][2] * det3_234_034 + mat[1][3] * det3_234_024 - mat[1][4] * det3_234_023;
  2166.     float det4_1234_0235 = mat[1][0] * det3_234_235 - mat[1][2] * det3_234_035 + mat[1][3] * det3_234_025 - mat[1][5] * det3_234_023;
  2167.     float det4_1234_0245 = mat[1][0] * det3_234_245 - mat[1][2] * det3_234_045 + mat[1][4] * det3_234_025 - mat[1][5] * det3_234_024;
  2168.     float det4_1234_0345 = mat[1][0] * det3_234_345 - mat[1][3] * det3_234_045 + mat[1][4] * det3_234_035 - mat[1][5] * det3_234_034;
  2169.     float det4_1234_1234 = mat[1][1] * det3_234_234 - mat[1][2] * det3_234_134 + mat[1][3] * det3_234_124 - mat[1][4] * det3_234_123;
  2170.     float det4_1234_1235 = mat[1][1] * det3_234_235 - mat[1][2] * det3_234_135 + mat[1][3] * det3_234_125 - mat[1][5] * det3_234_123;
  2171.     float det4_1234_1245 = mat[1][1] * det3_234_245 - mat[1][2] * det3_234_145 + mat[1][4] * det3_234_125 - mat[1][5] * det3_234_124;
  2172.     float det4_1234_1345 = mat[1][1] * det3_234_345 - mat[1][3] * det3_234_145 + mat[1][4] * det3_234_135 - mat[1][5] * det3_234_134;
  2173.     float det4_1234_2345 = mat[1][2] * det3_234_345 - mat[1][3] * det3_234_245 + mat[1][4] * det3_234_235 - mat[1][5] * det3_234_234;
  2174.     float det4_1235_0123 = mat[1][0] * det3_235_123 - mat[1][1] * det3_235_023 + mat[1][2] * det3_235_013 - mat[1][3] * det3_235_012;
  2175.     float det4_1235_0124 = mat[1][0] * det3_235_124 - mat[1][1] * det3_235_024 + mat[1][2] * det3_235_014 - mat[1][4] * det3_235_012;
  2176.     float det4_1235_0125 = mat[1][0] * det3_235_125 - mat[1][1] * det3_235_025 + mat[1][2] * det3_235_015 - mat[1][5] * det3_235_012;
  2177.     float det4_1235_0134 = mat[1][0] * det3_235_134 - mat[1][1] * det3_235_034 + mat[1][3] * det3_235_014 - mat[1][4] * det3_235_013;
  2178.     float det4_1235_0135 = mat[1][0] * det3_235_135 - mat[1][1] * det3_235_035 + mat[1][3] * det3_235_015 - mat[1][5] * det3_235_013;
  2179.     float det4_1235_0145 = mat[1][0] * det3_235_145 - mat[1][1] * det3_235_045 + mat[1][4] * det3_235_015 - mat[1][5] * det3_235_014;
  2180.     float det4_1235_0234 = mat[1][0] * det3_235_234 - mat[1][2] * det3_235_034 + mat[1][3] * det3_235_024 - mat[1][4] * det3_235_023;
  2181.     float det4_1235_0235 = mat[1][0] * det3_235_235 - mat[1][2] * det3_235_035 + mat[1][3] * det3_235_025 - mat[1][5] * det3_235_023;
  2182.     float det4_1235_0245 = mat[1][0] * det3_235_245 - mat[1][2] * det3_235_045 + mat[1][4] * det3_235_025 - mat[1][5] * det3_235_024;
  2183.     float det4_1235_0345 = mat[1][0] * det3_235_345 - mat[1][3] * det3_235_045 + mat[1][4] * det3_235_035 - mat[1][5] * det3_235_034;
  2184.     float det4_1235_1234 = mat[1][1] * det3_235_234 - mat[1][2] * det3_235_134 + mat[1][3] * det3_235_124 - mat[1][4] * det3_235_123;
  2185.     float det4_1235_1235 = mat[1][1] * det3_235_235 - mat[1][2] * det3_235_135 + mat[1][3] * det3_235_125 - mat[1][5] * det3_235_123;
  2186.     float det4_1235_1245 = mat[1][1] * det3_235_245 - mat[1][2] * det3_235_145 + mat[1][4] * det3_235_125 - mat[1][5] * det3_235_124;
  2187.     float det4_1235_1345 = mat[1][1] * det3_235_345 - mat[1][3] * det3_235_145 + mat[1][4] * det3_235_135 - mat[1][5] * det3_235_134;
  2188.     float det4_1235_2345 = mat[1][2] * det3_235_345 - mat[1][3] * det3_235_245 + mat[1][4] * det3_235_235 - mat[1][5] * det3_235_234;
  2189.     float det4_1245_0123 = mat[1][0] * det3_245_123 - mat[1][1] * det3_245_023 + mat[1][2] * det3_245_013 - mat[1][3] * det3_245_012;
  2190.     float det4_1245_0124 = mat[1][0] * det3_245_124 - mat[1][1] * det3_245_024 + mat[1][2] * det3_245_014 - mat[1][4] * det3_245_012;
  2191.     float det4_1245_0125 = mat[1][0] * det3_245_125 - mat[1][1] * det3_245_025 + mat[1][2] * det3_245_015 - mat[1][5] * det3_245_012;
  2192.     float det4_1245_0134 = mat[1][0] * det3_245_134 - mat[1][1] * det3_245_034 + mat[1][3] * det3_245_014 - mat[1][4] * det3_245_013;
  2193.     float det4_1245_0135 = mat[1][0] * det3_245_135 - mat[1][1] * det3_245_035 + mat[1][3] * det3_245_015 - mat[1][5] * det3_245_013;
  2194.     float det4_1245_0145 = mat[1][0] * det3_245_145 - mat[1][1] * det3_245_045 + mat[1][4] * det3_245_015 - mat[1][5] * det3_245_014;
  2195.     float det4_1245_0234 = mat[1][0] * det3_245_234 - mat[1][2] * det3_245_034 + mat[1][3] * det3_245_024 - mat[1][4] * det3_245_023;
  2196.     float det4_1245_0235 = mat[1][0] * det3_245_235 - mat[1][2] * det3_245_035 + mat[1][3] * det3_245_025 - mat[1][5] * det3_245_023;
  2197.     float det4_1245_0245 = mat[1][0] * det3_245_245 - mat[1][2] * det3_245_045 + mat[1][4] * det3_245_025 - mat[1][5] * det3_245_024;
  2198.     float det4_1245_0345 = mat[1][0] * det3_245_345 - mat[1][3] * det3_245_045 + mat[1][4] * det3_245_035 - mat[1][5] * det3_245_034;
  2199.     float det4_1245_1234 = mat[1][1] * det3_245_234 - mat[1][2] * det3_245_134 + mat[1][3] * det3_245_124 - mat[1][4] * det3_245_123;
  2200.     float det4_1245_1235 = mat[1][1] * det3_245_235 - mat[1][2] * det3_245_135 + mat[1][3] * det3_245_125 - mat[1][5] * det3_245_123;
  2201.     float det4_1245_1245 = mat[1][1] * det3_245_245 - mat[1][2] * det3_245_145 + mat[1][4] * det3_245_125 - mat[1][5] * det3_245_124;
  2202.     float det4_1245_1345 = mat[1][1] * det3_245_345 - mat[1][3] * det3_245_145 + mat[1][4] * det3_245_135 - mat[1][5] * det3_245_134;
  2203.     float det4_1245_2345 = mat[1][2] * det3_245_345 - mat[1][3] * det3_245_245 + mat[1][4] * det3_245_235 - mat[1][5] * det3_245_234;
  2204.     float det4_1345_0123 = mat[1][0] * det3_345_123 - mat[1][1] * det3_345_023 + mat[1][2] * det3_345_013 - mat[1][3] * det3_345_012;
  2205.     float det4_1345_0124 = mat[1][0] * det3_345_124 - mat[1][1] * det3_345_024 + mat[1][2] * det3_345_014 - mat[1][4] * det3_345_012;
  2206.     float det4_1345_0125 = mat[1][0] * det3_345_125 - mat[1][1] * det3_345_025 + mat[1][2] * det3_345_015 - mat[1][5] * det3_345_012;
  2207.     float det4_1345_0134 = mat[1][0] * det3_345_134 - mat[1][1] * det3_345_034 + mat[1][3] * det3_345_014 - mat[1][4] * det3_345_013;
  2208.     float det4_1345_0135 = mat[1][0] * det3_345_135 - mat[1][1] * det3_345_035 + mat[1][3] * det3_345_015 - mat[1][5] * det3_345_013;
  2209.     float det4_1345_0145 = mat[1][0] * det3_345_145 - mat[1][1] * det3_345_045 + mat[1][4] * det3_345_015 - mat[1][5] * det3_345_014;
  2210.     float det4_1345_0234 = mat[1][0] * det3_345_234 - mat[1][2] * det3_345_034 + mat[1][3] * det3_345_024 - mat[1][4] * det3_345_023;
  2211.     float det4_1345_0235 = mat[1][0] * det3_345_235 - mat[1][2] * det3_345_035 + mat[1][3] * det3_345_025 - mat[1][5] * det3_345_023;
  2212.     float det4_1345_0245 = mat[1][0] * det3_345_245 - mat[1][2] * det3_345_045 + mat[1][4] * det3_345_025 - mat[1][5] * det3_345_024;
  2213.     float det4_1345_0345 = mat[1][0] * det3_345_345 - mat[1][3] * det3_345_045 + mat[1][4] * det3_345_035 - mat[1][5] * det3_345_034;
  2214.     float det4_1345_1234 = mat[1][1] * det3_345_234 - mat[1][2] * det3_345_134 + mat[1][3] * det3_345_124 - mat[1][4] * det3_345_123;
  2215.     float det4_1345_1235 = mat[1][1] * det3_345_235 - mat[1][2] * det3_345_135 + mat[1][3] * det3_345_125 - mat[1][5] * det3_345_123;
  2216.     float det4_1345_1245 = mat[1][1] * det3_345_245 - mat[1][2] * det3_345_145 + mat[1][4] * det3_345_125 - mat[1][5] * det3_345_124;
  2217.     float det4_1345_1345 = mat[1][1] * det3_345_345 - mat[1][3] * det3_345_145 + mat[1][4] * det3_345_135 - mat[1][5] * det3_345_134;
  2218.     float det4_1345_2345 = mat[1][2] * det3_345_345 - mat[1][3] * det3_345_245 + mat[1][4] * det3_345_235 - mat[1][5] * det3_345_234;
  2219.  
  2220.     // remaining 5x5 sub-determinants
  2221.     float det5_01234_01234 = mat[0][0] * det4_1234_1234 - mat[0][1] * det4_1234_0234 + mat[0][2] * det4_1234_0134 - mat[0][3] * det4_1234_0124 + mat[0][4] * det4_1234_0123;
  2222.     float det5_01234_01235 = mat[0][0] * det4_1234_1235 - mat[0][1] * det4_1234_0235 + mat[0][2] * det4_1234_0135 - mat[0][3] * det4_1234_0125 + mat[0][5] * det4_1234_0123;
  2223.     float det5_01234_01245 = mat[0][0] * det4_1234_1245 - mat[0][1] * det4_1234_0245 + mat[0][2] * det4_1234_0145 - mat[0][4] * det4_1234_0125 + mat[0][5] * det4_1234_0124;
  2224.     float det5_01234_01345 = mat[0][0] * det4_1234_1345 - mat[0][1] * det4_1234_0345 + mat[0][3] * det4_1234_0145 - mat[0][4] * det4_1234_0135 + mat[0][5] * det4_1234_0134;
  2225.     float det5_01234_02345 = mat[0][0] * det4_1234_2345 - mat[0][2] * det4_1234_0345 + mat[0][3] * det4_1234_0245 - mat[0][4] * det4_1234_0235 + mat[0][5] * det4_1234_0234;
  2226.     float det5_01234_12345 = mat[0][1] * det4_1234_2345 - mat[0][2] * det4_1234_1345 + mat[0][3] * det4_1234_1245 - mat[0][4] * det4_1234_1235 + mat[0][5] * det4_1234_1234;
  2227.     float det5_01235_01234 = mat[0][0] * det4_1235_1234 - mat[0][1] * det4_1235_0234 + mat[0][2] * det4_1235_0134 - mat[0][3] * det4_1235_0124 + mat[0][4] * det4_1235_0123;
  2228.     float det5_01235_01235 = mat[0][0] * det4_1235_1235 - mat[0][1] * det4_1235_0235 + mat[0][2] * det4_1235_0135 - mat[0][3] * det4_1235_0125 + mat[0][5] * det4_1235_0123;
  2229.     float det5_01235_01245 = mat[0][0] * det4_1235_1245 - mat[0][1] * det4_1235_0245 + mat[0][2] * det4_1235_0145 - mat[0][4] * det4_1235_0125 + mat[0][5] * det4_1235_0124;
  2230.     float det5_01235_01345 = mat[0][0] * det4_1235_1345 - mat[0][1] * det4_1235_0345 + mat[0][3] * det4_1235_0145 - mat[0][4] * det4_1235_0135 + mat[0][5] * det4_1235_0134;
  2231.     float det5_01235_02345 = mat[0][0] * det4_1235_2345 - mat[0][2] * det4_1235_0345 + mat[0][3] * det4_1235_0245 - mat[0][4] * det4_1235_0235 + mat[0][5] * det4_1235_0234;
  2232.     float det5_01235_12345 = mat[0][1] * det4_1235_2345 - mat[0][2] * det4_1235_1345 + mat[0][3] * det4_1235_1245 - mat[0][4] * det4_1235_1235 + mat[0][5] * det4_1235_1234;
  2233.     float det5_01245_01234 = mat[0][0] * det4_1245_1234 - mat[0][1] * det4_1245_0234 + mat[0][2] * det4_1245_0134 - mat[0][3] * det4_1245_0124 + mat[0][4] * det4_1245_0123;
  2234.     float det5_01245_01235 = mat[0][0] * det4_1245_1235 - mat[0][1] * det4_1245_0235 + mat[0][2] * det4_1245_0135 - mat[0][3] * det4_1245_0125 + mat[0][5] * det4_1245_0123;
  2235.     float det5_01245_01245 = mat[0][0] * det4_1245_1245 - mat[0][1] * det4_1245_0245 + mat[0][2] * det4_1245_0145 - mat[0][4] * det4_1245_0125 + mat[0][5] * det4_1245_0124;
  2236.     float det5_01245_01345 = mat[0][0] * det4_1245_1345 - mat[0][1] * det4_1245_0345 + mat[0][3] * det4_1245_0145 - mat[0][4] * det4_1245_0135 + mat[0][5] * det4_1245_0134;
  2237.     float det5_01245_02345 = mat[0][0] * det4_1245_2345 - mat[0][2] * det4_1245_0345 + mat[0][3] * det4_1245_0245 - mat[0][4] * det4_1245_0235 + mat[0][5] * det4_1245_0234;
  2238.     float det5_01245_12345 = mat[0][1] * det4_1245_2345 - mat[0][2] * det4_1245_1345 + mat[0][3] * det4_1245_1245 - mat[0][4] * det4_1245_1235 + mat[0][5] * det4_1245_1234;
  2239.     float det5_01345_01234 = mat[0][0] * det4_1345_1234 - mat[0][1] * det4_1345_0234 + mat[0][2] * det4_1345_0134 - mat[0][3] * det4_1345_0124 + mat[0][4] * det4_1345_0123;
  2240.     float det5_01345_01235 = mat[0][0] * det4_1345_1235 - mat[0][1] * det4_1345_0235 + mat[0][2] * det4_1345_0135 - mat[0][3] * det4_1345_0125 + mat[0][5] * det4_1345_0123;
  2241.     float det5_01345_01245 = mat[0][0] * det4_1345_1245 - mat[0][1] * det4_1345_0245 + mat[0][2] * det4_1345_0145 - mat[0][4] * det4_1345_0125 + mat[0][5] * det4_1345_0124;
  2242.     float det5_01345_01345 = mat[0][0] * det4_1345_1345 - mat[0][1] * det4_1345_0345 + mat[0][3] * det4_1345_0145 - mat[0][4] * det4_1345_0135 + mat[0][5] * det4_1345_0134;
  2243.     float det5_01345_02345 = mat[0][0] * det4_1345_2345 - mat[0][2] * det4_1345_0345 + mat[0][3] * det4_1345_0245 - mat[0][4] * det4_1345_0235 + mat[0][5] * det4_1345_0234;
  2244.     float det5_01345_12345 = mat[0][1] * det4_1345_2345 - mat[0][2] * det4_1345_1345 + mat[0][3] * det4_1345_1245 - mat[0][4] * det4_1345_1235 + mat[0][5] * det4_1345_1234;
  2245.     float det5_02345_01234 = mat[0][0] * det4_2345_1234 - mat[0][1] * det4_2345_0234 + mat[0][2] * det4_2345_0134 - mat[0][3] * det4_2345_0124 + mat[0][4] * det4_2345_0123;
  2246.     float det5_02345_01235 = mat[0][0] * det4_2345_1235 - mat[0][1] * det4_2345_0235 + mat[0][2] * det4_2345_0135 - mat[0][3] * det4_2345_0125 + mat[0][5] * det4_2345_0123;
  2247.     float det5_02345_01245 = mat[0][0] * det4_2345_1245 - mat[0][1] * det4_2345_0245 + mat[0][2] * det4_2345_0145 - mat[0][4] * det4_2345_0125 + mat[0][5] * det4_2345_0124;
  2248.     float det5_02345_01345 = mat[0][0] * det4_2345_1345 - mat[0][1] * det4_2345_0345 + mat[0][3] * det4_2345_0145 - mat[0][4] * det4_2345_0135 + mat[0][5] * det4_2345_0134;
  2249.     float det5_02345_02345 = mat[0][0] * det4_2345_2345 - mat[0][2] * det4_2345_0345 + mat[0][3] * det4_2345_0245 - mat[0][4] * det4_2345_0235 + mat[0][5] * det4_2345_0234;
  2250.     float det5_02345_12345 = mat[0][1] * det4_2345_2345 - mat[0][2] * det4_2345_1345 + mat[0][3] * det4_2345_1245 - mat[0][4] * det4_2345_1235 + mat[0][5] * det4_2345_1234;
  2251.  
  2252.     mat[0][0] =  det5_12345_12345 * invDet;
  2253.     mat[0][1] = -det5_02345_12345 * invDet;
  2254.     mat[0][2] =  det5_01345_12345 * invDet;
  2255.     mat[0][3] = -det5_01245_12345 * invDet;
  2256.     mat[0][4] =  det5_01235_12345 * invDet;
  2257.     mat[0][5] = -det5_01234_12345 * invDet;
  2258.  
  2259.     mat[1][0] = -det5_12345_02345 * invDet;
  2260.     mat[1][1] =  det5_02345_02345 * invDet;
  2261.     mat[1][2] = -det5_01345_02345 * invDet;
  2262.     mat[1][3] =  det5_01245_02345 * invDet;
  2263.     mat[1][4] = -det5_01235_02345 * invDet;
  2264.     mat[1][5] =  det5_01234_02345 * invDet;
  2265.  
  2266.     mat[2][0] =  det5_12345_01345 * invDet;
  2267.     mat[2][1] = -det5_02345_01345 * invDet;
  2268.     mat[2][2] =  det5_01345_01345 * invDet;
  2269.     mat[2][3] = -det5_01245_01345 * invDet;
  2270.     mat[2][4] =  det5_01235_01345 * invDet;
  2271.     mat[2][5] = -det5_01234_01345 * invDet;
  2272.  
  2273.     mat[3][0] = -det5_12345_01245 * invDet;
  2274.     mat[3][1] =  det5_02345_01245 * invDet;
  2275.     mat[3][2] = -det5_01345_01245 * invDet;
  2276.     mat[3][3] =  det5_01245_01245 * invDet;
  2277.     mat[3][4] = -det5_01235_01245 * invDet;
  2278.     mat[3][5] =  det5_01234_01245 * invDet;
  2279.  
  2280.     mat[4][0] =  det5_12345_01235 * invDet;
  2281.     mat[4][1] = -det5_02345_01235 * invDet;
  2282.     mat[4][2] =  det5_01345_01235 * invDet;
  2283.     mat[4][3] = -det5_01245_01235 * invDet;
  2284.     mat[4][4] =  det5_01235_01235 * invDet;
  2285.     mat[4][5] = -det5_01234_01235 * invDet;
  2286.  
  2287.     mat[5][0] = -det5_12345_01234 * invDet;
  2288.     mat[5][1] =  det5_02345_01234 * invDet;
  2289.     mat[5][2] = -det5_01345_01234 * invDet;
  2290.     mat[5][3] =  det5_01245_01234 * invDet;
  2291.     mat[5][4] = -det5_01235_01234 * invDet;
  2292.     mat[5][5] =  det5_01234_01234 * invDet;
  2293.  
  2294.     return true;
  2295. }
  2296.  
  2297. /*
  2298. ============
  2299. idMat6::InverseFastSelf
  2300. ============
  2301. */
  2302. bool idMat6::InverseFastSelf( void ) {
  2303. #if 0
  2304.     // 810+6+36 = 852 multiplications
  2305.     //                1 division
  2306.     double det, invDet;
  2307.  
  2308.     // 2x2 sub-determinants required to calculate 6x6 determinant
  2309.     float det2_45_01 = mat[4][0] * mat[5][1] - mat[4][1] * mat[5][0];
  2310.     float det2_45_02 = mat[4][0] * mat[5][2] - mat[4][2] * mat[5][0];
  2311.     float det2_45_03 = mat[4][0] * mat[5][3] - mat[4][3] * mat[5][0];
  2312.     float det2_45_04 = mat[4][0] * mat[5][4] - mat[4][4] * mat[5][0];
  2313.     float det2_45_05 = mat[4][0] * mat[5][5] - mat[4][5] * mat[5][0];
  2314.     float det2_45_12 = mat[4][1] * mat[5][2] - mat[4][2] * mat[5][1];
  2315.     float det2_45_13 = mat[4][1] * mat[5][3] - mat[4][3] * mat[5][1];
  2316.     float det2_45_14 = mat[4][1] * mat[5][4] - mat[4][4] * mat[5][1];
  2317.     float det2_45_15 = mat[4][1] * mat[5][5] - mat[4][5] * mat[5][1];
  2318.     float det2_45_23 = mat[4][2] * mat[5][3] - mat[4][3] * mat[5][2];
  2319.     float det2_45_24 = mat[4][2] * mat[5][4] - mat[4][4] * mat[5][2];
  2320.     float det2_45_25 = mat[4][2] * mat[5][5] - mat[4][5] * mat[5][2];
  2321.     float det2_45_34 = mat[4][3] * mat[5][4] - mat[4][4] * mat[5][3];
  2322.     float det2_45_35 = mat[4][3] * mat[5][5] - mat[4][5] * mat[5][3];
  2323.     float det2_45_45 = mat[4][4] * mat[5][5] - mat[4][5] * mat[5][4];
  2324.  
  2325.     // 3x3 sub-determinants required to calculate 6x6 determinant
  2326.     float det3_345_012 = mat[3][0] * det2_45_12 - mat[3][1] * det2_45_02 + mat[3][2] * det2_45_01;
  2327.     float det3_345_013 = mat[3][0] * det2_45_13 - mat[3][1] * det2_45_03 + mat[3][3] * det2_45_01;
  2328.     float det3_345_014 = mat[3][0] * det2_45_14 - mat[3][1] * det2_45_04 + mat[3][4] * det2_45_01;
  2329.     float det3_345_015 = mat[3][0] * det2_45_15 - mat[3][1] * det2_45_05 + mat[3][5] * det2_45_01;
  2330.     float det3_345_023 = mat[3][0] * det2_45_23 - mat[3][2] * det2_45_03 + mat[3][3] * det2_45_02;
  2331.     float det3_345_024 = mat[3][0] * det2_45_24 - mat[3][2] * det2_45_04 + mat[3][4] * det2_45_02;
  2332.     float det3_345_025 = mat[3][0] * det2_45_25 - mat[3][2] * det2_45_05 + mat[3][5] * det2_45_02;
  2333.     float det3_345_034 = mat[3][0] * det2_45_34 - mat[3][3] * det2_45_04 + mat[3][4] * det2_45_03;
  2334.     float det3_345_035 = mat[3][0] * det2_45_35 - mat[3][3] * det2_45_05 + mat[3][5] * det2_45_03;
  2335.     float det3_345_045 = mat[3][0] * det2_45_45 - mat[3][4] * det2_45_05 + mat[3][5] * det2_45_04;
  2336.     float det3_345_123 = mat[3][1] * det2_45_23 - mat[3][2] * det2_45_13 + mat[3][3] * det2_45_12;
  2337.     float det3_345_124 = mat[3][1] * det2_45_24 - mat[3][2] * det2_45_14 + mat[3][4] * det2_45_12;
  2338.     float det3_345_125 = mat[3][1] * det2_45_25 - mat[3][2] * det2_45_15 + mat[3][5] * det2_45_12;
  2339.     float det3_345_134 = mat[3][1] * det2_45_34 - mat[3][3] * det2_45_14 + mat[3][4] * det2_45_13;
  2340.     float det3_345_135 = mat[3][1] * det2_45_35 - mat[3][3] * det2_45_15 + mat[3][5] * det2_45_13;
  2341.     float det3_345_145 = mat[3][1] * det2_45_45 - mat[3][4] * det2_45_15 + mat[3][5] * det2_45_14;
  2342.     float det3_345_234 = mat[3][2] * det2_45_34 - mat[3][3] * det2_45_24 + mat[3][4] * det2_45_23;
  2343.     float det3_345_235 = mat[3][2] * det2_45_35 - mat[3][3] * det2_45_25 + mat[3][5] * det2_45_23;
  2344.     float det3_345_245 = mat[3][2] * det2_45_45 - mat[3][4] * det2_45_25 + mat[3][5] * det2_45_24;
  2345.     float det3_345_345 = mat[3][3] * det2_45_45 - mat[3][4] * det2_45_35 + mat[3][5] * det2_45_34;
  2346.  
  2347.     // 4x4 sub-determinants required to calculate 6x6 determinant
  2348.     float det4_2345_0123 = mat[2][0] * det3_345_123 - mat[2][1] * det3_345_023 + mat[2][2] * det3_345_013 - mat[2][3] * det3_345_012;
  2349.     float det4_2345_0124 = mat[2][0] * det3_345_124 - mat[2][1] * det3_345_024 + mat[2][2] * det3_345_014 - mat[2][4] * det3_345_012;
  2350.     float det4_2345_0125 = mat[2][0] * det3_345_125 - mat[2][1] * det3_345_025 + mat[2][2] * det3_345_015 - mat[2][5] * det3_345_012;
  2351.     float det4_2345_0134 = mat[2][0] * det3_345_134 - mat[2][1] * det3_345_034 + mat[2][3] * det3_345_014 - mat[2][4] * det3_345_013;
  2352.     float det4_2345_0135 = mat[2][0] * det3_345_135 - mat[2][1] * det3_345_035 + mat[2][3] * det3_345_015 - mat[2][5] * det3_345_013;
  2353.     float det4_2345_0145 = mat[2][0] * det3_345_145 - mat[2][1] * det3_345_045 + mat[2][4] * det3_345_015 - mat[2][5] * det3_345_014;
  2354.     float det4_2345_0234 = mat[2][0] * det3_345_234 - mat[2][2] * det3_345_034 + mat[2][3] * det3_345_024 - mat[2][4] * det3_345_023;
  2355.     float det4_2345_0235 = mat[2][0] * det3_345_235 - mat[2][2] * det3_345_035 + mat[2][3] * det3_345_025 - mat[2][5] * det3_345_023;
  2356.     float det4_2345_0245 = mat[2][0] * det3_345_245 - mat[2][2] * det3_345_045 + mat[2][4] * det3_345_025 - mat[2][5] * det3_345_024;
  2357.     float det4_2345_0345 = mat[2][0] * det3_345_345 - mat[2][3] * det3_345_045 + mat[2][4] * det3_345_035 - mat[2][5] * det3_345_034;
  2358.     float det4_2345_1234 = mat[2][1] * det3_345_234 - mat[2][2] * det3_345_134 + mat[2][3] * det3_345_124 - mat[2][4] * det3_345_123;
  2359.     float det4_2345_1235 = mat[2][1] * det3_345_235 - mat[2][2] * det3_345_135 + mat[2][3] * det3_345_125 - mat[2][5] * det3_345_123;
  2360.     float det4_2345_1245 = mat[2][1] * det3_345_245 - mat[2][2] * det3_345_145 + mat[2][4] * det3_345_125 - mat[2][5] * det3_345_124;
  2361.     float det4_2345_1345 = mat[2][1] * det3_345_345 - mat[2][3] * det3_345_145 + mat[2][4] * det3_345_135 - mat[2][5] * det3_345_134;
  2362.     float det4_2345_2345 = mat[2][2] * det3_345_345 - mat[2][3] * det3_345_245 + mat[2][4] * det3_345_235 - mat[2][5] * det3_345_234;
  2363.  
  2364.     // 5x5 sub-determinants required to calculate 6x6 determinant
  2365.     float det5_12345_01234 = mat[1][0] * det4_2345_1234 - mat[1][1] * det4_2345_0234 + mat[1][2] * det4_2345_0134 - mat[1][3] * det4_2345_0124 + mat[1][4] * det4_2345_0123;
  2366.     float det5_12345_01235 = mat[1][0] * det4_2345_1235 - mat[1][1] * det4_2345_0235 + mat[1][2] * det4_2345_0135 - mat[1][3] * det4_2345_0125 + mat[1][5] * det4_2345_0123;
  2367.     float det5_12345_01245 = mat[1][0] * det4_2345_1245 - mat[1][1] * det4_2345_0245 + mat[1][2] * det4_2345_0145 - mat[1][4] * det4_2345_0125 + mat[1][5] * det4_2345_0124;
  2368.     float det5_12345_01345 = mat[1][0] * det4_2345_1345 - mat[1][1] * det4_2345_0345 + mat[1][3] * det4_2345_0145 - mat[1][4] * det4_2345_0135 + mat[1][5] * det4_2345_0134;
  2369.     float det5_12345_02345 = mat[1][0] * det4_2345_2345 - mat[1][2] * det4_2345_0345 + mat[1][3] * det4_2345_0245 - mat[1][4] * det4_2345_0235 + mat[1][5] * det4_2345_0234;
  2370.     float det5_12345_12345 = mat[1][1] * det4_2345_2345 - mat[1][2] * det4_2345_1345 + mat[1][3] * det4_2345_1245 - mat[1][4] * det4_2345_1235 + mat[1][5] * det4_2345_1234;
  2371.  
  2372.     // determinant of 6x6 matrix
  2373.     det = mat[0][0] * det5_12345_12345 - mat[0][1] * det5_12345_02345 + mat[0][2] * det5_12345_01345 -
  2374.                 mat[0][3] * det5_12345_01245 + mat[0][4] * det5_12345_01235 - mat[0][5] * det5_12345_01234;
  2375.  
  2376.     if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  2377.         return false;
  2378.     }
  2379.  
  2380.     invDet = 1.0f / det;
  2381.  
  2382.     // remaining 2x2 sub-determinants
  2383.     float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0];
  2384.     float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0];
  2385.     float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0];
  2386.     float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0];
  2387.     float det2_34_05 = mat[3][0] * mat[4][5] - mat[3][5] * mat[4][0];
  2388.     float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1];
  2389.     float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1];
  2390.     float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1];
  2391.     float det2_34_15 = mat[3][1] * mat[4][5] - mat[3][5] * mat[4][1];
  2392.     float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2];
  2393.     float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2];
  2394.     float det2_34_25 = mat[3][2] * mat[4][5] - mat[3][5] * mat[4][2];
  2395.     float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3];
  2396.     float det2_34_35 = mat[3][3] * mat[4][5] - mat[3][5] * mat[4][3];
  2397.     float det2_34_45 = mat[3][4] * mat[4][5] - mat[3][5] * mat[4][4];
  2398.     float det2_35_01 = mat[3][0] * mat[5][1] - mat[3][1] * mat[5][0];
  2399.     float det2_35_02 = mat[3][0] * mat[5][2] - mat[3][2] * mat[5][0];
  2400.     float det2_35_03 = mat[3][0] * mat[5][3] - mat[3][3] * mat[5][0];
  2401.     float det2_35_04 = mat[3][0] * mat[5][4] - mat[3][4] * mat[5][0];
  2402.     float det2_35_05 = mat[3][0] * mat[5][5] - mat[3][5] * mat[5][0];
  2403.     float det2_35_12 = mat[3][1] * mat[5][2] - mat[3][2] * mat[5][1];
  2404.     float det2_35_13 = mat[3][1] * mat[5][3] - mat[3][3] * mat[5][1];
  2405.     float det2_35_14 = mat[3][1] * mat[5][4] - mat[3][4] * mat[5][1];
  2406.     float det2_35_15 = mat[3][1] * mat[5][5] - mat[3][5] * mat[5][1];
  2407.     float det2_35_23 = mat[3][2] * mat[5][3] - mat[3][3] * mat[5][2];
  2408.     float det2_35_24 = mat[3][2] * mat[5][4] - mat[3][4] * mat[5][2];
  2409.     float det2_35_25 = mat[3][2] * mat[5][5] - mat[3][5] * mat[5][2];
  2410.     float det2_35_34 = mat[3][3] * mat[5][4] - mat[3][4] * mat[5][3];
  2411.     float det2_35_35 = mat[3][3] * mat[5][5] - mat[3][5] * mat[5][3];
  2412.     float det2_35_45 = mat[3][4] * mat[5][5] - mat[3][5] * mat[5][4];
  2413.  
  2414.     // remaining 3x3 sub-determinants
  2415.     float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01;
  2416.     float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01;
  2417.     float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01;
  2418.     float det3_234_015 = mat[2][0] * det2_34_15 - mat[2][1] * det2_34_05 + mat[2][5] * det2_34_01;
  2419.     float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02;
  2420.     float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02;
  2421.     float det3_234_025 = mat[2][0] * det2_34_25 - mat[2][2] * det2_34_05 + mat[2][5] * det2_34_02;
  2422.     float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03;
  2423.     float det3_234_035 = mat[2][0] * det2_34_35 - mat[2][3] * det2_34_05 + mat[2][5] * det2_34_03;
  2424.     float det3_234_045 = mat[2][0] * det2_34_45 - mat[2][4] * det2_34_05 + mat[2][5] * det2_34_04;
  2425.     float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12;
  2426.     float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12;
  2427.     float det3_234_125 = mat[2][1] * det2_34_25 - mat[2][2] * det2_34_15 + mat[2][5] * det2_34_12;
  2428.     float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13;
  2429.     float det3_234_135 = mat[2][1] * det2_34_35 - mat[2][3] * det2_34_15 + mat[2][5] * det2_34_13;
  2430.     float det3_234_145 = mat[2][1] * det2_34_45 - mat[2][4] * det2_34_15 + mat[2][5] * det2_34_14;
  2431.     float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23;
  2432.     float det3_234_235 = mat[2][2] * det2_34_35 - mat[2][3] * det2_34_25 + mat[2][5] * det2_34_23;
  2433.     float det3_234_245 = mat[2][2] * det2_34_45 - mat[2][4] * det2_34_25 + mat[2][5] * det2_34_24;
  2434.     float det3_234_345 = mat[2][3] * det2_34_45 - mat[2][4] * det2_34_35 + mat[2][5] * det2_34_34;
  2435.     float det3_235_012 = mat[2][0] * det2_35_12 - mat[2][1] * det2_35_02 + mat[2][2] * det2_35_01;
  2436.     float det3_235_013 = mat[2][0] * det2_35_13 - mat[2][1] * det2_35_03 + mat[2][3] * det2_35_01;
  2437.     float det3_235_014 = mat[2][0] * det2_35_14 - mat[2][1] * det2_35_04 + mat[2][4] * det2_35_01;
  2438.     float det3_235_015 = mat[2][0] * det2_35_15 - mat[2][1] * det2_35_05 + mat[2][5] * det2_35_01;
  2439.     float det3_235_023 = mat[2][0] * det2_35_23 - mat[2][2] * det2_35_03 + mat[2][3] * det2_35_02;
  2440.     float det3_235_024 = mat[2][0] * det2_35_24 - mat[2][2] * det2_35_04 + mat[2][4] * det2_35_02;
  2441.     float det3_235_025 = mat[2][0] * det2_35_25 - mat[2][2] * det2_35_05 + mat[2][5] * det2_35_02;
  2442.     float det3_235_034 = mat[2][0] * det2_35_34 - mat[2][3] * det2_35_04 + mat[2][4] * det2_35_03;
  2443.     float det3_235_035 = mat[2][0] * det2_35_35 - mat[2][3] * det2_35_05 + mat[2][5] * det2_35_03;
  2444.     float det3_235_045 = mat[2][0] * det2_35_45 - mat[2][4] * det2_35_05 + mat[2][5] * det2_35_04;
  2445.     float det3_235_123 = mat[2][1] * det2_35_23 - mat[2][2] * det2_35_13 + mat[2][3] * det2_35_12;
  2446.     float det3_235_124 = mat[2][1] * det2_35_24 - mat[2][2] * det2_35_14 + mat[2][4] * det2_35_12;
  2447.     float det3_235_125 = mat[2][1] * det2_35_25 - mat[2][2] * det2_35_15 + mat[2][5] * det2_35_12;
  2448.     float det3_235_134 = mat[2][1] * det2_35_34 - mat[2][3] * det2_35_14 + mat[2][4] * det2_35_13;
  2449.     float det3_235_135 = mat[2][1] * det2_35_35 - mat[2][3] * det2_35_15 + mat[2][5] * det2_35_13;
  2450.     float det3_235_145 = mat[2][1] * det2_35_45 - mat[2][4] * det2_35_15 + mat[2][5] * det2_35_14;
  2451.     float det3_235_234 = mat[2][2] * det2_35_34 - mat[2][3] * det2_35_24 + mat[2][4] * det2_35_23;
  2452.     float det3_235_235 = mat[2][2] * det2_35_35 - mat[2][3] * det2_35_25 + mat[2][5] * det2_35_23;
  2453.     float det3_235_245 = mat[2][2] * det2_35_45 - mat[2][4] * det2_35_25 + mat[2][5] * det2_35_24;
  2454.     float det3_235_345 = mat[2][3] * det2_35_45 - mat[2][4] * det2_35_35 + mat[2][5] * det2_35_34;
  2455.     float det3_245_012 = mat[2][0] * det2_45_12 - mat[2][1] * det2_45_02 + mat[2][2] * det2_45_01;
  2456.     float det3_245_013 = mat[2][0] * det2_45_13 - mat[2][1] * det2_45_03 + mat[2][3] * det2_45_01;
  2457.     float det3_245_014 = mat[2][0] * det2_45_14 - mat[2][1] * det2_45_04 + mat[2][4] * det2_45_01;
  2458.     float det3_245_015 = mat[2][0] * det2_45_15 - mat[2][1] * det2_45_05 + mat[2][5] * det2_45_01;
  2459.     float det3_245_023 = mat[2][0] * det2_45_23 - mat[2][2] * det2_45_03 + mat[2][3] * det2_45_02;
  2460.     float det3_245_024 = mat[2][0] * det2_45_24 - mat[2][2] * det2_45_04 + mat[2][4] * det2_45_02;
  2461.     float det3_245_025 = mat[2][0] * det2_45_25 - mat[2][2] * det2_45_05 + mat[2][5] * det2_45_02;
  2462.     float det3_245_034 = mat[2][0] * det2_45_34 - mat[2][3] * det2_45_04 + mat[2][4] * det2_45_03;
  2463.     float det3_245_035 = mat[2][0] * det2_45_35 - mat[2][3] * det2_45_05 + mat[2][5] * det2_45_03;
  2464.     float det3_245_045 = mat[2][0] * det2_45_45 - mat[2][4] * det2_45_05 + mat[2][5] * det2_45_04;
  2465.     float det3_245_123 = mat[2][1] * det2_45_23 - mat[2][2] * det2_45_13 + mat[2][3] * det2_45_12;
  2466.     float det3_245_124 = mat[2][1] * det2_45_24 - mat[2][2] * det2_45_14 + mat[2][4] * det2_45_12;
  2467.     float det3_245_125 = mat[2][1] * det2_45_25 - mat[2][2] * det2_45_15 + mat[2][5] * det2_45_12;
  2468.     float det3_245_134 = mat[2][1] * det2_45_34 - mat[2][3] * det2_45_14 + mat[2][4] * det2_45_13;
  2469.     float det3_245_135 = mat[2][1] * det2_45_35 - mat[2][3] * det2_45_15 + mat[2][5] * det2_45_13;
  2470.     float det3_245_145 = mat[2][1] * det2_45_45 - mat[2][4] * det2_45_15 + mat[2][5] * det2_45_14;
  2471.     float det3_245_234 = mat[2][2] * det2_45_34 - mat[2][3] * det2_45_24 + mat[2][4] * det2_45_23;
  2472.     float det3_245_235 = mat[2][2] * det2_45_35 - mat[2][3] * det2_45_25 + mat[2][5] * det2_45_23;
  2473.     float det3_245_245 = mat[2][2] * det2_45_45 - mat[2][4] * det2_45_25 + mat[2][5] * det2_45_24;
  2474.     float det3_245_345 = mat[2][3] * det2_45_45 - mat[2][4] * det2_45_35 + mat[2][5] * det2_45_34;
  2475.  
  2476.     // remaining 4x4 sub-determinants
  2477.     float det4_1234_0123 = mat[1][0] * det3_234_123 - mat[1][1] * det3_234_023 + mat[1][2] * det3_234_013 - mat[1][3] * det3_234_012;
  2478.     float det4_1234_0124 = mat[1][0] * det3_234_124 - mat[1][1] * det3_234_024 + mat[1][2] * det3_234_014 - mat[1][4] * det3_234_012;
  2479.     float det4_1234_0125 = mat[1][0] * det3_234_125 - mat[1][1] * det3_234_025 + mat[1][2] * det3_234_015 - mat[1][5] * det3_234_012;
  2480.     float det4_1234_0134 = mat[1][0] * det3_234_134 - mat[1][1] * det3_234_034 + mat[1][3] * det3_234_014 - mat[1][4] * det3_234_013;
  2481.     float det4_1234_0135 = mat[1][0] * det3_234_135 - mat[1][1] * det3_234_035 + mat[1][3] * det3_234_015 - mat[1][5] * det3_234_013;
  2482.     float det4_1234_0145 = mat[1][0] * det3_234_145 - mat[1][1] * det3_234_045 + mat[1][4] * det3_234_015 - mat[1][5] * det3_234_014;
  2483.     float det4_1234_0234 = mat[1][0] * det3_234_234 - mat[1][2] * det3_234_034 + mat[1][3] * det3_234_024 - mat[1][4] * det3_234_023;
  2484.     float det4_1234_0235 = mat[1][0] * det3_234_235 - mat[1][2] * det3_234_035 + mat[1][3] * det3_234_025 - mat[1][5] * det3_234_023;
  2485.     float det4_1234_0245 = mat[1][0] * det3_234_245 - mat[1][2] * det3_234_045 + mat[1][4] * det3_234_025 - mat[1][5] * det3_234_024;
  2486.     float det4_1234_0345 = mat[1][0] * det3_234_345 - mat[1][3] * det3_234_045 + mat[1][4] * det3_234_035 - mat[1][5] * det3_234_034;
  2487.     float det4_1234_1234 = mat[1][1] * det3_234_234 - mat[1][2] * det3_234_134 + mat[1][3] * det3_234_124 - mat[1][4] * det3_234_123;
  2488.     float det4_1234_1235 = mat[1][1] * det3_234_235 - mat[1][2] * det3_234_135 + mat[1][3] * det3_234_125 - mat[1][5] * det3_234_123;
  2489.     float det4_1234_1245 = mat[1][1] * det3_234_245 - mat[1][2] * det3_234_145 + mat[1][4] * det3_234_125 - mat[1][5] * det3_234_124;
  2490.     float det4_1234_1345 = mat[1][1] * det3_234_345 - mat[1][3] * det3_234_145 + mat[1][4] * det3_234_135 - mat[1][5] * det3_234_134;
  2491.     float det4_1234_2345 = mat[1][2] * det3_234_345 - mat[1][3] * det3_234_245 + mat[1][4] * det3_234_235 - mat[1][5] * det3_234_234;
  2492.     float det4_1235_0123 = mat[1][0] * det3_235_123 - mat[1][1] * det3_235_023 + mat[1][2] * det3_235_013 - mat[1][3] * det3_235_012;
  2493.     float det4_1235_0124 = mat[1][0] * det3_235_124 - mat[1][1] * det3_235_024 + mat[1][2] * det3_235_014 - mat[1][4] * det3_235_012;
  2494.     float det4_1235_0125 = mat[1][0] * det3_235_125 - mat[1][1] * det3_235_025 + mat[1][2] * det3_235_015 - mat[1][5] * det3_235_012;
  2495.     float det4_1235_0134 = mat[1][0] * det3_235_134 - mat[1][1] * det3_235_034 + mat[1][3] * det3_235_014 - mat[1][4] * det3_235_013;
  2496.     float det4_1235_0135 = mat[1][0] * det3_235_135 - mat[1][1] * det3_235_035 + mat[1][3] * det3_235_015 - mat[1][5] * det3_235_013;
  2497.     float det4_1235_0145 = mat[1][0] * det3_235_145 - mat[1][1] * det3_235_045 + mat[1][4] * det3_235_015 - mat[1][5] * det3_235_014;
  2498.     float det4_1235_0234 = mat[1][0] * det3_235_234 - mat[1][2] * det3_235_034 + mat[1][3] * det3_235_024 - mat[1][4] * det3_235_023;
  2499.     float det4_1235_0235 = mat[1][0] * det3_235_235 - mat[1][2] * det3_235_035 + mat[1][3] * det3_235_025 - mat[1][5] * det3_235_023;
  2500.     float det4_1235_0245 = mat[1][0] * det3_235_245 - mat[1][2] * det3_235_045 + mat[1][4] * det3_235_025 - mat[1][5] * det3_235_024;
  2501.     float det4_1235_0345 = mat[1][0] * det3_235_345 - mat[1][3] * det3_235_045 + mat[1][4] * det3_235_035 - mat[1][5] * det3_235_034;
  2502.     float det4_1235_1234 = mat[1][1] * det3_235_234 - mat[1][2] * det3_235_134 + mat[1][3] * det3_235_124 - mat[1][4] * det3_235_123;
  2503.     float det4_1235_1235 = mat[1][1] * det3_235_235 - mat[1][2] * det3_235_135 + mat[1][3] * det3_235_125 - mat[1][5] * det3_235_123;
  2504.     float det4_1235_1245 = mat[1][1] * det3_235_245 - mat[1][2] * det3_235_145 + mat[1][4] * det3_235_125 - mat[1][5] * det3_235_124;
  2505.     float det4_1235_1345 = mat[1][1] * det3_235_345 - mat[1][3] * det3_235_145 + mat[1][4] * det3_235_135 - mat[1][5] * det3_235_134;
  2506.     float det4_1235_2345 = mat[1][2] * det3_235_345 - mat[1][3] * det3_235_245 + mat[1][4] * det3_235_235 - mat[1][5] * det3_235_234;
  2507.     float det4_1245_0123 = mat[1][0] * det3_245_123 - mat[1][1] * det3_245_023 + mat[1][2] * det3_245_013 - mat[1][3] * det3_245_012;
  2508.     float det4_1245_0124 = mat[1][0] * det3_245_124 - mat[1][1] * det3_245_024 + mat[1][2] * det3_245_014 - mat[1][4] * det3_245_012;
  2509.     float det4_1245_0125 = mat[1][0] * det3_245_125 - mat[1][1] * det3_245_025 + mat[1][2] * det3_245_015 - mat[1][5] * det3_245_012;
  2510.     float det4_1245_0134 = mat[1][0] * det3_245_134 - mat[1][1] * det3_245_034 + mat[1][3] * det3_245_014 - mat[1][4] * det3_245_013;
  2511.     float det4_1245_0135 = mat[1][0] * det3_245_135 - mat[1][1] * det3_245_035 + mat[1][3] * det3_245_015 - mat[1][5] * det3_245_013;
  2512.     float det4_1245_0145 = mat[1][0] * det3_245_145 - mat[1][1] * det3_245_045 + mat[1][4] * det3_245_015 - mat[1][5] * det3_245_014;
  2513.     float det4_1245_0234 = mat[1][0] * det3_245_234 - mat[1][2] * det3_245_034 + mat[1][3] * det3_245_024 - mat[1][4] * det3_245_023;
  2514.     float det4_1245_0235 = mat[1][0] * det3_245_235 - mat[1][2] * det3_245_035 + mat[1][3] * det3_245_025 - mat[1][5] * det3_245_023;
  2515.     float det4_1245_0245 = mat[1][0] * det3_245_245 - mat[1][2] * det3_245_045 + mat[1][4] * det3_245_025 - mat[1][5] * det3_245_024;
  2516.     float det4_1245_0345 = mat[1][0] * det3_245_345 - mat[1][3] * det3_245_045 + mat[1][4] * det3_245_035 - mat[1][5] * det3_245_034;
  2517.     float det4_1245_1234 = mat[1][1] * det3_245_234 - mat[1][2] * det3_245_134 + mat[1][3] * det3_245_124 - mat[1][4] * det3_245_123;
  2518.     float det4_1245_1235 = mat[1][1] * det3_245_235 - mat[1][2] * det3_245_135 + mat[1][3] * det3_245_125 - mat[1][5] * det3_245_123;
  2519.     float det4_1245_1245 = mat[1][1] * det3_245_245 - mat[1][2] * det3_245_145 + mat[1][4] * det3_245_125 - mat[1][5] * det3_245_124;
  2520.     float det4_1245_1345 = mat[1][1] * det3_245_345 - mat[1][3] * det3_245_145 + mat[1][4] * det3_245_135 - mat[1][5] * det3_245_134;
  2521.     float det4_1245_2345 = mat[1][2] * det3_245_345 - mat[1][3] * det3_245_245 + mat[1][4] * det3_245_235 - mat[1][5] * det3_245_234;
  2522.     float det4_1345_0123 = mat[1][0] * det3_345_123 - mat[1][1] * det3_345_023 + mat[1][2] * det3_345_013 - mat[1][3] * det3_345_012;
  2523.     float det4_1345_0124 = mat[1][0] * det3_345_124 - mat[1][1] * det3_345_024 + mat[1][2] * det3_345_014 - mat[1][4] * det3_345_012;
  2524.     float det4_1345_0125 = mat[1][0] * det3_345_125 - mat[1][1] * det3_345_025 + mat[1][2] * det3_345_015 - mat[1][5] * det3_345_012;
  2525.     float det4_1345_0134 = mat[1][0] * det3_345_134 - mat[1][1] * det3_345_034 + mat[1][3] * det3_345_014 - mat[1][4] * det3_345_013;
  2526.     float det4_1345_0135 = mat[1][0] * det3_345_135 - mat[1][1] * det3_345_035 + mat[1][3] * det3_345_015 - mat[1][5] * det3_345_013;
  2527.     float det4_1345_0145 = mat[1][0] * det3_345_145 - mat[1][1] * det3_345_045 + mat[1][4] * det3_345_015 - mat[1][5] * det3_345_014;
  2528.     float det4_1345_0234 = mat[1][0] * det3_345_234 - mat[1][2] * det3_345_034 + mat[1][3] * det3_345_024 - mat[1][4] * det3_345_023;
  2529.     float det4_1345_0235 = mat[1][0] * det3_345_235 - mat[1][2] * det3_345_035 + mat[1][3] * det3_345_025 - mat[1][5] * det3_345_023;
  2530.     float det4_1345_0245 = mat[1][0] * det3_345_245 - mat[1][2] * det3_345_045 + mat[1][4] * det3_345_025 - mat[1][5] * det3_345_024;
  2531.     float det4_1345_0345 = mat[1][0] * det3_345_345 - mat[1][3] * det3_345_045 + mat[1][4] * det3_345_035 - mat[1][5] * det3_345_034;
  2532.     float det4_1345_1234 = mat[1][1] * det3_345_234 - mat[1][2] * det3_345_134 + mat[1][3] * det3_345_124 - mat[1][4] * det3_345_123;
  2533.     float det4_1345_1235 = mat[1][1] * det3_345_235 - mat[1][2] * det3_345_135 + mat[1][3] * det3_345_125 - mat[1][5] * det3_345_123;
  2534.     float det4_1345_1245 = mat[1][1] * det3_345_245 - mat[1][2] * det3_345_145 + mat[1][4] * det3_345_125 - mat[1][5] * det3_345_124;
  2535.     float det4_1345_1345 = mat[1][1] * det3_345_345 - mat[1][3] * det3_345_145 + mat[1][4] * det3_345_135 - mat[1][5] * det3_345_134;
  2536.     float det4_1345_2345 = mat[1][2] * det3_345_345 - mat[1][3] * det3_345_245 + mat[1][4] * det3_345_235 - mat[1][5] * det3_345_234;
  2537.  
  2538.     // remaining 5x5 sub-determinants
  2539.     float det5_01234_01234 = mat[0][0] * det4_1234_1234 - mat[0][1] * det4_1234_0234 + mat[0][2] * det4_1234_0134 - mat[0][3] * det4_1234_0124 + mat[0][4] * det4_1234_0123;
  2540.     float det5_01234_01235 = mat[0][0] * det4_1234_1235 - mat[0][1] * det4_1234_0235 + mat[0][2] * det4_1234_0135 - mat[0][3] * det4_1234_0125 + mat[0][5] * det4_1234_0123;
  2541.     float det5_01234_01245 = mat[0][0] * det4_1234_1245 - mat[0][1] * det4_1234_0245 + mat[0][2] * det4_1234_0145 - mat[0][4] * det4_1234_0125 + mat[0][5] * det4_1234_0124;
  2542.     float det5_01234_01345 = mat[0][0] * det4_1234_1345 - mat[0][1] * det4_1234_0345 + mat[0][3] * det4_1234_0145 - mat[0][4] * det4_1234_0135 + mat[0][5] * det4_1234_0134;
  2543.     float det5_01234_02345 = mat[0][0] * det4_1234_2345 - mat[0][2] * det4_1234_0345 + mat[0][3] * det4_1234_0245 - mat[0][4] * det4_1234_0235 + mat[0][5] * det4_1234_0234;
  2544.     float det5_01234_12345 = mat[0][1] * det4_1234_2345 - mat[0][2] * det4_1234_1345 + mat[0][3] * det4_1234_1245 - mat[0][4] * det4_1234_1235 + mat[0][5] * det4_1234_1234;
  2545.     float det5_01235_01234 = mat[0][0] * det4_1235_1234 - mat[0][1] * det4_1235_0234 + mat[0][2] * det4_1235_0134 - mat[0][3] * det4_1235_0124 + mat[0][4] * det4_1235_0123;
  2546.     float det5_01235_01235 = mat[0][0] * det4_1235_1235 - mat[0][1] * det4_1235_0235 + mat[0][2] * det4_1235_0135 - mat[0][3] * det4_1235_0125 + mat[0][5] * det4_1235_0123;
  2547.     float det5_01235_01245 = mat[0][0] * det4_1235_1245 - mat[0][1] * det4_1235_0245 + mat[0][2] * det4_1235_0145 - mat[0][4] * det4_1235_0125 + mat[0][5] * det4_1235_0124;
  2548.     float det5_01235_01345 = mat[0][0] * det4_1235_1345 - mat[0][1] * det4_1235_0345 + mat[0][3] * det4_1235_0145 - mat[0][4] * det4_1235_0135 + mat[0][5] * det4_1235_0134;
  2549.     float det5_01235_02345 = mat[0][0] * det4_1235_2345 - mat[0][2] * det4_1235_0345 + mat[0][3] * det4_1235_0245 - mat[0][4] * det4_1235_0235 + mat[0][5] * det4_1235_0234;
  2550.     float det5_01235_12345 = mat[0][1] * det4_1235_2345 - mat[0][2] * det4_1235_1345 + mat[0][3] * det4_1235_1245 - mat[0][4] * det4_1235_1235 + mat[0][5] * det4_1235_1234;
  2551.     float det5_01245_01234 = mat[0][0] * det4_1245_1234 - mat[0][1] * det4_1245_0234 + mat[0][2] * det4_1245_0134 - mat[0][3] * det4_1245_0124 + mat[0][4] * det4_1245_0123;
  2552.     float det5_01245_01235 = mat[0][0] * det4_1245_1235 - mat[0][1] * det4_1245_0235 + mat[0][2] * det4_1245_0135 - mat[0][3] * det4_1245_0125 + mat[0][5] * det4_1245_0123;
  2553.     float det5_01245_01245 = mat[0][0] * det4_1245_1245 - mat[0][1] * det4_1245_0245 + mat[0][2] * det4_1245_0145 - mat[0][4] * det4_1245_0125 + mat[0][5] * det4_1245_0124;
  2554.     float det5_01245_01345 = mat[0][0] * det4_1245_1345 - mat[0][1] * det4_1245_0345 + mat[0][3] * det4_1245_0145 - mat[0][4] * det4_1245_0135 + mat[0][5] * det4_1245_0134;
  2555.     float det5_01245_02345 = mat[0][0] * det4_1245_2345 - mat[0][2] * det4_1245_0345 + mat[0][3] * det4_1245_0245 - mat[0][4] * det4_1245_0235 + mat[0][5] * det4_1245_0234;
  2556.     float det5_01245_12345 = mat[0][1] * det4_1245_2345 - mat[0][2] * det4_1245_1345 + mat[0][3] * det4_1245_1245 - mat[0][4] * det4_1245_1235 + mat[0][5] * det4_1245_1234;
  2557.     float det5_01345_01234 = mat[0][0] * det4_1345_1234 - mat[0][1] * det4_1345_0234 + mat[0][2] * det4_1345_0134 - mat[0][3] * det4_1345_0124 + mat[0][4] * det4_1345_0123;
  2558.     float det5_01345_01235 = mat[0][0] * det4_1345_1235 - mat[0][1] * det4_1345_0235 + mat[0][2] * det4_1345_0135 - mat[0][3] * det4_1345_0125 + mat[0][5] * det4_1345_0123;
  2559.     float det5_01345_01245 = mat[0][0] * det4_1345_1245 - mat[0][1] * det4_1345_0245 + mat[0][2] * det4_1345_0145 - mat[0][4] * det4_1345_0125 + mat[0][5] * det4_1345_0124;
  2560.     float det5_01345_01345 = mat[0][0] * det4_1345_1345 - mat[0][1] * det4_1345_0345 + mat[0][3] * det4_1345_0145 - mat[0][4] * det4_1345_0135 + mat[0][5] * det4_1345_0134;
  2561.     float det5_01345_02345 = mat[0][0] * det4_1345_2345 - mat[0][2] * det4_1345_0345 + mat[0][3] * det4_1345_0245 - mat[0][4] * det4_1345_0235 + mat[0][5] * det4_1345_0234;
  2562.     float det5_01345_12345 = mat[0][1] * det4_1345_2345 - mat[0][2] * det4_1345_1345 + mat[0][3] * det4_1345_1245 - mat[0][4] * det4_1345_1235 + mat[0][5] * det4_1345_1234;
  2563.     float det5_02345_01234 = mat[0][0] * det4_2345_1234 - mat[0][1] * det4_2345_0234 + mat[0][2] * det4_2345_0134 - mat[0][3] * det4_2345_0124 + mat[0][4] * det4_2345_0123;
  2564.     float det5_02345_01235 = mat[0][0] * det4_2345_1235 - mat[0][1] * det4_2345_0235 + mat[0][2] * det4_2345_0135 - mat[0][3] * det4_2345_0125 + mat[0][5] * det4_2345_0123;
  2565.     float det5_02345_01245 = mat[0][0] * det4_2345_1245 - mat[0][1] * det4_2345_0245 + mat[0][2] * det4_2345_0145 - mat[0][4] * det4_2345_0125 + mat[0][5] * det4_2345_0124;
  2566.     float det5_02345_01345 = mat[0][0] * det4_2345_1345 - mat[0][1] * det4_2345_0345 + mat[0][3] * det4_2345_0145 - mat[0][4] * det4_2345_0135 + mat[0][5] * det4_2345_0134;
  2567.     float det5_02345_02345 = mat[0][0] * det4_2345_2345 - mat[0][2] * det4_2345_0345 + mat[0][3] * det4_2345_0245 - mat[0][4] * det4_2345_0235 + mat[0][5] * det4_2345_0234;
  2568.     float det5_02345_12345 = mat[0][1] * det4_2345_2345 - mat[0][2] * det4_2345_1345 + mat[0][3] * det4_2345_1245 - mat[0][4] * det4_2345_1235 + mat[0][5] * det4_2345_1234;
  2569.  
  2570.     mat[0][0] =  det5_12345_12345 * invDet;
  2571.     mat[0][1] = -det5_02345_12345 * invDet;
  2572.     mat[0][2] =  det5_01345_12345 * invDet;
  2573.     mat[0][3] = -det5_01245_12345 * invDet;
  2574.     mat[0][4] =  det5_01235_12345 * invDet;
  2575.     mat[0][5] = -det5_01234_12345 * invDet;
  2576.  
  2577.     mat[1][0] = -det5_12345_02345 * invDet;
  2578.     mat[1][1] =  det5_02345_02345 * invDet;
  2579.     mat[1][2] = -det5_01345_02345 * invDet;
  2580.     mat[1][3] =  det5_01245_02345 * invDet;
  2581.     mat[1][4] = -det5_01235_02345 * invDet;
  2582.     mat[1][5] =  det5_01234_02345 * invDet;
  2583.  
  2584.     mat[2][0] =  det5_12345_01345 * invDet;
  2585.     mat[2][1] = -det5_02345_01345 * invDet;
  2586.     mat[2][2] =  det5_01345_01345 * invDet;
  2587.     mat[2][3] = -det5_01245_01345 * invDet;
  2588.     mat[2][4] =  det5_01235_01345 * invDet;
  2589.     mat[2][5] = -det5_01234_01345 * invDet;
  2590.  
  2591.     mat[3][0] = -det5_12345_01245 * invDet;
  2592.     mat[3][1] =  det5_02345_01245 * invDet;
  2593.     mat[3][2] = -det5_01345_01245 * invDet;
  2594.     mat[3][3] =  det5_01245_01245 * invDet;
  2595.     mat[3][4] = -det5_01235_01245 * invDet;
  2596.     mat[3][5] =  det5_01234_01245 * invDet;
  2597.  
  2598.     mat[4][0] =  det5_12345_01235 * invDet;
  2599.     mat[4][1] = -det5_02345_01235 * invDet;
  2600.     mat[4][2] =  det5_01345_01235 * invDet;
  2601.     mat[4][3] = -det5_01245_01235 * invDet;
  2602.     mat[4][4] =  det5_01235_01235 * invDet;
  2603.     mat[4][5] = -det5_01234_01235 * invDet;
  2604.  
  2605.     mat[5][0] = -det5_12345_01234 * invDet;
  2606.     mat[5][1] =  det5_02345_01234 * invDet;
  2607.     mat[5][2] = -det5_01345_01234 * invDet;
  2608.     mat[5][3] =  det5_01245_01234 * invDet;
  2609.     mat[5][4] = -det5_01235_01234 * invDet;
  2610.     mat[5][5] =  det5_01234_01234 * invDet;
  2611.  
  2612.     return true;
  2613. #elif 0
  2614.     // 6*40 = 240 multiplications
  2615.     //            6 divisions
  2616.     float *mat = reinterpret_cast<float *>(this);
  2617.     float s;
  2618.     double d, di;
  2619.  
  2620.     di = mat[0];
  2621.     s = di;
  2622.     mat[0] = d = 1.0f / di;
  2623.     mat[1] *= d;
  2624.     mat[2] *= d;
  2625.     mat[3] *= d;
  2626.     mat[4] *= d;
  2627.     mat[5] *= d;
  2628.     d = -d;
  2629.     mat[6] *= d;
  2630.     mat[12] *= d;
  2631.     mat[18] *= d;
  2632.     mat[24] *= d;
  2633.     mat[30] *= d;
  2634.     d = mat[6] * di;
  2635.     mat[7] += mat[1] * d;
  2636.     mat[8] += mat[2] * d;
  2637.     mat[9] += mat[3] * d;
  2638.     mat[10] += mat[4] * d;
  2639.     mat[11] += mat[5] * d;
  2640.     d = mat[12] * di;
  2641.     mat[13] += mat[1] * d;
  2642.     mat[14] += mat[2] * d;
  2643.     mat[15] += mat[3] * d;
  2644.     mat[16] += mat[4] * d;
  2645.     mat[17] += mat[5] * d;
  2646.     d = mat[18] * di;
  2647.     mat[19] += mat[1] * d;
  2648.     mat[20] += mat[2] * d;
  2649.     mat[21] += mat[3] * d;
  2650.     mat[22] += mat[4] * d;
  2651.     mat[23] += mat[5] * d;
  2652.     d = mat[24] * di;
  2653.     mat[25] += mat[1] * d;
  2654.     mat[26] += mat[2] * d;
  2655.     mat[27] += mat[3] * d;
  2656.     mat[28] += mat[4] * d;
  2657.     mat[29] += mat[5] * d;
  2658.     d = mat[30] * di;
  2659.     mat[31] += mat[1] * d;
  2660.     mat[32] += mat[2] * d;
  2661.     mat[33] += mat[3] * d;
  2662.     mat[34] += mat[4] * d;
  2663.     mat[35] += mat[5] * d;
  2664.     di = mat[7];
  2665.     s *= di;
  2666.     mat[7] = d = 1.0f / di;
  2667.     mat[6] *= d;
  2668.     mat[8] *= d;
  2669.     mat[9] *= d;
  2670.     mat[10] *= d;
  2671.     mat[11] *= d;
  2672.     d = -d;
  2673.     mat[1] *= d;
  2674.     mat[13] *= d;
  2675.     mat[19] *= d;
  2676.     mat[25] *= d;
  2677.     mat[31] *= d;
  2678.     d = mat[1] * di;
  2679.     mat[0] += mat[6] * d;
  2680.     mat[2] += mat[8] * d;
  2681.     mat[3] += mat[9] * d;
  2682.     mat[4] += mat[10] * d;
  2683.     mat[5] += mat[11] * d;
  2684.     d = mat[13] * di;
  2685.     mat[12] += mat[6] * d;
  2686.     mat[14] += mat[8] * d;
  2687.     mat[15] += mat[9] * d;
  2688.     mat[16] += mat[10] * d;
  2689.     mat[17] += mat[11] * d;
  2690.     d = mat[19] * di;
  2691.     mat[18] += mat[6] * d;
  2692.     mat[20] += mat[8] * d;
  2693.     mat[21] += mat[9] * d;
  2694.     mat[22] += mat[10] * d;
  2695.     mat[23] += mat[11] * d;
  2696.     d = mat[25] * di;
  2697.     mat[24] += mat[6] * d;
  2698.     mat[26] += mat[8] * d;
  2699.     mat[27] += mat[9] * d;
  2700.     mat[28] += mat[10] * d;
  2701.     mat[29] += mat[11] * d;
  2702.     d = mat[31] * di;
  2703.     mat[30] += mat[6] * d;
  2704.     mat[32] += mat[8] * d;
  2705.     mat[33] += mat[9] * d;
  2706.     mat[34] += mat[10] * d;
  2707.     mat[35] += mat[11] * d;
  2708.     di = mat[14];
  2709.     s *= di;
  2710.     mat[14] = d = 1.0f / di;
  2711.     mat[12] *= d;
  2712.     mat[13] *= d;
  2713.     mat[15] *= d;
  2714.     mat[16] *= d;
  2715.     mat[17] *= d;
  2716.     d = -d;
  2717.     mat[2] *= d;
  2718.     mat[8] *= d;
  2719.     mat[20] *= d;
  2720.     mat[26] *= d;
  2721.     mat[32] *= d;
  2722.     d = mat[2] * di;
  2723.     mat[0] += mat[12] * d;
  2724.     mat[1] += mat[13] * d;
  2725.     mat[3] += mat[15] * d;
  2726.     mat[4] += mat[16] * d;
  2727.     mat[5] += mat[17] * d;
  2728.     d = mat[8] * di;
  2729.     mat[6] += mat[12] * d;
  2730.     mat[7] += mat[13] * d;
  2731.     mat[9] += mat[15] * d;
  2732.     mat[10] += mat[16] * d;
  2733.     mat[11] += mat[17] * d;
  2734.     d = mat[20] * di;
  2735.     mat[18] += mat[12] * d;
  2736.     mat[19] += mat[13] * d;
  2737.     mat[21] += mat[15] * d;
  2738.     mat[22] += mat[16] * d;
  2739.     mat[23] += mat[17] * d;
  2740.     d = mat[26] * di;
  2741.     mat[24] += mat[12] * d;
  2742.     mat[25] += mat[13] * d;
  2743.     mat[27] += mat[15] * d;
  2744.     mat[28] += mat[16] * d;
  2745.     mat[29] += mat[17] * d;
  2746.     d = mat[32] * di;
  2747.     mat[30] += mat[12] * d;
  2748.     mat[31] += mat[13] * d;
  2749.     mat[33] += mat[15] * d;
  2750.     mat[34] += mat[16] * d;
  2751.     mat[35] += mat[17] * d;
  2752.     di = mat[21];
  2753.     s *= di;
  2754.     mat[21] = d = 1.0f / di;
  2755.     mat[18] *= d;
  2756.     mat[19] *= d;
  2757.     mat[20] *= d;
  2758.     mat[22] *= d;
  2759.     mat[23] *= d;
  2760.     d = -d;
  2761.     mat[3] *= d;
  2762.     mat[9] *= d;
  2763.     mat[15] *= d;
  2764.     mat[27] *= d;
  2765.     mat[33] *= d;
  2766.     d = mat[3] * di;
  2767.     mat[0] += mat[18] * d;
  2768.     mat[1] += mat[19] * d;
  2769.     mat[2] += mat[20] * d;
  2770.     mat[4] += mat[22] * d;
  2771.     mat[5] += mat[23] * d;
  2772.     d = mat[9] * di;
  2773.     mat[6] += mat[18] * d;
  2774.     mat[7] += mat[19] * d;
  2775.     mat[8] += mat[20] * d;
  2776.     mat[10] += mat[22] * d;
  2777.     mat[11] += mat[23] * d;
  2778.     d = mat[15] * di;
  2779.     mat[12] += mat[18] * d;
  2780.     mat[13] += mat[19] * d;
  2781.     mat[14] += mat[20] * d;
  2782.     mat[16] += mat[22] * d;
  2783.     mat[17] += mat[23] * d;
  2784.     d = mat[27] * di;
  2785.     mat[24] += mat[18] * d;
  2786.     mat[25] += mat[19] * d;
  2787.     mat[26] += mat[20] * d;
  2788.     mat[28] += mat[22] * d;
  2789.     mat[29] += mat[23] * d;
  2790.     d = mat[33] * di;
  2791.     mat[30] += mat[18] * d;
  2792.     mat[31] += mat[19] * d;
  2793.     mat[32] += mat[20] * d;
  2794.     mat[34] += mat[22] * d;
  2795.     mat[35] += mat[23] * d;
  2796.     di = mat[28];
  2797.     s *= di;
  2798.     mat[28] = d = 1.0f / di;
  2799.     mat[24] *= d;
  2800.     mat[25] *= d;
  2801.     mat[26] *= d;
  2802.     mat[27] *= d;
  2803.     mat[29] *= d;
  2804.     d = -d;
  2805.     mat[4] *= d;
  2806.     mat[10] *= d;
  2807.     mat[16] *= d;
  2808.     mat[22] *= d;
  2809.     mat[34] *= d;
  2810.     d = mat[4] * di;
  2811.     mat[0] += mat[24] * d;
  2812.     mat[1] += mat[25] * d;
  2813.     mat[2] += mat[26] * d;
  2814.     mat[3] += mat[27] * d;
  2815.     mat[5] += mat[29] * d;
  2816.     d = mat[10] * di;
  2817.     mat[6] += mat[24] * d;
  2818.     mat[7] += mat[25] * d;
  2819.     mat[8] += mat[26] * d;
  2820.     mat[9] += mat[27] * d;
  2821.     mat[11] += mat[29] * d;
  2822.     d = mat[16] * di;
  2823.     mat[12] += mat[24] * d;
  2824.     mat[13] += mat[25] * d;
  2825.     mat[14] += mat[26] * d;
  2826.     mat[15] += mat[27] * d;
  2827.     mat[17] += mat[29] * d;
  2828.     d = mat[22] * di;
  2829.     mat[18] += mat[24] * d;
  2830.     mat[19] += mat[25] * d;
  2831.     mat[20] += mat[26] * d;
  2832.     mat[21] += mat[27] * d;
  2833.     mat[23] += mat[29] * d;
  2834.     d = mat[34] * di;
  2835.     mat[30] += mat[24] * d;
  2836.     mat[31] += mat[25] * d;
  2837.     mat[32] += mat[26] * d;
  2838.     mat[33] += mat[27] * d;
  2839.     mat[35] += mat[29] * d;
  2840.     di = mat[35];
  2841.     s *= di;
  2842.     mat[35] = d = 1.0f / di;
  2843.     mat[30] *= d;
  2844.     mat[31] *= d;
  2845.     mat[32] *= d;
  2846.     mat[33] *= d;
  2847.     mat[34] *= d;
  2848.     d = -d;
  2849.     mat[5] *= d;
  2850.     mat[11] *= d;
  2851.     mat[17] *= d;
  2852.     mat[23] *= d;
  2853.     mat[29] *= d;
  2854.     d = mat[5] * di;
  2855.     mat[0] += mat[30] * d;
  2856.     mat[1] += mat[31] * d;
  2857.     mat[2] += mat[32] * d;
  2858.     mat[3] += mat[33] * d;
  2859.     mat[4] += mat[34] * d;
  2860.     d = mat[11] * di;
  2861.     mat[6] += mat[30] * d;
  2862.     mat[7] += mat[31] * d;
  2863.     mat[8] += mat[32] * d;
  2864.     mat[9] += mat[33] * d;
  2865.     mat[10] += mat[34] * d;
  2866.     d = mat[17] * di;
  2867.     mat[12] += mat[30] * d;
  2868.     mat[13] += mat[31] * d;
  2869.     mat[14] += mat[32] * d;
  2870.     mat[15] += mat[33] * d;
  2871.     mat[16] += mat[34] * d;
  2872.     d = mat[23] * di;
  2873.     mat[18] += mat[30] * d;
  2874.     mat[19] += mat[31] * d;
  2875.     mat[20] += mat[32] * d;
  2876.     mat[21] += mat[33] * d;
  2877.     mat[22] += mat[34] * d;
  2878.     d = mat[29] * di;
  2879.     mat[24] += mat[30] * d;
  2880.     mat[25] += mat[31] * d;
  2881.     mat[26] += mat[32] * d;
  2882.     mat[27] += mat[33] * d;
  2883.     mat[28] += mat[34] * d;
  2884.  
  2885.     return ( s != 0.0f && !FLOAT_IS_NAN( s ) );
  2886. #else
  2887.     // 6*27+2*30 = 222 multiplications
  2888.     //        2*1  =     2 divisions
  2889.     idMat3 r0, r1, r2, r3;
  2890.     float c0, c1, c2, det, invDet;
  2891.     float *mat = reinterpret_cast<float *>(this);
  2892.  
  2893.     // r0 = m0.Inverse();
  2894.     c0 = mat[1*6+1] * mat[2*6+2] - mat[1*6+2] * mat[2*6+1];
  2895.     c1 = mat[1*6+2] * mat[2*6+0] - mat[1*6+0] * mat[2*6+2];
  2896.     c2 = mat[1*6+0] * mat[2*6+1] - mat[1*6+1] * mat[2*6+0];
  2897.  
  2898.     det = mat[0*6+0] * c0 + mat[0*6+1] * c1 + mat[0*6+2] * c2;
  2899.  
  2900.     if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  2901.         return false;
  2902.     }
  2903.  
  2904.     invDet = 1.0f / det;
  2905.  
  2906.     r0[0][0] = c0 * invDet;
  2907.     r0[0][1] = ( mat[0*6+2] * mat[2*6+1] - mat[0*6+1] * mat[2*6+2] ) * invDet;
  2908.     r0[0][2] = ( mat[0*6+1] * mat[1*6+2] - mat[0*6+2] * mat[1*6+1] ) * invDet;
  2909.     r0[1][0] = c1 * invDet;
  2910.     r0[1][1] = ( mat[0*6+0] * mat[2*6+2] - mat[0*6+2] * mat[2*6+0] ) * invDet;
  2911.     r0[1][2] = ( mat[0*6+2] * mat[1*6+0] - mat[0*6+0] * mat[1*6+2] ) * invDet;
  2912.     r0[2][0] = c2 * invDet;
  2913.     r0[2][1] = ( mat[0*6+1] * mat[2*6+0] - mat[0*6+0] * mat[2*6+1] ) * invDet;
  2914.     r0[2][2] = ( mat[0*6+0] * mat[1*6+1] - mat[0*6+1] * mat[1*6+0] ) * invDet;
  2915.  
  2916.     // r1 = r0 * m1;
  2917.     r1[0][0] = r0[0][0] * mat[0*6+3] + r0[0][1] * mat[1*6+3] + r0[0][2] * mat[2*6+3];
  2918.     r1[0][1] = r0[0][0] * mat[0*6+4] + r0[0][1] * mat[1*6+4] + r0[0][2] * mat[2*6+4];
  2919.     r1[0][2] = r0[0][0] * mat[0*6+5] + r0[0][1] * mat[1*6+5] + r0[0][2] * mat[2*6+5];
  2920.     r1[1][0] = r0[1][0] * mat[0*6+3] + r0[1][1] * mat[1*6+3] + r0[1][2] * mat[2*6+3];
  2921.     r1[1][1] = r0[1][0] * mat[0*6+4] + r0[1][1] * mat[1*6+4] + r0[1][2] * mat[2*6+4];
  2922.     r1[1][2] = r0[1][0] * mat[0*6+5] + r0[1][1] * mat[1*6+5] + r0[1][2] * mat[2*6+5];
  2923.     r1[2][0] = r0[2][0] * mat[0*6+3] + r0[2][1] * mat[1*6+3] + r0[2][2] * mat[2*6+3];
  2924.     r1[2][1] = r0[2][0] * mat[0*6+4] + r0[2][1] * mat[1*6+4] + r0[2][2] * mat[2*6+4];
  2925.     r1[2][2] = r0[2][0] * mat[0*6+5] + r0[2][1] * mat[1*6+5] + r0[2][2] * mat[2*6+5];
  2926.  
  2927.     // r2 = m2 * r1;
  2928.     r2[0][0] = mat[3*6+0] * r1[0][0] + mat[3*6+1] * r1[1][0] + mat[3*6+2] * r1[2][0];
  2929.     r2[0][1] = mat[3*6+0] * r1[0][1] + mat[3*6+1] * r1[1][1] + mat[3*6+2] * r1[2][1];
  2930.     r2[0][2] = mat[3*6+0] * r1[0][2] + mat[3*6+1] * r1[1][2] + mat[3*6+2] * r1[2][2];
  2931.     r2[1][0] = mat[4*6+0] * r1[0][0] + mat[4*6+1] * r1[1][0] + mat[4*6+2] * r1[2][0];
  2932.     r2[1][1] = mat[4*6+0] * r1[0][1] + mat[4*6+1] * r1[1][1] + mat[4*6+2] * r1[2][1];
  2933.     r2[1][2] = mat[4*6+0] * r1[0][2] + mat[4*6+1] * r1[1][2] + mat[4*6+2] * r1[2][2];
  2934.     r2[2][0] = mat[5*6+0] * r1[0][0] + mat[5*6+1] * r1[1][0] + mat[5*6+2] * r1[2][0];
  2935.     r2[2][1] = mat[5*6+0] * r1[0][1] + mat[5*6+1] * r1[1][1] + mat[5*6+2] * r1[2][1];
  2936.     r2[2][2] = mat[5*6+0] * r1[0][2] + mat[5*6+1] * r1[1][2] + mat[5*6+2] * r1[2][2];
  2937.  
  2938.     // r3 = r2 - m3;
  2939.     r3[0][0] = r2[0][0] - mat[3*6+3];
  2940.     r3[0][1] = r2[0][1] - mat[3*6+4];
  2941.     r3[0][2] = r2[0][2] - mat[3*6+5];
  2942.     r3[1][0] = r2[1][0] - mat[4*6+3];
  2943.     r3[1][1] = r2[1][1] - mat[4*6+4];
  2944.     r3[1][2] = r2[1][2] - mat[4*6+5];
  2945.     r3[2][0] = r2[2][0] - mat[5*6+3];
  2946.     r3[2][1] = r2[2][1] - mat[5*6+4];
  2947.     r3[2][2] = r2[2][2] - mat[5*6+5];
  2948.  
  2949.     // r3.InverseSelf();
  2950.     r2[0][0] = r3[1][1] * r3[2][2] - r3[1][2] * r3[2][1];
  2951.     r2[1][0] = r3[1][2] * r3[2][0] - r3[1][0] * r3[2][2];
  2952.     r2[2][0] = r3[1][0] * r3[2][1] - r3[1][1] * r3[2][0];
  2953.  
  2954.     det = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0] + r3[0][2] * r2[2][0];
  2955.  
  2956.     if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  2957.         return false;
  2958.     }
  2959.  
  2960.     invDet = 1.0f / det;
  2961.  
  2962.     r2[0][1] = r3[0][2] * r3[2][1] - r3[0][1] * r3[2][2];
  2963.     r2[0][2] = r3[0][1] * r3[1][2] - r3[0][2] * r3[1][1];
  2964.     r2[1][1] = r3[0][0] * r3[2][2] - r3[0][2] * r3[2][0];
  2965.     r2[1][2] = r3[0][2] * r3[1][0] - r3[0][0] * r3[1][2];
  2966.     r2[2][1] = r3[0][1] * r3[2][0] - r3[0][0] * r3[2][1];
  2967.     r2[2][2] = r3[0][0] * r3[1][1] - r3[0][1] * r3[1][0];
  2968.  
  2969.     r3[0][0] = r2[0][0] * invDet;
  2970.     r3[0][1] = r2[0][1] * invDet;
  2971.     r3[0][2] = r2[0][2] * invDet;
  2972.     r3[1][0] = r2[1][0] * invDet;
  2973.     r3[1][1] = r2[1][1] * invDet;
  2974.     r3[1][2] = r2[1][2] * invDet;
  2975.     r3[2][0] = r2[2][0] * invDet;
  2976.     r3[2][1] = r2[2][1] * invDet;
  2977.     r3[2][2] = r2[2][2] * invDet;
  2978.  
  2979.     // r2 = m2 * r0;
  2980.     r2[0][0] = mat[3*6+0] * r0[0][0] + mat[3*6+1] * r0[1][0] + mat[3*6+2] * r0[2][0];
  2981.     r2[0][1] = mat[3*6+0] * r0[0][1] + mat[3*6+1] * r0[1][1] + mat[3*6+2] * r0[2][1];
  2982.     r2[0][2] = mat[3*6+0] * r0[0][2] + mat[3*6+1] * r0[1][2] + mat[3*6+2] * r0[2][2];
  2983.     r2[1][0] = mat[4*6+0] * r0[0][0] + mat[4*6+1] * r0[1][0] + mat[4*6+2] * r0[2][0];
  2984.     r2[1][1] = mat[4*6+0] * r0[0][1] + mat[4*6+1] * r0[1][1] + mat[4*6+2] * r0[2][1];
  2985.     r2[1][2] = mat[4*6+0] * r0[0][2] + mat[4*6+1] * r0[1][2] + mat[4*6+2] * r0[2][2];
  2986.     r2[2][0] = mat[5*6+0] * r0[0][0] + mat[5*6+1] * r0[1][0] + mat[5*6+2] * r0[2][0];
  2987.     r2[2][1] = mat[5*6+0] * r0[0][1] + mat[5*6+1] * r0[1][1] + mat[5*6+2] * r0[2][1];
  2988.     r2[2][2] = mat[5*6+0] * r0[0][2] + mat[5*6+1] * r0[1][2] + mat[5*6+2] * r0[2][2];
  2989.  
  2990.     // m2 = r3 * r2;
  2991.     mat[3*6+0] = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0] + r3[0][2] * r2[2][0];
  2992.     mat[3*6+1] = r3[0][0] * r2[0][1] + r3[0][1] * r2[1][1] + r3[0][2] * r2[2][1];
  2993.     mat[3*6+2] = r3[0][0] * r2[0][2] + r3[0][1] * r2[1][2] + r3[0][2] * r2[2][2];
  2994.     mat[4*6+0] = r3[1][0] * r2[0][0] + r3[1][1] * r2[1][0] + r3[1][2] * r2[2][0];
  2995.     mat[4*6+1] = r3[1][0] * r2[0][1] + r3[1][1] * r2[1][1] + r3[1][2] * r2[2][1];
  2996.     mat[4*6+2] = r3[1][0] * r2[0][2] + r3[1][1] * r2[1][2] + r3[1][2] * r2[2][2];
  2997.     mat[5*6+0] = r3[2][0] * r2[0][0] + r3[2][1] * r2[1][0] + r3[2][2] * r2[2][0];
  2998.     mat[5*6+1] = r3[2][0] * r2[0][1] + r3[2][1] * r2[1][1] + r3[2][2] * r2[2][1];
  2999.     mat[5*6+2] = r3[2][0] * r2[0][2] + r3[2][1] * r2[1][2] + r3[2][2] * r2[2][2];
  3000.  
  3001.     // m0 = r0 - r1 * m2;
  3002.     mat[0*6+0] = r0[0][0] - r1[0][0] * mat[3*6+0] - r1[0][1] * mat[4*6+0] - r1[0][2] * mat[5*6+0];
  3003.     mat[0*6+1] = r0[0][1] - r1[0][0] * mat[3*6+1] - r1[0][1] * mat[4*6+1] - r1[0][2] * mat[5*6+1];
  3004.     mat[0*6+2] = r0[0][2] - r1[0][0] * mat[3*6+2] - r1[0][1] * mat[4*6+2] - r1[0][2] * mat[5*6+2];
  3005.     mat[1*6+0] = r0[1][0] - r1[1][0] * mat[3*6+0] - r1[1][1] * mat[4*6+0] - r1[1][2] * mat[5*6+0];
  3006.     mat[1*6+1] = r0[1][1] - r1[1][0] * mat[3*6+1] - r1[1][1] * mat[4*6+1] - r1[1][2] * mat[5*6+1];
  3007.     mat[1*6+2] = r0[1][2] - r1[1][0] * mat[3*6+2] - r1[1][1] * mat[4*6+2] - r1[1][2] * mat[5*6+2];
  3008.     mat[2*6+0] = r0[2][0] - r1[2][0] * mat[3*6+0] - r1[2][1] * mat[4*6+0] - r1[2][2] * mat[5*6+0];
  3009.     mat[2*6+1] = r0[2][1] - r1[2][0] * mat[3*6+1] - r1[2][1] * mat[4*6+1] - r1[2][2] * mat[5*6+1];
  3010.     mat[2*6+2] = r0[2][2] - r1[2][0] * mat[3*6+2] - r1[2][1] * mat[4*6+2] - r1[2][2] * mat[5*6+2];
  3011.  
  3012.     // m1 = r1 * r3;
  3013.     mat[0*6+3] = r1[0][0] * r3[0][0] + r1[0][1] * r3[1][0] + r1[0][2] * r3[2][0];
  3014.     mat[0*6+4] = r1[0][0] * r3[0][1] + r1[0][1] * r3[1][1] + r1[0][2] * r3[2][1];
  3015.     mat[0*6+5] = r1[0][0] * r3[0][2] + r1[0][1] * r3[1][2] + r1[0][2] * r3[2][2];
  3016.     mat[1*6+3] = r1[1][0] * r3[0][0] + r1[1][1] * r3[1][0] + r1[1][2] * r3[2][0];
  3017.     mat[1*6+4] = r1[1][0] * r3[0][1] + r1[1][1] * r3[1][1] + r1[1][2] * r3[2][1];
  3018.     mat[1*6+5] = r1[1][0] * r3[0][2] + r1[1][1] * r3[1][2] + r1[1][2] * r3[2][2];
  3019.     mat[2*6+3] = r1[2][0] * r3[0][0] + r1[2][1] * r3[1][0] + r1[2][2] * r3[2][0];
  3020.     mat[2*6+4] = r1[2][0] * r3[0][1] + r1[2][1] * r3[1][1] + r1[2][2] * r3[2][1];
  3021.     mat[2*6+5] = r1[2][0] * r3[0][2] + r1[2][1] * r3[1][2] + r1[2][2] * r3[2][2];
  3022.  
  3023.     // m3 = -r3;
  3024.     mat[3*6+3] = -r3[0][0];
  3025.     mat[3*6+4] = -r3[0][1];
  3026.     mat[3*6+5] = -r3[0][2];
  3027.     mat[4*6+3] = -r3[1][0];
  3028.     mat[4*6+4] = -r3[1][1];
  3029.     mat[4*6+5] = -r3[1][2];
  3030.     mat[5*6+3] = -r3[2][0];
  3031.     mat[5*6+4] = -r3[2][1];
  3032.     mat[5*6+5] = -r3[2][2];
  3033.  
  3034.     return true;
  3035. #endif
  3036. }
  3037.  
  3038. /*
  3039. =============
  3040. idMat6::ToString
  3041. =============
  3042. */
  3043. const char *idMat6::ToString( int precision ) const {
  3044.     return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
  3045. }
  3046.  
  3047.  
  3048. //===============================================================
  3049. //
  3050. //  idMatX
  3051. //
  3052. //===============================================================
  3053.  
  3054. // RAVEN BEGIN
  3055. // jscott: avoid pointer hackery pokery and use the compiler
  3056. float    idMatX::tempPtr[MATX_MAX_TEMP];
  3057. //float *    idMatX::tempPtr = (float *) ( ( (int) idMatX::temp + 15 ) & ~15 );
  3058. // RAVEN END
  3059. int        idMatX::tempIndex = 0;
  3060.  
  3061.  
  3062. /*
  3063. ============
  3064. idMatX::ChangeSize
  3065. ============
  3066. */
  3067. void idMatX::ChangeSize( int rows, int columns, bool makeZero ) {
  3068.     int alloc = ( rows * columns + 3 ) & ~3;
  3069.     if ( alloc > alloced && alloced != -1 ) {
  3070.         float *oldMat = mat;
  3071. // RAVEN BEGIN
  3072.         mat = (float *) Mem_Alloc16( alloc * sizeof( float ), MA_MATH );
  3073. // RAVEN END
  3074.         if ( makeZero ) {
  3075.             memset( mat, 0, alloc * sizeof( float ) );
  3076.         }
  3077.         alloced = alloc;
  3078.         if ( oldMat ) {
  3079.             int minRow = Min( numRows, rows );
  3080.             int minColumn = Min( numColumns, columns );
  3081.             for ( int i = 0; i < minRow; i++ ) {
  3082.                 for ( int j = 0; j < minColumn; j++ ) {
  3083.                     mat[ i * columns + j ] = oldMat[ i * numColumns + j ];
  3084.                 }
  3085.             }
  3086.             Mem_Free16( oldMat );
  3087.         }
  3088.     } else {
  3089.         if ( columns < numColumns ) {
  3090.             int minRow = Min( numRows, rows );
  3091.             for ( int i = 0; i < minRow; i++ ) {
  3092.                 for ( int j = 0; j < columns; j++ ) {
  3093.                     mat[ i * columns + j ] = mat[ i * numColumns + j ];
  3094.                 }
  3095.             }
  3096.         } else if ( columns > numColumns ) {
  3097.             for ( int i = Min( numRows, rows ) - 1; i >= 0; i-- ) {
  3098.                 if ( makeZero ) {
  3099.                     for ( int j = columns - 1; j >= numColumns; j-- ) {
  3100.                         mat[ i * columns + j ] = 0.0f;
  3101.                     }
  3102.                 }
  3103.                 for ( int j = numColumns - 1; j >= 0; j-- ) {
  3104.                     mat[ i * columns + j ] = mat[ i * numColumns + j ];
  3105.                 }
  3106.             }
  3107.         }
  3108.         if ( makeZero && rows > numRows ) {
  3109.             memset( mat + numRows * columns, 0, ( rows - numRows ) * columns * sizeof( float ) );
  3110.         }
  3111.     }
  3112.     numRows = rows;
  3113.     numColumns = columns;
  3114.     MATX_CLEAREND();
  3115. }
  3116.  
  3117. /*
  3118. ============
  3119. idMatX::RemoveRow
  3120. ============
  3121. */
  3122. idMatX &idMatX::RemoveRow( int r ) {
  3123.     int i;
  3124.  
  3125.     assert( r < numRows );
  3126.  
  3127.     numRows--;
  3128.  
  3129.     for ( i = r; i < numRows; i++ ) {
  3130.         memcpy( &mat[i * numColumns], &mat[( i + 1 ) * numColumns], numColumns * sizeof( float ) );
  3131.     }
  3132.  
  3133.     return *this;
  3134. }
  3135.  
  3136. /*
  3137. ============
  3138. idMatX::RemoveColumn
  3139. ============
  3140. */
  3141. idMatX &idMatX::RemoveColumn( int r ) {
  3142.     int i;
  3143.  
  3144.     assert( r < numColumns );
  3145.  
  3146.     numColumns--;
  3147.  
  3148.     for ( i = 0; i < numRows - 1; i++ ) {
  3149.         memmove( &mat[i * numColumns + r], &mat[i * ( numColumns + 1 ) + r + 1], numColumns * sizeof( float ) );
  3150.     }
  3151.     memmove( &mat[i * numColumns + r], &mat[i * ( numColumns + 1 ) + r + 1], ( numColumns - r ) * sizeof( float ) );
  3152.  
  3153.     return *this;
  3154. }
  3155.  
  3156. /*
  3157. ============
  3158. idMatX::RemoveRowColumn
  3159. ============
  3160. */
  3161. idMatX &idMatX::RemoveRowColumn( int r ) {
  3162.     int i;
  3163.  
  3164.     assert( r < numRows && r < numColumns );
  3165.  
  3166.     numRows--;
  3167.     numColumns--;
  3168.  
  3169.     if ( r > 0 ) {
  3170.         for ( i = 0; i < r - 1; i++ ) {
  3171.             memmove( &mat[i * numColumns + r], &mat[i * ( numColumns + 1 ) + r + 1], numColumns * sizeof( float ) );
  3172.         }
  3173.         memmove( &mat[i * numColumns + r], &mat[i * ( numColumns + 1 ) + r + 1], ( numColumns - r ) * sizeof( float ) );
  3174.     }
  3175.  
  3176.     memcpy( &mat[r * numColumns], &mat[( r + 1 ) * ( numColumns + 1 )], r * sizeof( float ) );
  3177.  
  3178.     for ( i = r; i < numRows - 1; i++ ) {
  3179.         memcpy( &mat[i * numColumns + r], &mat[( i + 1 ) * ( numColumns + 1 ) + r + 1], numColumns * sizeof( float ) );
  3180.     }
  3181.     memcpy( &mat[i * numColumns + r], &mat[( i + 1 ) * ( numColumns + 1 ) + r + 1], ( numColumns - r ) * sizeof( float ) );
  3182.  
  3183.     return *this;
  3184. }
  3185.  
  3186. /*
  3187. ============
  3188. idMatX::IsOrthogonal
  3189.  
  3190.   returns true if (*this) * this->Transpose() == Identity
  3191. ============
  3192. */
  3193. bool idMatX::IsOrthogonal( const float epsilon ) const {
  3194.     float *ptr1, *ptr2, sum;
  3195.  
  3196.     if ( !IsSquare() ) {
  3197.         return false;
  3198.     }
  3199.  
  3200.     ptr1 = mat;
  3201.     for ( int i = 0; i < numRows; i++ ) {
  3202.         for ( int j = 0; j < numColumns; j++ ) {
  3203.             ptr2 = mat + j;
  3204.             sum = ptr1[0] * ptr2[0] - (float) ( i == j );
  3205.             for ( int n = 1; n < numColumns; n++ ) {
  3206.                 ptr2 += numColumns;
  3207.                 sum += ptr1[n] * ptr2[0];
  3208.             }
  3209.             if ( idMath::Fabs( sum ) > epsilon ) {
  3210.                 return false;
  3211.             }
  3212.         }
  3213.         ptr1 += numColumns;
  3214.     }
  3215.     return true;
  3216. }
  3217.  
  3218. /*
  3219. ============
  3220. idMatX::IsOrthonormal
  3221.  
  3222.   returns true if (*this) * this->Transpose() == Identity and the length of each column vector is 1
  3223. ============
  3224. */
  3225. bool idMatX::IsOrthonormal( const float epsilon ) const {
  3226.     float *ptr1, *ptr2, sum;
  3227.  
  3228.     if ( !IsSquare() ) {
  3229.         return false;
  3230.     }
  3231.  
  3232.     ptr1 = mat;
  3233.     for ( int i = 0; i < numRows; i++ ) {
  3234.         for ( int j = 0; j < numColumns; j++ ) {
  3235.             ptr2 = mat + j;
  3236.             sum = ptr1[0] * ptr2[0] - (float) ( i == j );
  3237.             for ( int n = 1; n < numColumns; n++ ) {
  3238.                 ptr2 += numColumns;
  3239.                 sum += ptr1[n] * ptr2[0];
  3240.             }
  3241.             if ( idMath::Fabs( sum ) > epsilon ) {
  3242.                 return false;
  3243.             }
  3244.         }
  3245.         ptr1 += numColumns;
  3246.  
  3247.         ptr2 = mat + i;
  3248.         sum = ptr2[0] * ptr2[0] - 1.0f;
  3249.         for ( i = 1; i < numRows; i++ ) {
  3250.             ptr2 += numColumns;
  3251.             sum += ptr2[i] * ptr2[i];
  3252.         }
  3253.         if ( idMath::Fabs( sum ) > epsilon ) {
  3254.             return false;
  3255.         }
  3256.     }
  3257.     return true;
  3258. }
  3259.  
  3260. /*
  3261. ============
  3262. idMatX::IsPMatrix
  3263.  
  3264.   returns true if the matrix is a P-matrix
  3265.   A square matrix is a P-matrix if all its principal minors are positive.
  3266. ============
  3267. */
  3268. bool idMatX::IsPMatrix( const float epsilon ) const {
  3269.     int i, j;
  3270.     float d;
  3271.     idMatX m;
  3272.  
  3273.     if ( !IsSquare() ) {
  3274.         return false;
  3275.     }
  3276.  
  3277.     if ( numRows <= 0 ) {
  3278.         return true;
  3279.     }
  3280.  
  3281.     if ( (*this)[0][0] <= epsilon ) {
  3282.         return false;
  3283.     }
  3284.  
  3285.     if ( numRows <= 1 ) {
  3286.         return true;
  3287.     }
  3288.  
  3289.     m.SetData( numRows - 1, numColumns - 1, MATX_ALLOCA( ( numRows - 1 ) * ( numColumns - 1 ) ) );
  3290.  
  3291.     for ( i = 1; i < numRows; i++ ) {
  3292.         for ( j = 1; j < numColumns; j++ ) {
  3293.             m[i-1][j-1] = (*this)[i][j];
  3294.         }
  3295.     }
  3296.  
  3297.     if ( !m.IsPMatrix( epsilon ) ) {
  3298.         return false;
  3299.     }
  3300.  
  3301.     for ( i = 1; i < numRows; i++ ) {
  3302.         d = (*this)[i][0] / (*this)[0][0];
  3303.         for ( j = 1; j < numColumns; j++ ) {
  3304.             m[i-1][j-1] = (*this)[i][j] - d * (*this)[0][j];
  3305.         }
  3306.     }
  3307.  
  3308.     if ( !m.IsPMatrix( epsilon ) ) {
  3309.         return false;
  3310.     }
  3311.  
  3312.     return true;
  3313. }
  3314.  
  3315. /*
  3316. ============
  3317. idMatX::IsZMatrix
  3318.  
  3319.   returns true if the matrix is a Z-matrix
  3320.   A square matrix M is a Z-matrix if M[i][j] <= 0 for all i != j.
  3321. ============
  3322. */
  3323. bool idMatX::IsZMatrix( const float epsilon ) const {
  3324.     int i, j;
  3325.  
  3326.     if ( !IsSquare() ) {
  3327.         return false;
  3328.     }
  3329.  
  3330.     for ( i = 0; i < numRows; i++ ) {
  3331.         for ( j = 0; j < numColumns; j++ ) {
  3332.             if ( (*this)[i][j] > epsilon && i != j ) {
  3333.                 return false;
  3334.             }
  3335.         }
  3336.     }
  3337.     return true;
  3338. }
  3339.  
  3340. /*
  3341. ============
  3342. idMatX::IsPositiveDefinite
  3343.  
  3344.   returns true if the matrix is Positive Definite (PD)
  3345.   A square matrix M of order n is said to be PD if y'My > 0 for all vectors y of dimension n, y != 0.
  3346. ============
  3347. */
  3348. bool idMatX::IsPositiveDefinite( const float epsilon ) const {
  3349.     int i, j, k;
  3350.     float d, s;
  3351.     idMatX m;
  3352.  
  3353.     // the matrix must be square
  3354.     if ( !IsSquare() ) {
  3355.         return false;
  3356.     }
  3357.  
  3358.     // copy matrix
  3359.     m.SetData( numRows, numColumns, MATX_ALLOCA( numRows * numColumns ) );
  3360.     m = *this;
  3361.  
  3362.     // add transpose
  3363.     for ( i = 0; i < numRows; i++ ) {
  3364.         for ( j = 0; j < numColumns; j++ ) {
  3365.             m[i][j] += (*this)[j][i];
  3366.         }
  3367.     }
  3368.  
  3369.     // test Positive Definiteness with Gaussian pivot steps
  3370.     for ( i = 0; i < numRows; i++ ) {
  3371.  
  3372.         for ( j = i; j < numColumns; j++ ) {
  3373.             if ( m[j][j] <= epsilon ) {
  3374.                 return false;
  3375.             }
  3376.         }
  3377.  
  3378.         d = 1.0f / m[i][i];
  3379.         for ( j = i + 1; j < numColumns; j++ ) {
  3380.             s = d * m[j][i];
  3381.             m[j][i] = 0.0f;
  3382.             for ( k = i + 1; k < numRows; k++ ) {
  3383.                 m[j][k] -= s * m[i][k];
  3384.             }
  3385.         }
  3386.     }
  3387.  
  3388.     return true;
  3389. }
  3390.  
  3391. /*
  3392. ============
  3393. idMatX::IsSymmetricPositiveDefinite
  3394.  
  3395.   returns true if the matrix is Symmetric Positive Definite (PD)
  3396. ============
  3397. */
  3398. bool idMatX::IsSymmetricPositiveDefinite( const float epsilon ) const {
  3399.     idMatX m;
  3400.  
  3401.     // the matrix must be symmetric
  3402.     if ( !IsSymmetric( epsilon ) ) {
  3403.         return false;
  3404.     }
  3405.  
  3406.     // copy matrix
  3407.     m.SetData( numRows, numColumns, MATX_ALLOCA( numRows * numColumns ) );
  3408.     m = *this;
  3409.  
  3410.     // being able to obtain Cholesky factors is both a necessary and sufficient condition for positive definiteness
  3411.     return m.Cholesky_Factor();
  3412. }
  3413.  
  3414. /*
  3415. ============
  3416. idMatX::IsPositiveSemiDefinite
  3417.  
  3418.   returns true if the matrix is Positive Semi Definite (PSD)
  3419.   A square matrix M of order n is said to be PSD if y'My >= 0 for all vectors y of dimension n, y != 0.
  3420. ============
  3421. */
  3422. bool idMatX::IsPositiveSemiDefinite( const float epsilon ) const {
  3423.     int i, j, k;
  3424.     float d, s;
  3425.     idMatX m;
  3426.  
  3427.     // the matrix must be square
  3428.     if ( !IsSquare() ) {
  3429.         return false;
  3430.     }
  3431.  
  3432.     // copy original matrix
  3433.     m.SetData( numRows, numColumns, MATX_ALLOCA( numRows * numColumns ) );
  3434.     m = *this;
  3435.  
  3436.     // add transpose
  3437.     for ( i = 0; i < numRows; i++ ) {
  3438.         for ( j = 0; j < numColumns; j++ ) {
  3439.             m[i][j] += (*this)[j][i];
  3440.         }
  3441.     }
  3442.  
  3443.     // test Positive Semi Definiteness with Gaussian pivot steps
  3444.     for ( i = 0; i < numRows; i++ ) {
  3445.  
  3446.         for ( j = i; j < numColumns; j++ ) {
  3447.             if ( m[j][j] < -epsilon ) {
  3448.                 return false;
  3449.             }
  3450.             if ( m[j][j] > epsilon ) {
  3451.                 continue;
  3452.             }
  3453.             for ( k = 0; k < numRows; k++ ) {
  3454.                 if ( idMath::Fabs( m[k][j] ) > epsilon ) {
  3455.                     return false;
  3456.                 }
  3457.                 if ( idMath::Fabs( m[j][k] ) > epsilon ) {
  3458.                     return false;
  3459.                 }
  3460.             }
  3461.         }
  3462.  
  3463.         if ( m[i][i] <= epsilon ) {
  3464.             continue;
  3465.         }
  3466.  
  3467.         d = 1.0f / m[i][i];
  3468.         for ( j = i + 1; j < numColumns; j++ ) {
  3469.             s = d * m[j][i];
  3470.             m[j][i] = 0.0f;
  3471.             for ( k = i + 1; k < numRows; k++ ) {
  3472.                 m[j][k] -= s * m[i][k];
  3473.             }
  3474.         }
  3475.     }
  3476.  
  3477.     return true;
  3478. }
  3479.  
  3480. /*
  3481. ============
  3482. idMatX::IsSymmetricPositiveSemiDefinite
  3483.  
  3484.   returns true if the matrix is Symmetric Positive Semi Definite (PSD)
  3485. ============
  3486. */
  3487. bool idMatX::IsSymmetricPositiveSemiDefinite( const float epsilon ) const {
  3488.  
  3489.     // the matrix must be symmetric
  3490.     if ( !IsSymmetric( epsilon ) ) {
  3491.         return false;
  3492.     }
  3493.  
  3494.     return IsPositiveSemiDefinite( epsilon );
  3495. }
  3496.  
  3497. /*
  3498. ============
  3499. idMatX::LowerTriangularInverse
  3500.  
  3501.   in-place inversion of the lower triangular matrix
  3502. ============
  3503. */
  3504. bool idMatX::LowerTriangularInverse( void ) {
  3505.     int i, j, k;
  3506.     double d, sum;
  3507.  
  3508.     for ( i = 0; i < numRows; i++ ) {
  3509.         d = (*this)[i][i];
  3510.         if ( d == 0.0f ) {
  3511.             return false;
  3512.         }
  3513.         (*this)[i][i] = d = 1.0f / d;
  3514.  
  3515.         for ( j = 0; j < i; j++ ) {
  3516.             sum = 0.0f;
  3517.             for ( k = j; k < i; k++ ) {
  3518.                 sum -= (*this)[i][k] * (*this)[k][j];
  3519.             }
  3520.             (*this)[i][j] = sum * d;
  3521.         }
  3522.     }
  3523.     return true;
  3524. }
  3525.  
  3526. /*
  3527. ============
  3528. idMatX::UpperTriangularInverse
  3529.  
  3530.   in-place inversion of the upper triangular matrix
  3531. ============
  3532. */
  3533. bool idMatX::UpperTriangularInverse( void ) {
  3534.     int i, j, k;
  3535.     double d, sum;
  3536.  
  3537.     for ( i = numRows-1; i >= 0; i-- ) {
  3538.         d = (*this)[i][i];
  3539.         if ( d == 0.0f ) {
  3540.             return false;
  3541.         }
  3542.         (*this)[i][i] = d = 1.0f / d;
  3543.  
  3544.         for ( j = numRows-1; j > i; j-- ) {
  3545.             sum = 0.0f;
  3546.             for ( k = j; k > i; k-- ) {
  3547.                 sum -= (*this)[i][k] * (*this)[k][j];
  3548.             }
  3549.             (*this)[i][j] = sum * d;
  3550.         }
  3551.     }
  3552.     return true;
  3553. }
  3554.  
  3555. /*
  3556. =============
  3557. idMatX::ToString
  3558. =============
  3559. */
  3560. const char *idMatX::ToString( int precision ) const {
  3561.     return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
  3562. }
  3563.  
  3564. /*
  3565. ============
  3566. idMatX::Update_RankOne
  3567.  
  3568.   Updates the matrix to obtain the matrix: A + alpha * v * w'
  3569. ============
  3570. */
  3571. void idMatX::Update_RankOne( const idVecX &v, const idVecX &w, float alpha ) {
  3572.     int i, j;
  3573.     float s;
  3574.  
  3575.     assert( v.GetSize() >= numRows );
  3576.     assert( w.GetSize() >= numColumns );
  3577.  
  3578.     for ( i = 0; i < numRows; i++ ) {
  3579.         s = alpha * v[i];
  3580.         for ( j = 0; j < numColumns; j++ ) {
  3581.             (*this)[i][j] += s * w[j];
  3582.         }
  3583.     }
  3584. }
  3585.  
  3586. /*
  3587. ============
  3588. idMatX::Update_RankOneSymmetric
  3589.  
  3590.   Updates the matrix to obtain the matrix: A + alpha * v * v'
  3591. ============
  3592. */
  3593. void idMatX::Update_RankOneSymmetric( const idVecX &v, float alpha ) {
  3594.     int i, j;
  3595.     float s;
  3596.  
  3597.     assert( numRows == numColumns );
  3598.     assert( v.GetSize() >= numRows );
  3599.  
  3600.     for ( i = 0; i < numRows; i++ ) {
  3601.         s = alpha * v[i];
  3602.         for ( j = 0; j < numColumns; j++ ) {
  3603.             (*this)[i][j] += s * v[j];
  3604.         }
  3605.     }
  3606. }
  3607.  
  3608. /*
  3609. ============
  3610. idMatX::Update_RowColumn
  3611.  
  3612.   Updates the matrix to obtain the matrix:
  3613.  
  3614.       [ 0  a  0 ]
  3615.   A + [ d  b  e ]
  3616.       [ 0  c  0 ]
  3617.  
  3618.   where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1], d = w[0,r-1], w[r] = 0.0f, e = w[r+1,numColumns-1]
  3619. ============
  3620. */
  3621. void idMatX::Update_RowColumn( const idVecX &v, const idVecX &w, int r ) {
  3622.     int i;
  3623.  
  3624.     assert( w[r] == 0.0f );
  3625.     assert( v.GetSize() >= numColumns );
  3626.     assert( w.GetSize() >= numRows );
  3627.  
  3628.     for ( i = 0; i < numRows; i++ ) {
  3629.         (*this)[i][r] += v[i];
  3630.     }
  3631.     for ( i = 0; i < numColumns; i++ ) {
  3632.         (*this)[r][i] += w[i];
  3633.     }
  3634. }
  3635.  
  3636. /*
  3637. ============
  3638. idMatX::Update_RowColumnSymmetric
  3639.  
  3640.   Updates the matrix to obtain the matrix:
  3641.  
  3642.       [ 0  a  0 ]
  3643.   A + [ a  b  c ]
  3644.       [ 0  c  0 ]
  3645.  
  3646.   where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1]
  3647. ============
  3648. */
  3649. void idMatX::Update_RowColumnSymmetric( const idVecX &v, int r ) {
  3650.     int i;
  3651.  
  3652.     assert( numRows == numColumns );
  3653.     assert( v.GetSize() >= numRows );
  3654.  
  3655.     for ( i = 0; i < r; i++ ) {
  3656.         (*this)[i][r] += v[i];
  3657.         (*this)[r][i] += v[i];
  3658.     }
  3659.     (*this)[r][r] += v[r];
  3660.     for ( i = r+1; i < numRows; i++ ) {
  3661.         (*this)[i][r] += v[i];
  3662.         (*this)[r][i] += v[i];
  3663.     }
  3664. }
  3665.  
  3666. /*
  3667. ============
  3668. idMatX::Update_Increment
  3669.  
  3670.   Updates the matrix to obtain the matrix:
  3671.  
  3672.   [ A  a ]
  3673.   [ c  b ]
  3674.  
  3675.   where: a = v[0,numRows-1], b = v[numRows], c = w[0,numColumns-1]], w[numColumns] = 0
  3676. ============
  3677. */
  3678. void idMatX::Update_Increment( const idVecX &v, const idVecX &w ) {
  3679.     int i;
  3680.  
  3681.     assert( numRows == numColumns );
  3682.     assert( v.GetSize() >= numRows+1 );
  3683.     assert( w.GetSize() >= numColumns+1 );
  3684.  
  3685.     ChangeSize( numRows+1, numColumns+1, false );
  3686.  
  3687.     for ( i = 0; i < numRows; i++ ) {
  3688.         (*this)[i][numColumns-1] = v[i];
  3689.     }
  3690.     for ( i = 0; i < numColumns-1; i++ ) {
  3691.         (*this)[numRows-1][i] = w[i];
  3692.     }
  3693. }
  3694.  
  3695. /*
  3696. ============
  3697. idMatX::Update_IncrementSymmetric
  3698.  
  3699.   Updates the matrix to obtain the matrix:
  3700.  
  3701.   [ A  a ]
  3702.   [ a  b ]
  3703.  
  3704.   where: a = v[0,numRows-1], b = v[numRows]
  3705. ============
  3706. */
  3707. void idMatX::Update_IncrementSymmetric( const idVecX &v ) {
  3708.     int i;
  3709.  
  3710.     assert( numRows == numColumns );
  3711.     assert( v.GetSize() >= numRows+1 );
  3712.  
  3713.     ChangeSize( numRows+1, numColumns+1, false );
  3714.  
  3715.     for ( i = 0; i < numRows-1; i++ ) {
  3716.         (*this)[i][numColumns-1] = v[i];
  3717.     }
  3718.     for ( i = 0; i < numColumns; i++ ) {
  3719.         (*this)[numRows-1][i] = v[i];
  3720.     }
  3721. }
  3722.  
  3723. /*
  3724. ============
  3725. idMatX::Update_Decrement
  3726.  
  3727.   Updates the matrix to obtain a matrix with row r and column r removed.
  3728. ============
  3729. */
  3730. void idMatX::Update_Decrement( int r ) {
  3731.     RemoveRowColumn( r );
  3732. }
  3733.  
  3734. /*
  3735. ============
  3736. idMatX::Inverse_GaussJordan
  3737.  
  3738.   in-place inversion using Gauss-Jordan elimination
  3739. ============
  3740. */
  3741. bool idMatX::Inverse_GaussJordan( void ) {
  3742.     int i, j, k, r, c;
  3743.     float d, max;
  3744.  
  3745.     assert( numRows == numColumns );
  3746.  
  3747.     int *columnIndex = (int *) _alloca16( numRows * sizeof( int ) );
  3748.     int *rowIndex = (int *) _alloca16( numRows * sizeof( int ) );
  3749.     bool *pivot = (bool *) _alloca16( numRows * sizeof( bool ) );
  3750.  
  3751.     memset( pivot, 0, numRows * sizeof( bool ) );
  3752.  
  3753.     // elimination with full pivoting
  3754.     for ( i = 0; i < numRows; i++ ) {
  3755.  
  3756.         // search the whole matrix except for pivoted rows for the maximum absolute value
  3757.         max = 0.0f;
  3758.         r = c = 0;
  3759.         for ( j = 0; j < numRows; j++ ) {
  3760.             if ( !pivot[j] ) {
  3761.                 for ( k = 0; k < numRows; k++ ) {
  3762.                     if ( !pivot[k] ) {
  3763.                         d = idMath::Fabs( (*this)[j][k] );
  3764.                         if ( d > max ) {
  3765.                             max = d;
  3766.                             r = j;
  3767.                             c = k;
  3768.                         }
  3769.                     }
  3770.                 }
  3771.             }
  3772.         }
  3773.  
  3774.         if ( max == 0.0f ) {
  3775.             // matrix is not invertible
  3776.             return false;
  3777.         }
  3778.  
  3779.         pivot[c] = true;
  3780.  
  3781.         // swap rows such that entry (c,c) has the pivot entry
  3782.         if ( r != c ) {
  3783.             SwapRows( r, c );
  3784.         }
  3785.  
  3786.         // keep track of the row permutation
  3787.         rowIndex[i] = r;
  3788.         columnIndex[i] = c;
  3789.  
  3790.         // scale the row to make the pivot entry equal to 1
  3791.         d = 1.0f / (*this)[c][c];
  3792.         (*this)[c][c] = 1.0f;
  3793.         for ( k = 0; k < numRows; k++ ) {
  3794.             (*this)[c][k] *= d;
  3795.         }
  3796.  
  3797.         // zero out the pivot column entries in the other rows
  3798.         for ( j = 0; j < numRows; j++ ) {
  3799.             if ( j != c ) {
  3800.                 d = (*this)[j][c];
  3801.                 (*this)[j][c] = 0.0f;
  3802.                 for ( k = 0; k < numRows; k++ ) {
  3803.                     (*this)[j][k] -= (*this)[c][k] * d;
  3804.                 }
  3805.             }
  3806.         }
  3807.     }
  3808.  
  3809.     // reorder rows to store the inverse of the original matrix
  3810.     for ( j = numRows - 1; j >= 0; j-- ) {
  3811.         if ( rowIndex[j] != columnIndex[j] ) {
  3812.             for ( k = 0; k < numRows; k++ ) {
  3813.                 d = (*this)[k][rowIndex[j]];
  3814.                 (*this)[k][rowIndex[j]] = (*this)[k][columnIndex[j]];
  3815.                 (*this)[k][columnIndex[j]] = d;
  3816.             }
  3817.         }
  3818.     }
  3819.  
  3820.     return true;
  3821. }
  3822.  
  3823. /*
  3824. ============
  3825. idMatX::Inverse_UpdateRankOne
  3826.  
  3827.   Updates the in-place inverse using the Sherman-Morrison formula to obtain the inverse for the matrix: A + alpha * v * w'
  3828. ============
  3829. */
  3830. bool idMatX::Inverse_UpdateRankOne( const idVecX &v, const idVecX &w, float alpha ) {
  3831.     int i, j;
  3832.     float beta, s;
  3833.     idVecX y, z;
  3834.  
  3835.     assert( numRows == numColumns );
  3836.     assert( v.GetSize() >= numColumns );
  3837.     assert( w.GetSize() >= numRows );
  3838.  
  3839.     y.SetData( numRows, VECX_ALLOCA( numRows ) );
  3840.     z.SetData( numRows, VECX_ALLOCA( numRows ) );
  3841.  
  3842.     Multiply( y, v );
  3843.     TransposeMultiply( z, w );
  3844.     beta = 1.0f + ( w * y );
  3845.  
  3846.     if ( beta == 0.0f ) {
  3847.         return false;
  3848.     }
  3849.  
  3850.     alpha /= beta;
  3851.  
  3852.     for ( i = 0; i < numRows; i++ ) {
  3853.         s = y[i] * alpha;
  3854.         for ( j = 0; j < numColumns; j++ ) {
  3855.             (*this)[i][j] -= s * z[j];
  3856.         }
  3857.     }
  3858.     return true;
  3859. }
  3860.  
  3861. /*
  3862. ============
  3863. idMatX::Inverse_UpdateRowColumn
  3864.  
  3865.   Updates the in-place inverse to obtain the inverse for the matrix:
  3866.  
  3867.       [ 0  a  0 ]
  3868.   A + [ d  b  e ]
  3869.       [ 0  c  0 ]
  3870.  
  3871.   where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1], d = w[0,r-1], w[r] = 0.0f, e = w[r+1,numColumns-1]
  3872. ============
  3873. */
  3874. bool idMatX::Inverse_UpdateRowColumn( const idVecX &v, const idVecX &w, int r ) {
  3875.     idVecX s;
  3876.  
  3877.     assert( numRows == numColumns );
  3878.     assert( v.GetSize() >= numColumns );
  3879.     assert( w.GetSize() >= numRows );
  3880.     assert( r >= 0 && r < numRows && r < numColumns );
  3881.     assert( w[r] == 0.0f );
  3882.  
  3883.     s.SetData( Max( numRows, numColumns ), VECX_ALLOCA( Max( numRows, numColumns ) ) );
  3884.     s.Zero();
  3885.     s[r] = 1.0f;
  3886.  
  3887.     if ( !Inverse_UpdateRankOne( v, s, 1.0f ) ) {
  3888.         return false;
  3889.     }
  3890.     if ( !Inverse_UpdateRankOne( s, w, 1.0f ) ) {
  3891.         return false;
  3892.     }
  3893.     return true;
  3894. }
  3895.  
  3896. /*
  3897. ============
  3898. idMatX::Inverse_UpdateIncrement
  3899.  
  3900.   Updates the in-place inverse to obtain the inverse for the matrix:
  3901.  
  3902.   [ A  a ]
  3903.   [ c  b ]
  3904.  
  3905.   where: a = v[0,numRows-1], b = v[numRows], c = w[0,numColumns-1], w[numColumns] = 0
  3906. ============
  3907. */
  3908. bool idMatX::Inverse_UpdateIncrement( const idVecX &v, const idVecX &w ) {
  3909.     idVecX v2;
  3910.  
  3911.     assert( numRows == numColumns );
  3912.     assert( v.GetSize() >= numRows+1 );
  3913.     assert( w.GetSize() >= numColumns+1 );
  3914.  
  3915.     ChangeSize( numRows+1, numColumns+1, true );
  3916.     (*this)[numRows-1][numRows-1] = 1.0f;
  3917.  
  3918.     v2.SetData( numRows, VECX_ALLOCA( numRows ) );
  3919.     v2 = v;
  3920.     v2[numRows-1] -= 1.0f;
  3921.  
  3922.     return Inverse_UpdateRowColumn( v2, w, numRows-1 );
  3923. }
  3924.  
  3925. /*
  3926. ============
  3927. idMatX::Inverse_UpdateDecrement
  3928.  
  3929.   Updates the in-place inverse to obtain the inverse of the matrix with row r and column r removed.
  3930.   v and w should store the column and row of the original matrix respectively.
  3931. ============
  3932. */
  3933. bool idMatX::Inverse_UpdateDecrement( const idVecX &v, const idVecX &w, int r ) {
  3934.     idVecX v1, w1;
  3935.  
  3936.     assert( numRows == numColumns );
  3937.     assert( v.GetSize() >= numRows );
  3938.     assert( w.GetSize() >= numColumns );
  3939.     assert( r >= 0 && r < numRows && r < numColumns );
  3940.  
  3941.     v1.SetData( numRows, VECX_ALLOCA( numRows ) );
  3942.     w1.SetData( numRows, VECX_ALLOCA( numRows ) );
  3943.  
  3944.     // update the row and column to identity
  3945.     v1 = -v;
  3946.     w1 = -w;
  3947.     v1[r] += 1.0f;
  3948.     w1[r] = 0.0f;
  3949.  
  3950.     if ( !Inverse_UpdateRowColumn( v1, w1, r ) ) {
  3951.         return false;
  3952.     }
  3953.  
  3954.     // physically remove the row and column
  3955.     Update_Decrement( r );
  3956.  
  3957.     return true;
  3958. }
  3959.  
  3960. /*
  3961. ============
  3962. idMatX::Inverse_Solve
  3963.  
  3964.   Solve Ax = b with A inverted
  3965. ============
  3966. */
  3967. void idMatX::Inverse_Solve( idVecX &x, const idVecX &b ) const {
  3968.     Multiply( x, b );
  3969. }
  3970.  
  3971. /*
  3972. ============
  3973. idMatX::LU_Factor
  3974.  
  3975.   in-place factorization: LU
  3976.   L is a triangular matrix stored in the lower triangle.
  3977.   L has ones on the diagonal that are not stored.
  3978.   U is a triangular matrix stored in the upper triangle.
  3979.   If index != NULL partial pivoting is used for numerical stability.
  3980.   If index != NULL it must point to an array of numRow integers and is used to keep track of the row permutation.
  3981.   If det != NULL the determinant of the matrix is calculated and stored.
  3982. ============
  3983. */
  3984. bool idMatX::LU_Factor( int *index, float *det ) {
  3985.     int i, j, k, newi, min;
  3986.     double s, t, d, w;
  3987.  
  3988.     // if partial pivoting should be used
  3989.     if ( index ) {
  3990.         for ( i = 0; i < numRows; i++ ) {
  3991.             index[i] = i;
  3992.         }
  3993.     }
  3994.  
  3995.     w = 1.0f;
  3996.     min = Min( numRows, numColumns );
  3997.     for ( i = 0; i < min; i++ ) {
  3998.  
  3999.         newi = i;
  4000.         s = idMath::Fabs( (*this)[i][i] );
  4001.  
  4002.         if ( index ) {
  4003.             // find the largest absolute pivot
  4004.             for ( j = i + 1; j < numRows; j++ ) {
  4005.                 t = idMath::Fabs( (*this)[j][i] );
  4006.                 if ( t > s ) {
  4007.                     newi = j;
  4008.                     s = t;
  4009.                 }
  4010.             }
  4011.         }
  4012.  
  4013.         if ( s == 0.0f ) {
  4014.             return false;
  4015.         }
  4016.  
  4017.         if ( newi != i ) {
  4018.  
  4019.             w = -w;
  4020.  
  4021.             // swap index elements
  4022.             k = index[i];
  4023.             index[i] = index[newi];
  4024.             index[newi] = k;
  4025.  
  4026.             // swap rows
  4027.             for ( j = 0; j < numColumns; j++ ) {
  4028.                 t = (*this)[newi][j];
  4029.                 (*this)[newi][j] = (*this)[i][j];
  4030.                 (*this)[i][j] = t;
  4031.             }
  4032.         }
  4033.  
  4034.         if ( i < numRows ) {
  4035.             d = 1.0f / (*this)[i][i];
  4036.             for ( j = i + 1; j < numRows; j++ ) {
  4037.                 (*this)[j][i] *= d;
  4038.             }
  4039.         }
  4040.  
  4041.         if ( i < min-1 ) {
  4042.             for ( j = i + 1; j < numRows; j++ ) {
  4043.                 d = (*this)[j][i];
  4044.                 for ( k = i + 1; k < numColumns; k++ ) {
  4045.                     (*this)[j][k] -= d * (*this)[i][k];
  4046.                 }
  4047.             }
  4048.         }
  4049.     }
  4050.  
  4051.     if ( det ) {
  4052.         for ( i = 0; i < numRows; i++ ) {
  4053.             w *= (*this)[i][i];
  4054.         }
  4055.         *det = w;
  4056.     }
  4057.  
  4058.     return true;
  4059. }   
  4060.  
  4061. /*
  4062. ============
  4063. idMatX::LU_UpdateRankOne
  4064.  
  4065.   Updates the in-place LU factorization to obtain the factors for the matrix: LU + alpha * v * w'
  4066. ============
  4067. */
  4068. bool idMatX::LU_UpdateRankOne( const idVecX &v, const idVecX &w, float alpha, int *index ) {
  4069.     int i, j, max;
  4070.     float *y, *z;
  4071.     double diag, beta, p0, p1, d;
  4072.  
  4073.     assert( v.GetSize() >= numColumns );
  4074.     assert( w.GetSize() >= numRows );
  4075.  
  4076.     y = (float *) _alloca16( v.GetSize() * sizeof( float ) );
  4077.     z = (float *) _alloca16( w.GetSize() * sizeof( float ) );
  4078.  
  4079.     if ( index != NULL ) {
  4080.         for ( i = 0; i < numRows; i++ ) {
  4081.             y[i] = alpha * v[index[i]];
  4082.         }
  4083.     } else {
  4084.         for ( i = 0; i < numRows; i++ ) {
  4085.             y[i] = alpha * v[i];
  4086.         }
  4087.     }
  4088.  
  4089.     memcpy( z, w.ToFloatPtr(), w.GetSize() * sizeof( float ) );
  4090.  
  4091.     max = Min( numRows, numColumns );
  4092.     for ( i = 0; i < max; i++ ) {
  4093.         diag = (*this)[i][i];
  4094.  
  4095.         p0 = y[i];
  4096.         p1 = z[i];
  4097.         diag += p0 * p1;
  4098.  
  4099.         if ( diag == 0.0f ) {
  4100.             return false;
  4101.         }
  4102.  
  4103.         beta = p1 / diag;
  4104.  
  4105.         (*this)[i][i] = diag;
  4106.  
  4107.         for ( j = i+1; j < numColumns; j++ ) {
  4108.  
  4109.             d = (*this)[i][j];
  4110.  
  4111.             d += p0 * z[j];
  4112.             z[j] -= beta * d;
  4113.  
  4114.             (*this)[i][j] = d;
  4115.         }
  4116.  
  4117.         for ( j = i+1; j < numRows; j++ ) {
  4118.  
  4119.             d = (*this)[j][i];
  4120.  
  4121.             y[j] -= p0 * d;
  4122.             d += beta * y[j];
  4123.  
  4124.             (*this)[j][i] = d;
  4125.         }
  4126.     }
  4127.     return true;
  4128. }
  4129.  
  4130. /*
  4131. ============
  4132. idMatX::LU_UpdateRowColumn
  4133.  
  4134.   Updates the in-place LU factorization to obtain the factors for the matrix:
  4135.  
  4136.        [ 0  a  0 ]
  4137.   LU + [ d  b  e ]
  4138.        [ 0  c  0 ]
  4139.  
  4140.   where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1], d = w[0,r-1], w[r] = 0.0f, e = w[r+1,numColumns-1]
  4141. ============
  4142. */
  4143. bool idMatX::LU_UpdateRowColumn( const idVecX &v, const idVecX &w, int r, int *index ) {
  4144. #if 0
  4145.  
  4146.     idVecX s;
  4147.  
  4148.     assert( v.GetSize() >= numColumns );
  4149.     assert( w.GetSize() >= numRows );
  4150.     assert( r >= 0 && r < numRows && r < numColumns );
  4151.     assert( w[r] == 0.0f );
  4152.  
  4153.     s.SetData( Max( numRows, numColumns ), VECX_ALLOCA( Max( numRows, numColumns ) ) );
  4154.     s.Zero();
  4155.     s[r] = 1.0f;
  4156.  
  4157.     if ( !LU_UpdateRankOne( v, s, 1.0f, index ) ) {
  4158.         return false;
  4159.     }
  4160.     if ( !LU_UpdateRankOne( s, w, 1.0f, index ) ) {
  4161.         return false;
  4162.     }
  4163.     return true;
  4164.  
  4165. #else
  4166.  
  4167.     int i, j, min, max, rp;
  4168.     float *y0, *y1, *z0, *z1;
  4169.     double diag, beta0, beta1, p0, p1, q0, q1, d;
  4170.  
  4171.     assert( v.GetSize() >= numColumns );
  4172.     assert( w.GetSize() >= numRows );
  4173.     assert( r >= 0 && r < numColumns && r < numRows );
  4174.     assert( w[r] == 0.0f );
  4175.  
  4176.     y0 = (float *) _alloca16( v.GetSize() * sizeof( float ) );
  4177.     z0 = (float *) _alloca16( w.GetSize() * sizeof( float ) );
  4178.     y1 = (float *) _alloca16( v.GetSize() * sizeof( float ) );
  4179.     z1 = (float *) _alloca16( w.GetSize() * sizeof( float ) );
  4180.  
  4181.     if ( index != NULL ) {
  4182.         for ( i = 0; i < numRows; i++ ) {
  4183.             y0[i] = v[index[i]];
  4184.         }
  4185.         rp = r;
  4186.         for ( i = 0; i < numRows; i++ ) {
  4187.             if ( index[i] == r ) {
  4188.                 rp = i;
  4189.                 break;
  4190.             }
  4191.         }
  4192.     } else {
  4193.         memcpy( y0, v.ToFloatPtr(), v.GetSize() * sizeof( float ) );
  4194.         rp = r;
  4195.     }
  4196.  
  4197.     memset( y1, 0, v.GetSize() * sizeof( float ) );
  4198.     y1[rp] = 1.0f;
  4199.  
  4200.     memset( z0, 0, w.GetSize() * sizeof( float ) );
  4201.     z0[r] = 1.0f;
  4202.  
  4203.     memcpy( z1, w.ToFloatPtr(), w.GetSize() * sizeof( float ) );
  4204.  
  4205.     // update the beginning of the to be updated row and column
  4206.     min = Min( r, rp );
  4207.     for ( i = 0; i < min; i++ ) {
  4208.         p0 = y0[i];
  4209.         beta1 = z1[i] / (*this)[i][i];
  4210.  
  4211.         (*this)[i][r] += p0;
  4212.         for ( j = i+1; j < numColumns; j++ ) {
  4213.             z1[j] -= beta1 * (*this)[i][j];
  4214.         }
  4215.         for ( j = i+1; j < numRows; j++ ) {
  4216.             y0[j] -= p0 * (*this)[j][i];
  4217.         }
  4218.         (*this)[rp][i] += beta1;
  4219.     }
  4220.  
  4221.     // update the lower right corner starting at r,r
  4222.     max = Min( numRows, numColumns );
  4223.     for ( i = min; i < max; i++ ) {
  4224.         diag = (*this)[i][i];
  4225.  
  4226.         p0 = y0[i];
  4227.         p1 = z0[i];
  4228.         diag += p0 * p1;
  4229.  
  4230.         if ( diag == 0.0f ) {
  4231.             return false;
  4232.         }
  4233.  
  4234.         beta0 = p1 / diag;
  4235.  
  4236.         q0 = y1[i];
  4237.         q1 = z1[i];
  4238.         diag += q0 * q1;
  4239.  
  4240.         if ( diag == 0.0f ) {
  4241.             return false;
  4242.         }
  4243.  
  4244.         beta1 = q1 / diag;
  4245.  
  4246.         (*this)[i][i] = diag;
  4247.  
  4248.         for ( j = i+1; j < numColumns; j++ ) {
  4249.  
  4250.             d = (*this)[i][j];
  4251.  
  4252.             d += p0 * z0[j];
  4253.             z0[j] -= beta0 * d;
  4254.  
  4255.             d += q0 * z1[j];
  4256.             z1[j] -= beta1 * d;
  4257.  
  4258.             (*this)[i][j] = d;
  4259.         }
  4260.  
  4261.         for ( j = i+1; j < numRows; j++ ) {
  4262.  
  4263.             d = (*this)[j][i];
  4264.  
  4265.             y0[j] -= p0 * d;
  4266.             d += beta0 * y0[j];
  4267.  
  4268.             y1[j] -= q0 * d;
  4269.             d += beta1 * y1[j];
  4270.  
  4271.             (*this)[j][i] = d;
  4272.         }
  4273.     }
  4274.     return true;
  4275.  
  4276. #endif
  4277. }
  4278.  
  4279. /*
  4280. ============
  4281. idMatX::LU_UpdateIncrement
  4282.  
  4283.   Updates the in-place LU factorization to obtain the factors for the matrix:
  4284.  
  4285.   [ A  a ]
  4286.   [ c  b ]
  4287.  
  4288.   where: a = v[0,numRows-1], b = v[numRows], c = w[0,numColumns-1], w[numColumns] = 0
  4289. ============
  4290. */
  4291. bool idMatX::LU_UpdateIncrement( const idVecX &v, const idVecX &w, int *index ) {
  4292.     int i, j;
  4293.     float sum;
  4294.  
  4295.     assert( numRows == numColumns );
  4296.     assert( v.GetSize() >= numRows+1 );
  4297.     assert( w.GetSize() >= numColumns+1 );
  4298.  
  4299.     ChangeSize( numRows+1, numColumns+1, true );
  4300.  
  4301.     // add row to L
  4302.     for ( i = 0; i < numRows - 1; i++ ) {
  4303.         sum = w[i];
  4304.         for ( j = 0; j < i; j++ ) {
  4305.             sum -= (*this)[numRows - 1][j] * (*this)[j][i];
  4306.         }
  4307.         (*this)[numRows - 1 ][i] = sum / (*this)[i][i];
  4308.     }
  4309.  
  4310.     // add row to the permutation index
  4311.     if ( index != NULL ) {
  4312.         index[numRows - 1] = numRows - 1;
  4313.     }
  4314.  
  4315.     // add column to U
  4316.     for ( i = 0; i < numRows; i++ ) {
  4317.         if ( index != NULL ) {
  4318.             sum = v[index[i]];
  4319.         } else {
  4320.             sum = v[i];
  4321.         }
  4322.         for ( j = 0; j < i; j++ ) {
  4323.             sum -= (*this)[i][j] * (*this)[j][numRows - 1];
  4324.         }
  4325.         (*this)[i][numRows - 1] = sum;
  4326.     }
  4327.  
  4328.     return true;
  4329. }
  4330.  
  4331. /*
  4332. ============
  4333. idMatX::LU_UpdateDecrement
  4334.  
  4335.   Updates the in-place LU factorization to obtain the factors for the matrix with row r and column r removed.
  4336.   v and w should store the column and row of the original matrix respectively.
  4337.   If index != NULL then u should store row index[r] of the original matrix. If index == NULL then u = w.
  4338. ============
  4339. */
  4340. bool idMatX::LU_UpdateDecrement( const idVecX &v, const idVecX &w, const idVecX &u, int r, int *index ) {
  4341.     int i, p;
  4342.     idVecX v1, w1;
  4343.  
  4344.     assert( numRows == numColumns );
  4345.     assert( v.GetSize() >= numColumns );
  4346.     assert( w.GetSize() >= numRows );
  4347.     assert( r >= 0 && r < numRows && r < numColumns );
  4348.  
  4349.     v1.SetData( numRows, VECX_ALLOCA( numRows ) );
  4350.     w1.SetData( numRows, VECX_ALLOCA( numRows ) );
  4351.  
  4352.     if ( index != NULL ) {
  4353.  
  4354.         // find the pivot row
  4355.         for ( p = i = 0; i < numRows; i++ ) {
  4356.             if ( index[i] == r ) {
  4357.                 p = i;
  4358.                 break;
  4359.             }
  4360.         }
  4361.  
  4362.         // update the row and column to identity
  4363.         v1 = -v;
  4364.         w1 = -u;
  4365.  
  4366.         if ( p != r ) {
  4367.             idSwap( v1[index[r]], v1[index[p]] );
  4368.             idSwap( index[r], index[p] );
  4369.         }
  4370.  
  4371.         v1[r] += 1.0f;
  4372.         w1[r] = 0.0f;
  4373.  
  4374.         if ( !LU_UpdateRowColumn( v1, w1, r, index ) ) {
  4375.             return false;
  4376.         }
  4377.  
  4378.         if ( p != r ) {
  4379.  
  4380.             if ( idMath::Fabs( u[p] ) < 1e-4f ) {
  4381.                 // NOTE: an additional row interchange is required for numerical stability
  4382.             }
  4383.  
  4384.             // move row index[r] of the original matrix to row index[p] of the original matrix
  4385.             v1.Zero();
  4386.             v1[index[p]] = 1.0f;
  4387.             w1 = u - w;
  4388.  
  4389.             if ( !LU_UpdateRankOne( v1, w1, 1.0f, index ) ) {
  4390.                 return false;
  4391.             }
  4392.         }
  4393.  
  4394.         // remove the row from the permutation index
  4395.         for ( i = r; i < numRows - 1; i++ ) {
  4396.             index[i] = index[i+1];
  4397.         }
  4398.         for ( i = 0; i < numRows - 1; i++ ) {
  4399.             if ( index[i] > r ) {
  4400.                 index[i]--;
  4401.             }
  4402.         }
  4403.  
  4404.     } else {
  4405.  
  4406.         v1 = -v;
  4407.         w1 = -w;
  4408.         v1[r] += 1.0f;
  4409.         w1[r] = 0.0f;
  4410.  
  4411.         if ( !LU_UpdateRowColumn( v1, w1, r, index ) ) {
  4412.             return false;
  4413.         }
  4414.     }
  4415.  
  4416.     // physically remove the row and column
  4417.     Update_Decrement( r );
  4418.  
  4419.     return true;
  4420. }
  4421.  
  4422. /*
  4423. ============
  4424. idMatX::LU_Solve
  4425.  
  4426.   Solve Ax = b with A factored in-place as: LU
  4427. ============
  4428. */
  4429. void idMatX::LU_Solve( idVecX &x, const idVecX &b, const int *index ) const {
  4430.     int i, j;
  4431.     double sum;
  4432.  
  4433.     assert( x.GetSize() == numColumns && b.GetSize() == numRows );
  4434.  
  4435.     // solve L
  4436.     for ( i = 0; i < numRows; i++ ) {
  4437.         if ( index != NULL ) {
  4438.             sum = b[index[i]];
  4439.         } else {
  4440.             sum = b[i];
  4441.         }
  4442.         for ( j = 0; j < i; j++ ) {
  4443.             sum -= (*this)[i][j] * x[j];
  4444.         }
  4445.         x[i] = sum;
  4446.     }
  4447.  
  4448.     // solve U
  4449.     for ( i = numRows - 1; i >= 0; i-- ) {
  4450.         sum = x[i];
  4451.         for ( j = i + 1; j < numRows; j++ ) {
  4452.             sum -= (*this)[i][j] * x[j];
  4453.         }
  4454.         x[i] = sum / (*this)[i][i];
  4455.     }
  4456. }
  4457.  
  4458. /*
  4459. ============
  4460. idMatX::LU_Inverse
  4461.  
  4462.   Calculates the inverse of the matrix which is factored in-place as LU
  4463. ============
  4464. */
  4465. void idMatX::LU_Inverse( idMatX &inv, const int *index ) const {
  4466.     int i, j;
  4467.     idVecX x, b;
  4468.  
  4469.     assert( numRows == numColumns );
  4470.  
  4471.     x.SetData( numRows, VECX_ALLOCA( numRows ) );
  4472.     b.SetData( numRows, VECX_ALLOCA( numRows ) );
  4473.     b.Zero();
  4474.     inv.SetSize( numRows, numColumns );
  4475.  
  4476.     for ( i = 0; i < numRows; i++ ) {
  4477.  
  4478.         b[i] = 1.0f;
  4479.         LU_Solve( x, b, index );
  4480.         for ( j = 0; j < numRows; j++ ) {
  4481.             inv[j][i] = x[j];
  4482.         }
  4483.         b[i] = 0.0f;
  4484.     }
  4485. }
  4486.  
  4487. /*
  4488. ============
  4489. idMatX::LU_UnpackFactors
  4490.  
  4491.   Unpacks the in-place LU factorization.
  4492. ============
  4493. */
  4494. void idMatX::LU_UnpackFactors( idMatX &L, idMatX &U ) const {
  4495.     int i, j;
  4496.  
  4497.     L.Zero( numRows, numColumns );
  4498.     U.Zero( numRows, numColumns );
  4499.     for ( i = 0; i < numRows; i++ ) {
  4500.         for ( j = 0; j < i; j++ ) {
  4501.             L[i][j] = (*this)[i][j];
  4502.         }
  4503.         L[i][i] = 1.0f;
  4504.         for ( j = i; j < numColumns; j++ ) {
  4505.             U[i][j] = (*this)[i][j];
  4506.         }
  4507.     }
  4508. }
  4509.  
  4510. /*
  4511. ============
  4512. idMatX::LU_MultiplyFactors
  4513.  
  4514.   Multiplies the factors of the in-place LU factorization to form the original matrix.
  4515. ============
  4516. */
  4517. void idMatX::LU_MultiplyFactors( idMatX &m, const int *index ) const {
  4518.     int r, rp, i, j;
  4519.     double sum;
  4520.  
  4521.     m.SetSize( numRows, numColumns );
  4522.  
  4523.     for ( r = 0; r < numRows; r++ ) {
  4524.  
  4525.         if ( index != NULL ) {
  4526.             rp = index[r];
  4527.         } else {
  4528.             rp = r;
  4529.         }
  4530.  
  4531.         // calculate row of matrix
  4532.         for ( i = 0; i < numColumns; i++ ) {
  4533.             if ( i >= r ) {
  4534.                 sum = (*this)[r][i];
  4535.             } else {
  4536.                 sum = 0.0f;
  4537.             }
  4538.             for ( j = 0; j <= i && j < r; j++ ) {
  4539.                 sum += (*this)[r][j] * (*this)[j][i];
  4540.             }
  4541.             m[rp][i] = sum;
  4542.         }
  4543.     }
  4544. }
  4545.  
  4546. /*
  4547. ============
  4548. idMatX::QR_Factor
  4549.  
  4550.   in-place factorization: QR
  4551.   Q is an orthogonal matrix represented as a product of Householder matrices stored in the lower triangle and c.
  4552.   R is a triangular matrix stored in the upper triangle except for the diagonal elements which are stored in d.
  4553.   The initial matrix has to be square.
  4554. ============
  4555. */
  4556. bool idMatX::QR_Factor( idVecX &c, idVecX &d ) {
  4557.     int i, j, k;
  4558.     double scale, s, t, sum;
  4559.     bool singular = false;
  4560.  
  4561.     assert( numRows == numColumns );
  4562.     assert( c.GetSize() >= numRows && d.GetSize() >= numRows );
  4563.  
  4564.     for ( k = 0; k < numRows-1; k++ ) {
  4565.  
  4566.         scale = 0.0f;
  4567.         for ( i = k; i < numRows; i++ ) {
  4568.             s = idMath::Fabs( (*this)[i][k] );
  4569.             if ( s > scale ) {
  4570.                 scale = s;
  4571.             }
  4572.         }
  4573.         if ( scale == 0.0f ) {
  4574.             singular = true;
  4575.             c[k] = d[k] = 0.0f;
  4576.         } else {
  4577.  
  4578.             s = 1.0f / scale;
  4579.             for ( i = k; i < numRows; i++ ) {
  4580.                 (*this)[i][k] *= s;
  4581.             }
  4582.  
  4583.             sum = 0.0f;
  4584.             for ( i = k; i < numRows; i++ ) {
  4585.                 s = (*this)[i][k];
  4586.                 sum += s * s;
  4587.             }
  4588.  
  4589.             s = idMath::Sqrt( sum );
  4590.             if ( (*this)[k][k] < 0.0f ) {
  4591.                 s = -s;
  4592.             }
  4593.             (*this)[k][k] += s;
  4594.             c[k] = s * (*this)[k][k];
  4595.             d[k] = -scale * s;
  4596.  
  4597.             for ( j = k+1; j < numRows; j++ ) {
  4598.  
  4599.                 sum = 0.0f;
  4600.                 for ( i = k; i < numRows; i++ ) {
  4601.                     sum += (*this)[i][k] * (*this)[i][j];
  4602.                 }
  4603.                 t = sum / c[k];
  4604.                 for ( i = k; i < numRows; i++ ) {
  4605.                     (*this)[i][j] -= t * (*this)[i][k];
  4606.                 }
  4607.             }
  4608.         }
  4609.     }
  4610.     d[numRows-1] = (*this)[ (numRows-1) ][ (numRows-1) ];
  4611.     if ( d[numRows-1] == 0.0f ) {
  4612.         singular = true;
  4613.     }
  4614.  
  4615.     return !singular;
  4616. }
  4617.  
  4618. /*
  4619. ============
  4620. idMatX::QR_Rotate
  4621.  
  4622.   Performs a Jacobi rotation on the rows i and i+1 of the unpacked QR factors.
  4623. ============
  4624. */
  4625. void idMatX::QR_Rotate( idMatX &R, int i, float a, float b ) {
  4626.     int j;
  4627.     float f, c, s, w, y;
  4628.  
  4629.     if ( a == 0.0f ) {
  4630.         c = 0.0f;
  4631.         s = ( b >= 0.0f ) ? 1.0f : -1.0f;
  4632.     } else if ( idMath::Fabs( a ) > idMath::Fabs( b ) ) {
  4633.         f = b / a;
  4634.         c = idMath::Fabs( 1.0f / idMath::Sqrt( 1.0f + f * f ) );
  4635.         if ( a < 0.0f ) {
  4636.             c = -c;
  4637.         }
  4638.         s = f * c;
  4639.     } else {
  4640.         f = a / b;
  4641.         s = idMath::Fabs( 1.0f / idMath::Sqrt( 1.0f + f * f ) );
  4642.         if ( b < 0.0f ) {
  4643.             s = -s;
  4644.         }
  4645.         c = f * s;
  4646.     }
  4647.     for ( j = i; j < numRows; j++ ) {
  4648.         y = R[i][j];
  4649.         w = R[i+1][j];
  4650.         R[i][j] = c * y - s * w;
  4651.         R[i+1][j] = s * y + c * w;
  4652.     }
  4653.     for ( j = 0; j < numRows; j++ ) {
  4654.         y = (*this)[j][i];
  4655.         w = (*this)[j][i+1];
  4656.         (*this)[j][i] = c * y - s * w;
  4657.         (*this)[j][i+1] = s * y + c * w;
  4658.     }
  4659. }
  4660.  
  4661. /*
  4662. ============
  4663. idMatX::QR_UpdateRankOne
  4664.  
  4665.   Updates the unpacked QR factorization to obtain the factors for the matrix: QR + alpha * v * w'
  4666. ============
  4667. */
  4668. bool idMatX::QR_UpdateRankOne( idMatX &R, const idVecX &v, const idVecX &w, float alpha ) {
  4669.     int i, k;
  4670.     float f;
  4671.     idVecX u;
  4672.  
  4673.     assert( v.GetSize() >= numColumns );
  4674.     assert( w.GetSize() >= numRows );
  4675.  
  4676.     u.SetData( v.GetSize(), VECX_ALLOCA( v.GetSize() ) );
  4677.     TransposeMultiply( u, v );
  4678.     u *= alpha;
  4679.  
  4680.     for ( k = v.GetSize()-1; k > 0; k-- ) {
  4681.         if ( u[k] != 0.0f ) {
  4682.             break;
  4683.         }
  4684.     }
  4685.     for ( i = k-1; i >= 0; i-- ) {
  4686.         QR_Rotate( R, i, u[i], -u[i+1] );
  4687.         if ( u[i] == 0.0f ) {
  4688.             u[i] = idMath::Fabs( u[i+1] );
  4689.         } else if ( idMath::Fabs( u[i] ) > idMath::Fabs( u[i+1] ) ) {
  4690.             f = u[i+1] / u[i];
  4691.             u[i] = idMath::Fabs( u[i] ) * idMath::Sqrt( 1.0f + f * f );
  4692.         } else {
  4693.             f = u[i] / u[i+1];
  4694.             u[i] = idMath::Fabs( u[i+1] ) * idMath::Sqrt( 1.0f + f * f );
  4695.         }
  4696.     }
  4697.     for ( i = 0; i < v.GetSize(); i++ ) {
  4698.         R[0][i] += u[0] * w[i];
  4699.     }
  4700.     for ( i = 0; i < k; i++ ) {
  4701.         QR_Rotate( R, i, -R[i][i], R[i+1][i] );
  4702.     }
  4703.     return true;
  4704. }
  4705.  
  4706. /*
  4707. ============
  4708. idMatX::QR_UpdateRowColumn
  4709.  
  4710.   Updates the unpacked QR factorization to obtain the factors for the matrix:
  4711.  
  4712.        [ 0  a  0 ]
  4713.   QR + [ d  b  e ]
  4714.        [ 0  c  0 ]
  4715.  
  4716.   where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1], d = w[0,r-1], w[r] = 0.0f, e = w[r+1,numColumns-1]
  4717. ============
  4718. */
  4719. bool idMatX::QR_UpdateRowColumn( idMatX &R, const idVecX &v, const idVecX &w, int r ) {
  4720.     idVecX s;
  4721.  
  4722.     assert( v.GetSize() >= numColumns );
  4723.     assert( w.GetSize() >= numRows );
  4724.     assert( r >= 0 && r < numRows && r < numColumns );
  4725.     assert( w[r] == 0.0f );
  4726.  
  4727.     s.SetData( Max( numRows, numColumns ), VECX_ALLOCA( Max( numRows, numColumns ) ) );
  4728.     s.Zero();
  4729.     s[r] = 1.0f;
  4730.  
  4731.     if ( !QR_UpdateRankOne( R, v, s, 1.0f ) ) {
  4732.         return false;
  4733.     }
  4734.     if ( !QR_UpdateRankOne( R, s, w, 1.0f ) ) {
  4735.         return false;
  4736.     }
  4737.     return true;
  4738. }
  4739.  
  4740. /*
  4741. ============
  4742. idMatX::QR_UpdateIncrement
  4743.  
  4744.   Updates the unpacked QR factorization to obtain the factors for the matrix:
  4745.  
  4746.   [ A  a ]
  4747.   [ c  b ]
  4748.  
  4749.   where: a = v[0,numRows-1], b = v[numRows], c = w[0,numColumns-1], w[numColumns] = 0
  4750. ============
  4751. */
  4752. bool idMatX::QR_UpdateIncrement( idMatX &R, const idVecX &v, const idVecX &w ) {
  4753.     idVecX v2;
  4754.  
  4755.     assert( numRows == numColumns );
  4756.     assert( v.GetSize() >= numRows+1 );
  4757.     assert( w.GetSize() >= numColumns+1 );
  4758.  
  4759.     ChangeSize( numRows+1, numColumns+1, true );
  4760.     (*this)[numRows-1][numRows-1] = 1.0f;
  4761.  
  4762.     R.ChangeSize( R.numRows+1, R.numColumns+1, true );
  4763.     R[R.numRows-1][R.numRows-1] = 1.0f;
  4764.  
  4765.     v2.SetData( numRows, VECX_ALLOCA( numRows ) );
  4766.     v2 = v;
  4767.     v2[numRows-1] -= 1.0f;
  4768.  
  4769.     return QR_UpdateRowColumn( R, v2, w, numRows-1 );
  4770. }
  4771.  
  4772. /*
  4773. ============
  4774. idMatX::QR_UpdateDecrement
  4775.  
  4776.   Updates the unpacked QR factorization to obtain the factors for the matrix with row r and column r removed.
  4777.   v and w should store the column and row of the original matrix respectively.
  4778. ============
  4779. */
  4780. bool idMatX::QR_UpdateDecrement( idMatX &R, const idVecX &v, const idVecX &w, int r ) {
  4781.     idVecX v1, w1;
  4782.  
  4783.     assert( numRows == numColumns );
  4784.     assert( v.GetSize() >= numRows );
  4785.     assert( w.GetSize() >= numColumns );
  4786.     assert( r >= 0 && r < numRows && r < numColumns );
  4787.  
  4788.     v1.SetData( numRows, VECX_ALLOCA( numRows ) );
  4789.     w1.SetData( numRows, VECX_ALLOCA( numRows ) );
  4790.  
  4791.     // update the row and column to identity
  4792.     v1 = -v;
  4793.     w1 = -w;
  4794.     v1[r] += 1.0f;
  4795.     w1[r] = 0.0f;
  4796.  
  4797.     if ( !QR_UpdateRowColumn( R, v1, w1, r ) ) {
  4798.         return false;
  4799.     }
  4800.  
  4801.     // physically remove the row and column
  4802.     Update_Decrement( r );
  4803.     R.Update_Decrement( r );
  4804.  
  4805.     return true;
  4806. }
  4807.  
  4808. /*
  4809. ============
  4810. idMatX::QR_Solve
  4811.  
  4812.   Solve Ax = b with A factored in-place as: QR
  4813. ============
  4814. */
  4815. void idMatX::QR_Solve( idVecX &x, const idVecX &b, const idVecX &c, const idVecX &d ) const {
  4816.     int i, j;
  4817.     double sum, t;
  4818.  
  4819.     assert( numRows == numColumns );
  4820.     assert( x.GetSize() >= numRows && b.GetSize() >= numRows );
  4821.     assert( c.GetSize() >= numRows && d.GetSize() >= numRows );
  4822.  
  4823.     for ( i = 0; i < numRows; i++ ) {
  4824.         x[i] = b[i];
  4825.     }
  4826.  
  4827.     // multiply b with transpose of Q
  4828.     for ( i = 0; i < numRows-1; i++ ) {
  4829.  
  4830.         sum = 0.0f;
  4831.         for ( j = i; j < numRows; j++ ) {
  4832.             sum += (*this)[j][i] * x[j];
  4833.         }
  4834.         t = sum / c[i];
  4835.         for ( j = i; j < numRows; j++ ) {
  4836.             x[j] -= t * (*this)[j][i];
  4837.         }
  4838.     }
  4839.  
  4840.     // backsubstitution with R
  4841.     for ( i = numRows-1; i >= 0; i-- ) {
  4842.  
  4843.         sum = x[i];
  4844.         for ( j = i + 1; j < numRows; j++ ) {
  4845.             sum -= (*this)[i][j] * x[j];
  4846.         }
  4847.         x[i] = sum / d[i];
  4848.     }
  4849. }
  4850.  
  4851. /*
  4852. ============
  4853. idMatX::QR_Solve
  4854.  
  4855.   Solve Ax = b with A factored as: QR
  4856. ============
  4857. */
  4858. void idMatX::QR_Solve( idVecX &x, const idVecX &b, const idMatX &R ) const {
  4859.     int i, j;
  4860.     double sum;
  4861.  
  4862.     assert( numRows == numColumns );
  4863.  
  4864.     // multiply b with transpose of Q
  4865.     TransposeMultiply( x, b );
  4866.  
  4867.     // backsubstitution with R
  4868.     for ( i = numRows-1; i >= 0; i-- ) {
  4869.  
  4870.         sum = x[i];
  4871.         for ( j = i + 1; j < numRows; j++ ) {
  4872.             sum -= R[i][j] * x[j];
  4873.         }
  4874.         x[i] = sum / R[i][i];
  4875.     }
  4876. }
  4877.  
  4878. /*
  4879. ============
  4880. idMatX::QR_Inverse
  4881.  
  4882.   Calculates the inverse of the matrix which is factored in-place as: QR
  4883. ============
  4884. */
  4885. void idMatX::QR_Inverse( idMatX &inv, const idVecX &c, const idVecX &d ) const {
  4886.     int i, j;
  4887.     idVecX x, b;
  4888.  
  4889.     assert( numRows == numColumns );
  4890.  
  4891.     x.SetData( numRows, VECX_ALLOCA( numRows ) );
  4892.     b.SetData( numRows, VECX_ALLOCA( numRows ) );
  4893.     b.Zero();
  4894.     inv.SetSize( numRows, numColumns );
  4895.  
  4896.     for ( i = 0; i < numRows; i++ ) {
  4897.  
  4898.         b[i] = 1.0f;
  4899.         QR_Solve( x, b, c, d );
  4900.         for ( j = 0; j < numRows; j++ ) {
  4901.             inv[j][i] = x[j];
  4902.         }
  4903.         b[i] = 0.0f;
  4904.     }
  4905. }
  4906.  
  4907. /*
  4908. ============
  4909. idMatX::QR_UnpackFactors
  4910.  
  4911.   Unpacks the in-place QR factorization.
  4912. ============
  4913. */
  4914. void idMatX::QR_UnpackFactors( idMatX &Q, idMatX &R, const idVecX &c, const idVecX &d ) const {
  4915.     int i, j, k;
  4916.     double sum;
  4917.  
  4918.     Q.Identity( numRows, numColumns );
  4919.     for ( i = 0; i < numColumns-1; i++ ) {
  4920.         if ( c[i] == 0.0f ) {
  4921.             continue;
  4922.         }
  4923.         for ( j = 0; j < numRows; j++ ) {
  4924.             sum = 0.0f;
  4925.             for ( k = i; k < numColumns; k++ ) {
  4926.                 sum += (*this)[k][i] * Q[j][k];
  4927.             }
  4928.             sum /= c[i];
  4929.             for ( k = i; k < numColumns; k++ ) {
  4930.                 Q[j][k] -= sum * (*this)[k][i];
  4931.             }
  4932.         }
  4933.     }
  4934.  
  4935.     R.Zero( numRows, numColumns );
  4936.     for ( i = 0; i < numRows; i++ ) {
  4937.         R[i][i] = d[i];
  4938.         for ( j = i+1; j < numColumns; j++ ) {
  4939.             R[i][j] = (*this)[i][j];
  4940.         }
  4941.     }
  4942. }
  4943.  
  4944. /*
  4945. ============
  4946. idMatX::QR_MultiplyFactors
  4947.  
  4948.   Multiplies the factors of the in-place QR factorization to form the original matrix.
  4949. ============
  4950. */
  4951. void idMatX::QR_MultiplyFactors( idMatX &m, const idVecX &c, const idVecX &d ) const {
  4952.     int i, j, k;
  4953.     double sum;
  4954.     idMatX Q;
  4955.  
  4956.     Q.Identity( numRows, numColumns );
  4957.     for ( i = 0; i < numColumns-1; i++ ) {
  4958.         if ( c[i] == 0.0f ) {
  4959.             continue;
  4960.         }
  4961.         for ( j = 0; j < numRows; j++ ) {
  4962.             sum = 0.0f;
  4963.             for ( k = i; k < numColumns; k++ ) {
  4964.                 sum += (*this)[k][i] * Q[j][k];
  4965.             }
  4966.             sum /= c[i];
  4967.             for ( k = i; k < numColumns; k++ ) {
  4968.                 Q[j][k] -= sum * (*this)[k][i];
  4969.             }
  4970.         }
  4971.     }
  4972.  
  4973.     for ( i = 0; i < numRows; i++ ) {
  4974.         for ( j = 0; j < numColumns; j++ ) {
  4975.             sum = Q[i][j] * d[i];
  4976.             for ( k = 0; k < i; k++ ) {
  4977.                 sum += Q[i][k] * (*this)[j][k];
  4978.             }
  4979.             m[i][j] = sum;
  4980.         }
  4981.     }
  4982. }
  4983.  
  4984. /*
  4985. ============
  4986. idMatX::Pythag
  4987.  
  4988.   Computes (a^2 + b^2)^1/2 without underflow or overflow.
  4989. ============
  4990. */
  4991. float idMatX::Pythag( float a, float b ) const {
  4992.     double at, bt, ct;
  4993.  
  4994.     at = idMath::Fabs( a );
  4995.     bt = idMath::Fabs( b );
  4996.     if ( at > bt ) {
  4997.         ct = bt / at;
  4998.         return at * idMath::Sqrt( 1.0f + ct * ct );
  4999.     } else {
  5000.         if ( bt ) {
  5001.             ct = at / bt;
  5002.             return bt * idMath::Sqrt( 1.0f + ct * ct );
  5003.         } else {
  5004.             return 0.0f;
  5005.         }
  5006.     }
  5007. }
  5008.  
  5009. /*
  5010. ============
  5011. idMatX::SVD_BiDiag
  5012. ============
  5013. */
  5014. void idMatX::SVD_BiDiag( idVecX &w, idVecX &rv1, float &anorm ) {
  5015.     int i, j, k, l;
  5016.     double f, h, r, g, s, scale;
  5017.  
  5018.     anorm = 0.0f;
  5019.     g = s = scale = 0.0f;
  5020.     for ( i = 0; i < numColumns; i++ ) {
  5021.         l = i + 1;
  5022.         rv1[i] = scale * g;
  5023.         g = s = scale = 0.0f;
  5024.         if ( i < numRows ) {
  5025.             for ( k = i; k < numRows; k++ ) {
  5026.                 scale += idMath::Fabs( (*this)[k][i] );
  5027.             }
  5028.             if ( scale ) {
  5029.                 for ( k = i; k < numRows; k++ ) {
  5030.                     (*this)[k][i] /= scale;
  5031.                     s += (*this)[k][i] * (*this)[k][i];
  5032.                 }
  5033.                 f = (*this)[i][i];
  5034.                 g = idMath::Sqrt( s );
  5035.                 if ( f >= 0.0f ) {
  5036.                     g = -g;
  5037.                 }
  5038.                 h = f * g - s;
  5039.                 (*this)[i][i] = f - g;
  5040.                 if ( i != (numColumns-1) ) {
  5041.                     for ( j = l; j < numColumns; j++ ) {
  5042.                         for ( s = 0.0f, k = i; k < numRows; k++ ) {
  5043.                             s += (*this)[k][i] * (*this)[k][j];
  5044.                         }
  5045.                         f = s / h;
  5046.                         for ( k = i; k < numRows; k++ ) {
  5047.                             (*this)[k][j] += f * (*this)[k][i];
  5048.                         }
  5049.                     }
  5050.                 }
  5051.                 for ( k = i; k < numRows; k++ ) {
  5052.                     (*this)[k][i] *= scale;
  5053.                 }
  5054.             }
  5055.         }
  5056.         w[i] = scale * g;
  5057.         g = s = scale = 0.0f;
  5058.         if ( i < numRows && i != (numColumns-1) ) {
  5059.             for ( k = l; k < numColumns; k++ ) {
  5060.                 scale += idMath::Fabs( (*this)[i][k] );
  5061.             }
  5062.             if ( scale ) {
  5063.                 for ( k = l; k < numColumns; k++ ) {
  5064.                     (*this)[i][k] /= scale;
  5065.                     s += (*this)[i][k] * (*this)[i][k];
  5066.                 }
  5067.                 f = (*this)[i][l];
  5068.                 g = idMath::Sqrt( s );
  5069.                 if ( f >= 0.0f ) {
  5070.                     g = -g;
  5071.                 }
  5072.                 h = 1.0f / ( f * g - s );
  5073.                 (*this)[i][l] = f - g;
  5074.                 for ( k = l; k < numColumns; k++ ) {
  5075.                     rv1[k] = (*this)[i][k] * h;
  5076.                 }
  5077.                 if ( i != (numRows-1) ) {
  5078.                     for ( j = l; j < numRows; j++ ) {
  5079.                         for ( s = 0.0f, k = l; k < numColumns; k++ ) {
  5080.                             s += (*this)[j][k] * (*this)[i][k];
  5081.                         }
  5082.                         for ( k = l; k < numColumns; k++ ) {
  5083.                             (*this)[j][k] += s * rv1[k];
  5084.                         }
  5085.                     }
  5086.                 }
  5087.                 for ( k = l; k < numColumns; k++ ) {
  5088.                     (*this)[i][k] *= scale;
  5089.                 }
  5090.             }
  5091.         }
  5092.         r = idMath::Fabs( w[i] ) + idMath::Fabs( rv1[i] );
  5093.         if ( r > anorm ) {
  5094.             anorm = r;
  5095.         }
  5096.     }
  5097. }
  5098.  
  5099. /*
  5100. ============
  5101. idMatX::SVD_InitialWV
  5102. ============
  5103. */
  5104. void idMatX::SVD_InitialWV( idVecX &w, idMatX &V, idVecX &rv1 ) {
  5105.     int i, j, k, l;
  5106.     double f, g, s;
  5107.  
  5108.     g = 0.0f;
  5109.     for ( i = (numColumns-1); i >= 0; i-- ) {
  5110.         l = i + 1;
  5111.         if ( i < ( numColumns - 1 ) ) {
  5112.             if ( g ) {
  5113.                 for ( j = l; j < numColumns; j++ ) {
  5114.                     V[j][i] = ((*this)[i][j] / (*this)[i][l]) / g;
  5115.                 }
  5116.                 // double division to reduce underflow
  5117.                 for ( j = l; j < numColumns; j++ ) {
  5118.                     for ( s = 0.0f, k = l; k < numColumns; k++ ) {
  5119.                         s += (*this)[i][k] * V[k][j];
  5120.                     }
  5121.                     for ( k = l; k < numColumns; k++ ) {
  5122.                         V[k][j] += s * V[k][i];
  5123.                     }
  5124.                 }
  5125.             }
  5126.             for ( j = l; j < numColumns; j++ ) {
  5127.                 V[i][j] = V[j][i] = 0.0f;
  5128.             }
  5129.         }
  5130.         V[i][i] = 1.0f;
  5131.         g = rv1[i];
  5132.     }
  5133.     for ( i = numColumns - 1 ; i >= 0; i-- ) {
  5134.         l = i + 1;
  5135.         g = w[i];
  5136.         if ( i < (numColumns-1) ) {
  5137.             for ( j = l; j < numColumns; j++ ) {
  5138.                 (*this)[i][j] = 0.0f;
  5139.             }
  5140.         }
  5141.         if ( g ) {
  5142.             g = 1.0f / g;
  5143.             if ( i != (numColumns-1) ) {
  5144.                 for ( j = l; j < numColumns; j++ ) {
  5145.                     for ( s = 0.0f, k = l; k < numRows; k++ ) {
  5146.                         s += (*this)[k][i] * (*this)[k][j];
  5147.                     }
  5148.                     f = (s / (*this)[i][i]) * g;
  5149.                     for ( k = i; k < numRows; k++ ) {
  5150.                         (*this)[k][j] += f * (*this)[k][i];
  5151.                     }
  5152.                 }
  5153.             }
  5154.             for ( j = i; j < numRows; j++ ) {
  5155.                 (*this)[j][i] *= g;
  5156.             }
  5157.         }
  5158.         else {
  5159.             for ( j = i; j < numRows; j++ ) {
  5160.                 (*this)[j][i] = 0.0f;
  5161.             }
  5162.         }
  5163.         (*this)[i][i] += 1.0f;
  5164.     }
  5165. }
  5166.  
  5167. /*
  5168. ============
  5169. idMatX::SVD_Factor
  5170.  
  5171.   in-place factorization: U * Diag(w) * V.Transpose()
  5172.   known as the Singular Value Decomposition.
  5173.   U is a column-orthogonal matrix which overwrites the original matrix.
  5174.   w is a diagonal matrix with all elements >= 0 which are the singular values.
  5175.   V is the transpose of an orthogonal matrix.
  5176. ============
  5177. */
  5178. bool idMatX::SVD_Factor( idVecX &w, idMatX &V ) {
  5179.     int flag, i, its, j, jj, k, l, nm;
  5180.     double c, f, h, s, x, y, z, r, g = 0.0f;
  5181.     float anorm = 0.0f;
  5182.     idVecX rv1;
  5183.  
  5184.     if ( numRows < numColumns ) {
  5185.         return false;
  5186.     }
  5187.  
  5188.     rv1.SetData( numColumns, VECX_ALLOCA( numColumns ) );
  5189.     rv1.Zero();
  5190.     w.Zero( numColumns );
  5191.     V.Zero( numColumns, numColumns );
  5192.  
  5193.     SVD_BiDiag( w, rv1, anorm );
  5194.     SVD_InitialWV( w, V, rv1 );
  5195.  
  5196.     for ( k = numColumns - 1; k >= 0; k-- ) {
  5197.         for ( its = 1; its <= 30; its++ ) {
  5198.             flag = 1;
  5199.             nm = 0;
  5200.             for ( l = k; l >= 0; l-- ) {
  5201.                 nm = l - 1;
  5202.                 if ( ( idMath::Fabs( rv1[l] ) + anorm ) == anorm /* idMath::Fabs( rv1[l] ) < idMath::FLT_EPSILON */ ) {
  5203.                     flag = 0;
  5204.                     break;
  5205.                 }
  5206.                 if ( ( idMath::Fabs( w[nm] ) + anorm ) == anorm /* idMath::Fabs( w[nm] ) < idMath::FLT_EPSILON */ ) {
  5207.                     break;
  5208.                 }
  5209.             }
  5210.             if ( flag ) {
  5211.                 c = 0.0f;
  5212.                 s = 1.0f;
  5213.                 for ( i = l; i <= k; i++ ) {
  5214.                     f = s * rv1[i];
  5215.  
  5216.                     if ( ( idMath::Fabs( f ) + anorm ) != anorm /* idMath::Fabs( f ) > idMath::FLT_EPSILON */ ) {
  5217.                         g = w[i];
  5218.                         h = Pythag( f, g );
  5219.                         w[i] = h;
  5220.                         h = 1.0f / h;
  5221.                         c = g * h;
  5222.                         s = -f * h;
  5223.                         for ( j = 0; j < numRows; j++ ) {
  5224.                             y = (*this)[j][nm];
  5225.                             z = (*this)[j][i];
  5226.                             (*this)[j][nm] = y * c + z * s;
  5227.                             (*this)[j][i] = z * c - y * s;
  5228.                         }
  5229.                     }
  5230.                 }
  5231.             }
  5232.             z = w[k];
  5233.             if ( l == k ) {
  5234.                 if ( z < 0.0f ) {
  5235.                     w[k] = -z;
  5236.                     for ( j = 0; j < numColumns; j++ ) {
  5237.                         V[j][k] = -V[j][k];
  5238.                     }
  5239.                 }
  5240.                 break;
  5241.             }
  5242.             if ( its == 30 ) {
  5243.                 return false;        // no convergence
  5244.             }
  5245.             x = w[l];
  5246.             nm = k - 1;
  5247.             y = w[nm];
  5248.             g = rv1[nm];
  5249.             h = rv1[k];
  5250.             f = ( ( y - z ) * ( y + z ) + ( g - h ) * ( g + h ) ) / ( 2.0f * h * y );
  5251.             g = Pythag( f, 1.0f );
  5252.             r = ( f >= 0.0f ? g : - g );
  5253.             f= ( ( x - z ) * ( x + z ) + h * ( ( y / ( f + r ) ) - h ) ) / x;
  5254.             c = s = 1.0f;
  5255.             for ( j = l; j <= nm; j++ ) {
  5256.                 i = j + 1;
  5257.                 g = rv1[i];
  5258.                 y = w[i];
  5259.                 h = s * g;
  5260.                 g = c * g;
  5261.                 z = Pythag( f, h );
  5262.                 rv1[j] = z;
  5263.                 c = f / z;
  5264.                 s = h / z;
  5265.                 f = x * c + g * s;
  5266.                 g = g * c - x * s;
  5267.                 h = y * s;
  5268.                 y = y * c;
  5269.                 for ( jj = 0; jj < numColumns; jj++ ) {
  5270.                     x = V[jj][j];
  5271.                     z = V[jj][i];
  5272.                     V[jj][j] = x * c + z * s;
  5273.                     V[jj][i] = z * c - x * s;
  5274.                 }
  5275.                 z = Pythag( f, h );
  5276.                 w[j] = z;
  5277.                 if ( z ) {
  5278.                     z = 1.0f / z;
  5279.                     c = f * z;
  5280.                     s = h * z;
  5281.                 }
  5282.                 f = ( c * g ) + ( s * y );
  5283.                 x = ( c * y ) - ( s * g );
  5284.                 for ( jj = 0; jj < numRows; jj++ ) {
  5285.                     y = (*this)[jj][j];
  5286.                     z = (*this)[jj][i];
  5287.                     (*this)[jj][j] = y * c + z * s;
  5288.                     (*this)[jj][i] = z * c - y * s;
  5289.                 }
  5290.             }
  5291.             rv1[l] = 0.0f;
  5292.             rv1[k] = f;
  5293.             w[k] = x;
  5294.         }
  5295.     }
  5296.     return true;
  5297. }
  5298.  
  5299. /*
  5300. ============
  5301. idMatX::SVD_Solve
  5302.  
  5303.   Solve Ax = b with A factored as: U * Diag(w) * V.Transpose()
  5304. ============
  5305. */
  5306. void idMatX::SVD_Solve( idVecX &x, const idVecX &b, const idVecX &w, const idMatX &V ) const {
  5307.     int i, j;
  5308.     double sum;
  5309.     idVecX tmp;
  5310.  
  5311.     assert( x.GetSize() >= numColumns );
  5312.     assert( b.GetSize() >= numColumns );
  5313.     assert( w.GetSize() == numColumns );
  5314.     assert( V.GetNumRows() == numColumns && V.GetNumColumns() == numColumns );
  5315.  
  5316.     tmp.SetData( numColumns, VECX_ALLOCA( numColumns ) );
  5317.  
  5318.     for ( i = 0; i < numColumns; i++ ) {
  5319.         sum = 0.0f;
  5320. // RAVEN BEGIN
  5321. // jscott: renamed to prevent name clash
  5322.         if ( w[i] >= idMath::FLOAT_EPSILON ) {
  5323. // RAVEN END
  5324.             for ( j = 0; j < numRows; j++ ) {
  5325.                 sum += (*this)[j][i] * b[j];
  5326.             }
  5327.             sum /= w[i];
  5328.         }
  5329.         tmp[i] = sum;
  5330.     }
  5331.     for ( i = 0; i < numColumns; i++ ) {
  5332.         sum = 0.0f;
  5333.         for ( j = 0; j < numColumns; j++ ) {
  5334.             sum += V[i][j] * tmp[j];
  5335.         }
  5336.         x[i] = sum;
  5337.     }
  5338. }
  5339.  
  5340. /*
  5341. ============
  5342. idMatX::SVD_Inverse
  5343.  
  5344.   Calculates the inverse of the matrix which is factored in-place as: U * Diag(w) * V.Transpose()
  5345. ============
  5346. */
  5347. void idMatX::SVD_Inverse( idMatX &inv, const idVecX &w, const idMatX &V ) const {
  5348.     int i, j, k;
  5349.     double wi, sum;
  5350.     idMatX V2;
  5351.  
  5352.     assert( numRows == numColumns );
  5353.  
  5354.     V2 = V;
  5355.  
  5356.     // V * [diag(1/w[i])]
  5357.     for ( i = 0; i < numRows; i++ ) {
  5358.         wi = w[i];
  5359. // RAVEN BEGIN
  5360. // jscott: renamed to prevent name clash
  5361.         wi = ( wi < idMath::FLOAT_EPSILON ) ? 0.0f : 1.0f / wi;
  5362. // RAVEN END
  5363.         for ( j = 0; j < numColumns; j++ ) {
  5364.             V2[j][i] *= wi;
  5365.         }
  5366.     }
  5367.  
  5368.     // V * [diag(1/w[i])] * Ut
  5369.     for ( i = 0; i < numRows; i++ ) {
  5370.         for ( j = 0; j < numColumns; j++ ) {
  5371.             sum = V2[i][0] * (*this)[j][0];
  5372.             for ( k = 1; k < numColumns; k++ ) {
  5373.                 sum += V2[i][k] * (*this)[j][k];
  5374.             }
  5375.             inv[i][j] = sum;
  5376.         }
  5377.     }
  5378. }
  5379.  
  5380. /*
  5381. ============
  5382. idMatX::SVD_MultiplyFactors
  5383.  
  5384.   Multiplies the factors of the in-place SVD factorization to form the original matrix.
  5385. ============
  5386. */
  5387. void idMatX::SVD_MultiplyFactors( idMatX &m, const idVecX &w, const idMatX &V ) const {
  5388.     int r, i, j;
  5389.     double sum;
  5390.  
  5391.     m.SetSize( numRows, V.GetNumRows() );
  5392.  
  5393.     for ( r = 0; r < numRows; r++ ) {
  5394.         // calculate row of matrix
  5395. // RAVEN BEGIN
  5396. // jscott: renamed to prevent name clash
  5397.         if ( w[r] >= idMath::FLOAT_EPSILON ) {
  5398. // RAVEN END
  5399.             for ( i = 0; i < V.GetNumRows(); i++ ) {
  5400.                 sum = 0.0f;
  5401.                 for ( j = 0; j < numColumns; j++ ) {
  5402.                     sum += (*this)[r][j] * V[i][j];
  5403.                 }
  5404.                 m[r][i] = sum * w[r];
  5405.             }
  5406.         } else {
  5407.             for ( i = 0; i < V.GetNumRows(); i++ ) {
  5408.                 m[r][i] = 0.0f;
  5409.             }
  5410.         }
  5411.     }
  5412. }
  5413.  
  5414. /*
  5415. ============
  5416. idMatX::Cholesky_Factor
  5417.  
  5418.   in-place Cholesky factorization: LL'
  5419.   L is a triangular matrix stored in the lower triangle.
  5420.   The upper triangle is not cleared.
  5421.   The initial matrix has to be symmetric positive definite.
  5422. ============
  5423. */
  5424. bool idMatX::Cholesky_Factor( void ) {
  5425.     int i, j, k;
  5426.     float *invSqrt;
  5427.     double sum;
  5428.  
  5429.     assert( numRows == numColumns );
  5430.  
  5431.     invSqrt = (float *) _alloca16( numRows * sizeof( float ) );
  5432.  
  5433.     for ( i = 0; i < numRows; i++ ) {
  5434.  
  5435.         for ( j = 0; j < i; j++ ) {
  5436.  
  5437.             sum = (*this)[i][j];
  5438.             for ( k = 0; k < j; k++ ) {
  5439.                 sum -= (*this)[i][k] * (*this)[j][k];
  5440.             }
  5441.             (*this)[i][j] = sum * invSqrt[j];
  5442.         }
  5443.  
  5444.         sum = (*this)[i][i];
  5445.         for ( k = 0; k < i; k++ ) {
  5446.             sum -= (*this)[i][k] * (*this)[i][k];
  5447.         }
  5448.  
  5449.         if ( sum <= 0.0f ) {
  5450.             return false;
  5451.         }
  5452.  
  5453.         invSqrt[i] = idMath::InvSqrt( sum );
  5454.         (*this)[i][i] = invSqrt[i] * sum;
  5455.     }
  5456.     return true;
  5457. }
  5458.  
  5459. /*
  5460. ============
  5461. idMatX::Cholesky_UpdateRankOne
  5462.  
  5463.   Updates the in-place Cholesky factorization to obtain the factors for the matrix: LL' + alpha * v * v'
  5464.   If offset > 0 only the lower right corner starting at (offset, offset) is updated.
  5465. ============
  5466. */
  5467. bool idMatX::Cholesky_UpdateRankOne( const idVecX &v, float alpha, int offset ) {
  5468.     int i, j;
  5469.     float *y;
  5470.     double diag, invDiag, diagSqr, newDiag, newDiagSqr, beta, p, d;
  5471.  
  5472.     assert( numRows == numColumns );
  5473.     assert( v.GetSize() >= numRows );
  5474.     assert( offset >= 0 && offset < numRows );
  5475.  
  5476.     y = (float *) _alloca16( v.GetSize() * sizeof( float ) );
  5477.     memcpy( y, v.ToFloatPtr(), v.GetSize() * sizeof( float ) );
  5478.  
  5479.     for ( i = offset; i < numColumns; i++ ) {
  5480.         p = y[i];
  5481.         diag = (*this)[i][i];
  5482.         invDiag = 1.0f / diag;
  5483.         diagSqr = diag * diag;
  5484.         newDiagSqr = diagSqr + alpha * p * p;
  5485.  
  5486.         if ( newDiagSqr <= 0.0f ) {
  5487.             return false;
  5488.         }
  5489.  
  5490.         (*this)[i][i] = newDiag = idMath::Sqrt( newDiagSqr );
  5491.  
  5492.         alpha /= newDiagSqr;
  5493.         beta = p * alpha;
  5494.         alpha *= diagSqr;
  5495.  
  5496.         for ( j = i+1; j < numRows; j++ ) {
  5497.  
  5498.             d = (*this)[j][i] * invDiag;
  5499.  
  5500.             y[j] -= p * d;
  5501.             d += beta * y[j];
  5502.  
  5503.             (*this)[j][i] = d * newDiag;
  5504.         }
  5505.     }
  5506.     return true;
  5507. }
  5508.  
  5509. /*
  5510. ============
  5511. idMatX::Cholesky_UpdateRowColumn
  5512.  
  5513.   Updates the in-place Cholesky factorization to obtain the factors for the matrix:
  5514.  
  5515.         [ 0  a  0 ]
  5516.   LL' + [ a  b  c ]
  5517.         [ 0  c  0 ]
  5518.  
  5519.   where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1]
  5520. ============
  5521. */
  5522. bool idMatX::Cholesky_UpdateRowColumn( const idVecX &v, int r ) {
  5523.  
  5524.     int i, j;
  5525. // RAVEN BEGIN
  5526. // jscott: VC7.1 fix
  5527.     float sum;
  5528. // RAVEN END
  5529.     float *original, *y;
  5530.     idVecX addSub;
  5531.  
  5532.     assert( numRows == numColumns );
  5533.     assert( v.GetSize() >= numRows );
  5534.     assert( r >= 0 && r < numRows );
  5535.  
  5536.     addSub.SetData( numColumns, (float *) _alloca16( numColumns * sizeof( float ) ) );
  5537.  
  5538.     if ( r == 0 ) {
  5539.  
  5540.         if ( numColumns == 1 ) {
  5541.             sum = (*this)[0][0];
  5542. // RAVEN BEGIN
  5543. // jscott: VC7.1 fix
  5544.             sum = ( sum * sum ) + v[0];
  5545. // RAVEN END
  5546.             if ( sum <= 0.0f ) {
  5547.                 return false;
  5548.             }
  5549.             (*this)[0][0] = idMath::Sqrt( sum );
  5550.             return true;
  5551.         }
  5552.         for ( i = 0; i < numColumns; i++ ) {
  5553.             addSub[i] = v[i];
  5554.         }
  5555.  
  5556.     } else {
  5557.  
  5558.         original = (float *) _alloca16( numColumns * sizeof( float ) );
  5559.         y = (float *) _alloca16( numColumns * sizeof( float ) );
  5560.  
  5561.         // calculate original row/column of matrix
  5562.         for ( i = 0; i < numRows; i++ ) {
  5563.             sum = 0.0f;
  5564.             for ( j = 0; j <= i; j++ ) {
  5565.                 sum += (*this)[r][j] * (*this)[i][j];
  5566.             }
  5567.             original[i] = sum;
  5568.         }
  5569.  
  5570.         // solve for y in L * y = original + v
  5571.         for ( i = 0; i < r; i++ ) {
  5572.             sum = original[i] + v[i];
  5573.             for ( j = 0; j < i; j++ ) {
  5574.                 sum -= (*this)[r][j] * (*this)[i][j];
  5575.             }
  5576.             (*this)[r][i] = sum / (*this)[i][i];
  5577.         }
  5578.  
  5579.         // if the last row/column of the matrix is updated
  5580.         if ( r == numColumns - 1 ) {
  5581.             // only calculate new diagonal
  5582.             sum = original[r] + v[r];
  5583.             for ( j = 0; j < r; j++) {
  5584.                 sum -= (*this)[r][j] * (*this)[r][j];
  5585.             }
  5586.             if ( sum <= 0.0f ) {
  5587.                 return false;
  5588.             }
  5589.             (*this)[r][r] = idMath::Sqrt( sum );
  5590.             return true;
  5591.         }
  5592.  
  5593.         // calculate the row/column to be added to the lower right sub matrix starting at (r, r)
  5594.         for ( i = r; i < numColumns; i++ ) {
  5595.             sum = 0.0f;
  5596.             for ( j = 0; j <= r; j++ ) {
  5597.                 sum += (*this)[r][j] * (*this)[i][j];
  5598.             }
  5599.             addSub[i] = v[i] - ( sum - original[i] );
  5600.         }
  5601.     }
  5602.  
  5603.     // add row/column to the lower right sub matrix starting at (r, r)
  5604.  
  5605. #if 0
  5606.  
  5607.     idVecX v1, v2;
  5608.     double d;
  5609.  
  5610.     v1.SetData( numColumns, (float *) _alloca16( numColumns * sizeof( float ) ) );
  5611.     v2.SetData( numColumns, (float *) _alloca16( numColumns * sizeof( float ) ) );
  5612.  
  5613.     d = idMath::SQRT_1OVER2;
  5614.     v1[r] = ( 0.5f * addSub[r] + 1.0f ) * d;
  5615.     v2[r] = ( 0.5f * addSub[r] - 1.0f ) * d;
  5616.     for ( i = r+1; i < numColumns; i++ ) {
  5617.         v1[i] = v2[i] = addSub[i] * d;
  5618.     }
  5619.  
  5620.     // update
  5621.     if ( !Cholesky_UpdateRankOne( v1, 1.0f, r ) ) {
  5622.         return false;
  5623.     }
  5624.     // downdate
  5625.     if ( !Cholesky_UpdateRankOne( v2, -1.0f, r ) ) {
  5626.         return false;
  5627.     }
  5628.  
  5629. #else
  5630.  
  5631.     float *v1, *v2;
  5632.     double diag, invDiag, diagSqr, newDiag, newDiagSqr;
  5633.     double alpha1, alpha2, beta1, beta2, p1, p2, d;
  5634.  
  5635.     v1 = (float *) _alloca16( numColumns * sizeof( float ) );
  5636.     v2 = (float *) _alloca16( numColumns * sizeof( float ) );
  5637.  
  5638.     d = idMath::SQRT_1OVER2;
  5639.     v1[r] = ( 0.5f * addSub[r] + 1.0f ) * d;
  5640.     v2[r] = ( 0.5f * addSub[r] - 1.0f ) * d;
  5641.     for ( i = r+1; i < numColumns; i++ ) {
  5642.         v1[i] = v2[i] = addSub[i] * d;
  5643.     }
  5644.  
  5645.     alpha1 = 1.0f;
  5646.     alpha2 = -1.0f;
  5647.  
  5648.     // simultaneous update/downdate of the sub matrix starting at (r, r)
  5649.     for ( i = r; i < numColumns; i++ ) {
  5650.         p1 = v1[i];
  5651.         diag = (*this)[i][i];
  5652.         invDiag = 1.0f / diag;
  5653.         diagSqr = diag * diag;
  5654.         newDiagSqr = diagSqr + alpha1 * p1 * p1;
  5655.  
  5656.         if ( newDiagSqr <= 0.0f ) {
  5657.             return false;
  5658.         }
  5659.  
  5660.         alpha1 /= newDiagSqr;
  5661.         beta1 = p1 * alpha1;
  5662.         alpha1 *= diagSqr;
  5663.  
  5664.         p2 = v2[i];
  5665.         diagSqr = newDiagSqr;
  5666.         newDiagSqr = diagSqr + alpha2 * p2 * p2;
  5667.  
  5668.         if ( newDiagSqr <= 0.0f ) {
  5669.             return false;
  5670.         }
  5671.  
  5672.         (*this)[i][i] = newDiag = idMath::Sqrt( newDiagSqr );
  5673.  
  5674.         alpha2 /= newDiagSqr;
  5675.         beta2 = p2 * alpha2;
  5676.         alpha2 *= diagSqr;
  5677.  
  5678.         for ( j = i+1; j < numRows; j++ ) {
  5679.  
  5680.             d = (*this)[j][i] * invDiag;
  5681.  
  5682.             v1[j] -= p1 * d;
  5683.             d += beta1 * v1[j];
  5684.  
  5685.             v2[j] -= p2 * d;
  5686.             d += beta2 * v2[j];
  5687.  
  5688.             (*this)[j][i] = d * newDiag;
  5689.         }
  5690.     }
  5691.  
  5692. #endif
  5693.     return true;
  5694. }
  5695.  
  5696. /*
  5697. ============
  5698. idMatX::Cholesky_UpdateIncrement
  5699.  
  5700.   Updates the in-place Cholesky factorization to obtain the factors for the matrix:
  5701.  
  5702.   [ A  a ]
  5703.   [ a  b ]
  5704.  
  5705.   where: a = v[0,numRows-1], b = v[numRows]
  5706. ============
  5707. */
  5708. bool idMatX::Cholesky_UpdateIncrement( const idVecX &v ) {
  5709.     int i, j;
  5710.     float *x;
  5711.     double sum;
  5712.  
  5713.     assert( numRows == numColumns );
  5714.     assert( v.GetSize() >= numRows+1 );
  5715.  
  5716.     ChangeSize( numRows+1, numColumns+1, false );
  5717.  
  5718.     x = (float *) _alloca16( numRows * sizeof( float ) );
  5719.  
  5720.     // solve for x in L * x = v
  5721.     for ( i = 0; i < numRows - 1; i++ ) {
  5722.         sum = v[i];
  5723.         for ( j = 0; j < i; j++ ) {
  5724.             sum -= (*this)[i][j] * x[j];
  5725.         }
  5726.         x[i] = sum / (*this)[i][i];
  5727.     }
  5728.  
  5729.     // calculate new row of L and calculate the square of the diagonal entry
  5730.     sum = v[numRows - 1];
  5731.     for ( i = 0; i < numRows - 1; i++ ) {
  5732.         (*this)[numRows - 1][i] = x[i];
  5733.         sum -= x[i] * x[i];
  5734.     }
  5735.  
  5736.     if ( sum <= 0.0f ) {
  5737.         return false;
  5738.     }
  5739.  
  5740.     // store the diagonal entry
  5741.     (*this)[numRows - 1][numRows - 1] = idMath::Sqrt( sum );
  5742.  
  5743.     return true;
  5744. }
  5745.  
  5746. /*
  5747. ============
  5748. idMatX::Cholesky_UpdateDecrement
  5749.  
  5750.   Updates the in-place Cholesky factorization to obtain the factors for the matrix with row r and column r removed.
  5751.   v should store the row of the original matrix.
  5752. ============
  5753. */
  5754. bool idMatX::Cholesky_UpdateDecrement( const idVecX &v, int r ) {
  5755.     idVecX v1;
  5756.  
  5757.     assert( numRows == numColumns );
  5758.     assert( v.GetSize() >= numRows );
  5759.     assert( r >= 0 && r < numRows );
  5760.  
  5761.     v1.SetData( numRows, VECX_ALLOCA( numRows ) );
  5762.  
  5763.     // update the row and column to identity
  5764.     v1 = -v;
  5765.     v1[r] += 1.0f;
  5766.  
  5767.     if ( !Cholesky_UpdateRowColumn( v1, r ) ) {
  5768.         return false;
  5769.     }
  5770.  
  5771.     // physically remove the row and column
  5772.     Update_Decrement( r );
  5773.  
  5774.     return true;
  5775. }
  5776.  
  5777. /*
  5778. ============
  5779. idMatX::Cholesky_Solve
  5780.  
  5781.   Solve Ax = b with A factored in-place as: LL'
  5782. ============
  5783. */
  5784. void idMatX::Cholesky_Solve( idVecX &x, const idVecX &b ) const {
  5785.     int i, j;
  5786.     double sum;
  5787.  
  5788.     assert( numRows == numColumns );
  5789.     assert( x.GetSize() >= numRows && b.GetSize() >= numRows );
  5790.  
  5791.     // solve L
  5792.     for ( i = 0; i < numRows; i++ ) {
  5793.         sum = b[i];
  5794.         for ( j = 0; j < i; j++ ) {
  5795.             sum -= (*this)[i][j] * x[j];
  5796.         }
  5797.         x[i] = sum / (*this)[i][i];
  5798.     }
  5799.  
  5800.     // solve Lt
  5801.     for ( i = numRows - 1; i >= 0; i-- ) {
  5802.         sum = x[i];
  5803.         for ( j = i + 1; j < numRows; j++ ) {
  5804.             sum -= (*this)[j][i] * x[j];
  5805.         }
  5806.         x[i] = sum / (*this)[i][i];
  5807.     }
  5808. }
  5809.  
  5810. /*
  5811. ============
  5812. idMatX::Cholesky_Inverse
  5813.  
  5814.   Calculates the inverse of the matrix which is factored in-place as: LL'
  5815. ============
  5816. */
  5817. void idMatX::Cholesky_Inverse( idMatX &inv ) const {
  5818.     int i, j;
  5819.     idVecX x, b;
  5820.  
  5821.     assert( numRows == numColumns );
  5822.  
  5823.     x.SetData( numRows, VECX_ALLOCA( numRows ) );
  5824.     b.SetData( numRows, VECX_ALLOCA( numRows ) );
  5825.     b.Zero();
  5826.     inv.SetSize( numRows, numColumns );
  5827.  
  5828.     for ( i = 0; i < numRows; i++ ) {
  5829.  
  5830.         b[i] = 1.0f;
  5831.         Cholesky_Solve( x, b );
  5832.         for ( j = 0; j < numRows; j++ ) {
  5833.             inv[j][i] = x[j];
  5834.         }
  5835.         b[i] = 0.0f;
  5836.     }
  5837. }
  5838.  
  5839. /*
  5840. ============
  5841. idMatX::Cholesky_MultiplyFactors
  5842.  
  5843.   Multiplies the factors of the in-place Cholesky factorization to form the original matrix.
  5844. ============
  5845. */
  5846. void idMatX::Cholesky_MultiplyFactors( idMatX &m ) const {
  5847.     int r, i, j;
  5848.     double sum;
  5849.  
  5850.     m.SetSize( numRows, numColumns );
  5851.  
  5852.     for ( r = 0; r < numRows; r++ ) {
  5853.  
  5854.         // calculate row of matrix
  5855.         for ( i = 0; i < numRows; i++ ) {
  5856.             sum = 0.0f;
  5857.             for ( j = 0; j <= i && j <= r; j++ ) {
  5858.                 sum += (*this)[r][j] * (*this)[i][j];
  5859.             }
  5860.             m[r][i] = sum;
  5861.         }
  5862.     }
  5863. }
  5864.  
  5865. /*
  5866. ============
  5867. idMatX::LDLT_Factor
  5868.  
  5869.   in-place factorization: LDL'
  5870.   L is a triangular matrix stored in the lower triangle.
  5871.   L has ones on the diagonal that are not stored.
  5872.   D is a diagonal matrix stored on the diagonal.
  5873.   The upper triangle is not cleared.
  5874.   The initial matrix has to be symmetric.
  5875. ============
  5876. */
  5877. bool idMatX::LDLT_Factor( void ) {
  5878.     int i, j, k;
  5879.     float *v;
  5880.     double d, sum;
  5881.  
  5882.     assert( numRows == numColumns );
  5883.  
  5884.     v = (float *) _alloca16( numRows * sizeof( float ) );
  5885.  
  5886.     for ( i = 0; i < numRows; i++ ) {
  5887.  
  5888.         sum = (*this)[i][i];
  5889.         for ( j = 0; j < i; j++ ) {
  5890.             d = (*this)[i][j];
  5891.             v[j] = (*this)[j][j] * d;
  5892.             sum -= v[j] * d;
  5893.         }
  5894.  
  5895.         if ( sum == 0.0f ) {
  5896.             return false;
  5897.         }
  5898.  
  5899.         (*this)[i][i] = sum;
  5900.         d = 1.0f / sum;
  5901.  
  5902.         for ( j = i + 1; j < numRows; j++ ) {
  5903.             sum = (*this)[j][i];
  5904.             for ( k = 0; k < i; k++ ) {
  5905.                 sum -= (*this)[j][k] * v[k];
  5906.             }
  5907.             (*this)[j][i] = sum * d;
  5908.         }
  5909.     }
  5910.  
  5911.     return true;
  5912. }
  5913.  
  5914. /*
  5915. ============
  5916. idMatX::LDLT_UpdateRankOne
  5917.  
  5918.   Updates the in-place LDL' factorization to obtain the factors for the matrix: LDL' + alpha * v * v'
  5919.   If offset > 0 only the lower right corner starting at (offset, offset) is updated.
  5920. ============
  5921. */
  5922. bool idMatX::LDLT_UpdateRankOne( const idVecX &v, float alpha, int offset ) {
  5923.     int i, j;
  5924.     float *y;
  5925.     double diag, newDiag, beta, p, d;
  5926.  
  5927.     assert( numRows == numColumns );
  5928.     assert( v.GetSize() >= numRows );
  5929.     assert( offset >= 0 && offset < numRows );
  5930.  
  5931.     y = (float *) _alloca16( v.GetSize() * sizeof( float ) );
  5932.     memcpy( y, v.ToFloatPtr(), v.GetSize() * sizeof( float ) );
  5933.  
  5934.     for ( i = offset; i < numColumns; i++ ) {
  5935.         p = y[i];
  5936.         diag = (*this)[i][i];
  5937.         (*this)[i][i] = newDiag = diag + alpha * p * p;
  5938.  
  5939.         if ( newDiag == 0.0f ) {
  5940.             return false;
  5941.         }
  5942.  
  5943.         alpha /= newDiag;
  5944.         beta = p * alpha;
  5945.         alpha *= diag;
  5946.  
  5947.         for ( j = i+1; j < numRows; j++ ) {
  5948.  
  5949.             d = (*this)[j][i];
  5950.  
  5951.             y[j] -= p * d;
  5952.             d += beta * y[j];
  5953.  
  5954.             (*this)[j][i] = d;
  5955.         }
  5956.     }
  5957.  
  5958.     return true;
  5959. }
  5960.  
  5961. /*
  5962. ============
  5963. idMatX::LDLT_UpdateRowColumn
  5964.  
  5965.   Updates the in-place LDL' factorization to obtain the factors for the matrix:
  5966.  
  5967.          [ 0  a  0 ]
  5968.   LDL' + [ a  b  c ]
  5969.          [ 0  c  0 ]
  5970.  
  5971.   where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1]
  5972. ============
  5973. */
  5974. bool idMatX::LDLT_UpdateRowColumn( const idVecX &v, int r ) {
  5975.     int i, j;
  5976.     double sum;
  5977.     float *original, *y;
  5978.     idVecX addSub;
  5979.  
  5980.     assert( numRows == numColumns );
  5981.     assert( v.GetSize() >= numRows );
  5982.     assert( r >= 0 && r < numRows );
  5983.  
  5984.     addSub.SetData( numColumns, (float *) _alloca16( numColumns * sizeof( float ) ) );
  5985.  
  5986.     if ( r == 0 ) {
  5987.  
  5988.         if ( numColumns == 1 ) {
  5989.             (*this)[0][0] += v[0];
  5990.             return true;
  5991.         }
  5992.         for ( i = 0; i < numColumns; i++ ) {
  5993.             addSub[i] = v[i];
  5994.         }
  5995.  
  5996.     } else {
  5997.  
  5998.         original = (float *) _alloca16( numColumns * sizeof( float ) );
  5999.         y = (float *) _alloca16( numColumns * sizeof( float ) );
  6000.  
  6001.         // calculate original row/column of matrix
  6002.         for ( i = 0; i < r; i++ ) {
  6003.             y[i] = (*this)[r][i] * (*this)[i][i];
  6004.         }
  6005.         for ( i = 0; i < numColumns; i++ ) {
  6006.             if ( i < r ) {
  6007.                 sum = (*this)[i][i] * (*this)[r][i];
  6008.             } else if ( i == r ) {
  6009.                 sum = (*this)[r][r];
  6010.             } else {
  6011.                 sum = (*this)[r][r] * (*this)[i][r];
  6012.             }
  6013.             for ( j = 0; j < i && j < r; j++ ) {
  6014.                 sum += (*this)[i][j] * y[j];
  6015.             }
  6016.             original[i] = sum;
  6017.         }
  6018.  
  6019.         // solve for y in L * y = original + v
  6020.         for ( i = 0; i < r; i++ ) {
  6021.             sum = original[i] + v[i];
  6022.             for ( j = 0; j < i; j++ ) {
  6023.                 sum -= (*this)[i][j] * y[j];
  6024.             }
  6025.             y[i] = sum;
  6026.         }
  6027.  
  6028.         // calculate new row of L
  6029.         for ( i = 0; i < r; i++ ) {
  6030.             (*this)[r][i] = y[i] / (*this)[i][i];
  6031.         }
  6032.  
  6033.         // if the last row/column of the matrix is updated
  6034.         if ( r == numColumns - 1 ) {
  6035.             // only calculate new diagonal
  6036.             sum = original[r] + v[r];
  6037.             for ( j = 0; j < r; j++ ) {
  6038.                 sum -= (*this)[r][j] * y[j];
  6039.             }
  6040.             if ( sum == 0.0f ) {
  6041.                 return false;
  6042.             }
  6043.             (*this)[r][r] = sum;
  6044.             return true;
  6045.         }
  6046.  
  6047.         // calculate the row/column to be added to the lower right sub matrix starting at (r, r)
  6048.         for ( i = 0; i < r; i++ ) {
  6049.             y[i] = (*this)[r][i] * (*this)[i][i];
  6050.         }
  6051.         for ( i = r; i < numColumns; i++ ) {
  6052.             if ( i == r ) {
  6053.                 sum = (*this)[r][r];
  6054.             } else {
  6055.                 sum = (*this)[r][r] * (*this)[i][r];
  6056.             }
  6057.             for ( j = 0; j < r; j++ ) {
  6058.                 sum += (*this)[i][j] * y[j];
  6059.             }
  6060.             addSub[i] = v[i] - ( sum - original[i] );
  6061.         }
  6062.     }
  6063.  
  6064.     // add row/column to the lower right sub matrix starting at (r, r)
  6065.  
  6066. #if 0
  6067.  
  6068.     idVecX v1, v2;
  6069.     double d;
  6070.  
  6071.     v1.SetData( numColumns, (float *) _alloca16( numColumns * sizeof( float ) ) );
  6072.     v2.SetData( numColumns, (float *) _alloca16( numColumns * sizeof( float ) ) );
  6073.  
  6074.     d = idMath::SQRT_1OVER2;
  6075.     v1[r] = ( 0.5f * addSub[r] + 1.0f ) * d;
  6076.     v2[r] = ( 0.5f * addSub[r] - 1.0f ) * d;
  6077.     for ( i = r+1; i < numColumns; i++ ) {
  6078.         v1[i] = v2[i] = addSub[i] * d;
  6079.     }
  6080.  
  6081.     // update
  6082.     if ( !LDLT_UpdateRankOne( v1, 1.0f, r ) ) {
  6083.         return false;
  6084.     }
  6085.     // downdate
  6086.     if ( !LDLT_UpdateRankOne( v2, -1.0f, r ) ) {
  6087.         return false;
  6088.     }
  6089.  
  6090. #else
  6091.  
  6092.     float *v1, *v2;
  6093.     double d, diag, newDiag, p1, p2, alpha1, alpha2, beta1, beta2;
  6094.  
  6095.     v1 = (float *) _alloca16( numColumns * sizeof( float ) );
  6096.     v2 = (float *) _alloca16( numColumns * sizeof( float ) );
  6097.  
  6098.     d = idMath::SQRT_1OVER2;
  6099.     v1[r] = ( 0.5f * addSub[r] + 1.0f ) * d;
  6100.     v2[r] = ( 0.5f * addSub[r] - 1.0f ) * d;
  6101.     for ( i = r+1; i < numColumns; i++ ) {
  6102.         v1[i] = v2[i] = addSub[i] * d;
  6103.     }
  6104.  
  6105.     alpha1 = 1.0f;
  6106.     alpha2 = -1.0f;
  6107.  
  6108.     // simultaneous update/downdate of the sub matrix starting at (r, r)
  6109.     for ( i = r; i < numColumns; i++ ) {
  6110.  
  6111.         diag = (*this)[i][i];
  6112.         p1 = v1[i];
  6113.         newDiag = diag + alpha1 * p1 * p1;
  6114.  
  6115.         if ( newDiag == 0.0f ) {
  6116.             return false;
  6117.         }
  6118.  
  6119.         alpha1 /= newDiag;
  6120.         beta1 = p1 * alpha1;
  6121.         alpha1 *= diag;
  6122.  
  6123.         diag = newDiag;
  6124.         p2 = v2[i];
  6125.         newDiag = diag + alpha2 * p2 * p2;
  6126.  
  6127.         if ( newDiag == 0.0f ) {
  6128.             return false;
  6129.         }
  6130.  
  6131.         alpha2 /= newDiag;
  6132.         beta2 = p2 * alpha2;
  6133.         alpha2 *= diag;
  6134.  
  6135.         (*this)[i][i] = newDiag;
  6136.  
  6137.         for ( j = i+1; j < numRows; j++ ) {
  6138.  
  6139.             d = (*this)[j][i];
  6140.  
  6141.             v1[j] -= p1 * d;
  6142.             d += beta1 * v1[j];
  6143.  
  6144.             v2[j] -= p2 * d;
  6145.             d += beta2 * v2[j];
  6146.  
  6147.             (*this)[j][i] = d;
  6148.         }
  6149.     }
  6150.  
  6151. #endif
  6152.  
  6153.     return true;
  6154. }
  6155.  
  6156. /*
  6157. ============
  6158. idMatX::LDLT_UpdateIncrement
  6159.  
  6160.   Updates the in-place LDL' factorization to obtain the factors for the matrix:
  6161.  
  6162.   [ A  a ]
  6163.   [ a  b ]
  6164.  
  6165.   where: a = v[0,numRows-1], b = v[numRows]
  6166. ============
  6167. */
  6168. bool idMatX::LDLT_UpdateIncrement( const idVecX &v ) {
  6169.     int i, j;
  6170.     float *x;
  6171.     double sum, d;
  6172.  
  6173.     assert( numRows == numColumns );
  6174.     assert( v.GetSize() >= numRows+1 );
  6175.  
  6176.     ChangeSize( numRows+1, numColumns+1, false );
  6177.  
  6178.     x = (float *) _alloca16( numRows * sizeof( float ) );
  6179.  
  6180.     // solve for x in L * x = v
  6181.     for ( i = 0; i < numRows - 1; i++ ) {
  6182.         sum = v[i];
  6183.         for ( j = 0; j < i; j++ ) {
  6184.             sum -= (*this)[i][j] * x[j];
  6185.         }
  6186.         x[i] = sum;
  6187.     }
  6188.  
  6189.     // calculate new row of L and calculate the diagonal entry
  6190.     sum = v[numRows - 1];
  6191.     for ( i = 0; i < numRows - 1; i++ ) {
  6192.         (*this)[numRows - 1][i] = d = x[i] / (*this)[i][i];
  6193.         sum -= d * x[i];
  6194.     }
  6195.  
  6196.     if ( sum == 0.0f ) {
  6197.         return false;
  6198.     }
  6199.  
  6200.     // store the diagonal entry
  6201.     (*this)[numRows - 1][numRows - 1] = sum;
  6202.  
  6203.     return true;
  6204. }
  6205.  
  6206. /*
  6207. ============
  6208. idMatX::LDLT_UpdateDecrement
  6209.  
  6210.   Updates the in-place LDL' factorization to obtain the factors for the matrix with row r and column r removed.
  6211.   v should store the row of the original matrix.
  6212. ============
  6213. */
  6214. bool idMatX::LDLT_UpdateDecrement( const idVecX &v, int r ) {
  6215.     idVecX v1;
  6216.  
  6217.     assert( numRows == numColumns );
  6218.     assert( v.GetSize() >= numRows );
  6219.     assert( r >= 0 && r < numRows );
  6220.  
  6221.     v1.SetData( numRows, VECX_ALLOCA( numRows ) );
  6222.  
  6223.     // update the row and column to identity
  6224.     v1 = -v;
  6225.     v1[r] += 1.0f;
  6226.  
  6227.     // NOTE:    msvc compiler bug: the this pointer stored in edi is expected to stay
  6228.     //            untouched when calling LDLT_UpdateRowColumn in the if statement
  6229. #if 0
  6230.     if ( !LDLT_UpdateRowColumn( v1, r ) ) {
  6231. #else
  6232.     bool ret = LDLT_UpdateRowColumn( v1, r );
  6233.     if ( !ret ) {
  6234. #endif
  6235.         return false;
  6236.     }
  6237.  
  6238.     // physically remove the row and column
  6239.     Update_Decrement( r );
  6240.  
  6241.     return true;
  6242. }
  6243.  
  6244. /*
  6245. ============
  6246. idMatX::LDLT_Solve
  6247.  
  6248.   Solve Ax = b with A factored in-place as: LDL'
  6249. ============
  6250. */
  6251. void idMatX::LDLT_Solve( idVecX &x, const idVecX &b ) const {
  6252.     int i, j;
  6253.     double sum;
  6254.  
  6255.     assert( numRows == numColumns );
  6256.     assert( x.GetSize() >= numRows && b.GetSize() >= numRows );
  6257.  
  6258.     // solve L
  6259.     for ( i = 0; i < numRows; i++ ) {
  6260.         sum = b[i];
  6261.         for ( j = 0; j < i; j++ ) {
  6262.             sum -= (*this)[i][j] * x[j];
  6263.         }
  6264.         x[i] = sum;
  6265.     }
  6266.  
  6267.     // solve D
  6268.     for ( i = 0; i < numRows; i++ ) {
  6269.         x[i] /= (*this)[i][i];
  6270.     }
  6271.  
  6272.     // solve Lt
  6273.     for ( i = numRows - 2; i >= 0; i-- ) {
  6274.         sum = x[i];
  6275.         for ( j = i + 1; j < numRows; j++ ) {
  6276.             sum -= (*this)[j][i] * x[j];
  6277.         }
  6278.         x[i] = sum;
  6279.     }
  6280. }
  6281.  
  6282. /*
  6283. ============
  6284. idMatX::LDLT_Inverse
  6285.  
  6286.   Calculates the inverse of the matrix which is factored in-place as: LDL'
  6287. ============
  6288. */
  6289. void idMatX::LDLT_Inverse( idMatX &inv ) const {
  6290.     int i, j;
  6291.     idVecX x, b;
  6292.  
  6293.     assert( numRows == numColumns );
  6294.  
  6295.     x.SetData( numRows, VECX_ALLOCA( numRows ) );
  6296.     b.SetData( numRows, VECX_ALLOCA( numRows ) );
  6297.     b.Zero();
  6298.     inv.SetSize( numRows, numColumns );
  6299.  
  6300.     for ( i = 0; i < numRows; i++ ) {
  6301.  
  6302.         b[i] = 1.0f;
  6303.         LDLT_Solve( x, b );
  6304.         for ( j = 0; j < numRows; j++ ) {
  6305.             inv[j][i] = x[j];
  6306.         }
  6307.         b[i] = 0.0f;
  6308.     }
  6309. }
  6310.  
  6311. /*
  6312. ============
  6313. idMatX::LDLT_UnpackFactors
  6314.  
  6315.   Unpacks the in-place LDL' factorization.
  6316. ============
  6317. */
  6318. void idMatX::LDLT_UnpackFactors( idMatX &L, idMatX &D ) const {
  6319.     int i, j;
  6320.  
  6321.     L.Zero( numRows, numColumns );
  6322.     D.Zero( numRows, numColumns );
  6323.     for ( i = 0; i < numRows; i++ ) {
  6324.         for ( j = 0; j < i; j++ ) {
  6325.             L[i][j] = (*this)[i][j];
  6326.         }
  6327.         L[i][i] = 1.0f;
  6328.         D[i][i] = (*this)[i][i];
  6329.     }
  6330. }
  6331.  
  6332. /*
  6333. ============
  6334. idMatX::LDLT_MultiplyFactors
  6335.  
  6336.   Multiplies the factors of the in-place LDL' factorization to form the original matrix.
  6337. ============
  6338. */
  6339. void idMatX::LDLT_MultiplyFactors( idMatX &m ) const {
  6340.     int r, i, j;
  6341.     float *v;
  6342.     double sum;
  6343.  
  6344.     v = (float *) _alloca16( numRows * sizeof( float ) );
  6345.     m.SetSize( numRows, numColumns );
  6346.  
  6347.     for ( r = 0; r < numRows; r++ ) {
  6348.  
  6349.         // calculate row of matrix
  6350.         for ( i = 0; i < r; i++ ) {
  6351.             v[i] = (*this)[r][i] * (*this)[i][i];
  6352.         }
  6353.         for ( i = 0; i < numColumns; i++ ) {
  6354.             if ( i < r ) {
  6355.                 sum = (*this)[i][i] * (*this)[r][i];
  6356.             } else if ( i == r ) {
  6357.                 sum = (*this)[r][r];
  6358.             } else {
  6359.                 sum = (*this)[r][r] * (*this)[i][r];
  6360.             }
  6361.             for ( j = 0; j < i && j < r; j++ ) {
  6362.                 sum += (*this)[i][j] * v[j];
  6363.             }
  6364.             m[r][i] = sum;
  6365.         }
  6366.     }
  6367. }
  6368.  
  6369. /*
  6370. ============
  6371. idMatX::TriDiagonal_ClearTriangles
  6372. ============
  6373. */
  6374. void idMatX::TriDiagonal_ClearTriangles( void ) {
  6375.     int i, j;
  6376.  
  6377.     assert( numRows == numColumns );
  6378.     for ( i = 0; i < numRows-2; i++ ) {
  6379.         for ( j = i+2; j < numColumns; j++ ) {
  6380.             (*this)[i][j] = 0.0f;
  6381.             (*this)[j][i] = 0.0f;
  6382.         }
  6383.     }
  6384. }
  6385.  
  6386. /*
  6387. ============
  6388. idMatX::TriDiagonal_Solve
  6389.  
  6390.   Solve Ax = b with A being tridiagonal.
  6391. ============
  6392. */
  6393. bool idMatX::TriDiagonal_Solve( idVecX &x, const idVecX &b ) const {
  6394.     int i;
  6395.     float d;
  6396.     idVecX tmp;
  6397.  
  6398.     assert( numRows == numColumns );
  6399.     assert( x.GetSize() >= numRows && b.GetSize() >= numRows );
  6400.  
  6401.     tmp.SetData( numRows, VECX_ALLOCA( numRows ) );
  6402.  
  6403.     d = (*this)[0][0];
  6404.     if ( d == 0.0f ) {
  6405.         return false;
  6406.     }
  6407.     d = 1.0f / d;
  6408.     x[0] = b[0] * d;
  6409.     for ( i = 1; i < numRows; i++ ) {
  6410.         tmp[i] = (*this)[i-1][i] * d;
  6411.         d = (*this)[i][i] - (*this)[i][i-1] * tmp[i];
  6412.         if ( d == 0.0f ) {
  6413.             return false;
  6414.         }
  6415.         d = 1.0f / d;
  6416.         x[i] = ( b[i] - (*this)[i][i-1] * x[i-1] ) * d;
  6417.     }
  6418.     for ( i = numRows - 2; i >= 0; i-- ) {
  6419.         x[i] -= tmp[i+1] * x[i+1];
  6420.     }
  6421.     return true;
  6422. }
  6423.  
  6424. /*
  6425. ============
  6426. idMatX::TriDiagonal_Inverse
  6427.  
  6428.   Calculates the inverse of a tri-diagonal matrix.
  6429. ============
  6430. */
  6431. void idMatX::TriDiagonal_Inverse( idMatX &inv ) const {
  6432.     int i, j;
  6433.     idVecX x, b;
  6434.  
  6435.     assert( numRows == numColumns );
  6436.  
  6437.     x.SetData( numRows, VECX_ALLOCA( numRows ) );
  6438.     b.SetData( numRows, VECX_ALLOCA( numRows ) );
  6439.     b.Zero();
  6440.     inv.SetSize( numRows, numColumns );
  6441.  
  6442.     for ( i = 0; i < numRows; i++ ) {
  6443.  
  6444.         b[i] = 1.0f;
  6445.         TriDiagonal_Solve( x, b );
  6446.         for ( j = 0; j < numRows; j++ ) {
  6447.             inv[j][i] = x[j];
  6448.         }
  6449.         b[i] = 0.0f;
  6450.     }
  6451. }
  6452.  
  6453. /*
  6454. ============
  6455. idMatX::HouseholderReduction
  6456.  
  6457.   Householder reduction to symmetric tri-diagonal form.
  6458.   The original matrix is replaced by an orthogonal matrix effecting the accumulated householder transformations.
  6459.   The diagonal elements of the diagonal matrix are stored in diag.
  6460.   The off-diagonal elements of the diagonal matrix are stored in subd.
  6461.   The initial matrix has to be symmetric.
  6462. ============
  6463. */
  6464. void idMatX::HouseholderReduction( idVecX &diag, idVecX &subd ) {
  6465.     int i0, i1, i2, i3;
  6466.     float h, f, g, invH, halfFdivH, scale, invScale, sum;
  6467.  
  6468.     assert( numRows == numColumns );
  6469.  
  6470.     diag.SetSize( numRows );
  6471.     subd.SetSize( numRows );
  6472.  
  6473.     for ( i0 = numRows-1, i3 = numRows-2; i0 >= 1; i0--, i3-- ) {
  6474.         h = 0.0f;
  6475.         scale = 0.0f;
  6476.  
  6477.         if ( i3 > 0 ) {
  6478.             for ( i2 = 0; i2 <= i3; i2++ ) {
  6479.                 scale += idMath::Fabs( (*this)[i0][i2] );
  6480.             }
  6481.             if ( scale == 0 ) {
  6482.                 subd[i0] = (*this)[i0][i3];
  6483.             } else {
  6484.                 invScale = 1.0f / scale;
  6485.                 for (i2 = 0; i2 <= i3; i2++)
  6486.                 {
  6487.                     (*this)[i0][i2] *= invScale;
  6488.                     h += (*this)[i0][i2] * (*this)[i0][i2];
  6489.                 }
  6490.                 f = (*this)[i0][i3];
  6491.                 g = idMath::Sqrt( h );
  6492.                 if ( f > 0.0f ) {
  6493.                     g = -g;
  6494.                 }
  6495.                 subd[i0] = scale * g;
  6496.                 h -= f * g;
  6497.                 (*this)[i0][i3] = f - g;
  6498.                 f = 0.0f;
  6499.                 invH = 1.0f / h;
  6500.                 for (i1 = 0; i1 <= i3; i1++) {
  6501.                     (*this)[i1][i0] = (*this)[i0][i1] * invH;
  6502.                     g = 0.0f;
  6503.                     for (i2 = 0; i2 <= i1; i2++) {
  6504.                         g += (*this)[i1][i2] * (*this)[i0][i2];
  6505.                     }
  6506.                     for (i2 = i1+1; i2 <= i3; i2++) {
  6507.                         g += (*this)[i2][i1] * (*this)[i0][i2];
  6508.                     }
  6509.                     subd[i1] = g * invH;
  6510.                     f += subd[i1] * (*this)[i0][i1];
  6511.                 }
  6512.                 halfFdivH = 0.5f * f * invH;
  6513.                 for ( i1 = 0; i1 <= i3; i1++ ) {
  6514.                     f = (*this)[i0][i1];
  6515.                     g = subd[i1] - halfFdivH * f;
  6516.                     subd[i1] = g;
  6517.                     for ( i2 = 0; i2 <= i1; i2++ ) {
  6518.                         (*this)[i1][i2] -= f * subd[i2] + g * (*this)[i0][i2];
  6519.                     }
  6520.                 }
  6521.             }
  6522.         } else {
  6523.             subd[i0] = (*this)[i0][i3];
  6524.         }
  6525.  
  6526.         diag[i0] = h;
  6527.     }
  6528.  
  6529.     diag[0] = 0.0f;
  6530.     subd[0] = 0.0f;
  6531.     for ( i0 = 0, i3 = -1; i0 <= numRows-1; i0++, i3++ ) {
  6532.         if ( diag[i0] ) {
  6533.             for ( i1 = 0; i1 <= i3; i1++ ) {
  6534.                 sum = 0.0f;
  6535.                 for (i2 = 0; i2 <= i3; i2++) {
  6536.                     sum += (*this)[i0][i2] * (*this)[i2][i1];
  6537.                 }
  6538.                 for ( i2 = 0; i2 <= i3; i2++ ) {
  6539.                     (*this)[i2][i1] -= sum * (*this)[i2][i0];
  6540.                 }
  6541.             }
  6542.         }
  6543.         diag[i0] = (*this)[i0][i0];
  6544.         (*this)[i0][i0] = 1.0f;
  6545.         for ( i1 = 0; i1 <= i3; i1++ ) {
  6546.             (*this)[i1][i0] = 0.0f;
  6547.             (*this)[i0][i1] = 0.0f;
  6548.         }
  6549.     }
  6550.  
  6551.     // re-order
  6552.     for ( i0 = 1, i3 = 0; i0 < numRows; i0++, i3++ ) {
  6553.         subd[i3] = subd[i0];
  6554.     }
  6555.     subd[numRows-1] = 0.0f;
  6556. }
  6557.  
  6558. /*
  6559. ============
  6560. idMatX::QL
  6561.  
  6562.   QL algorithm with implicit shifts to determine the eigenvalues and eigenvectors of a symmetric tri-diagonal matrix.
  6563.   diag contains the diagonal elements of the symmetric tri-diagonal matrix on input and is overwritten with the eigenvalues.
  6564.   subd contains the off-diagonal elements of the symmetric tri-diagonal matrix and is destroyed.
  6565.   This matrix has to be either the identity matrix to determine the eigenvectors for a symmetric tri-diagonal matrix,
  6566.   or the matrix returned by the Householder reduction to determine the eigenvalues for the original symmetric matrix.
  6567. ============
  6568. */
  6569. bool idMatX::QL( idVecX &diag, idVecX &subd ) {
  6570.     const int maxIter = 32;
  6571.     int i0, i1, i2, i3;
  6572.     float a, b, f, g, r, p, s, c;
  6573.  
  6574.     assert( numRows == numColumns );
  6575.  
  6576.     for ( i0 = 0; i0 < numRows; i0++ ) {
  6577.         for ( i1 = 0; i1 < maxIter; i1++ ) {
  6578.             for ( i2 = i0; i2 <= numRows - 2; i2++ ) {
  6579.                 a = idMath::Fabs( diag[i2] ) + idMath::Fabs( diag[i2+1] );
  6580.                 if ( idMath::Fabs( subd[i2] ) + a == a ) {
  6581.                     break;
  6582.                 }
  6583.             }
  6584.             if ( i2 == i0 ) {
  6585.                 break;
  6586.             }
  6587.  
  6588.             g = ( diag[i0+1] - diag[i0] ) / ( 2.0f * subd[i0] );
  6589.             r = idMath::Sqrt( g * g + 1.0f );
  6590.             if ( g < 0.0f ) {
  6591.                 g = diag[i2] - diag[i0] + subd[i0] / ( g - r );
  6592.             } else {
  6593.                 g = diag[i2] - diag[i0] + subd[i0] / ( g + r );
  6594.             }
  6595.             s = 1.0f;
  6596.             c = 1.0f;
  6597.             p = 0.0f;
  6598.             for ( i3 = i2 - 1; i3 >= i0; i3-- ) {
  6599.                 f = s * subd[i3];
  6600.                 b = c * subd[i3];
  6601.                 if ( idMath::Fabs( f ) >= idMath::Fabs( g ) ) {
  6602.                     c = g / f;
  6603.                     r = idMath::Sqrt( c * c + 1.0f );
  6604.                     subd[i3+1] = f * r;
  6605.                     s = 1.0f / r;
  6606.                     c *= s;
  6607.                 } else {
  6608.                     s = f / g;
  6609.                     r = idMath::Sqrt( s * s + 1.0f );
  6610.                     subd[i3+1] = g * r;
  6611.                     c = 1.0f / r;
  6612.                     s *= c;
  6613.                 }
  6614.                 g = diag[i3+1] - p;
  6615.                 r = ( diag[i3] - g ) * s + 2.0f * b * c;
  6616.                 p = s * r;
  6617.                 diag[i3+1] = g + p;
  6618.                 g = c * r - b;
  6619.  
  6620.                 for ( int i4 = 0; i4 < numRows; i4++ ) {
  6621.                     f = (*this)[i4][i3+1];
  6622.                     (*this)[i4][i3+1] = s * (*this)[i4][i3] + c * f;
  6623.                     (*this)[i4][i3] = c * (*this)[i4][i3] - s * f;
  6624.                 }
  6625.             }
  6626.             diag[i0] -= p;
  6627.             subd[i0] = g;
  6628.             subd[i2] = 0.0f;
  6629.         }
  6630.         if ( i1 == maxIter ) {
  6631.             return false;
  6632.         }
  6633.     }
  6634.     return true;
  6635. }
  6636.  
  6637. /*
  6638. ============
  6639. idMatX::Eigen_SolveSymmetricTriDiagonal
  6640.  
  6641.   Determine eigen values and eigen vectors for a symmetric tri-diagonal matrix.
  6642.   The eigen values are stored in 'eigenValues'.
  6643.   Column i of the original matrix will store the eigen vector corresponding to the eigenValues[i].
  6644.   The initial matrix has to be symmetric tri-diagonal.
  6645. ============
  6646. */
  6647. bool idMatX::Eigen_SolveSymmetricTriDiagonal( idVecX &eigenValues ) {
  6648.     int i;
  6649.     idVecX subd;
  6650.  
  6651.     assert( numRows == numColumns );
  6652.  
  6653.     subd.SetData( numRows, VECX_ALLOCA( numRows ) );
  6654.     eigenValues.SetSize( numRows );
  6655.  
  6656.     for ( i = 0; i < numRows-1; i++ ) {
  6657.         eigenValues[i] = (*this)[i][i];
  6658.         subd[i] = (*this)[i+1][i];
  6659.     }
  6660.     eigenValues[numRows-1] = (*this)[numRows-1][numRows-1];
  6661.  
  6662.     Identity();
  6663.  
  6664.     return QL( eigenValues, subd );
  6665. }
  6666.  
  6667. /*
  6668. ============
  6669. idMatX::Eigen_SolveSymmetric
  6670.  
  6671.   Determine eigen values and eigen vectors for a symmetric matrix.
  6672.   The eigen values are stored in 'eigenValues'.
  6673.   Column i of the original matrix will store the eigen vector corresponding to the eigenValues[i].
  6674.   The initial matrix has to be symmetric.
  6675. ============
  6676. */
  6677. bool idMatX::Eigen_SolveSymmetric( idVecX &eigenValues ) {
  6678.     idVecX subd;
  6679.  
  6680.     assert( numRows == numColumns );
  6681.  
  6682.     subd.SetData( numRows, VECX_ALLOCA( numRows ) );
  6683.     eigenValues.SetSize( numRows );
  6684.  
  6685.     HouseholderReduction( eigenValues, subd );
  6686.     return QL( eigenValues, subd );
  6687. }
  6688.  
  6689. /*
  6690. ============
  6691. idMatX::HessenbergReduction
  6692.  
  6693.   Reduction to Hessenberg form.
  6694. ============
  6695. */
  6696. void idMatX::HessenbergReduction( idMatX &H ) {
  6697.     int i, j, m;
  6698.     int low = 0;
  6699.     int high = numRows - 1;
  6700.     float scale, f, g, h;
  6701.     idVecX v;
  6702.  
  6703.     v.SetData( numRows, VECX_ALLOCA( numRows ) );
  6704.  
  6705.     for ( m = low + 1; m <= high - 1; m++ ) {
  6706.  
  6707.         scale = 0.0f;
  6708.         for ( i = m; i <= high; i++ ) {
  6709.             scale = scale + idMath::Fabs( H[i][m-1] );
  6710.         }
  6711.         if ( scale != 0.0f ) {
  6712.  
  6713.             // compute Householder transformation.
  6714.             h = 0.0f;
  6715.             for ( i = high; i >= m; i-- ) {
  6716.                 v[i] = H[i][m-1] / scale;
  6717.                 h += v[i] * v[i];
  6718.             }
  6719.             g = idMath::Sqrt( h );
  6720.             if ( v[m] > 0.0f ) {
  6721.                 g = -g;
  6722.             }
  6723.             h = h - v[m] * g;
  6724.             v[m] = v[m] - g;
  6725.  
  6726.             // apply Householder similarity transformation
  6727.             // H = (I-u*u'/h)*H*(I-u*u')/h)
  6728.             for ( j = m; j < numRows; j++) {
  6729.                 f = 0.0f;
  6730.                 for ( i = high; i >= m; i-- ) {
  6731.                     f += v[i] * H[i][j];
  6732.                 }
  6733.                 f = f / h;
  6734.                 for ( i = m; i <= high; i++ ) {
  6735.                     H[i][j] -= f * v[i];
  6736.                 }
  6737.             }
  6738.  
  6739.             for ( i = 0; i <= high; i++ ) {
  6740.                 f = 0.0f;
  6741.                 for ( j = high; j >= m; j-- ) {
  6742.                     f += v[j] * H[i][j];
  6743.                 }
  6744.                 f = f / h;
  6745.                 for ( j = m; j <= high; j++ ) {
  6746.                     H[i][j] -= f * v[j];
  6747.                 }
  6748.             }
  6749.             v[m] = scale * v[m];
  6750.             H[m][m-1] = scale * g;
  6751.         }
  6752.     }
  6753.  
  6754.     // accumulate transformations
  6755.     Identity();
  6756.     for ( int m = high - 1; m >= low + 1; m-- ) {
  6757.         if ( H[m][m-1] != 0.0f ) {
  6758.             for ( i = m + 1; i <= high; i++ ) {
  6759.                 v[i] = H[i][m-1];
  6760.             }
  6761.             for ( j = m; j <= high; j++ ) {
  6762.                 g = 0.0f;
  6763.                 for ( i = m; i <= high; i++ ) {
  6764.                     g += v[i] * (*this)[i][j];
  6765.                 }
  6766.                 // float division to avoid possible underflow
  6767.                 g = ( g / v[m] ) / H[m][m-1];
  6768.                 for ( i = m; i <= high; i++ ) {
  6769.                     (*this)[i][j] += g * v[i];
  6770.                 }
  6771.             }
  6772.         }
  6773.     }
  6774. }
  6775.  
  6776. /*
  6777. ============
  6778. idMatX::ComplexDivision
  6779.  
  6780.   Complex scalar division.
  6781. ============
  6782. */
  6783. void idMatX::ComplexDivision( float xr, float xi, float yr, float yi, float &cdivr, float &cdivi ) {
  6784.     float r, d;
  6785.     if ( idMath::Fabs( yr ) > idMath::Fabs( yi ) ) {
  6786.         r = yi / yr;
  6787.         d = yr + r * yi;
  6788.         cdivr = ( xr + r * xi ) / d;
  6789.         cdivi = ( xi - r * xr ) / d;
  6790.     } else {
  6791.         r = yr / yi;
  6792.         d = yi + r * yr;
  6793.         cdivr = ( r * xr + xi ) / d;
  6794.         cdivi = ( r * xi - xr ) / d;
  6795.     }
  6796. }
  6797.  
  6798. /*
  6799. ============
  6800. idMatX::HessenbergToRealSchur
  6801.  
  6802.   Reduction from Hessenberg to real Schur form.
  6803. ============
  6804. */
  6805. bool idMatX::HessenbergToRealSchur( idMatX &H, idVecX &realEigenValues, idVecX &imaginaryEigenValues ) {
  6806.     int i, j, k;
  6807.     int n = numRows - 1;
  6808.     int low = 0;
  6809.     int high = numRows - 1;
  6810.     float eps = 2e-16f, exshift = 0.0f;
  6811.     float p = 0.0f, q = 0.0f, r = 0.0f, s = 0.0f, z = 0.0f, t, w, x, y;
  6812.  
  6813.     // store roots isolated by balanc and compute matrix norm
  6814.     float norm = 0.0f;
  6815.     for ( i = 0; i < numRows; i++ ) {
  6816.         if ( i < low || i > high ) {
  6817.             realEigenValues[i] = H[i][i];
  6818.             imaginaryEigenValues[i] = 0.0f;
  6819.         }
  6820.         for ( j = Max( i - 1, 0 ); j < numRows; j++ ) {
  6821.             norm = norm + idMath::Fabs( H[i][j] );
  6822.         }
  6823.     }
  6824.  
  6825.     int iter = 0;
  6826.     while( n >= low ) {
  6827.  
  6828.         // look for single small sub-diagonal element
  6829.         int l = n;
  6830.         while ( l > low ) {
  6831.             s = idMath::Fabs( H[l-1][l-1] ) + idMath::Fabs( H[l][l] );
  6832.             if ( s == 0.0f ) {
  6833.                 s = norm;
  6834.             }
  6835.             if ( idMath::Fabs( H[l][l-1] ) < eps * s ) {
  6836.                 break;
  6837.             }
  6838.             l--;
  6839.         }
  6840.        
  6841.         // check for convergence
  6842.         if ( l == n ) {            // one root found
  6843.             H[n][n] = H[n][n] + exshift;
  6844.             realEigenValues[n] = H[n][n];
  6845.             imaginaryEigenValues[n] = 0.0f;
  6846.             n--;
  6847.             iter = 0;
  6848.         } else if ( l == n-1 ) {    // two roots found
  6849.             w = H[n][n-1] * H[n-1][n];
  6850.             p = ( H[n-1][n-1] - H[n][n] ) / 2.0f;
  6851.             q = p * p + w;
  6852.             z = idMath::Sqrt( idMath::Fabs( q ) );
  6853.             H[n][n] = H[n][n] + exshift;
  6854.             H[n-1][n-1] = H[n-1][n-1] + exshift;
  6855.             x = H[n][n];
  6856.  
  6857.             if ( q >= 0.0f ) {        // real pair
  6858.                 if ( p >= 0.0f ) {
  6859.                     z = p + z;
  6860.                 } else {
  6861.                     z = p - z;
  6862.                 }
  6863.                 realEigenValues[n-1] = x + z;
  6864.                 realEigenValues[n] = realEigenValues[n-1];
  6865.                 if ( z != 0.0f ) {
  6866.                     realEigenValues[n] = x - w / z;
  6867.                 }
  6868.                 imaginaryEigenValues[n-1] = 0.0f;
  6869.                 imaginaryEigenValues[n] = 0.0f;
  6870.                 x = H[n][n-1];
  6871.                 s = idMath::Fabs( x ) + idMath::Fabs( z );
  6872.                 p = x / s;
  6873.                 q = z / s;
  6874.                 r = idMath::Sqrt( p * p + q * q );
  6875.                 p = p / r;
  6876.                 q = q / r;
  6877.  
  6878.                 // modify row
  6879.                 for ( j = n-1; j < numRows; j++ ) {
  6880.                     z = H[n-1][j];
  6881.                     H[n-1][j] = q * z + p * H[n][j];
  6882.                     H[n][j] = q * H[n][j] - p * z;
  6883.                 }
  6884.  
  6885.                 // modify column
  6886.                 for ( i = 0; i <= n; i++ ) {
  6887.                     z = H[i][n-1];
  6888.                     H[i][n-1] = q * z + p * H[i][n];
  6889.                     H[i][n] = q * H[i][n] - p * z;
  6890.                 }
  6891.  
  6892.                 // accumulate transformations
  6893.                 for ( i = low; i <= high; i++ ) {
  6894.                     z = (*this)[i][n-1];
  6895.                     (*this)[i][n-1] = q * z + p * (*this)[i][n];
  6896.                     (*this)[i][n] = q * (*this)[i][n] - p * z;
  6897.                 }
  6898.             } else {        // complex pair
  6899.                 realEigenValues[n-1] = x + p;
  6900.                 realEigenValues[n] = x + p;
  6901.                 imaginaryEigenValues[n-1] = z;
  6902.                 imaginaryEigenValues[n] = -z;
  6903.             }
  6904.             n = n - 2;
  6905.             iter = 0;
  6906.  
  6907.         } else {    // no convergence yet
  6908.  
  6909.             // form shift
  6910.             x = H[n][n];
  6911.             y = 0.0f;
  6912.             w = 0.0f;
  6913.             if ( l < n ) {
  6914.                 y = H[n-1][n-1];
  6915.                 w = H[n][n-1] * H[n-1][n];
  6916.             }
  6917.  
  6918.             // Wilkinson's original ad hoc shift
  6919.             if ( iter == 10 ) {
  6920.                 exshift += x;
  6921.                 for ( i = low; i <= n; i++ ) {
  6922.                     H[i][i] -= x;
  6923.                 }
  6924.                 s = idMath::Fabs( H[n][n-1] ) + idMath::Fabs( H[n-1][n-2] );
  6925.                 x = y = 0.75f * s;
  6926.                 w = -0.4375f * s * s;
  6927.             }
  6928.  
  6929.             // new ad hoc shift
  6930.             if ( iter == 30 ) {
  6931.                 s = ( y - x ) / 2.0f;
  6932.                 s = s * s + w;
  6933.                 if ( s > 0 ) {
  6934.                     s = idMath::Sqrt( s );
  6935.                     if ( y < x ) {
  6936.                         s = -s;
  6937.                     }
  6938.                     s = x - w / ( ( y - x ) / 2.0f + s );
  6939.                     for ( i = low; i <= n; i++ ) {
  6940.                         H[i][i] -= s;
  6941.                     }
  6942.                     exshift += s;
  6943.                     x = y = w = 0.964f;
  6944.                 }
  6945.             }
  6946.  
  6947.             iter = iter + 1;
  6948.  
  6949.             // look for two consecutive small sub-diagonal elements
  6950.             int m;
  6951.             for( m = n-2; m >= l; m-- ) {
  6952.                 z = H[m][m];
  6953.                 r = x - z;
  6954.                 s = y - z;
  6955.                 p = ( r * s - w ) / H[m+1][m] + H[m][m+1];
  6956.                 q = H[m+1][m+1] - z - r - s;
  6957.                 r = H[m+2][m+1];
  6958.                 s = idMath::Fabs( p ) + idMath::Fabs( q ) + idMath::Fabs( r );
  6959.                 p = p / s;
  6960.                 q = q / s;
  6961.                 r = r / s;
  6962.                 if ( m == l ) {
  6963.                     break;
  6964.                 }
  6965.                 if ( idMath::Fabs( H[m][m-1] ) * ( idMath::Fabs( q ) + idMath::Fabs( r ) ) <
  6966.                         eps * ( idMath::Fabs( p ) * ( idMath::Fabs( H[m-1][m-1] ) + idMath::Fabs( z ) + idMath::Fabs( H[m+1][m+1] ) ) ) ) {
  6967.                     break;
  6968.                 }
  6969.             }
  6970.  
  6971.             for ( i = m+2; i <= n; i++ ) {
  6972.                 H[i][i-2] = 0.0f;
  6973.                 if ( i > m+2 ) {
  6974.                     H[i][i-3] = 0.0f;
  6975.                 }
  6976.             }
  6977.  
  6978.             // double QR step involving rows l:n and columns m:n
  6979.             for ( k = m; k <= n-1; k++ ) {
  6980.                 bool notlast = ( k != n-1 );
  6981.                 if ( k != m ) {
  6982.                     p = H[k][k-1];
  6983.                     q = H[k+1][k-1];
  6984.                     r = ( notlast ? H[k+2][k-1] : 0.0f );
  6985.                     x = idMath::Fabs( p ) + idMath::Fabs( q ) + idMath::Fabs( r );
  6986.                     if ( x != 0.0f ) {
  6987.                         p = p / x;
  6988.                         q = q / x;
  6989.                         r = r / x;
  6990.                     }
  6991.                 }
  6992.                 if ( x == 0.0f ) {
  6993.                     break;
  6994.                 }
  6995.                 s = idMath::Sqrt( p * p + q * q + r * r );
  6996.                 if ( p < 0.0f ) {
  6997.                     s = -s;
  6998.                 }
  6999.                 if ( s != 0.0f ) {
  7000.                     if ( k != m ) {
  7001.                         H[k][k-1] = -s * x;
  7002.                     } else if ( l != m ) {
  7003.                         H[k][k-1] = -H[k][k-1];
  7004.                     }
  7005.                     p = p + s;
  7006.                     x = p / s;
  7007.                     y = q / s;
  7008.                     z = r / s;
  7009.                     q = q / p;
  7010.                     r = r / p;
  7011.  
  7012.                     // modify row
  7013.                     for ( j = k; j < numRows; j++ ) {
  7014.                         p = H[k][j] + q * H[k+1][j];
  7015.                         if ( notlast ) {
  7016.                             p = p + r * H[k+2][j];
  7017.                             H[k+2][j] = H[k+2][j] - p * z;
  7018.                         }
  7019.                         H[k][j] = H[k][j] - p * x;
  7020.                         H[k+1][j] = H[k+1][j] - p * y;
  7021.                     }
  7022.  
  7023.                     // modify column
  7024.                     for ( i = 0; i <= Min( n, k + 3 ); i++ ) {
  7025.                         p = x * H[i][k] + y * H[i][k+1];
  7026.                         if ( notlast ) {
  7027.                             p = p + z * H[i][k+2];
  7028.                             H[i][k+2] = H[i][k+2] - p * r;
  7029.                         }
  7030.                         H[i][k] = H[i][k] - p;
  7031.                         H[i][k+1] = H[i][k+1] - p * q;
  7032.                     }
  7033.  
  7034.                     // accumulate transformations
  7035.                     for ( i = low; i <= high; i++ ) {
  7036.                         p = x * (*this)[i][k] + y * (*this)[i][k+1];
  7037.                         if ( notlast ) {
  7038.                             p = p + z * (*this)[i][k+2];
  7039.                             (*this)[i][k+2] = (*this)[i][k+2] - p * r;
  7040.                         }
  7041.                         (*this)[i][k] = (*this)[i][k] - p;
  7042.                         (*this)[i][k+1] = (*this)[i][k+1] - p * q;
  7043.                     }
  7044.                 }
  7045.             }
  7046.         }
  7047.     }
  7048.     
  7049.     // backsubstitute to find vectors of upper triangular form
  7050.     if ( norm == 0.0f ) {
  7051.         return false;
  7052.     }
  7053.  
  7054.     for ( n = numRows-1; n >= 0; n-- ) {
  7055.         p = realEigenValues[n];
  7056.         q = imaginaryEigenValues[n];
  7057.  
  7058.         if ( q == 0.0f ) {        // real vector
  7059.             int l = n;
  7060.             H[n][n] = 1.0f;
  7061.             for ( i = n-1; i >= 0; i-- ) {
  7062.                 w = H[i][i] - p;
  7063.                 r = 0.0f;
  7064.                 for ( j = l; j <= n; j++ ) {
  7065.                     r = r + H[i][j] * H[j][n];
  7066.                 }
  7067.                 if ( imaginaryEigenValues[i] < 0.0f ) {
  7068.                     z = w;
  7069.                     s = r;
  7070.                 } else {
  7071.                     l = i;
  7072.                     if ( imaginaryEigenValues[i] == 0.0f ) {
  7073.                         if ( w != 0.0f ) {
  7074.                             H[i][n] = -r / w;
  7075.                         } else {
  7076.                             H[i][n] = -r / ( eps * norm );
  7077.                         }
  7078.                     } else {        // solve real equations
  7079.                         x = H[i][i+1];
  7080.                         y = H[i+1][i];
  7081.                         q = ( realEigenValues[i] - p ) * ( realEigenValues[i] - p ) + imaginaryEigenValues[i] * imaginaryEigenValues[i];
  7082.                         t = ( x * s - z * r ) / q;
  7083.                         H[i][n] = t;
  7084.                         if ( idMath::Fabs(x) > idMath::Fabs( z ) ) {
  7085.                             H[i+1][n] = ( -r - w * t ) / x;
  7086.                         } else {
  7087.                             H[i+1][n] = ( -s - y * t ) / z;
  7088.                         }
  7089.                     }
  7090.  
  7091.                     // overflow control
  7092.                     t = idMath::Fabs(H[i][n]);
  7093.                     if ( ( eps * t ) * t > 1 ) {
  7094.                         for ( j = i; j <= n; j++ ) {
  7095.                             H[j][n] = H[j][n] / t;
  7096.                         }
  7097.                     }
  7098.                 }
  7099.             }
  7100.         } else if ( q < 0.0f ) {    // complex vector
  7101.             int l = n-1;
  7102.  
  7103.             // last vector component imaginary so matrix is triangular
  7104.             if ( idMath::Fabs( H[n][n-1] ) > idMath::Fabs( H[n-1][n] ) ) {
  7105.                 H[n-1][n-1] = q / H[n][n-1];
  7106.                 H[n-1][n] = -( H[n][n] - p ) / H[n][n-1];
  7107.             } else {
  7108.                 ComplexDivision( 0.0f, -H[n-1][n], H[n-1][n-1]-p, q, H[n-1][n-1], H[n-1][n] );
  7109.             }
  7110.             H[n][n-1] = 0.0f;
  7111.             H[n][n] = 1.0f;
  7112.             for ( i = n-2; i >= 0; i-- ) {
  7113.                 float ra, sa, vr, vi;
  7114.                 ra = 0.0f;
  7115.                 sa = 0.0f;
  7116.                 for ( j = l; j <= n; j++ ) {
  7117.                     ra = ra + H[i][j] * H[j][n-1];
  7118.                     sa = sa + H[i][j] * H[j][n];
  7119.                 }
  7120.                 w = H[i][i] - p;
  7121.  
  7122.                 if ( imaginaryEigenValues[i] < 0.0f ) {
  7123.                     z = w;
  7124.                     r = ra;
  7125.                     s = sa;
  7126.                 } else {
  7127.                     l = i;
  7128.                     if ( imaginaryEigenValues[i] == 0.0f ) {
  7129.                         ComplexDivision( -ra, -sa, w, q, H[i][n-1], H[i][n] );
  7130.                     } else {
  7131.                         // solve complex equations
  7132.                         x = H[i][i+1];
  7133.                         y = H[i+1][i];
  7134.                         vr = ( realEigenValues[i] - p ) * ( realEigenValues[i] - p ) + imaginaryEigenValues[i] * imaginaryEigenValues[i] - q * q;
  7135.                         vi = ( realEigenValues[i] - p ) * 2.0f * q;
  7136.                         if ( vr == 0.0f && vi == 0.0f ) {
  7137.                             vr = eps * norm * ( idMath::Fabs( w ) + idMath::Fabs( q ) + idMath::Fabs( x ) + idMath::Fabs( y ) + idMath::Fabs( z ) );
  7138.                         }
  7139.                         ComplexDivision( x * r - z * ra + q * sa, x * s - z * sa - q * ra, vr, vi, H[i][n-1], H[i][n] );
  7140.                         if ( idMath::Fabs( x ) > ( idMath::Fabs( z ) + idMath::Fabs( q ) ) ) {
  7141.                             H[i+1][n-1] = ( -ra - w * H[i][n-1] + q * H[i][n] ) / x;
  7142.                             H[i+1][n] = ( -sa - w * H[i][n] - q * H[i][n-1] ) / x;
  7143.                         } else {
  7144.                             ComplexDivision( -r - y * H[i][n-1], -s - y * H[i][n], z, q, H[i+1][n-1], H[i+1][n] );
  7145.                         }
  7146.                     }
  7147.  
  7148.                     // overflow control
  7149.                     t = Max( idMath::Fabs( H[i][n-1] ), idMath::Fabs( H[i][n] ) );
  7150.                     if ( ( eps * t ) * t > 1 ) {
  7151.                         for ( j = i; j <= n; j++ ) {
  7152.                             H[j][n-1] = H[j][n-1] / t;
  7153.                             H[j][n] = H[j][n] / t;
  7154.                         }
  7155.                     }
  7156.                 }
  7157.             }
  7158.         }
  7159.     }
  7160.  
  7161.     // vectors of isolated roots
  7162.     for ( i = 0; i < numRows; i++ ) {
  7163.         if ( i < low || i > high ) {
  7164.             for ( j = i; j < numRows; j++ ) {
  7165.                 (*this)[i][j] = H[i][j];
  7166.             }
  7167.         }
  7168.     }
  7169.  
  7170.     // back transformation to get eigenvectors of original matrix
  7171.     for ( j = numRows - 1; j >= low; j-- ) {
  7172.         for ( i = low; i <= high; i++ ) {
  7173.             z = 0.0f;
  7174.             for ( k = low; k <= Min( j, high ); k++ ) {
  7175.                 z = z + (*this)[i][k] * H[k][j];
  7176.             }
  7177.             (*this)[i][j] = z;
  7178.         }
  7179.     }
  7180.  
  7181.     return true;
  7182. }
  7183.  
  7184. /*
  7185. ============
  7186. idMatX::Eigen_Solve
  7187.  
  7188.   Determine eigen values and eigen vectors for a square matrix.
  7189.   The eigen values are stored in 'realEigenValues' and 'imaginaryEigenValues'.
  7190.   Column i of the original matrix will store the eigen vector corresponding to the realEigenValues[i] and imaginaryEigenValues[i].
  7191. ============
  7192. */
  7193. bool idMatX::Eigen_Solve( idVecX &realEigenValues, idVecX &imaginaryEigenValues ) {
  7194.     idMatX H;
  7195.  
  7196.     assert( numRows == numColumns );
  7197.  
  7198.     realEigenValues.SetSize( numRows );
  7199.     imaginaryEigenValues.SetSize( numRows );
  7200.  
  7201.     H = *this;
  7202.  
  7203.     // reduce to Hessenberg form
  7204.     HessenbergReduction( H );
  7205.  
  7206.     // reduce Hessenberg to real Schur form
  7207.     return HessenbergToRealSchur( H, realEigenValues, imaginaryEigenValues );
  7208. }
  7209.  
  7210. /*
  7211. ============
  7212. idMatX::Eigen_SortIncreasing
  7213. ============
  7214. */
  7215. void idMatX::Eigen_SortIncreasing( idVecX &eigenValues ) {
  7216.     int i, j, k;
  7217.     float min;
  7218.  
  7219.     for ( i = 0, j; i <= numRows - 2; i++ ) {
  7220.         j = i;
  7221.         min = eigenValues[j];
  7222.         for ( k = i + 1; k < numRows; k++ ) {
  7223.             if ( eigenValues[k] < min ) {
  7224.                 j = k;
  7225.                 min = eigenValues[j];
  7226.             }
  7227.         }
  7228.         if ( j != i ) {
  7229.             eigenValues.SwapElements( i, j );
  7230.             SwapColumns( i, j );
  7231.         }
  7232.     }
  7233. }
  7234.  
  7235. /*
  7236. ============
  7237. idMatX::Eigen_SortDecreasing
  7238. ============
  7239. */
  7240. void idMatX::Eigen_SortDecreasing( idVecX &eigenValues ) {
  7241.     int i, j, k;
  7242.     float max;
  7243.  
  7244.     for ( i = 0, j; i <= numRows - 2; i++ ) {
  7245.         j = i;
  7246.         max = eigenValues[j];
  7247.         for ( k = i + 1; k < numRows; k++ ) {
  7248.             if ( eigenValues[k] > max ) {
  7249.                 j = k;
  7250.                 max = eigenValues[j];
  7251.             }
  7252.         }
  7253.         if ( j != i ) {
  7254.             eigenValues.SwapElements( i, j );
  7255.             SwapColumns( i, j );
  7256.         }
  7257.     }
  7258. }
  7259.  
  7260. /*
  7261. ============
  7262. idMatX::DeterminantGeneric
  7263. ============
  7264. */
  7265. float idMatX::DeterminantGeneric( void ) const {
  7266.     int *index;
  7267.     float det;
  7268.     idMatX tmp;
  7269.  
  7270.     index = (int *) _alloca16( numRows * sizeof( int ) );
  7271.     tmp.SetData( numRows, numColumns, MATX_ALLOCA( numRows * numColumns ) );
  7272.     tmp = *this;
  7273.  
  7274.     if ( !tmp.LU_Factor( index, &det ) ) {
  7275.         return 0.0f;
  7276.     }
  7277.  
  7278.     return det;
  7279. }
  7280.  
  7281. /*
  7282. ============
  7283. idMatX::InverseSelfGeneric
  7284. ============
  7285. */
  7286. bool idMatX::InverseSelfGeneric( void ) {
  7287.     int i, j, *index;
  7288.     idMatX tmp;
  7289.     idVecX x, b;
  7290.  
  7291.     index = (int *) _alloca16( numRows * sizeof( int ) );
  7292.     tmp.SetData( numRows, numColumns, MATX_ALLOCA( numRows * numColumns ) );
  7293.     tmp = *this;
  7294.  
  7295.     if ( !tmp.LU_Factor( index ) ) {
  7296.         return false;
  7297.     }
  7298.  
  7299.     x.SetData( numRows, VECX_ALLOCA( numRows ) );
  7300.     b.SetData( numRows, VECX_ALLOCA( numRows ) );
  7301.     b.Zero();
  7302.  
  7303.     for ( i = 0; i < numRows; i++ ) {
  7304.  
  7305.         b[i] = 1.0f;
  7306.         tmp.LU_Solve( x, b, index );
  7307.         for ( j = 0; j < numRows; j++ ) {
  7308.             (*this)[j][i] = x[j];
  7309.         }
  7310.         b[i] = 0.0f;
  7311.     }
  7312.     return true;
  7313. }
  7314.  
  7315. /*
  7316. ============
  7317. idMatX::Test
  7318. ============
  7319. */
  7320. void idMatX::Test( void ) {
  7321.     idMatX original, m1, m2, m3, q1, q2, r1, r2;
  7322.     idVecX v, w, u, c, d;
  7323.     int offset, size, *index1, *index2;
  7324.  
  7325.     size = 6;
  7326.     original.Random( size, size, 0 );
  7327.     original = original * original.Transpose();
  7328.  
  7329.     index1 = (int *) _alloca16( ( size + 1 ) * sizeof( index1[0] ) );
  7330.     index2 = (int *) _alloca16( ( size + 1 ) * sizeof( index2[0] ) );
  7331.  
  7332.     /*
  7333.         idMatX::LowerTriangularInverse
  7334.     */
  7335.  
  7336.     m1 = original;
  7337.     m1.ClearUpperTriangle();
  7338.     m2 = m1;
  7339.  
  7340.     m2.InverseSelf();
  7341.     m1.LowerTriangularInverse();
  7342.  
  7343.     if ( !m1.Compare( m2, 1e-4f ) ) {
  7344.         idLib::common->Warning( "idMatX::LowerTriangularInverse failed" );
  7345.     }
  7346.  
  7347.     /*
  7348.         idMatX::UpperTriangularInverse
  7349.     */
  7350.  
  7351.     m1 = original;
  7352.     m1.ClearLowerTriangle();
  7353.     m2 = m1;
  7354.  
  7355.     m2.InverseSelf();
  7356.     m1.UpperTriangularInverse();
  7357.  
  7358.     if ( !m1.Compare( m2, 1e-4f ) ) {
  7359.         idLib::common->Warning( "idMatX::UpperTriangularInverse failed" );
  7360.     }
  7361.  
  7362.     /*
  7363.         idMatX::Inverse_GaussJordan
  7364.     */
  7365.  
  7366.     m1 = original;
  7367.  
  7368.     m1.Inverse_GaussJordan();
  7369.     m1 *= original;
  7370.  
  7371.     if ( !m1.IsIdentity( 1e-4f ) ) {
  7372.         idLib::common->Warning( "idMatX::Inverse_GaussJordan failed" );
  7373.     }
  7374.  
  7375.     /*
  7376.         idMatX::Inverse_UpdateRankOne
  7377.     */
  7378.  
  7379.     m1 = original;
  7380.     m2 = original;
  7381.  
  7382.     w.Random( size, 1 );
  7383.     v.Random( size, 2 );
  7384.  
  7385.     // invert m1
  7386.     m1.Inverse_GaussJordan();
  7387.  
  7388.     // modify and invert m2 
  7389.     m2.Update_RankOne( v, w, 1.0f );
  7390.     if ( !m2.Inverse_GaussJordan() ) {
  7391.         assert( 0 );
  7392.     }
  7393.  
  7394.     // update inverse of m1
  7395.     m1.Inverse_UpdateRankOne( v, w, 1.0f );
  7396.  
  7397.     if ( !m1.Compare( m2, 1e-4f ) ) {
  7398.         idLib::common->Warning( "idMatX::Inverse_UpdateRankOne failed" );
  7399.     }
  7400.  
  7401.     /*
  7402.         idMatX::Inverse_UpdateRowColumn
  7403.     */
  7404.  
  7405.     for ( offset = 0; offset < size; offset++ ) {
  7406.         m1 = original;
  7407.         m2 = original;
  7408.  
  7409.         v.Random( size, 1 );
  7410.         w.Random( size, 2 );
  7411.         w[offset] = 0.0f;
  7412.  
  7413.         // invert m1
  7414.         m1.Inverse_GaussJordan();
  7415.  
  7416.         // modify and invert m2
  7417.         m2.Update_RowColumn( v, w, offset );
  7418.         if ( !m2.Inverse_GaussJordan() ) {
  7419.             assert( 0 );
  7420.         }
  7421.  
  7422.         // update inverse of m1
  7423.         m1.Inverse_UpdateRowColumn( v, w, offset );
  7424.  
  7425.         if ( !m1.Compare( m2, 1e-3f ) ) {
  7426.             idLib::common->Warning( "idMatX::Inverse_UpdateRowColumn failed" );
  7427.         }
  7428.     }
  7429.  
  7430.     /*
  7431.         idMatX::Inverse_UpdateIncrement
  7432.     */
  7433.  
  7434.     m1 = original;
  7435.     m2 = original;
  7436.  
  7437.     v.Random( size + 1, 1 );
  7438.     w.Random( size + 1, 2 );
  7439.     w[size] = 0.0f;
  7440.  
  7441.     // invert m1
  7442.     m1.Inverse_GaussJordan();
  7443.  
  7444.     // modify and invert m2 
  7445.     m2.Update_Increment( v, w );
  7446.     if ( !m2.Inverse_GaussJordan() ) {
  7447.         assert( 0 );
  7448.     }
  7449.  
  7450.     // update inverse of m1
  7451.     m1.Inverse_UpdateIncrement( v, w );
  7452.  
  7453.     if ( !m1.Compare( m2, 1e-4f ) ) {
  7454.         idLib::common->Warning( "idMatX::Inverse_UpdateIncrement failed" );
  7455.     }
  7456.  
  7457.     /*
  7458.         idMatX::Inverse_UpdateDecrement
  7459.     */
  7460.  
  7461.     for ( offset = 0; offset < size; offset++ ) {
  7462.         m1 = original;
  7463.         m2 = original;
  7464.  
  7465.         v.SetSize( 6 );
  7466.         w.SetSize( 6 );
  7467.         for ( int i = 0; i < size; i++ ) {
  7468.             v[i] = original[i][offset];
  7469.             w[i] = original[offset][i];
  7470.         }
  7471.  
  7472.         // invert m1
  7473.         m1.Inverse_GaussJordan();
  7474.  
  7475.         // modify and invert m2
  7476.         m2.Update_Decrement( offset );
  7477.         if ( !m2.Inverse_GaussJordan() ) {
  7478.             assert( 0 );
  7479.         }
  7480.  
  7481.         // update inverse of m1
  7482.         m1.Inverse_UpdateDecrement( v, w, offset );
  7483.  
  7484.         if ( !m1.Compare( m2, 1e-3f ) ) {
  7485.             idLib::common->Warning( "idMatX::Inverse_UpdateDecrement failed" );
  7486.         }
  7487.     }
  7488.  
  7489.     /*
  7490.         idMatX::LU_Factor
  7491.     */
  7492.  
  7493.     m1 = original;
  7494.  
  7495.     m1.LU_Factor( NULL );    // no pivoting
  7496.     m1.LU_UnpackFactors( m2, m3 );
  7497.     m1 = m2 * m3;
  7498.  
  7499.     if ( !original.Compare( m1, 1e-4f ) ) {
  7500.         idLib::common->Warning( "idMatX::LU_Factor failed" );
  7501.     }
  7502.  
  7503.     /*
  7504.         idMatX::LU_UpdateRankOne
  7505.     */
  7506.  
  7507.     m1 = original;
  7508.     m2 = original;
  7509.  
  7510.     w.Random( size, 1 );
  7511.     v.Random( size, 2 );
  7512.  
  7513.     // factor m1
  7514.     m1.LU_Factor( index1 );
  7515.  
  7516.     // modify and factor m2 
  7517.     m2.Update_RankOne( v, w, 1.0f );
  7518.     if ( !m2.LU_Factor( index2 ) ) {
  7519.         assert( 0 );
  7520.     }
  7521.     m2.LU_MultiplyFactors( m3, index2 );
  7522.     m2 = m3;
  7523.  
  7524.     // update factored m1
  7525.     m1.LU_UpdateRankOne( v, w, 1.0f, index1 );
  7526.     m1.LU_MultiplyFactors( m3, index1 );
  7527.     m1 = m3;
  7528.  
  7529.     if ( !m1.Compare( m2, 1e-4f ) ) {
  7530.         idLib::common->Warning( "idMatX::LU_UpdateRankOne failed" );
  7531.     }
  7532.  
  7533.     /*
  7534.         idMatX::LU_UpdateRowColumn
  7535.     */
  7536.  
  7537.     for ( offset = 0; offset < size; offset++ ) {
  7538.         m1 = original;
  7539.         m2 = original;
  7540.  
  7541.         v.Random( size, 1 );
  7542.         w.Random( size, 2 );
  7543.         w[offset] = 0.0f;
  7544.  
  7545.         // factor m1
  7546.         m1.LU_Factor( index1 );
  7547.  
  7548.         // modify and factor m2
  7549.         m2.Update_RowColumn( v, w, offset );
  7550.         if ( !m2.LU_Factor( index2 ) ) {
  7551.             assert( 0 );
  7552.         }
  7553.         m2.LU_MultiplyFactors( m3, index2 );
  7554.         m2 = m3;
  7555.  
  7556.         // update m1
  7557.         m1.LU_UpdateRowColumn( v, w, offset, index1  );
  7558.         m1.LU_MultiplyFactors( m3, index1 );
  7559.         m1 = m3;
  7560.  
  7561.         if ( !m1.Compare( m2, 1e-3f ) ) {
  7562.             idLib::common->Warning( "idMatX::LU_UpdateRowColumn failed" );
  7563.         }
  7564.     }
  7565.  
  7566.     /*
  7567.         idMatX::LU_UpdateIncrement
  7568.     */
  7569.  
  7570.     m1 = original;
  7571.     m2 = original;
  7572.  
  7573.     v.Random( size + 1, 1 );
  7574.     w.Random( size + 1, 2 );
  7575.     w[size] = 0.0f;
  7576.  
  7577.     // factor m1
  7578.     m1.LU_Factor( index1 );
  7579.  
  7580.     // modify and factor m2 
  7581.     m2.Update_Increment( v, w );
  7582.     if ( !m2.LU_Factor( index2 ) ) {
  7583.         assert( 0 );
  7584.     }
  7585.     m2.LU_MultiplyFactors( m3, index2 );
  7586.     m2 = m3;
  7587.  
  7588.     // update factored m1
  7589.     m1.LU_UpdateIncrement( v, w, index1 );
  7590.     m1.LU_MultiplyFactors( m3, index1 );
  7591.     m1 = m3;
  7592.  
  7593.     if ( !m1.Compare( m2, 1e-4f ) ) {
  7594.         idLib::common->Warning( "idMatX::LU_UpdateIncrement failed" );
  7595.     }
  7596.  
  7597.     /*
  7598.         idMatX::LU_UpdateDecrement
  7599.     */
  7600.  
  7601.     for ( offset = 0; offset < size; offset++ ) {
  7602.         m1 = original;
  7603.         m2 = original;
  7604.  
  7605.         v.SetSize( 6 );
  7606.         w.SetSize( 6 );
  7607.         for ( int i = 0; i < size; i++ ) {
  7608.             v[i] = original[i][offset];
  7609.             w[i] = original[offset][i];
  7610.         }
  7611.  
  7612.         // factor m1
  7613.         m1.LU_Factor( index1 );
  7614.  
  7615.         // modify and factor m2
  7616.         m2.Update_Decrement( offset );
  7617.         if ( !m2.LU_Factor( index2 ) ) {
  7618.             assert( 0 );
  7619.         }
  7620.         m2.LU_MultiplyFactors( m3, index2 );
  7621.         m2 = m3;
  7622.  
  7623.         u.SetSize( 6 );
  7624.         for ( int i = 0; i < size; i++ ) {
  7625.             u[i] = original[index1[offset]][i];
  7626.         }
  7627.  
  7628.         // update factors of m1
  7629.         m1.LU_UpdateDecrement( v, w, u, offset, index1 );
  7630.         m1.LU_MultiplyFactors( m3, index1 );
  7631.         m1 = m3;
  7632.  
  7633.         if ( !m1.Compare( m2, 1e-3f ) ) {
  7634.             idLib::common->Warning( "idMatX::LU_UpdateDecrement failed" );
  7635.         }
  7636.     }
  7637.  
  7638.     /*
  7639.         idMatX::LU_Inverse
  7640.     */
  7641.  
  7642.     m2 = original;
  7643.  
  7644.     m2.LU_Factor( NULL );
  7645.     m2.LU_Inverse( m1, NULL );
  7646.     m1 *= original;
  7647.  
  7648.     if ( !m1.IsIdentity( 1e-4f ) ) {
  7649.         idLib::common->Warning( "idMatX::LU_Inverse failed" );
  7650.     }
  7651.  
  7652.     /*
  7653.         idMatX::QR_Factor
  7654.     */
  7655.  
  7656.     c.SetSize( size );
  7657.     d.SetSize( size );
  7658.  
  7659.     m1 = original;
  7660.  
  7661.     m1.QR_Factor( c, d );
  7662.     m1.QR_UnpackFactors( q1, r1, c, d );
  7663.     m1 = q1 * r1;
  7664.  
  7665.     if ( !original.Compare( m1, 1e-4f ) ) {
  7666.         idLib::common->Warning( "idMatX::QR_Factor failed" );
  7667.     }
  7668.  
  7669.     /*
  7670.         idMatX::QR_UpdateRankOne
  7671.     */
  7672.  
  7673.     c.SetSize( size );
  7674.     d.SetSize( size );
  7675.  
  7676.     m1 = original;
  7677.     m2 = original;
  7678.  
  7679.     w.Random( size, 0 );
  7680.     v = w;
  7681.  
  7682.     // factor m1
  7683.     m1.QR_Factor( c, d );
  7684.     m1.QR_UnpackFactors( q1, r1, c, d );
  7685.  
  7686.     // modify and factor m2 
  7687.     m2.Update_RankOne( v, w, 1.0f );
  7688.     if ( !m2.QR_Factor( c, d ) ) {
  7689.         assert( 0 );
  7690.     }
  7691.     m2.QR_UnpackFactors( q2, r2, c, d );
  7692.     m2 = q2 * r2;
  7693.  
  7694.     // update factored m1
  7695.     q1.QR_UpdateRankOne( r1, v, w, 1.0f );
  7696.     m1 = q1 * r1;
  7697.  
  7698.     if ( !m1.Compare( m2, 1e-4f ) ) {
  7699.         idLib::common->Warning( "idMatX::QR_UpdateRankOne failed" );
  7700.     }
  7701.  
  7702.     /*
  7703.         idMatX::QR_UpdateRowColumn
  7704.     */
  7705.  
  7706.     for ( offset = 0; offset < size; offset++ ) {
  7707.         c.SetSize( size );
  7708.         d.SetSize( size );
  7709.  
  7710.         m1 = original;
  7711.         m2 = original;
  7712.  
  7713.         v.Random( size, 1 );
  7714.         w.Random( size, 2 );
  7715.         w[offset] = 0.0f;
  7716.  
  7717.         // factor m1
  7718.         m1.QR_Factor( c, d );
  7719.         m1.QR_UnpackFactors( q1, r1, c, d );
  7720.  
  7721.         // modify and factor m2
  7722.         m2.Update_RowColumn( v, w, offset );
  7723.         if ( !m2.QR_Factor( c, d ) ) {
  7724.             assert( 0 );
  7725.         }
  7726.         m2.QR_UnpackFactors( q2, r2, c, d );
  7727.         m2 = q2 * r2;
  7728.  
  7729.         // update m1
  7730.         q1.QR_UpdateRowColumn( r1, v, w, offset );
  7731.         m1 = q1 * r1;
  7732.  
  7733.         if ( !m1.Compare( m2, 1e-3f ) ) {
  7734.             idLib::common->Warning( "idMatX::QR_UpdateRowColumn failed" );
  7735.         }
  7736.     }
  7737.  
  7738.     /*
  7739.         idMatX::QR_UpdateIncrement
  7740.     */
  7741.  
  7742.     c.SetSize( size+1 );
  7743.     d.SetSize( size+1 );
  7744.  
  7745.     m1 = original;
  7746.     m2 = original;
  7747.  
  7748.     v.Random( size + 1, 1 );
  7749.     w.Random( size + 1, 2 );
  7750.     w[size] = 0.0f;
  7751.  
  7752.     // factor m1
  7753.     m1.QR_Factor( c, d );
  7754.     m1.QR_UnpackFactors( q1, r1, c, d );
  7755.  
  7756.     // modify and factor m2 
  7757.     m2.Update_Increment( v, w );
  7758.     if ( !m2.QR_Factor( c, d ) ) {
  7759.         assert( 0 );
  7760.     }
  7761.     m2.QR_UnpackFactors( q2, r2, c, d );
  7762.     m2 = q2 * r2;
  7763.  
  7764.     // update factored m1
  7765.     q1.QR_UpdateIncrement( r1, v, w );
  7766.     m1 = q1 * r1;
  7767.  
  7768.     if ( !m1.Compare( m2, 1e-4f ) ) {
  7769.         idLib::common->Warning( "idMatX::QR_UpdateIncrement failed" );
  7770.     }
  7771.  
  7772.     /*
  7773.         idMatX::QR_UpdateDecrement
  7774.     */
  7775.  
  7776.     for ( offset = 0; offset < size; offset++ ) {
  7777.         c.SetSize( size+1 );
  7778.         d.SetSize( size+1 );
  7779.  
  7780.         m1 = original;
  7781.         m2 = original;
  7782.  
  7783.         v.SetSize( 6 );
  7784.         w.SetSize( 6 );
  7785.         for ( int i = 0; i < size; i++ ) {
  7786.             v[i] = original[i][offset];
  7787.             w[i] = original[offset][i];
  7788.         }
  7789.  
  7790.         // factor m1
  7791.         m1.QR_Factor( c, d );
  7792.         m1.QR_UnpackFactors( q1, r1, c, d );
  7793.  
  7794.         // modify and factor m2
  7795.         m2.Update_Decrement( offset );
  7796.         if ( !m2.QR_Factor( c, d ) ) {
  7797.             assert( 0 );
  7798.         }
  7799.         m2.QR_UnpackFactors( q2, r2, c, d );
  7800.         m2 = q2 * r2;
  7801.  
  7802.         // update factors of m1
  7803.         q1.QR_UpdateDecrement( r1, v, w, offset );
  7804.         m1 = q1 * r1;
  7805.  
  7806.         if ( !m1.Compare( m2, 1e-3f ) ) {
  7807.             idLib::common->Warning( "idMatX::QR_UpdateDecrement failed" );
  7808.         }
  7809.     }
  7810.  
  7811.     /*
  7812.         idMatX::QR_Inverse
  7813.     */
  7814.  
  7815.     m2 = original;
  7816.  
  7817.     m2.QR_Factor( c, d );
  7818.     m2.QR_Inverse( m1, c, d );
  7819.     m1 *= original;
  7820.  
  7821.     if ( !m1.IsIdentity( 1e-4f ) ) {
  7822.         idLib::common->Warning( "idMatX::QR_Inverse failed" );
  7823.     }
  7824.  
  7825.     /*
  7826.         idMatX::SVD_Factor
  7827.     */
  7828.  
  7829.     m1 = original;
  7830.     m3.Zero( size, size );
  7831.     w.Zero( size );
  7832.  
  7833.     m1.SVD_Factor( w, m3 );
  7834.     m2.Diag( w );
  7835.     m3.TransposeSelf();
  7836.     m1 = m1 * m2 * m3;
  7837.  
  7838.     if ( !original.Compare( m1, 1e-4f ) ) {
  7839.         idLib::common->Warning( "idMatX::SVD_Factor failed" );
  7840.     }
  7841.  
  7842.     /*
  7843.         idMatX::SVD_Inverse
  7844.     */
  7845.  
  7846.     m2 = original;
  7847.  
  7848.     m2.SVD_Factor( w, m3 );
  7849.     m2.SVD_Inverse( m1, w, m3 );
  7850.     m1 *= original;
  7851.  
  7852.     if ( !m1.IsIdentity( 1e-4f ) ) {
  7853.         idLib::common->Warning( "idMatX::SVD_Inverse failed" );
  7854.     }
  7855.  
  7856.     /*
  7857.         idMatX::Cholesky_Factor
  7858.     */
  7859.  
  7860.     m1 = original;
  7861.  
  7862.     m1.Cholesky_Factor();
  7863.     m1.Cholesky_MultiplyFactors( m2 );
  7864.  
  7865.     if ( !original.Compare( m2, 1e-4f ) ) {
  7866.         idLib::common->Warning( "idMatX::Cholesky_Factor failed" );
  7867.     }
  7868.  
  7869.     /*
  7870.         idMatX::Cholesky_UpdateRankOne
  7871.     */
  7872.  
  7873.     m1 = original;
  7874.     m2 = original;
  7875.  
  7876.     w.Random( size, 0 );
  7877.  
  7878.     // factor m1
  7879.     m1.Cholesky_Factor();
  7880.     m1.ClearUpperTriangle();
  7881.  
  7882.     // modify and factor m2 
  7883.     m2.Update_RankOneSymmetric( w, 1.0f );
  7884.     if ( !m2.Cholesky_Factor() ) {
  7885.         assert( 0 );
  7886.     }
  7887.     m2.ClearUpperTriangle();
  7888.  
  7889.     // update factored m1
  7890.     m1.Cholesky_UpdateRankOne( w, 1.0f, 0 );
  7891.  
  7892.     if ( !m1.Compare( m2, 1e-4f ) ) {
  7893.         idLib::common->Warning( "idMatX::Cholesky_UpdateRankOne failed" );
  7894.     }
  7895.  
  7896.     /*
  7897.         idMatX::Cholesky_UpdateRowColumn
  7898.     */
  7899.  
  7900.     for ( offset = 0; offset < size; offset++ ) {
  7901.         m1 = original;
  7902.         m2 = original;
  7903.  
  7904.         // factor m1
  7905.         m1.Cholesky_Factor();
  7906.         m1.ClearUpperTriangle();
  7907.  
  7908.         int pdtable[] = { 1, 0, 1, 0, 0, 0 };
  7909.         w.Random( size, pdtable[offset] );
  7910.         w *= 0.1f;
  7911.  
  7912.         // modify and factor m2
  7913.         m2.Update_RowColumnSymmetric( w, offset );
  7914.         if ( !m2.Cholesky_Factor() ) {
  7915.             assert( 0 );
  7916.         }
  7917.         m2.ClearUpperTriangle();
  7918.  
  7919.         // update m1
  7920.         m1.Cholesky_UpdateRowColumn( w, offset );
  7921.  
  7922.         if ( !m1.Compare( m2, 1e-3f ) ) {
  7923.             idLib::common->Warning( "idMatX::Cholesky_UpdateRowColumn failed" );
  7924.         }
  7925.     }
  7926.  
  7927.     /*
  7928.         idMatX::Cholesky_UpdateIncrement
  7929.     */
  7930.  
  7931.     m1.Random( size + 1, size + 1, 0 );
  7932.     m3 = m1 * m1.Transpose();
  7933.  
  7934.     m1.SquareSubMatrix( m3, size );
  7935.     m2 = m1;
  7936.  
  7937.     w.SetSize( size + 1 );
  7938.     for ( int i = 0; i < size + 1; i++ ) {
  7939.         w[i] = m3[size][i];
  7940.     }
  7941.  
  7942.     // factor m1
  7943.     m1.Cholesky_Factor();
  7944.  
  7945.     // modify and factor m2 
  7946.     m2.Update_IncrementSymmetric( w );
  7947.     if ( !m2.Cholesky_Factor() ) {
  7948.         assert( 0 );
  7949.     }
  7950.  
  7951.     // update factored m1
  7952.     m1.Cholesky_UpdateIncrement( w );
  7953.  
  7954.     m1.ClearUpperTriangle();
  7955.     m2.ClearUpperTriangle();
  7956.  
  7957.     if ( !m1.Compare( m2, 1e-4f ) ) {
  7958.         idLib::common->Warning( "idMatX::Cholesky_UpdateIncrement failed" );
  7959.     }
  7960.  
  7961.     /*
  7962.         idMatX::Cholesky_UpdateDecrement
  7963.     */
  7964.  
  7965.     for ( offset = 0; offset < size; offset += size - 1 ) {
  7966.         m1 = original;
  7967.         m2 = original;
  7968.  
  7969.         v.SetSize( 6 );
  7970.         for ( int i = 0; i < size; i++ ) {
  7971.             v[i] = original[i][offset];
  7972.         }
  7973.  
  7974.         // factor m1
  7975.         m1.Cholesky_Factor();
  7976.  
  7977.         // modify and factor m2
  7978.         m2.Update_Decrement( offset );
  7979.         if ( !m2.Cholesky_Factor() ) {
  7980.             assert( 0 );
  7981.         }
  7982.  
  7983.         // update factors of m1
  7984.         m1.Cholesky_UpdateDecrement( v, offset );
  7985.  
  7986.         if ( !m1.Compare( m2, 1e-3f ) ) {
  7987.             idLib::common->Warning( "idMatX::Cholesky_UpdateDecrement failed" );
  7988.         }
  7989.     }
  7990.  
  7991.     /*
  7992.         idMatX::Cholesky_Inverse
  7993.     */
  7994.  
  7995.     m2 = original;
  7996.  
  7997.     m2.Cholesky_Factor();
  7998.     m2.Cholesky_Inverse( m1 );
  7999.     m1 *= original;
  8000.  
  8001.     if ( !m1.IsIdentity( 1e-4f ) ) {
  8002.         idLib::common->Warning( "idMatX::Cholesky_Inverse failed" );
  8003.     }
  8004.  
  8005.     /*
  8006.         idMatX::LDLT_Factor
  8007.     */
  8008.  
  8009.     m1 = original;
  8010.  
  8011.     m1.LDLT_Factor();
  8012.     m1.LDLT_MultiplyFactors( m2 );
  8013.  
  8014.     if ( !original.Compare( m2, 1e-4f ) ) {
  8015.         idLib::common->Warning( "idMatX::LDLT_Factor failed" );
  8016.     }
  8017.  
  8018.     m1.LDLT_UnpackFactors( m2, m3 );
  8019.     m2 = m2 * m3 * m2.Transpose();
  8020.  
  8021.     if ( !original.Compare( m2, 1e-4f ) ) {
  8022.         idLib::common->Warning( "idMatX::LDLT_Factor failed" );
  8023.     }
  8024.  
  8025.     /*
  8026.         idMatX::LDLT_UpdateRankOne
  8027.     */
  8028.  
  8029.     m1 = original;
  8030.     m2 = original;
  8031.  
  8032.     w.Random( size, 0 );
  8033.  
  8034.     // factor m1
  8035.     m1.LDLT_Factor();
  8036.     m1.ClearUpperTriangle();
  8037.  
  8038.     // modify and factor m2 
  8039.     m2.Update_RankOneSymmetric( w, 1.0f );
  8040.     if ( !m2.LDLT_Factor() ) {
  8041.         assert( 0 );
  8042.     }
  8043.     m2.ClearUpperTriangle();
  8044.  
  8045.     // update factored m1
  8046.     m1.LDLT_UpdateRankOne( w, 1.0f, 0 );
  8047.  
  8048.     if ( !m1.Compare( m2, 1e-4f ) ) {
  8049.         idLib::common->Warning( "idMatX::LDLT_UpdateRankOne failed" );
  8050.     }
  8051.  
  8052.     /*
  8053.         idMatX::LDLT_UpdateRowColumn
  8054.     */
  8055.  
  8056.     for ( offset = 0; offset < size; offset++ ) {
  8057.         m1 = original;
  8058.         m2 = original;
  8059.  
  8060.         w.Random( size, 0 );
  8061.  
  8062.         // factor m1
  8063.         m1.LDLT_Factor();
  8064.         m1.ClearUpperTriangle();
  8065.  
  8066.         // modify and factor m2
  8067.         m2.Update_RowColumnSymmetric( w, offset );
  8068.         if ( !m2.LDLT_Factor() ) {
  8069.             assert( 0 );
  8070.         }
  8071.         m2.ClearUpperTriangle();
  8072.  
  8073.         // update m1
  8074.         m1.LDLT_UpdateRowColumn( w, offset );
  8075.  
  8076.         if ( !m1.Compare( m2, 1e-3f ) ) {
  8077.             idLib::common->Warning( "idMatX::LDLT_UpdateRowColumn failed" );
  8078.         }
  8079.     }
  8080.  
  8081.     /*
  8082.         idMatX::LDLT_UpdateIncrement
  8083.     */
  8084.  
  8085.     m1.Random( size + 1, size + 1, 0 );
  8086.     m3 = m1 * m1.Transpose();
  8087.  
  8088.     m1.SquareSubMatrix( m3, size );
  8089.     m2 = m1;
  8090.  
  8091.     w.SetSize( size + 1 );
  8092.     for ( int i = 0; i < size + 1; i++ ) {
  8093.         w[i] = m3[size][i];
  8094.     }
  8095.  
  8096.     // factor m1
  8097.     m1.LDLT_Factor();
  8098.  
  8099.     // modify and factor m2 
  8100.     m2.Update_IncrementSymmetric( w );
  8101.     if ( !m2.LDLT_Factor() ) {
  8102.         assert( 0 );
  8103.     }
  8104.  
  8105.     // update factored m1
  8106.     m1.LDLT_UpdateIncrement( w );
  8107.  
  8108.     m1.ClearUpperTriangle();
  8109.     m2.ClearUpperTriangle();
  8110.  
  8111.     if ( !m1.Compare( m2, 1e-4f ) ) {
  8112.         idLib::common->Warning( "idMatX::LDLT_UpdateIncrement failed" );
  8113.     }
  8114.  
  8115.     /*
  8116.         idMatX::LDLT_UpdateDecrement
  8117.     */
  8118.  
  8119.     for ( offset = 0; offset < size; offset++ ) {
  8120.         m1 = original;
  8121.         m2 = original;
  8122.  
  8123.         v.SetSize( 6 );
  8124.         for ( int i = 0; i < size; i++ ) {
  8125.             v[i] = original[i][offset];
  8126.         }
  8127.  
  8128.         // factor m1
  8129.         m1.LDLT_Factor();
  8130.  
  8131.         // modify and factor m2
  8132.         m2.Update_Decrement( offset );
  8133.         if ( !m2.LDLT_Factor() ) {
  8134.             assert( 0 );
  8135.         }
  8136.  
  8137.         // update factors of m1
  8138.         m1.LDLT_UpdateDecrement( v, offset );
  8139.  
  8140.         if ( !m1.Compare( m2, 1e-3f ) ) {
  8141.             idLib::common->Warning( "idMatX::LDLT_UpdateDecrement failed" );
  8142.         }
  8143.     }
  8144.  
  8145.     /*
  8146.         idMatX::LDLT_Inverse
  8147.     */
  8148.  
  8149.     m2 = original;
  8150.  
  8151.     m2.LDLT_Factor();
  8152.     m2.LDLT_Inverse( m1 );
  8153.     m1 *= original;
  8154.  
  8155.     if ( !m1.IsIdentity( 1e-4f ) ) {
  8156.         idLib::common->Warning( "idMatX::LDLT_Inverse failed" );
  8157.     }
  8158.  
  8159.     /*
  8160.         idMatX::Eigen_SolveSymmetricTriDiagonal
  8161.     */
  8162.  
  8163.     m3 = original;
  8164.     m3.TriDiagonal_ClearTriangles();
  8165.     m1 = m3;
  8166.  
  8167.     v.SetSize( size );
  8168.  
  8169.     m1.Eigen_SolveSymmetricTriDiagonal( v );
  8170.  
  8171.     m3.TransposeMultiply( m2, m1 );
  8172.  
  8173.     for ( int i = 0; i < size; i++ ) {
  8174.         for ( int j = 0; j < size; j++ ) {
  8175.             m1[i][j] *= v[j];
  8176.         }
  8177.     }
  8178.  
  8179.     if ( !m1.Compare( m2, 1e-4f ) ) {
  8180.         idLib::common->Warning( "idMatX::Eigen_SolveSymmetricTriDiagonal failed" );
  8181.     }
  8182.  
  8183.     /*
  8184.         idMatX::Eigen_SolveSymmetric
  8185.     */
  8186.  
  8187.     m3 = original;
  8188.     m1 = m3;
  8189.  
  8190.     v.SetSize( size );
  8191.  
  8192.     m1.Eigen_SolveSymmetric( v );
  8193.  
  8194.     m3.TransposeMultiply( m2, m1 );
  8195.  
  8196.     for ( int i = 0; i < size; i++ ) {
  8197.         for ( int j = 0; j < size; j++ ) {
  8198.             m1[i][j] *= v[j];
  8199.         }
  8200.     }
  8201.  
  8202.     if ( !m1.Compare( m2, 1e-4f ) ) {
  8203.         idLib::common->Warning( "idMatX::Eigen_SolveSymmetric failed" );
  8204.     }
  8205.  
  8206.     /*
  8207.         idMatX::Eigen_Solve
  8208.     */
  8209.  
  8210.     m3 = original;
  8211.     m1 = m3;
  8212.  
  8213.     v.SetSize( size );
  8214.     w.SetSize( size );
  8215.  
  8216.     m1.Eigen_Solve( v, w );
  8217.  
  8218.     m3.TransposeMultiply( m2, m1 );
  8219.  
  8220.     for ( int i = 0; i < size; i++ ) {
  8221.         for ( int j = 0; j < size; j++ ) {
  8222.             m1[i][j] *= v[j];
  8223.         }
  8224.     }
  8225.  
  8226.     if ( !m1.Compare( m2, 1e-4f ) ) {
  8227.         idLib::common->Warning( "idMatX::Eigen_Solve failed" );
  8228.     }
  8229. }
  8230.