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.h < prev    next >
C/C++ Source or Header  |  2005-11-14  |  94KB  |  3,017 lines
  1.  
  2. #ifndef __MATH_MATRIX_H__
  3. #define __MATH_MATRIX_H__
  4.  
  5. /*
  6. ===============================================================================
  7.  
  8.   Matrix classes, all matrices are row-major except idMat3
  9.  
  10. ===============================================================================
  11. */
  12.  
  13. #define MATRIX_INVERSE_EPSILON        1e-14
  14. #define MATRIX_EPSILON                1e-6
  15.  
  16. class idAngles;
  17. class idQuat;
  18. class idCQuat;
  19. class idRotation;
  20. class idMat4;
  21.  
  22. //===============================================================
  23. //
  24. //    idMat2 - 2x2 matrix
  25. //
  26. //===============================================================
  27.  
  28. class idMat2 {
  29. public:
  30.                     idMat2( void );
  31.                     explicit idMat2( const idVec2 &x, const idVec2 &y );
  32.                     explicit idMat2( const float xx, const float xy, const float yx, const float yy );
  33.                     explicit idMat2( const float src[ 2 ][ 2 ] );
  34.  
  35.     const idVec2 &    operator[]( int index ) const;
  36.     idVec2 &        operator[]( int index );
  37.     idMat2            operator-() const;
  38.     idMat2            operator*( const float a ) const;
  39.     idVec2            operator*( const idVec2 &vec ) const;
  40.     idMat2            operator*( const idMat2 &a ) const;
  41.     idMat2            operator+( const idMat2 &a ) const;
  42.     idMat2            operator-( const idMat2 &a ) const;
  43.     idMat2 &        operator*=( const float a );
  44.     idMat2 &        operator*=( const idMat2 &a );
  45.     idMat2 &        operator+=( const idMat2 &a );
  46.     idMat2 &        operator-=( const idMat2 &a );
  47.  
  48.     friend idMat2    operator*( const float a, const idMat2 &mat );
  49.     friend idVec2    operator*( const idVec2 &vec, const idMat2 &mat );
  50.     friend idVec2 &    operator*=( idVec2 &vec, const idMat2 &mat );
  51.  
  52.     bool            Compare( const idMat2 &a ) const;                        // exact compare, no epsilon
  53.     bool            Compare( const idMat2 &a, const float epsilon ) const;    // compare with epsilon
  54.     bool            operator==( const idMat2 &a ) const;                    // exact compare, no epsilon
  55.     bool            operator!=( const idMat2 &a ) const;                    // exact compare, no epsilon
  56.  
  57.     void            Zero( void );
  58.     void            Identity( void );
  59.     bool            IsIdentity( const float epsilon = MATRIX_EPSILON ) const;
  60.     bool            IsSymmetric( const float epsilon = MATRIX_EPSILON ) const;
  61.     bool            IsDiagonal( const float epsilon = MATRIX_EPSILON ) const;
  62.  
  63.     float            Trace( void ) const;
  64.     float            Determinant( void ) const;
  65.     idMat2            Transpose( void ) const;    // returns transpose
  66.     idMat2 &        TransposeSelf( void );
  67.     idMat2            Inverse( void ) const;        // returns the inverse ( m * m.Inverse() = identity )
  68.     bool            InverseSelf( void );        // returns false if determinant is zero
  69.     idMat2            InverseFast( void ) const;    // returns the inverse ( m * m.Inverse() = identity )
  70.     bool            InverseFastSelf( void );    // returns false if determinant is zero
  71.  
  72.     int                GetDimension( void ) const;
  73.  
  74.     const float *    ToFloatPtr( void ) const;
  75.     float *            ToFloatPtr( void );
  76.     const char *    ToString( int precision = 2 ) const;
  77.  
  78. private:
  79.     idVec2            mat[ 2 ];
  80. };
  81.  
  82. extern idMat2 mat2_zero;
  83. extern idMat2 mat2_identity;
  84. #define mat2_default    mat2_identity
  85.  
  86. ID_INLINE idMat2::idMat2( void ) {
  87. }
  88.  
  89. ID_INLINE idMat2::idMat2( const idVec2 &x, const idVec2 &y ) {
  90.     mat[ 0 ].x = x.x; mat[ 0 ].y = x.y;
  91.     mat[ 1 ].x = y.x; mat[ 1 ].y = y.y;
  92. }
  93.  
  94. ID_INLINE idMat2::idMat2( const float xx, const float xy, const float yx, const float yy ) {
  95.     mat[ 0 ].x = xx; mat[ 0 ].y = xy;
  96.     mat[ 1 ].x = yx; mat[ 1 ].y = yy;
  97. }
  98.  
  99. ID_INLINE idMat2::idMat2( const float src[ 2 ][ 2 ] ) {
  100.     memcpy( mat, src, 2 * 2 * sizeof( float ) );
  101. }
  102.  
  103. ID_INLINE const idVec2 &idMat2::operator[]( int index ) const {
  104.     //assert( ( index >= 0 ) && ( index < 2 ) );
  105.     return mat[ index ];
  106. }
  107.  
  108. ID_INLINE idVec2 &idMat2::operator[]( int index ) {
  109.     //assert( ( index >= 0 ) && ( index < 2 ) );
  110.     return mat[ index ];
  111. }
  112.  
  113. ID_INLINE idMat2 idMat2::operator-() const {
  114.     return idMat2(    -mat[0][0], -mat[0][1],
  115.                     -mat[1][0], -mat[1][1] );
  116. }
  117.  
  118. ID_INLINE idVec2 idMat2::operator*( const idVec2 &vec ) const {
  119.     return idVec2(
  120.         mat[ 0 ].x * vec.x + mat[ 0 ].y * vec.y,
  121.         mat[ 1 ].x * vec.x + mat[ 1 ].y * vec.y );
  122. }
  123.  
  124. ID_INLINE idMat2 idMat2::operator*( const idMat2 &a ) const {
  125.     return idMat2(
  126.         mat[0].x * a[0].x + mat[0].y * a[1].x,
  127.         mat[0].x * a[0].y + mat[0].y * a[1].y,
  128.         mat[1].x * a[0].x + mat[1].y * a[1].x,
  129.         mat[1].x * a[0].y + mat[1].y * a[1].y );
  130. }
  131.  
  132. ID_INLINE idMat2 idMat2::operator*( const float a ) const {
  133.     return idMat2(
  134.         mat[0].x * a, mat[0].y * a, 
  135.         mat[1].x * a, mat[1].y * a );
  136. }
  137.  
  138. ID_INLINE idMat2 idMat2::operator+( const idMat2 &a ) const {
  139.     return idMat2(
  140.         mat[0].x + a[0].x, mat[0].y + a[0].y, 
  141.         mat[1].x + a[1].x, mat[1].y + a[1].y );
  142. }
  143.     
  144. ID_INLINE idMat2 idMat2::operator-( const idMat2 &a ) const {
  145.     return idMat2(
  146.         mat[0].x - a[0].x, mat[0].y - a[0].y,
  147.         mat[1].x - a[1].x, mat[1].y - a[1].y );
  148. }
  149.  
  150. ID_INLINE idMat2 &idMat2::operator*=( const float a ) {
  151.     mat[0].x *= a; mat[0].y *= a;
  152.     mat[1].x *= a; mat[1].y *= a;
  153.  
  154.     return *this;
  155. }
  156.  
  157. ID_INLINE idMat2 &idMat2::operator*=( const idMat2 &a ) {
  158.     float x, y;
  159.     x = mat[0].x; y = mat[0].y;
  160.     mat[0].x = x * a[0].x + y * a[1].x;
  161.     mat[0].y = x * a[0].y + y * a[1].y;
  162.     x = mat[1].x; y = mat[1].y;
  163.     mat[1].x = x * a[0].x + y * a[1].x;
  164.     mat[1].y = x * a[0].y + y * a[1].y;
  165.     return *this;
  166. }
  167.  
  168. ID_INLINE idMat2 &idMat2::operator+=( const idMat2 &a ) {
  169.     mat[0].x += a[0].x; mat[0].y += a[0].y;
  170.     mat[1].x += a[1].x; mat[1].y += a[1].y;
  171.  
  172.     return *this;
  173. }
  174.  
  175. ID_INLINE idMat2 &idMat2::operator-=( const idMat2 &a ) {
  176.     mat[0].x -= a[0].x; mat[0].y -= a[0].y;
  177.     mat[1].x -= a[1].x; mat[1].y -= a[1].y;
  178.  
  179.     return *this;
  180. }
  181.  
  182. ID_INLINE idVec2 operator*( const idVec2 &vec, const idMat2 &mat ) {
  183.     return mat * vec;
  184. }
  185.  
  186. ID_INLINE idMat2 operator*( const float a, idMat2 const &mat ) {
  187.     return mat * a;
  188. }
  189.  
  190. ID_INLINE idVec2 &operator*=( idVec2 &vec, const idMat2 &mat ) {
  191.     vec = mat * vec;
  192.     return vec;
  193. }
  194.  
  195. ID_INLINE bool idMat2::Compare( const idMat2 &a ) const {
  196.     if ( mat[0].Compare( a[0] ) &&
  197.         mat[1].Compare( a[1] ) ) {
  198.         return true;
  199.     }
  200.     return false;
  201. }
  202.  
  203. ID_INLINE bool idMat2::Compare( const idMat2 &a, const float epsilon ) const {
  204.     if ( mat[0].Compare( a[0], epsilon ) &&
  205.         mat[1].Compare( a[1], epsilon ) ) {
  206.         return true;
  207.     }
  208.     return false;
  209. }
  210.  
  211. ID_INLINE bool idMat2::operator==( const idMat2 &a ) const {
  212.     return Compare( a );
  213. }
  214.  
  215. ID_INLINE bool idMat2::operator!=( const idMat2 &a ) const {
  216.     return !Compare( a );
  217. }
  218.  
  219. ID_INLINE void idMat2::Zero( void ) {
  220.     mat[0].Zero();
  221.     mat[1].Zero();
  222. }
  223.  
  224. ID_INLINE void idMat2::Identity( void ) {
  225.     *this = mat2_identity;
  226. }
  227.  
  228. ID_INLINE bool idMat2::IsIdentity( const float epsilon ) const {
  229.     return Compare( mat2_identity, epsilon );
  230. }
  231.  
  232. ID_INLINE bool idMat2::IsSymmetric( const float epsilon ) const {
  233.     return ( idMath::Fabs( mat[0][1] - mat[1][0] ) < epsilon );
  234. }
  235.  
  236. ID_INLINE bool idMat2::IsDiagonal( const float epsilon ) const {
  237.     if ( idMath::Fabs( mat[0][1] ) > epsilon ||
  238.         idMath::Fabs( mat[1][0] ) > epsilon ) {
  239.         return false;
  240.     }
  241.     return true;
  242. }
  243.  
  244. ID_INLINE float idMat2::Trace( void ) const {
  245.     return ( mat[0][0] + mat[1][1] );
  246. }
  247.  
  248. ID_INLINE float idMat2::Determinant( void ) const {
  249.     return mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
  250. }
  251.  
  252. ID_INLINE idMat2 idMat2::Transpose( void ) const {
  253.     return idMat2(    mat[0][0], mat[1][0],
  254.                     mat[0][1], mat[1][1] );
  255. }
  256.  
  257. ID_INLINE idMat2 &idMat2::TransposeSelf( void ) {
  258.     float tmp;
  259.  
  260.     tmp = mat[0][1];
  261.     mat[0][1] = mat[1][0];
  262.     mat[1][0] = tmp;
  263.  
  264.     return *this;
  265. }
  266.  
  267. ID_INLINE idMat2 idMat2::Inverse( void ) const {
  268.     idMat2 invMat;
  269.  
  270.     invMat = *this;
  271. #ifdef _DEBUG    
  272.     assert( invMat.InverseSelf() );
  273. #else
  274.     invMat.InverseSelf();
  275. #endif
  276.     return invMat;
  277. }
  278.  
  279. ID_INLINE idMat2 idMat2::InverseFast( void ) const {
  280.     idMat2 invMat;
  281.  
  282.     invMat = *this;
  283. #ifdef _DEBUG    
  284.     assert ( invMat.InverseFastSelf() );
  285. #else
  286.     invMat.InverseFastSelf();
  287. #endif
  288.     return invMat;
  289. }
  290.  
  291. ID_INLINE int idMat2::GetDimension( void ) const {
  292.     return 4;
  293. }
  294.  
  295. ID_INLINE const float *idMat2::ToFloatPtr( void ) const {
  296.     return mat[0].ToFloatPtr();
  297. }
  298.  
  299. ID_INLINE float *idMat2::ToFloatPtr( void ) {
  300.     return mat[0].ToFloatPtr();
  301. }
  302.  
  303.  
  304. //===============================================================
  305. //
  306. //    idMat3 - 3x3 matrix
  307. //
  308. //    NOTE:    matrix is column-major
  309. //
  310. //===============================================================
  311.  
  312. class idMat3 {
  313. public:
  314.                     idMat3( void );
  315.                     explicit idMat3( const idVec3 &x, const idVec3 &y, const idVec3 &z );
  316.                     explicit idMat3( const float xx, const float xy, const float xz, const float yx, const float yy, const float yz, const float zx, const float zy, const float zz );
  317.                     explicit idMat3( const float src[ 3 ][ 3 ] );
  318.  
  319.     const idVec3 &    operator[]( int index ) const;
  320.     idVec3 &        operator[]( int index );
  321.     idMat3            operator-() const;
  322.     idMat3            operator*( const float a ) const;
  323.     idVec3            operator*( const idVec3 &vec ) const;
  324.     idMat3            operator*( const idMat3 &a ) const;
  325. // RAVEN BEGIN
  326. // jscott: multiply by the transpose
  327.     idVec3            operator/( const idVec3 &vec ) const;
  328.     idMat3            operator/( const idMat3 &a ) const;
  329. // RAVEN END
  330.     idMat3            operator+( const idMat3 &a ) const;
  331.     idMat3            operator-( const idMat3 &a ) const;
  332.     idMat3 &        operator*=( const float a );
  333.     idMat3 &        operator*=( const idMat3 &a );
  334.     idMat3 &        operator+=( const idMat3 &a );
  335.     idMat3 &        operator-=( const idMat3 &a );
  336.  
  337.     friend idMat3    operator*( const float a, const idMat3 &mat );
  338.     friend idVec3    operator*( const idVec3 &vec, const idMat3 &mat );
  339.     friend idVec3 &    operator*=( idVec3 &vec, const idMat3 &mat );
  340.  
  341.     bool            Compare( const idMat3 &a ) const;                        // exact compare, no epsilon
  342.     bool            Compare( const idMat3 &a, const float epsilon ) const;    // compare with epsilon
  343.     bool            operator==( const idMat3 &a ) const;                    // exact compare, no epsilon
  344.     bool            operator!=( const idMat3 &a ) const;                    // exact compare, no epsilon
  345.  
  346.     void            Zero( void );
  347.     void            Identity( void );
  348.     bool            IsIdentity( const float epsilon = MATRIX_EPSILON ) const;
  349.     bool            IsSymmetric( const float epsilon = MATRIX_EPSILON ) const;
  350.     bool            IsDiagonal( const float epsilon = MATRIX_EPSILON ) const;
  351.     bool            IsRotated( void ) const;
  352.  
  353.     void            ProjectVector( const idVec3 &src, idVec3 &dst ) const;
  354.     void            UnprojectVector( const idVec3 &src, idVec3 &dst ) const;
  355.  
  356.     bool            FixDegeneracies( void );    // fix degenerate axial cases
  357.     bool            FixDenormals( void );        // change tiny numbers to zero
  358.  
  359.     float            Trace( void ) const;
  360.     float            Determinant( void ) const;
  361.     idMat3            OrthoNormalize( void ) const;
  362.     idMat3 &        OrthoNormalizeSelf( void );
  363.     idMat3            Transpose( void ) const;    // returns transpose
  364.     idMat3 &        TransposeSelf( void );
  365.     idMat3            Inverse( void ) const;        // returns the inverse ( m * m.Inverse() = identity )
  366.     bool            InverseSelf( void );        // returns false if determinant is zero
  367.     idMat3            InverseFast( void ) const;    // returns the inverse ( m * m.Inverse() = identity )
  368.     bool            InverseFastSelf( void );    // returns false if determinant is zero
  369.     idMat3            TransposeMultiply( const idMat3 &b ) const;
  370.  
  371.     idMat3            InertiaTranslate( const float mass, const idVec3 ¢erOfMass, const idVec3 &translation ) const;
  372.     idMat3 &        InertiaTranslateSelf( const float mass, const idVec3 ¢erOfMass, const idVec3 &translation );
  373.     idMat3            InertiaRotate( const idMat3 &rotation ) const;
  374.     idMat3 &        InertiaRotateSelf( const idMat3 &rotation );
  375.  
  376.     int                GetDimension( void ) const;
  377. // RAVEN BEGIN
  378. // abahr:
  379.     int                GetVec3Dimension( void ) const;
  380. // RAVEN END
  381.  
  382.     idAngles        ToAngles( void ) const;
  383. // RAVEN BEGIN
  384.     void            RotateAbsolute(int whichAxis, float    howManyDegrees);
  385.     void            RotateRelative(int whichAxis, float    howManyDegrees);
  386.     void            RotateArbitrary(const idVec3 &rotAxis, float howManyDegrees);
  387. // RAVEN END
  388.     idQuat            ToQuat( void ) const;
  389.     idCQuat            ToCQuat( void ) const;
  390.     idRotation        ToRotation( void ) const;
  391.     idMat4            ToMat4( void ) const;
  392.     idVec3            ToAngularVelocity( void ) const;
  393.     const float *    ToFloatPtr( void ) const;
  394.     float *            ToFloatPtr( void );
  395.     const char *    ToString( int precision = 2 ) const;
  396.  
  397.     friend void        TransposeMultiply( const idMat3 &inv, const idMat3 &b, idMat3 &dst );
  398.     friend idMat3    SkewSymmetric( idVec3 const &src );
  399.  
  400. private:
  401.     idVec3            mat[ 3 ];
  402. };
  403.  
  404. extern idMat3 mat3_zero;
  405. extern idMat3 mat3_identity;
  406. #define mat3_default    mat3_identity
  407.  
  408. ID_INLINE idMat3::idMat3( void ) {
  409. }
  410.  
  411. ID_INLINE idMat3::idMat3( const idVec3 &x, const idVec3 &y, const idVec3 &z ) {
  412.     mat[ 0 ].x = x.x; mat[ 0 ].y = x.y; mat[ 0 ].z = x.z;
  413.     mat[ 1 ].x = y.x; mat[ 1 ].y = y.y; mat[ 1 ].z = y.z;
  414.     mat[ 2 ].x = z.x; mat[ 2 ].y = z.y; mat[ 2 ].z = z.z;
  415. }
  416.  
  417. ID_INLINE idMat3::idMat3( const float xx, const float xy, const float xz, const float yx, const float yy, const float yz, const float zx, const float zy, const float zz ) {
  418.     mat[ 0 ].x = xx; mat[ 0 ].y = xy; mat[ 0 ].z = xz;
  419.     mat[ 1 ].x = yx; mat[ 1 ].y = yy; mat[ 1 ].z = yz;
  420.     mat[ 2 ].x = zx; mat[ 2 ].y = zy; mat[ 2 ].z = zz;
  421. }
  422.  
  423. ID_INLINE idMat3::idMat3( const float src[ 3 ][ 3 ] ) {
  424.     memcpy( mat, src, 3 * 3 * sizeof( float ) );
  425. }
  426.  
  427. ID_INLINE const idVec3 &idMat3::operator[]( int index ) const {
  428.     //assert( ( index >= 0 ) && ( index < 3 ) );
  429.     return mat[ index ];
  430. }
  431.  
  432. ID_INLINE idVec3 &idMat3::operator[]( int index ) {
  433.     //assert( ( index >= 0 ) && ( index < 3 ) );
  434.     return mat[ index ];
  435. }
  436.  
  437. ID_INLINE idMat3 idMat3::operator-() const {
  438.     return idMat3(    -mat[0][0], -mat[0][1], -mat[0][2],
  439.                     -mat[1][0], -mat[1][1], -mat[1][2],
  440.                     -mat[2][0], -mat[2][1], -mat[2][2] );
  441. }
  442.  
  443. ID_INLINE idVec3 idMat3::operator*( const idVec3 &vec ) const {
  444.     return idVec3(
  445.         mat[ 0 ].x * vec.x + mat[ 1 ].x * vec.y + mat[ 2 ].x * vec.z,
  446.         mat[ 0 ].y * vec.x + mat[ 1 ].y * vec.y + mat[ 2 ].y * vec.z,
  447.         mat[ 0 ].z * vec.x + mat[ 1 ].z * vec.y + mat[ 2 ].z * vec.z );
  448. }
  449.  
  450. ID_INLINE idMat3 idMat3::operator*( const idMat3 &a ) const {
  451.     int i, j;
  452.     const float *m1Ptr, *m2Ptr;
  453.     float *dstPtr;
  454.     idMat3 dst;
  455.  
  456.     m1Ptr = reinterpret_cast<const float *>(this);
  457.     m2Ptr = reinterpret_cast<const float *>(&a);
  458.     dstPtr = reinterpret_cast<float *>(&dst);
  459.  
  460.     for ( i = 0; i < 3; i++ ) {
  461.         for ( j = 0; j < 3; j++ ) {
  462.             *dstPtr = m1Ptr[0] * m2Ptr[ 0 * 3 + j ]
  463.                     + m1Ptr[1] * m2Ptr[ 1 * 3 + j ]
  464.                     + m1Ptr[2] * m2Ptr[ 2 * 3 + j ];
  465.             dstPtr++;
  466.         }
  467.         m1Ptr += 3;
  468.     }
  469.     return dst;
  470. }
  471.  
  472. // RAVEN BEGIN
  473. // jscott: divide is overridden to multiply by transpose
  474. ID_INLINE idVec3 idMat3::operator/( const idVec3 &vec ) const 
  475. {
  476.     return( idVec3(
  477.         mat[0].x * vec.x + mat[0].y * vec.y + mat[0].z * vec.z,
  478.         mat[1].x * vec.x + mat[1].y * vec.y + mat[1].z * vec.z,
  479.         mat[2].x * vec.x + mat[2].y * vec.y + mat[2].z * vec.z ) );
  480. }
  481.  
  482. ID_INLINE idMat3 idMat3::operator/( const idMat3 &a ) const 
  483. {
  484.     idMat3        dst;
  485.  
  486.     dst[0].x = mat[0].x * a.mat[0].x + mat[0].y * a.mat[0].y + mat[0].z * a.mat[0].z;
  487.     dst[0].y = mat[0].x * a.mat[1].x + mat[0].y * a.mat[1].y + mat[0].z * a.mat[1].z;
  488.     dst[0].z = mat[0].x * a.mat[2].x + mat[0].y * a.mat[2].y + mat[0].z * a.mat[2].z;
  489.  
  490.     dst[1].x = mat[1].x * a.mat[0].x + mat[1].y * a.mat[0].y + mat[1].z * a.mat[0].z;
  491.     dst[1].y = mat[1].x * a.mat[1].x + mat[1].y * a.mat[1].y + mat[1].z * a.mat[1].z;
  492.     dst[1].z = mat[1].x * a.mat[2].x + mat[1].y * a.mat[2].y + mat[1].z * a.mat[2].z;
  493.  
  494.     dst[2].x = mat[2].x * a.mat[0].x + mat[2].y * a.mat[0].y + mat[2].z * a.mat[0].z;
  495.     dst[2].y = mat[2].x * a.mat[1].x + mat[2].y * a.mat[1].y + mat[2].z * a.mat[1].z;
  496.     dst[2].z = mat[2].x * a.mat[2].x + mat[2].y * a.mat[2].y + mat[2].z * a.mat[2].z;
  497.  
  498.     return( dst );
  499. }
  500. // RAVEN END
  501.  
  502. ID_INLINE idMat3 idMat3::operator*( const float a ) const {
  503.     return idMat3(
  504.         mat[0].x * a, mat[0].y * a, mat[0].z * a,
  505.         mat[1].x * a, mat[1].y * a, mat[1].z * a,
  506.         mat[2].x * a, mat[2].y * a, mat[2].z * a );
  507. }
  508.  
  509. ID_INLINE idMat3 idMat3::operator+( const idMat3 &a ) const {
  510.     return idMat3(
  511.         mat[0].x + a[0].x, mat[0].y + a[0].y, mat[0].z + a[0].z,
  512.         mat[1].x + a[1].x, mat[1].y + a[1].y, mat[1].z + a[1].z,
  513.         mat[2].x + a[2].x, mat[2].y + a[2].y, mat[2].z + a[2].z );
  514. }
  515.     
  516. ID_INLINE idMat3 idMat3::operator-( const idMat3 &a ) const {
  517.     return idMat3(
  518.         mat[0].x - a[0].x, mat[0].y - a[0].y, mat[0].z - a[0].z,
  519.         mat[1].x - a[1].x, mat[1].y - a[1].y, mat[1].z - a[1].z,
  520.         mat[2].x - a[2].x, mat[2].y - a[2].y, mat[2].z - a[2].z );
  521. }
  522.  
  523. ID_INLINE idMat3 &idMat3::operator*=( const float a ) {
  524.     mat[0].x *= a; mat[0].y *= a; mat[0].z *= a;
  525.     mat[1].x *= a; mat[1].y *= a; mat[1].z *= a; 
  526.     mat[2].x *= a; mat[2].y *= a; mat[2].z *= a;
  527.  
  528.     return *this;
  529. }
  530.  
  531. ID_INLINE idMat3 &idMat3::operator*=( const idMat3 &a ) {
  532.     int i, j;
  533.     const float *m2Ptr;
  534.     float *m1Ptr, dst[3];
  535.  
  536.     m1Ptr = reinterpret_cast<float *>(this);
  537.     m2Ptr = reinterpret_cast<const float *>(&a);
  538.  
  539.     for ( i = 0; i < 3; i++ ) {
  540.         for ( j = 0; j < 3; j++ ) {
  541.             dst[j]  = m1Ptr[0] * m2Ptr[ 0 * 3 + j ]
  542.                     + m1Ptr[1] * m2Ptr[ 1 * 3 + j ]
  543.                     + m1Ptr[2] * m2Ptr[ 2 * 3 + j ];
  544.         }
  545.         m1Ptr[0] = dst[0]; m1Ptr[1] = dst[1]; m1Ptr[2] = dst[2];
  546.         m1Ptr += 3;
  547.     }
  548.     return *this;
  549. }
  550.  
  551. ID_INLINE idMat3 &idMat3::operator+=( const idMat3 &a ) {
  552.     mat[0].x += a[0].x; mat[0].y += a[0].y; mat[0].z += a[0].z;
  553.     mat[1].x += a[1].x; mat[1].y += a[1].y; mat[1].z += a[1].z;
  554.     mat[2].x += a[2].x; mat[2].y += a[2].y; mat[2].z += a[2].z;
  555.  
  556.     return *this;
  557. }
  558.  
  559. ID_INLINE idMat3 &idMat3::operator-=( const idMat3 &a ) {
  560.     mat[0].x -= a[0].x; mat[0].y -= a[0].y; mat[0].z -= a[0].z;
  561.     mat[1].x -= a[1].x; mat[1].y -= a[1].y; mat[1].z -= a[1].z;
  562.     mat[2].x -= a[2].x; mat[2].y -= a[2].y; mat[2].z -= a[2].z;
  563.  
  564.     return *this;
  565. }
  566.  
  567. ID_INLINE idVec3 operator*( const idVec3 &vec, const idMat3 &mat ) {
  568.     return mat * vec;
  569. }
  570.  
  571. ID_INLINE idMat3 operator*( const float a, const idMat3 &mat ) {
  572.     return mat * a;
  573. }
  574.  
  575. ID_INLINE idVec3 &operator*=( idVec3 &vec, const idMat3 &mat ) {
  576.     float x = mat[ 0 ].x * vec.x + mat[ 1 ].x * vec.y + mat[ 2 ].x * vec.z;
  577.     float y = mat[ 0 ].y * vec.x + mat[ 1 ].y * vec.y + mat[ 2 ].y * vec.z;
  578.     vec.z = mat[ 0 ].z * vec.x + mat[ 1 ].z * vec.y + mat[ 2 ].z * vec.z;
  579.     vec.x = x;
  580.     vec.y = y;
  581.     return vec;
  582. }
  583.  
  584. ID_INLINE bool idMat3::Compare( const idMat3 &a ) const {
  585.     if ( mat[0].Compare( a[0] ) &&
  586.         mat[1].Compare( a[1] ) &&
  587.         mat[2].Compare( a[2] ) ) {
  588.         return true;
  589.     }
  590.     return false;
  591. }
  592.  
  593. ID_INLINE bool idMat3::Compare( const idMat3 &a, const float epsilon ) const {
  594.     if ( mat[0].Compare( a[0], epsilon ) &&
  595.         mat[1].Compare( a[1], epsilon ) &&
  596.         mat[2].Compare( a[2], epsilon ) ) {
  597.         return true;
  598.     }
  599.     return false;
  600. }
  601.  
  602. ID_INLINE bool idMat3::operator==( const idMat3 &a ) const {
  603.     return Compare( a );
  604. }
  605.  
  606. ID_INLINE bool idMat3::operator!=( const idMat3 &a ) const {
  607.     return !Compare( a );
  608. }
  609.  
  610. ID_INLINE void idMat3::Zero( void ) {
  611.     memset( mat, 0, sizeof( idMat3 ) );
  612. }
  613.  
  614. ID_INLINE void idMat3::Identity( void ) {
  615.     *this = mat3_identity;
  616. }
  617.  
  618. ID_INLINE bool idMat3::IsIdentity( const float epsilon ) const {
  619.     return Compare( mat3_identity, epsilon );
  620. }
  621.  
  622. ID_INLINE bool idMat3::IsSymmetric( const float epsilon ) const {
  623.     if ( idMath::Fabs( mat[0][1] - mat[1][0] ) > epsilon ) {
  624.         return false;
  625.     }
  626.     if ( idMath::Fabs( mat[0][2] - mat[2][0] ) > epsilon ) {
  627.         return false;
  628.     }
  629.     if ( idMath::Fabs( mat[1][2] - mat[2][1] ) > epsilon ) {
  630.         return false;
  631.     }
  632.     return true;
  633. }
  634.  
  635. ID_INLINE bool idMat3::IsDiagonal( const float epsilon ) const {
  636.     if ( idMath::Fabs( mat[0][1] ) > epsilon ||
  637.         idMath::Fabs( mat[0][2] ) > epsilon ||
  638.         idMath::Fabs( mat[1][0] ) > epsilon ||
  639.         idMath::Fabs( mat[1][2] ) > epsilon ||
  640.         idMath::Fabs( mat[2][0] ) > epsilon ||
  641.         idMath::Fabs( mat[2][1] ) > epsilon ) {
  642.         return false;
  643.     }
  644.     return true;
  645. }
  646.  
  647. ID_INLINE bool idMat3::IsRotated( void ) const {
  648.     // NOTE: assumes the 3x3 matrix is orthonormal
  649.     return ( mat[0][0] != 1.0f || mat[1][1] != 1.0f || mat[2][2] != 1.0f );
  650. }
  651.  
  652. ID_INLINE void idMat3::ProjectVector( const idVec3 &src, idVec3 &dst ) const {
  653.     dst.x = src * mat[ 0 ];
  654.     dst.y = src * mat[ 1 ];
  655.     dst.z = src * mat[ 2 ];
  656. }
  657.  
  658. ID_INLINE void idMat3::UnprojectVector( const idVec3 &src, idVec3 &dst ) const {
  659.     dst = mat[ 0 ] * src.x + mat[ 1 ] * src.y + mat[ 2 ] * src.z;
  660. }
  661.  
  662. ID_INLINE bool idMat3::FixDegeneracies( void ) {
  663.     bool r = mat[0].FixDegenerateNormal();
  664.     r |= mat[1].FixDegenerateNormal();
  665.     r |= mat[2].FixDegenerateNormal();
  666.     return r;
  667. }
  668.  
  669. ID_INLINE bool idMat3::FixDenormals( void ) {
  670.     bool r = mat[0].FixDenormals();
  671.     r |= mat[1].FixDenormals();
  672.     r |= mat[2].FixDenormals();
  673.     return r;
  674. }
  675.  
  676. ID_INLINE float idMat3::Trace( void ) const {
  677.     return ( mat[0][0] + mat[1][1] + mat[2][2] );
  678. }
  679.  
  680. ID_INLINE idMat3 idMat3::OrthoNormalize( void ) const {
  681.     idMat3 ortho;
  682.  
  683.     ortho = *this;
  684.     ortho[ 0 ].Normalize();
  685.     ortho[ 2 ].Cross( mat[ 0 ], mat[ 1 ] );
  686.     ortho[ 2 ].Normalize();
  687.     ortho[ 1 ].Cross( mat[ 2 ], mat[ 0 ] );
  688.     ortho[ 1 ].Normalize();
  689.     return ortho;
  690. }
  691.  
  692. ID_INLINE idMat3 &idMat3::OrthoNormalizeSelf( void ) {
  693.     mat[ 0 ].Normalize();
  694.     mat[ 2 ].Cross( mat[ 0 ], mat[ 1 ] );
  695.     mat[ 2 ].Normalize();
  696.     mat[ 1 ].Cross( mat[ 2 ], mat[ 0 ] );
  697.     mat[ 1 ].Normalize();
  698.     return *this;
  699. }
  700.  
  701. ID_INLINE idMat3 idMat3::Transpose( void ) const {
  702.     return idMat3(    mat[0][0], mat[1][0], mat[2][0],
  703.                     mat[0][1], mat[1][1], mat[2][1],
  704.                     mat[0][2], mat[1][2], mat[2][2] );
  705. }
  706.  
  707. ID_INLINE idMat3 &idMat3::TransposeSelf( void ) {
  708.     float tmp0, tmp1, tmp2;
  709.  
  710.     tmp0 = mat[0][1];
  711.     mat[0][1] = mat[1][0];
  712.     mat[1][0] = tmp0;
  713.     tmp1 = mat[0][2];
  714.     mat[0][2] = mat[2][0];
  715.     mat[2][0] = tmp1;
  716.     tmp2 = mat[1][2];
  717.     mat[1][2] = mat[2][1];
  718.     mat[2][1] = tmp2;
  719.  
  720.     return *this;
  721. }
  722.  
  723. ID_INLINE idMat3 idMat3::Inverse( void ) const {
  724.     idMat3 invMat;
  725.  
  726.     invMat = *this;
  727. #ifdef _DEBUG    
  728.     assert ( invMat.InverseSelf() );
  729. #else
  730.     invMat.InverseSelf();
  731. #endif
  732.     return invMat;
  733. }
  734.  
  735. ID_INLINE idMat3 idMat3::InverseFast( void ) const {
  736.     idMat3 invMat;
  737.  
  738.     invMat = *this;
  739. #ifdef _DEBUG    
  740.     assert ( invMat.InverseFastSelf() );
  741. #else
  742.     invMat.InverseFastSelf();
  743. #endif
  744.     return invMat;
  745. }
  746.  
  747. ID_INLINE idMat3 idMat3::TransposeMultiply( const idMat3 &b ) const {
  748.     return idMat3(    mat[0].x * b[0].x + mat[1].x * b[1].x + mat[2].x * b[2].x,
  749.                     mat[0].x * b[0].y + mat[1].x * b[1].y + mat[2].x * b[2].y,
  750.                     mat[0].x * b[0].z + mat[1].x * b[1].z + mat[2].x * b[2].z,
  751.                     mat[0].y * b[0].x + mat[1].y * b[1].x + mat[2].y * b[2].x,
  752.                     mat[0].y * b[0].y + mat[1].y * b[1].y + mat[2].y * b[2].y,
  753.                     mat[0].y * b[0].z + mat[1].y * b[1].z + mat[2].y * b[2].z,
  754.                     mat[0].z * b[0].x + mat[1].z * b[1].x + mat[2].z * b[2].x,
  755.                     mat[0].z * b[0].y + mat[1].z * b[1].y + mat[2].z * b[2].y,
  756.                     mat[0].z * b[0].z + mat[1].z * b[1].z + mat[2].z * b[2].z );
  757. }
  758.  
  759. ID_INLINE void TransposeMultiply( const idMat3 &transpose, const idMat3 &b, idMat3 &dst ) {
  760.     dst[0].x = transpose[0].x * b[0].x + transpose[1].x * b[1].x + transpose[2].x * b[2].x;
  761.     dst[0].y = transpose[0].x * b[0].y + transpose[1].x * b[1].y + transpose[2].x * b[2].y;
  762.     dst[0].z = transpose[0].x * b[0].z + transpose[1].x * b[1].z + transpose[2].x * b[2].z;
  763.     dst[1].x = transpose[0].y * b[0].x + transpose[1].y * b[1].x + transpose[2].y * b[2].x;
  764.     dst[1].y = transpose[0].y * b[0].y + transpose[1].y * b[1].y + transpose[2].y * b[2].y;
  765.     dst[1].z = transpose[0].y * b[0].z + transpose[1].y * b[1].z + transpose[2].y * b[2].z;
  766.     dst[2].x = transpose[0].z * b[0].x + transpose[1].z * b[1].x + transpose[2].z * b[2].x;
  767.     dst[2].y = transpose[0].z * b[0].y + transpose[1].z * b[1].y + transpose[2].z * b[2].y;
  768.     dst[2].z = transpose[0].z * b[0].z + transpose[1].z * b[1].z + transpose[2].z * b[2].z;
  769. }
  770.  
  771. ID_INLINE idMat3 SkewSymmetric( idVec3 const &src ) {
  772.     return idMat3( 0.0f, -src.z,  src.y, src.z,   0.0f, -src.x, -src.y,  src.x,   0.0f );
  773. }
  774.  
  775. ID_INLINE int idMat3::GetDimension( void ) const {
  776.     return 9;
  777. }
  778.  
  779. // RAVEN BEGIN
  780. // abahr: made version for when getting vectors
  781. ID_INLINE int idMat3::GetVec3Dimension( void ) const {
  782.     return 3;
  783. }
  784.  
  785. ID_INLINE const float *idMat3::ToFloatPtr( void ) const {
  786.     return mat[0].ToFloatPtr();
  787. }
  788.  
  789. ID_INLINE float *idMat3::ToFloatPtr( void ) {
  790.     return mat[0].ToFloatPtr();
  791. }
  792.  
  793.  
  794. //===============================================================
  795. //
  796. //    idMat4 - 4x4 matrix
  797. //
  798. //===============================================================
  799.  
  800. class idMat4 {
  801. public:
  802.                     idMat4( void );
  803.                     explicit idMat4( const idVec4 &x, const idVec4 &y, const idVec4 &z, const idVec4 &w );
  804.                     explicit idMat4(const float xx, const float xy, const float xz, const float xw,
  805.                                     const float yx, const float yy, const float yz, const float yw,
  806.                                     const float zx, const float zy, const float zz, const float zw,
  807.                                     const float wx, const float wy, const float wz, const float ww );
  808.                     explicit idMat4( const idMat3 &rotation, const idVec3 &translation );
  809.                     explicit idMat4( const float src[ 4 ][ 4 ] );
  810.  
  811.     const idVec4 &    operator[]( int index ) const;
  812.     idVec4 &        operator[]( int index );
  813.     idMat4            operator*( const float a ) const;
  814.     idVec4            operator*( const idVec4 &vec ) const;
  815.     idVec3            operator*( const idVec3 &vec ) const;
  816.     idMat4            operator*( const idMat4 &a ) const;
  817.     idMat4            operator+( const idMat4 &a ) const;
  818.     idMat4            operator-( const idMat4 &a ) const;
  819.     idMat4 &        operator*=( const float a );
  820.     idMat4 &        operator*=( const idMat4 &a );
  821.     idMat4 &        operator+=( const idMat4 &a );
  822.     idMat4 &        operator-=( const idMat4 &a );
  823.  
  824.     friend idMat4    operator*( const float a, const idMat4 &mat );
  825.     friend idVec4    operator*( const idVec4 &vec, const idMat4 &mat );
  826.     friend idVec3    operator*( const idVec3 &vec, const idMat4 &mat );
  827.     friend idVec4 &    operator*=( idVec4 &vec, const idMat4 &mat );
  828.     friend idVec3 &    operator*=( idVec3 &vec, const idMat4 &mat );
  829.  
  830.     bool            Compare( const idMat4 &a ) const;                        // exact compare, no epsilon
  831.     bool            Compare( const idMat4 &a, const float epsilon ) const;    // compare with epsilon
  832.     bool            operator==( const idMat4 &a ) const;                    // exact compare, no epsilon
  833.     bool            operator!=( const idMat4 &a ) const;                    // exact compare, no epsilon
  834.  
  835.     void            Zero( void );
  836.     void            Identity( void );
  837.     bool            IsIdentity( const float epsilon = MATRIX_EPSILON ) const;
  838.     bool            IsSymmetric( const float epsilon = MATRIX_EPSILON ) const;
  839.     bool            IsDiagonal( const float epsilon = MATRIX_EPSILON ) const;
  840.     bool            IsRotated( void ) const;
  841.  
  842.     void            ProjectVector( const idVec4 &src, idVec4 &dst ) const;
  843.     void            UnprojectVector( const idVec4 &src, idVec4 &dst ) const;
  844.  
  845.     float            Trace( void ) const;
  846.     float            Determinant( void ) const;
  847.     idMat4            Transpose( void ) const;    // returns transpose
  848.     idMat4 &        TransposeSelf( void );
  849.     idMat4            Inverse( void ) const;        // returns the inverse ( m * m.Inverse() = identity )
  850.     bool            InverseSelf( void );        // returns false if determinant is zero
  851.     idMat4            InverseFast( void ) const;    // returns the inverse ( m * m.Inverse() = identity )
  852.     bool            InverseFastSelf( void );    // returns false if determinant is zero
  853.     idMat4            TransposeMultiply( const idMat4 &b ) const;
  854.  
  855.     int                GetDimension( void ) const;
  856.  
  857.     const float *    ToFloatPtr( void ) const;
  858.     float *            ToFloatPtr( void );
  859.     const char *    ToString( int precision = 2 ) const;
  860.  
  861. private:
  862.     idVec4            mat[ 4 ];
  863. };
  864.  
  865. extern idMat4 mat4_zero;
  866. extern idMat4 mat4_identity;
  867. #define mat4_default    mat4_identity
  868.  
  869. ID_INLINE idMat4::idMat4( void ) {
  870. }
  871.  
  872. ID_INLINE idMat4::idMat4( const idVec4 &x, const idVec4 &y, const idVec4 &z, const idVec4 &w ) {
  873.     mat[ 0 ] = x;
  874.     mat[ 1 ] = y;
  875.     mat[ 2 ] = z;
  876.     mat[ 3 ] = w;
  877. }
  878.  
  879. ID_INLINE idMat4::idMat4( const float xx, const float xy, const float xz, const float xw,
  880.                             const float yx, const float yy, const float yz, const float yw,
  881.                             const float zx, const float zy, const float zz, const float zw,
  882.                             const float wx, const float wy, const float wz, const float ww ) {
  883.     mat[0][0] = xx; mat[0][1] = xy; mat[0][2] = xz; mat[0][3] = xw;
  884.     mat[1][0] = yx; mat[1][1] = yy; mat[1][2] = yz; mat[1][3] = yw;
  885.     mat[2][0] = zx; mat[2][1] = zy; mat[2][2] = zz; mat[2][3] = zw;
  886.     mat[3][0] = wx; mat[3][1] = wy; mat[3][2] = wz; mat[3][3] = ww;
  887. }
  888.  
  889. ID_INLINE idMat4::idMat4( const idMat3 &rotation, const idVec3 &translation ) {
  890.     // NOTE: idMat3 is transposed because it is column-major
  891.     mat[ 0 ][ 0 ] = rotation[0][0];
  892.     mat[ 0 ][ 1 ] = rotation[1][0];
  893.     mat[ 0 ][ 2 ] = rotation[2][0];
  894.     mat[ 0 ][ 3 ] = translation[0];
  895.     mat[ 1 ][ 0 ] = rotation[0][1];
  896.     mat[ 1 ][ 1 ] = rotation[1][1];
  897.     mat[ 1 ][ 2 ] = rotation[2][1];
  898.     mat[ 1 ][ 3 ] = translation[1];
  899.     mat[ 2 ][ 0 ] = rotation[0][2];
  900.     mat[ 2 ][ 1 ] = rotation[1][2];
  901.     mat[ 2 ][ 2 ] = rotation[2][2];
  902.     mat[ 2 ][ 3 ] = translation[2];
  903.     mat[ 3 ][ 0 ] = 0.0f;
  904.     mat[ 3 ][ 1 ] = 0.0f;
  905.     mat[ 3 ][ 2 ] = 0.0f;
  906.     mat[ 3 ][ 3 ] = 1.0f;
  907. }
  908.  
  909. ID_INLINE idMat4::idMat4( const float src[ 4 ][ 4 ] ) {
  910.     memcpy( mat, src, 4 * 4 * sizeof( float ) );
  911. }
  912.  
  913. ID_INLINE const idVec4 &idMat4::operator[]( int index ) const {
  914.     //assert( ( index >= 0 ) && ( index < 4 ) );
  915.     return mat[ index ];
  916. }
  917.  
  918. ID_INLINE idVec4 &idMat4::operator[]( int index ) {
  919.     //assert( ( index >= 0 ) && ( index < 4 ) );
  920.     return mat[ index ];
  921. }
  922.  
  923. ID_INLINE idMat4 idMat4::operator*( const float a ) const {
  924.     return idMat4(
  925.         mat[0].x * a, mat[0].y * a, mat[0].z * a, mat[0].w * a,
  926.         mat[1].x * a, mat[1].y * a, mat[1].z * a, mat[1].w * a,
  927.         mat[2].x * a, mat[2].y * a, mat[2].z * a, mat[2].w * a,
  928.         mat[3].x * a, mat[3].y * a, mat[3].z * a, mat[3].w * a );
  929. }
  930.  
  931. ID_INLINE idVec4 idMat4::operator*( const idVec4 &vec ) const {
  932.     return idVec4(
  933.         mat[ 0 ].x * vec.x + mat[ 0 ].y * vec.y + mat[ 0 ].z * vec.z + mat[ 0 ].w * vec.w,
  934.         mat[ 1 ].x * vec.x + mat[ 1 ].y * vec.y + mat[ 1 ].z * vec.z + mat[ 1 ].w * vec.w,
  935.         mat[ 2 ].x * vec.x + mat[ 2 ].y * vec.y + mat[ 2 ].z * vec.z + mat[ 2 ].w * vec.w,
  936.         mat[ 3 ].x * vec.x + mat[ 3 ].y * vec.y + mat[ 3 ].z * vec.z + mat[ 3 ].w * vec.w );
  937. }
  938.  
  939. ID_INLINE idVec3 idMat4::operator*( const idVec3 &vec ) const {
  940.     float s = mat[ 3 ].x * vec.x + mat[ 3 ].y * vec.y + mat[ 3 ].z * vec.z + mat[ 3 ].w;
  941.     if ( s == 0.0f ) {
  942.         return idVec3( 0.0f, 0.0f, 0.0f );
  943.     }
  944.     if ( s == 1.0f ) {
  945.         return idVec3(
  946.             mat[ 0 ].x * vec.x + mat[ 0 ].y * vec.y + mat[ 0 ].z * vec.z + mat[ 0 ].w,
  947.             mat[ 1 ].x * vec.x + mat[ 1 ].y * vec.y + mat[ 1 ].z * vec.z + mat[ 1 ].w,
  948.             mat[ 2 ].x * vec.x + mat[ 2 ].y * vec.y + mat[ 2 ].z * vec.z + mat[ 2 ].w );
  949.     }
  950.     else {
  951.         float invS = 1.0f / s;
  952.         return idVec3(
  953.             (mat[ 0 ].x * vec.x + mat[ 0 ].y * vec.y + mat[ 0 ].z * vec.z + mat[ 0 ].w) * invS,
  954.             (mat[ 1 ].x * vec.x + mat[ 1 ].y * vec.y + mat[ 1 ].z * vec.z + mat[ 1 ].w) * invS,
  955.             (mat[ 2 ].x * vec.x + mat[ 2 ].y * vec.y + mat[ 2 ].z * vec.z + mat[ 2 ].w) * invS );
  956.     }
  957. }
  958.  
  959. ID_INLINE idMat4 idMat4::operator*( const idMat4 &a ) const {
  960.     int i, j;
  961.     const float *m1Ptr, *m2Ptr;
  962.     float *dstPtr;
  963.     idMat4 dst;
  964.  
  965.     m1Ptr = reinterpret_cast<const float *>(this);
  966.     m2Ptr = reinterpret_cast<const float *>(&a);
  967.     dstPtr = reinterpret_cast<float *>(&dst);
  968.  
  969.     for ( i = 0; i < 4; i++ ) {
  970.         for ( j = 0; j < 4; j++ ) {
  971.             *dstPtr = m1Ptr[0] * m2Ptr[ 0 * 4 + j ]
  972.                     + m1Ptr[1] * m2Ptr[ 1 * 4 + j ]
  973.                     + m1Ptr[2] * m2Ptr[ 2 * 4 + j ]
  974.                     + m1Ptr[3] * m2Ptr[ 3 * 4 + j ];
  975.             dstPtr++;
  976.         }
  977.         m1Ptr += 4;
  978.     }
  979.     return dst;
  980. }
  981.  
  982. ID_INLINE idMat4 idMat4::operator+( const idMat4 &a ) const {
  983.     return idMat4( 
  984.         mat[0].x + a[0].x, mat[0].y + a[0].y, mat[0].z + a[0].z, mat[0].w + a[0].w,
  985.         mat[1].x + a[1].x, mat[1].y + a[1].y, mat[1].z + a[1].z, mat[1].w + a[1].w,
  986.         mat[2].x + a[2].x, mat[2].y + a[2].y, mat[2].z + a[2].z, mat[2].w + a[2].w,
  987.         mat[3].x + a[3].x, mat[3].y + a[3].y, mat[3].z + a[3].z, mat[3].w + a[3].w );
  988. }
  989.     
  990. ID_INLINE idMat4 idMat4::operator-( const idMat4 &a ) const {
  991.     return idMat4( 
  992.         mat[0].x - a[0].x, mat[0].y - a[0].y, mat[0].z - a[0].z, mat[0].w - a[0].w,
  993.         mat[1].x - a[1].x, mat[1].y - a[1].y, mat[1].z - a[1].z, mat[1].w - a[1].w,
  994.         mat[2].x - a[2].x, mat[2].y - a[2].y, mat[2].z - a[2].z, mat[2].w - a[2].w,
  995.         mat[3].x - a[3].x, mat[3].y - a[3].y, mat[3].z - a[3].z, mat[3].w - a[3].w );
  996. }
  997.  
  998. ID_INLINE idMat4 &idMat4::operator*=( const float a ) {
  999.     mat[0].x *= a; mat[0].y *= a; mat[0].z *= a; mat[0].w *= a;
  1000.     mat[1].x *= a; mat[1].y *= a; mat[1].z *= a; mat[1].w *= a;
  1001.     mat[2].x *= a; mat[2].y *= a; mat[2].z *= a; mat[2].w *= a;
  1002.     mat[3].x *= a; mat[3].y *= a; mat[3].z *= a; mat[3].w *= a;
  1003.     return *this;
  1004. }
  1005.  
  1006. ID_INLINE idMat4 &idMat4::operator*=( const idMat4 &a ) {
  1007.     *this = (*this) * a;
  1008.     return *this;
  1009. }
  1010.  
  1011. ID_INLINE idMat4 &idMat4::operator+=( const idMat4 &a ) {
  1012.     mat[0].x += a[0].x; mat[0].y += a[0].y; mat[0].z += a[0].z; mat[0].w += a[0].w;
  1013.     mat[1].x += a[1].x; mat[1].y += a[1].y; mat[1].z += a[1].z; mat[1].w += a[1].w;
  1014.     mat[2].x += a[2].x; mat[2].y += a[2].y; mat[2].z += a[2].z; mat[2].w += a[2].w;
  1015.     mat[3].x += a[3].x; mat[3].y += a[3].y; mat[3].z += a[3].z; mat[3].w += a[3].w;
  1016.     return *this;
  1017. }
  1018.  
  1019. ID_INLINE idMat4 &idMat4::operator-=( const idMat4 &a ) {
  1020.     mat[0].x -= a[0].x; mat[0].y -= a[0].y; mat[0].z -= a[0].z; mat[0].w -= a[0].w;
  1021.     mat[1].x -= a[1].x; mat[1].y -= a[1].y; mat[1].z -= a[1].z; mat[1].w -= a[1].w;
  1022.     mat[2].x -= a[2].x; mat[2].y -= a[2].y; mat[2].z -= a[2].z; mat[2].w -= a[2].w;
  1023.     mat[3].x -= a[3].x; mat[3].y -= a[3].y; mat[3].z -= a[3].z; mat[3].w -= a[3].w;
  1024.     return *this;
  1025. }
  1026.  
  1027. ID_INLINE idMat4 operator*( const float a, const idMat4 &mat ) {
  1028.     return mat * a;
  1029. }
  1030.  
  1031. ID_INLINE idVec4 operator*( const idVec4 &vec, const idMat4 &mat ) {
  1032.     return mat * vec;
  1033. }
  1034.  
  1035. ID_INLINE idVec3 operator*( const idVec3 &vec, const idMat4 &mat ) {
  1036.     return mat * vec;
  1037. }
  1038.  
  1039. ID_INLINE idVec4 &operator*=( idVec4 &vec, const idMat4 &mat ) {
  1040.     vec = mat * vec;
  1041.     return vec;
  1042. }
  1043.  
  1044. ID_INLINE idVec3 &operator*=( idVec3 &vec, const idMat4 &mat ) {
  1045.     vec = mat * vec;
  1046.     return vec;
  1047. }
  1048.  
  1049. ID_INLINE bool idMat4::Compare( const idMat4 &a ) const {
  1050.     dword i;
  1051.     const float *ptr1, *ptr2;
  1052.  
  1053.     ptr1 = reinterpret_cast<const float *>(mat);
  1054.     ptr2 = reinterpret_cast<const float *>(a.mat);
  1055.     for ( i = 0; i < 4*4; i++ ) {
  1056.         if ( ptr1[i] != ptr2[i] ) {
  1057.             return false;
  1058.         }
  1059.     }
  1060.     return true;
  1061. }
  1062.  
  1063. ID_INLINE bool idMat4::Compare( const idMat4 &a, const float epsilon ) const {
  1064.     dword i;
  1065.     const float *ptr1, *ptr2;
  1066.  
  1067.     ptr1 = reinterpret_cast<const float *>(mat);
  1068.     ptr2 = reinterpret_cast<const float *>(a.mat);
  1069.     for ( i = 0; i < 4*4; i++ ) {
  1070.         if ( idMath::Fabs( ptr1[i] - ptr2[i] ) > epsilon ) {
  1071.             return false;
  1072.         }
  1073.     }
  1074.     return true;
  1075. }
  1076.  
  1077. ID_INLINE bool idMat4::operator==( const idMat4 &a ) const {
  1078.     return Compare( a );
  1079. }
  1080.  
  1081. ID_INLINE bool idMat4::operator!=( const idMat4 &a ) const {
  1082.     return !Compare( a );
  1083. }
  1084.  
  1085. ID_INLINE void idMat4::Zero( void ) {
  1086.     memset( mat, 0, sizeof( idMat4 ) );
  1087. }
  1088.  
  1089. ID_INLINE void idMat4::Identity( void ) {
  1090.     *this = mat4_identity;
  1091. }
  1092.  
  1093. ID_INLINE bool idMat4::IsIdentity( const float epsilon ) const {
  1094.     return Compare( mat4_identity, epsilon );
  1095. }
  1096.  
  1097. ID_INLINE bool idMat4::IsSymmetric( const float epsilon ) const {
  1098.     for ( int i = 1; i < 4; i++ ) {
  1099.         for ( int j = 0; j < i; j++ ) {
  1100.             if ( idMath::Fabs( mat[i][j] - mat[j][i] ) > epsilon ) {
  1101.                 return false;
  1102.             }
  1103.         }
  1104.     }
  1105.     return true;
  1106. }
  1107.  
  1108. ID_INLINE bool idMat4::IsDiagonal( const float epsilon ) const {
  1109.     for ( int i = 0; i < 4; i++ ) {
  1110.         for ( int j = 0; j < 4; j++ ) {
  1111.             if ( i != j && idMath::Fabs( mat[i][j] ) > epsilon ) {
  1112.                 return false;
  1113.             }
  1114.         }
  1115.     }
  1116.     return true;
  1117. }
  1118.  
  1119. ID_INLINE bool idMat4::IsRotated( void ) const {
  1120.     if ( !mat[ 0 ][ 1 ] && !mat[ 0 ][ 2 ] &&
  1121.         !mat[ 1 ][ 0 ] && !mat[ 1 ][ 2 ] &&
  1122.         !mat[ 2 ][ 0 ] && !mat[ 2 ][ 1 ] ) {
  1123.         return false;
  1124.     }
  1125.     return true;
  1126. }
  1127.  
  1128. ID_INLINE void idMat4::ProjectVector( const idVec4 &src, idVec4 &dst ) const {
  1129.     dst.x = src * mat[ 0 ];
  1130.     dst.y = src * mat[ 1 ];
  1131.     dst.z = src * mat[ 2 ];
  1132.     dst.w = src * mat[ 3 ];
  1133. }
  1134.  
  1135. ID_INLINE void idMat4::UnprojectVector( const idVec4 &src, idVec4 &dst ) const {
  1136.     dst = mat[ 0 ] * src.x + mat[ 1 ] * src.y + mat[ 2 ] * src.z + mat[ 3 ] * src.w;
  1137. }
  1138.  
  1139. ID_INLINE float idMat4::Trace( void ) const {
  1140.     return ( mat[0][0] + mat[1][1] + mat[2][2] + mat[3][3] );
  1141. }
  1142.  
  1143. ID_INLINE idMat4 idMat4::Inverse( void ) const {
  1144.     idMat4 invMat;
  1145.  
  1146.     invMat = *this;
  1147. #ifdef _DEBUG    
  1148.     assert ( invMat.InverseSelf() );
  1149. #else
  1150.     invMat.InverseSelf();
  1151. #endif
  1152.     return invMat;
  1153. }
  1154.  
  1155. ID_INLINE idMat4 idMat4::InverseFast( void ) const {
  1156.     idMat4 invMat;
  1157.  
  1158.     invMat = *this;
  1159. #ifdef _DEBUG    
  1160.     assert( invMat.InverseFastSelf() );
  1161. #else
  1162.     invMat.InverseFastSelf();
  1163. #endif
  1164.     return invMat;
  1165. }
  1166.  
  1167. ID_INLINE idMat4 idMat3::ToMat4( void ) const {
  1168.     // NOTE: idMat3 is transposed because it is column-major
  1169.     return idMat4(    mat[0][0],    mat[1][0],    mat[2][0],    0.0f,
  1170.                     mat[0][1],    mat[1][1],    mat[2][1],    0.0f,
  1171.                     mat[0][2],    mat[1][2],    mat[2][2],    0.0f,
  1172.                     0.0f,        0.0f,        0.0f,        1.0f );
  1173. }
  1174.  
  1175. ID_INLINE int idMat4::GetDimension( void ) const {
  1176.     return 16;
  1177. }
  1178.  
  1179. ID_INLINE const float *idMat4::ToFloatPtr( void ) const {
  1180.     return mat[0].ToFloatPtr();
  1181. }
  1182.  
  1183. ID_INLINE float *idMat4::ToFloatPtr( void ) {
  1184.     return mat[0].ToFloatPtr();
  1185. }
  1186.  
  1187.  
  1188. //===============================================================
  1189. //
  1190. //    idMat5 - 5x5 matrix
  1191. //
  1192. //===============================================================
  1193.  
  1194. class idMat5 {
  1195. public:
  1196.                     idMat5( void );
  1197.                     explicit idMat5( const idVec5 &v0, const idVec5 &v1, const idVec5 &v2, const idVec5 &v3, const idVec5 &v4 );
  1198.                     explicit idMat5( const float src[ 5 ][ 5 ] );
  1199.  
  1200.     const idVec5 &    operator[]( int index ) const;
  1201.     idVec5 &        operator[]( int index );
  1202.     idMat5            operator*( const float a ) const;
  1203.     idVec5            operator*( const idVec5 &vec ) const;
  1204.     idMat5            operator*( const idMat5 &a ) const;
  1205.     idMat5            operator+( const idMat5 &a ) const;
  1206.     idMat5            operator-( const idMat5 &a ) const;
  1207.     idMat5 &        operator*=( const float a );
  1208.     idMat5 &        operator*=( const idMat5 &a );
  1209.     idMat5 &        operator+=( const idMat5 &a );
  1210.     idMat5 &        operator-=( const idMat5 &a );
  1211.  
  1212.     friend idMat5    operator*( const float a, const idMat5 &mat );
  1213.     friend idVec5    operator*( const idVec5 &vec, const idMat5 &mat );
  1214.     friend idVec5 &    operator*=( idVec5 &vec, const idMat5 &mat );
  1215.  
  1216.     bool            Compare( const idMat5 &a ) const;                        // exact compare, no epsilon
  1217.     bool            Compare( const idMat5 &a, const float epsilon ) const;    // compare with epsilon
  1218.     bool            operator==( const idMat5 &a ) const;                    // exact compare, no epsilon
  1219.     bool            operator!=( const idMat5 &a ) const;                    // exact compare, no epsilon
  1220.  
  1221.     void            Zero( void );
  1222.     void            Identity( void );
  1223.     bool            IsIdentity( const float epsilon = MATRIX_EPSILON ) const;
  1224.     bool            IsSymmetric( const float epsilon = MATRIX_EPSILON ) const;
  1225.     bool            IsDiagonal( const float epsilon = MATRIX_EPSILON ) const;
  1226.  
  1227.     float            Trace( void ) const;
  1228.     float            Determinant( void ) const;
  1229.     idMat5            Transpose( void ) const;    // returns transpose
  1230.     idMat5 &        TransposeSelf( void );
  1231.     idMat5            Inverse( void ) const;        // returns the inverse ( m * m.Inverse() = identity )
  1232.     bool            InverseSelf( void );        // returns false if determinant is zero
  1233.     idMat5            InverseFast( void ) const;    // returns the inverse ( m * m.Inverse() = identity )
  1234.     bool            InverseFastSelf( void );    // returns false if determinant is zero
  1235.  
  1236.     int                GetDimension( void ) const;
  1237.  
  1238.     const float *    ToFloatPtr( void ) const;
  1239.     float *            ToFloatPtr( void );
  1240.     const char *    ToString( int precision = 2 ) const;
  1241.  
  1242. private:
  1243.     idVec5            mat[ 5 ];
  1244. };
  1245.  
  1246. extern idMat5 mat5_zero;
  1247. extern idMat5 mat5_identity;
  1248. #define mat5_default    mat5_identity
  1249.  
  1250. ID_INLINE idMat5::idMat5( void ) {
  1251. }
  1252.  
  1253. ID_INLINE idMat5::idMat5( const float src[ 5 ][ 5 ] ) {
  1254.     memcpy( mat, src, 5 * 5 * sizeof( float ) );
  1255. }
  1256.  
  1257. ID_INLINE idMat5::idMat5( const idVec5 &v0, const idVec5 &v1, const idVec5 &v2, const idVec5 &v3, const idVec5 &v4 ) {
  1258.     mat[0] = v0;
  1259.     mat[1] = v1;
  1260.     mat[2] = v2;
  1261.     mat[3] = v3;
  1262.     mat[4] = v4;
  1263. }
  1264.  
  1265. ID_INLINE const idVec5 &idMat5::operator[]( int index ) const {
  1266.     //assert( ( index >= 0 ) && ( index < 5 ) );
  1267.     return mat[ index ];
  1268. }
  1269.  
  1270. ID_INLINE idVec5 &idMat5::operator[]( int index ) {
  1271.     //assert( ( index >= 0 ) && ( index < 5 ) );
  1272.     return mat[ index ];
  1273. }
  1274.  
  1275. ID_INLINE idMat5 idMat5::operator*( const idMat5 &a ) const {
  1276.     int i, j;
  1277.     const float *m1Ptr, *m2Ptr;
  1278.     float *dstPtr;
  1279.     idMat5 dst;
  1280.  
  1281.     m1Ptr = reinterpret_cast<const float *>(this);
  1282.     m2Ptr = reinterpret_cast<const float *>(&a);
  1283.     dstPtr = reinterpret_cast<float *>(&dst);
  1284.  
  1285.     for ( i = 0; i < 5; i++ ) {
  1286.         for ( j = 0; j < 5; j++ ) {
  1287.             *dstPtr = m1Ptr[0] * m2Ptr[ 0 * 5 + j ]
  1288.                     + m1Ptr[1] * m2Ptr[ 1 * 5 + j ]
  1289.                     + m1Ptr[2] * m2Ptr[ 2 * 5 + j ]
  1290.                     + m1Ptr[3] * m2Ptr[ 3 * 5 + j ]
  1291.                     + m1Ptr[4] * m2Ptr[ 4 * 5 + j ];
  1292.             dstPtr++;
  1293.         }
  1294.         m1Ptr += 5;
  1295.     }
  1296.     return dst;
  1297. }
  1298.  
  1299. ID_INLINE idMat5 idMat5::operator*( const float a ) const {
  1300.     return idMat5(
  1301.         idVec5( mat[0][0] * a, mat[0][1] * a, mat[0][2] * a, mat[0][3] * a, mat[0][4] * a ),
  1302.         idVec5( mat[1][0] * a, mat[1][1] * a, mat[1][2] * a, mat[1][3] * a, mat[1][4] * a ),
  1303.         idVec5( mat[2][0] * a, mat[2][1] * a, mat[2][2] * a, mat[2][3] * a, mat[2][4] * a ),
  1304.         idVec5( mat[3][0] * a, mat[3][1] * a, mat[3][2] * a, mat[3][3] * a, mat[3][4] * a ),
  1305.         idVec5( mat[4][0] * a, mat[4][1] * a, mat[4][2] * a, mat[4][3] * a, mat[4][4] * a ) );
  1306. }
  1307.  
  1308. ID_INLINE idVec5 idMat5::operator*( const idVec5 &vec ) const {
  1309.     return idVec5(
  1310.         mat[0][0] * vec[0] + mat[0][1] * vec[1] + mat[0][2] * vec[2] + mat[0][3] * vec[3] + mat[0][4] * vec[4],
  1311.         mat[1][0] * vec[0] + mat[1][1] * vec[1] + mat[1][2] * vec[2] + mat[1][3] * vec[3] + mat[1][4] * vec[4],
  1312.         mat[2][0] * vec[0] + mat[2][1] * vec[1] + mat[2][2] * vec[2] + mat[2][3] * vec[3] + mat[2][4] * vec[4],
  1313.         mat[3][0] * vec[0] + mat[3][1] * vec[1] + mat[3][2] * vec[2] + mat[3][3] * vec[3] + mat[3][4] * vec[4],
  1314.         mat[4][0] * vec[0] + mat[4][1] * vec[1] + mat[4][2] * vec[2] + mat[4][3] * vec[3] + mat[4][4] * vec[4] );
  1315. }
  1316.  
  1317. ID_INLINE idMat5 idMat5::operator+( const idMat5 &a ) const {
  1318.     return idMat5(
  1319.         idVec5( mat[0][0] + a[0][0], mat[0][1] + a[0][1], mat[0][2] + a[0][2], mat[0][3] + a[0][3], mat[0][4] + a[0][4] ),
  1320.         idVec5( mat[1][0] + a[1][0], mat[1][1] + a[1][1], mat[1][2] + a[1][2], mat[1][3] + a[1][3], mat[1][4] + a[1][4] ),
  1321.         idVec5( mat[2][0] + a[2][0], mat[2][1] + a[2][1], mat[2][2] + a[2][2], mat[2][3] + a[2][3], mat[2][4] + a[2][4] ),
  1322.         idVec5( mat[3][0] + a[3][0], mat[3][1] + a[3][1], mat[3][2] + a[3][2], mat[3][3] + a[3][3], mat[3][4] + a[3][4] ),
  1323.         idVec5( mat[4][0] + a[4][0], mat[4][1] + a[4][1], mat[4][2] + a[4][2], mat[4][3] + a[4][3], mat[4][4] + a[4][4] ) );
  1324. }
  1325.  
  1326. ID_INLINE idMat5 idMat5::operator-( const idMat5 &a ) const {
  1327.     return idMat5(
  1328.         idVec5( mat[0][0] - a[0][0], mat[0][1] - a[0][1], mat[0][2] - a[0][2], mat[0][3] - a[0][3], mat[0][4] - a[0][4] ),
  1329.         idVec5( mat[1][0] - a[1][0], mat[1][1] - a[1][1], mat[1][2] - a[1][2], mat[1][3] - a[1][3], mat[1][4] - a[1][4] ),
  1330.         idVec5( mat[2][0] - a[2][0], mat[2][1] - a[2][1], mat[2][2] - a[2][2], mat[2][3] - a[2][3], mat[2][4] - a[2][4] ),
  1331.         idVec5( mat[3][0] - a[3][0], mat[3][1] - a[3][1], mat[3][2] - a[3][2], mat[3][3] - a[3][3], mat[3][4] - a[3][4] ),
  1332.         idVec5( mat[4][0] - a[4][0], mat[4][1] - a[4][1], mat[4][2] - a[4][2], mat[4][3] - a[4][3], mat[4][4] - a[4][4] ) );
  1333. }
  1334.  
  1335. ID_INLINE idMat5 &idMat5::operator*=( const float a ) {
  1336.     mat[0][0] *= a; mat[0][1] *= a; mat[0][2] *= a; mat[0][3] *= a; mat[0][4] *= a;
  1337.     mat[1][0] *= a; mat[1][1] *= a; mat[1][2] *= a; mat[1][3] *= a; mat[1][4] *= a;
  1338.     mat[2][0] *= a; mat[2][1] *= a; mat[2][2] *= a; mat[2][3] *= a; mat[2][4] *= a;
  1339.     mat[3][0] *= a; mat[3][1] *= a; mat[3][2] *= a; mat[3][3] *= a; mat[3][4] *= a;
  1340.     mat[4][0] *= a; mat[4][1] *= a; mat[4][2] *= a; mat[4][3] *= a; mat[4][4] *= a;
  1341.     return *this;
  1342. }
  1343.  
  1344. ID_INLINE idMat5 &idMat5::operator*=( const idMat5 &a ) {
  1345.     *this = *this * a;
  1346.     return *this;
  1347. }
  1348.  
  1349. ID_INLINE idMat5 &idMat5::operator+=( const idMat5 &a ) {
  1350.     mat[0][0] += a[0][0]; mat[0][1] += a[0][1]; mat[0][2] += a[0][2]; mat[0][3] += a[0][3]; mat[0][4] += a[0][4];
  1351.     mat[1][0] += a[1][0]; mat[1][1] += a[1][1]; mat[1][2] += a[1][2]; mat[1][3] += a[1][3]; mat[1][4] += a[1][4];
  1352.     mat[2][0] += a[2][0]; mat[2][1] += a[2][1]; mat[2][2] += a[2][2]; mat[2][3] += a[2][3]; mat[2][4] += a[2][4];
  1353.     mat[3][0] += a[3][0]; mat[3][1] += a[3][1]; mat[3][2] += a[3][2]; mat[3][3] += a[3][3]; mat[3][4] += a[3][4];
  1354.     mat[4][0] += a[4][0]; mat[4][1] += a[4][1]; mat[4][2] += a[4][2]; mat[4][3] += a[4][3]; mat[4][4] += a[4][4];
  1355.     return *this;
  1356. }
  1357.  
  1358. ID_INLINE idMat5 &idMat5::operator-=( const idMat5 &a ) {
  1359.     mat[0][0] -= a[0][0]; mat[0][1] -= a[0][1]; mat[0][2] -= a[0][2]; mat[0][3] -= a[0][3]; mat[0][4] -= a[0][4];
  1360.     mat[1][0] -= a[1][0]; mat[1][1] -= a[1][1]; mat[1][2] -= a[1][2]; mat[1][3] -= a[1][3]; mat[1][4] -= a[1][4];
  1361.     mat[2][0] -= a[2][0]; mat[2][1] -= a[2][1]; mat[2][2] -= a[2][2]; mat[2][3] -= a[2][3]; mat[2][4] -= a[2][4];
  1362.     mat[3][0] -= a[3][0]; mat[3][1] -= a[3][1]; mat[3][2] -= a[3][2]; mat[3][3] -= a[3][3]; mat[3][4] -= a[3][4];
  1363.     mat[4][0] -= a[4][0]; mat[4][1] -= a[4][1]; mat[4][2] -= a[4][2]; mat[4][3] -= a[4][3]; mat[4][4] -= a[4][4];
  1364.     return *this;
  1365. }
  1366.  
  1367. ID_INLINE idVec5 operator*( const idVec5 &vec, const idMat5 &mat ) {
  1368.     return mat * vec;
  1369. }
  1370.  
  1371. ID_INLINE idMat5 operator*( const float a, idMat5 const &mat ) {
  1372.     return mat * a;
  1373. }
  1374.  
  1375. ID_INLINE idVec5 &operator*=( idVec5 &vec, const idMat5 &mat ) {
  1376.     vec = mat * vec;
  1377.     return vec;
  1378. }
  1379.  
  1380. ID_INLINE bool idMat5::Compare( const idMat5 &a ) const {
  1381.     dword i;
  1382.     const float *ptr1, *ptr2;
  1383.  
  1384.     ptr1 = reinterpret_cast<const float *>(mat);
  1385.     ptr2 = reinterpret_cast<const float *>(a.mat);
  1386.     for ( i = 0; i < 5*5; i++ ) {
  1387.         if ( ptr1[i] != ptr2[i] ) {
  1388.             return false;
  1389.         }
  1390.     }
  1391.     return true;
  1392. }
  1393.  
  1394. ID_INLINE bool idMat5::Compare( const idMat5 &a, const float epsilon ) const {
  1395.     dword i;
  1396.     const float *ptr1, *ptr2;
  1397.  
  1398.     ptr1 = reinterpret_cast<const float *>(mat);
  1399.     ptr2 = reinterpret_cast<const float *>(a.mat);
  1400.     for ( i = 0; i < 5*5; i++ ) {
  1401.         if ( idMath::Fabs( ptr1[i] - ptr2[i] ) > epsilon ) {
  1402.             return false;
  1403.         }
  1404.     }
  1405.     return true;
  1406. }
  1407.  
  1408. ID_INLINE bool idMat5::operator==( const idMat5 &a ) const {
  1409.     return Compare( a );
  1410. }
  1411.  
  1412. ID_INLINE bool idMat5::operator!=( const idMat5 &a ) const {
  1413.     return !Compare( a );
  1414. }
  1415.  
  1416. ID_INLINE void idMat5::Zero( void ) {
  1417.     memset( mat, 0, sizeof( idMat5 ) );
  1418. }
  1419.  
  1420. ID_INLINE void idMat5::Identity( void ) {
  1421.     *this = mat5_identity;
  1422. }
  1423.  
  1424. ID_INLINE bool idMat5::IsIdentity( const float epsilon ) const {
  1425.     return Compare( mat5_identity, epsilon );
  1426. }
  1427.  
  1428. ID_INLINE bool idMat5::IsSymmetric( const float epsilon ) const {
  1429.     for ( int i = 1; i < 5; i++ ) {
  1430.         for ( int j = 0; j < i; j++ ) {
  1431.             if ( idMath::Fabs( mat[i][j] - mat[j][i] ) > epsilon ) {
  1432.                 return false;
  1433.             }
  1434.         }
  1435.     }
  1436.     return true;
  1437. }
  1438.  
  1439. ID_INLINE bool idMat5::IsDiagonal( const float epsilon ) const {
  1440.     for ( int i = 0; i < 5; i++ ) {
  1441.         for ( int j = 0; j < 5; j++ ) {
  1442.             if ( i != j && idMath::Fabs( mat[i][j] ) > epsilon ) {
  1443.                 return false;
  1444.             }
  1445.         }
  1446.     }
  1447.     return true;
  1448. }
  1449.  
  1450. ID_INLINE float idMat5::Trace( void ) const {
  1451.     return ( mat[0][0] + mat[1][1] + mat[2][2] + mat[3][3] + mat[4][4] );
  1452. }
  1453.  
  1454. ID_INLINE idMat5 idMat5::Inverse( void ) const {
  1455.     idMat5 invMat;
  1456.  
  1457.     invMat = *this;
  1458. #ifdef _DEBUG
  1459.     assert ( invMat.InverseSelf() );
  1460. #else
  1461.     invMat.InverseSelf();
  1462. #endif
  1463.     return invMat;
  1464. }
  1465.  
  1466. ID_INLINE idMat5 idMat5::InverseFast( void ) const {
  1467.     idMat5 invMat;
  1468.  
  1469.     invMat = *this;
  1470. #ifdef _DEBUG
  1471.     assert ( invMat.InverseFastSelf() );
  1472. #else
  1473.     invMat.InverseFastSelf();
  1474. #endif
  1475.     return invMat;
  1476. }
  1477.  
  1478. ID_INLINE int idMat5::GetDimension( void ) const {
  1479.     return 25;
  1480. }
  1481.  
  1482. ID_INLINE const float *idMat5::ToFloatPtr( void ) const {
  1483.     return mat[0].ToFloatPtr();
  1484. }
  1485.  
  1486. ID_INLINE float *idMat5::ToFloatPtr( void ) {
  1487.     return mat[0].ToFloatPtr();
  1488. }
  1489.  
  1490.  
  1491. //===============================================================
  1492. //
  1493. //    idMat6 - 6x6 matrix
  1494. //
  1495. //===============================================================
  1496.  
  1497. class idMat6 {
  1498. public:
  1499.                     idMat6( void );
  1500.                     explicit idMat6( const idVec6 &v0, const idVec6 &v1, const idVec6 &v2, const idVec6 &v3, const idVec6 &v4, const idVec6 &v5 );
  1501.                     explicit idMat6( const idMat3 &m0, const idMat3 &m1, const idMat3 &m2, const idMat3 &m3 );
  1502.                     explicit idMat6( const float src[ 6 ][ 6 ] );
  1503.  
  1504.     const idVec6 &    operator[]( int index ) const;
  1505.     idVec6 &        operator[]( int index );
  1506.     idMat6            operator*( const float a ) const;
  1507.     idVec6            operator*( const idVec6 &vec ) const;
  1508.     idMat6            operator*( const idMat6 &a ) const;
  1509.     idMat6            operator+( const idMat6 &a ) const;
  1510.     idMat6            operator-( const idMat6 &a ) const;
  1511.     idMat6 &        operator*=( const float a );
  1512.     idMat6 &        operator*=( const idMat6 &a );
  1513.     idMat6 &        operator+=( const idMat6 &a );
  1514.     idMat6 &        operator-=( const idMat6 &a );
  1515.  
  1516.     friend idMat6    operator*( const float a, const idMat6 &mat );
  1517.     friend idVec6    operator*( const idVec6 &vec, const idMat6 &mat );
  1518.     friend idVec6 &    operator*=( idVec6 &vec, const idMat6 &mat );
  1519.  
  1520.     bool            Compare( const idMat6 &a ) const;                        // exact compare, no epsilon
  1521.     bool            Compare( const idMat6 &a, const float epsilon ) const;    // compare with epsilon
  1522.     bool            operator==( const idMat6 &a ) const;                    // exact compare, no epsilon
  1523.     bool            operator!=( const idMat6 &a ) const;                    // exact compare, no epsilon
  1524.  
  1525.     void            Zero( void );
  1526.     void            Identity( void );
  1527.     bool            IsIdentity( const float epsilon = MATRIX_EPSILON ) const;
  1528.     bool            IsSymmetric( const float epsilon = MATRIX_EPSILON ) const;
  1529.     bool            IsDiagonal( const float epsilon = MATRIX_EPSILON ) const;
  1530.  
  1531.     idMat3            SubMat3( int n ) const;
  1532.     float            Trace( void ) const;
  1533.     float            Determinant( void ) const;
  1534.     idMat6            Transpose( void ) const;    // returns transpose
  1535.     idMat6 &        TransposeSelf( void );
  1536.     idMat6            Inverse( void ) const;        // returns the inverse ( m * m.Inverse() = identity )
  1537.     bool            InverseSelf( void );        // returns false if determinant is zero
  1538.     idMat6            InverseFast( void ) const;    // returns the inverse ( m * m.Inverse() = identity )
  1539.     bool            InverseFastSelf( void );    // returns false if determinant is zero
  1540.  
  1541.     int                GetDimension( void ) const;
  1542.  
  1543.     const float *    ToFloatPtr( void ) const;
  1544.     float *            ToFloatPtr( void );
  1545.     const char *    ToString( int precision = 2 ) const;
  1546.  
  1547. private:
  1548.     idVec6            mat[ 6 ];
  1549. };
  1550.  
  1551. extern idMat6 mat6_zero;
  1552. extern idMat6 mat6_identity;
  1553. #define mat6_default    mat6_identity
  1554.  
  1555. ID_INLINE idMat6::idMat6( void ) {
  1556. }
  1557.  
  1558. ID_INLINE idMat6::idMat6( const idMat3 &m0, const idMat3 &m1, const idMat3 &m2, const idMat3 &m3 ) {
  1559.     mat[0] = idVec6( m0[0][0], m0[0][1], m0[0][2], m1[0][0], m1[0][1], m1[0][2] );
  1560.     mat[1] = idVec6( m0[1][0], m0[1][1], m0[1][2], m1[1][0], m1[1][1], m1[1][2] );
  1561.     mat[2] = idVec6( m0[2][0], m0[2][1], m0[2][2], m1[2][0], m1[2][1], m1[2][2] );
  1562.     mat[3] = idVec6( m2[0][0], m2[0][1], m2[0][2], m3[0][0], m3[0][1], m3[0][2] );
  1563.     mat[4] = idVec6( m2[1][0], m2[1][1], m2[1][2], m3[1][0], m3[1][1], m3[1][2] );
  1564.     mat[5] = idVec6( m2[2][0], m2[2][1], m2[2][2], m3[2][0], m3[2][1], m3[2][2] );
  1565. }
  1566.  
  1567. ID_INLINE idMat6::idMat6( const idVec6 &v0, const idVec6 &v1, const idVec6 &v2, const idVec6 &v3, const idVec6 &v4, const idVec6 &v5 ) {
  1568.     mat[0] = v0;
  1569.     mat[1] = v1;
  1570.     mat[2] = v2;
  1571.     mat[3] = v3;
  1572.     mat[4] = v4;
  1573.     mat[5] = v5;
  1574. }
  1575.  
  1576. ID_INLINE idMat6::idMat6( const float src[ 6 ][ 6 ] ) {
  1577.     memcpy( mat, src, 6 * 6 * sizeof( float ) );
  1578. }
  1579.  
  1580. ID_INLINE const idVec6 &idMat6::operator[]( int index ) const {
  1581.     //assert( ( index >= 0 ) && ( index < 6 ) );
  1582.     return mat[ index ];
  1583. }
  1584.  
  1585. ID_INLINE idVec6 &idMat6::operator[]( int index ) {
  1586.     //assert( ( index >= 0 ) && ( index < 6 ) );
  1587.     return mat[ index ];
  1588. }
  1589.  
  1590. ID_INLINE idMat6 idMat6::operator*( const idMat6 &a ) const {
  1591.     int i, j;
  1592.     const float *m1Ptr, *m2Ptr;
  1593.     float *dstPtr;
  1594.     idMat6 dst;
  1595.  
  1596.     m1Ptr = reinterpret_cast<const float *>(this);
  1597.     m2Ptr = reinterpret_cast<const float *>(&a);
  1598.     dstPtr = reinterpret_cast<float *>(&dst);
  1599.  
  1600.     for ( i = 0; i < 6; i++ ) {
  1601.         for ( j = 0; j < 6; j++ ) {
  1602.             *dstPtr = m1Ptr[0] * m2Ptr[ 0 * 6 + j ]
  1603.                     + m1Ptr[1] * m2Ptr[ 1 * 6 + j ]
  1604.                     + m1Ptr[2] * m2Ptr[ 2 * 6 + j ]
  1605.                     + m1Ptr[3] * m2Ptr[ 3 * 6 + j ]
  1606.                     + m1Ptr[4] * m2Ptr[ 4 * 6 + j ]
  1607.                     + m1Ptr[5] * m2Ptr[ 5 * 6 + j ];
  1608.             dstPtr++;
  1609.         }
  1610.         m1Ptr += 6;
  1611.     }
  1612.     return dst;
  1613. }
  1614.  
  1615. ID_INLINE idMat6 idMat6::operator*( const float a ) const {
  1616.     return idMat6(
  1617.         idVec6( mat[0][0] * a, mat[0][1] * a, mat[0][2] * a, mat[0][3] * a, mat[0][4] * a, mat[0][5] * a ),
  1618.         idVec6( mat[1][0] * a, mat[1][1] * a, mat[1][2] * a, mat[1][3] * a, mat[1][4] * a, mat[1][5] * a ),
  1619.         idVec6( mat[2][0] * a, mat[2][1] * a, mat[2][2] * a, mat[2][3] * a, mat[2][4] * a, mat[2][5] * a ),
  1620.         idVec6( mat[3][0] * a, mat[3][1] * a, mat[3][2] * a, mat[3][3] * a, mat[3][4] * a, mat[3][5] * a ),
  1621.         idVec6( mat[4][0] * a, mat[4][1] * a, mat[4][2] * a, mat[4][3] * a, mat[4][4] * a, mat[4][5] * a ),
  1622.         idVec6( mat[5][0] * a, mat[5][1] * a, mat[5][2] * a, mat[5][3] * a, mat[5][4] * a, mat[5][5] * a ) );
  1623. }
  1624.  
  1625. ID_INLINE idVec6 idMat6::operator*( const idVec6 &vec ) const {
  1626.     return idVec6(
  1627.         mat[0][0] * vec[0] + mat[0][1] * vec[1] + mat[0][2] * vec[2] + mat[0][3] * vec[3] + mat[0][4] * vec[4] + mat[0][5] * vec[5],
  1628.         mat[1][0] * vec[0] + mat[1][1] * vec[1] + mat[1][2] * vec[2] + mat[1][3] * vec[3] + mat[1][4] * vec[4] + mat[1][5] * vec[5],
  1629.         mat[2][0] * vec[0] + mat[2][1] * vec[1] + mat[2][2] * vec[2] + mat[2][3] * vec[3] + mat[2][4] * vec[4] + mat[2][5] * vec[5],
  1630.         mat[3][0] * vec[0] + mat[3][1] * vec[1] + mat[3][2] * vec[2] + mat[3][3] * vec[3] + mat[3][4] * vec[4] + mat[3][5] * vec[5],
  1631.         mat[4][0] * vec[0] + mat[4][1] * vec[1] + mat[4][2] * vec[2] + mat[4][3] * vec[3] + mat[4][4] * vec[4] + mat[4][5] * vec[5],
  1632.         mat[5][0] * vec[0] + mat[5][1] * vec[1] + mat[5][2] * vec[2] + mat[5][3] * vec[3] + mat[5][4] * vec[4] + mat[5][5] * vec[5] );
  1633. }
  1634.  
  1635. ID_INLINE idMat6 idMat6::operator+( const idMat6 &a ) const {
  1636.     return idMat6(
  1637.         idVec6( mat[0][0] + a[0][0], mat[0][1] + a[0][1], mat[0][2] + a[0][2], mat[0][3] + a[0][3], mat[0][4] + a[0][4], mat[0][5] + a[0][5] ),
  1638.         idVec6( mat[1][0] + a[1][0], mat[1][1] + a[1][1], mat[1][2] + a[1][2], mat[1][3] + a[1][3], mat[1][4] + a[1][4], mat[1][5] + a[1][5] ),
  1639.         idVec6( mat[2][0] + a[2][0], mat[2][1] + a[2][1], mat[2][2] + a[2][2], mat[2][3] + a[2][3], mat[2][4] + a[2][4], mat[2][5] + a[2][5] ),
  1640.         idVec6( mat[3][0] + a[3][0], mat[3][1] + a[3][1], mat[3][2] + a[3][2], mat[3][3] + a[3][3], mat[3][4] + a[3][4], mat[3][5] + a[3][5] ),
  1641.         idVec6( mat[4][0] + a[4][0], mat[4][1] + a[4][1], mat[4][2] + a[4][2], mat[4][3] + a[4][3], mat[4][4] + a[4][4], mat[4][5] + a[4][5] ),
  1642.         idVec6( mat[5][0] + a[5][0], mat[5][1] + a[5][1], mat[5][2] + a[5][2], mat[5][3] + a[5][3], mat[5][4] + a[5][4], mat[5][5] + a[5][5] ) );
  1643. }
  1644.  
  1645. ID_INLINE idMat6 idMat6::operator-( const idMat6 &a ) const {
  1646.     return idMat6(
  1647.         idVec6( mat[0][0] - a[0][0], mat[0][1] - a[0][1], mat[0][2] - a[0][2], mat[0][3] - a[0][3], mat[0][4] - a[0][4], mat[0][5] - a[0][5] ),
  1648.         idVec6( mat[1][0] - a[1][0], mat[1][1] - a[1][1], mat[1][2] - a[1][2], mat[1][3] - a[1][3], mat[1][4] - a[1][4], mat[1][5] - a[1][5] ),
  1649.         idVec6( mat[2][0] - a[2][0], mat[2][1] - a[2][1], mat[2][2] - a[2][2], mat[2][3] - a[2][3], mat[2][4] - a[2][4], mat[2][5] - a[2][5] ),
  1650.         idVec6( mat[3][0] - a[3][0], mat[3][1] - a[3][1], mat[3][2] - a[3][2], mat[3][3] - a[3][3], mat[3][4] - a[3][4], mat[3][5] - a[3][5] ),
  1651.         idVec6( mat[4][0] - a[4][0], mat[4][1] - a[4][1], mat[4][2] - a[4][2], mat[4][3] - a[4][3], mat[4][4] - a[4][4], mat[4][5] - a[4][5] ),
  1652.         idVec6( mat[5][0] - a[5][0], mat[5][1] - a[5][1], mat[5][2] - a[5][2], mat[5][3] - a[5][3], mat[5][4] - a[5][4], mat[5][5] - a[5][5] ) );
  1653. }
  1654.  
  1655. ID_INLINE idMat6 &idMat6::operator*=( const float a ) {
  1656.     mat[0][0] *= a; mat[0][1] *= a; mat[0][2] *= a; mat[0][3] *= a; mat[0][4] *= a; mat[0][5] *= a;
  1657.     mat[1][0] *= a; mat[1][1] *= a; mat[1][2] *= a; mat[1][3] *= a; mat[1][4] *= a; mat[1][5] *= a;
  1658.     mat[2][0] *= a; mat[2][1] *= a; mat[2][2] *= a; mat[2][3] *= a; mat[2][4] *= a; mat[2][5] *= a;
  1659.     mat[3][0] *= a; mat[3][1] *= a; mat[3][2] *= a; mat[3][3] *= a; mat[3][4] *= a; mat[3][5] *= a;
  1660.     mat[4][0] *= a; mat[4][1] *= a; mat[4][2] *= a; mat[4][3] *= a; mat[4][4] *= a; mat[4][5] *= a;
  1661.     mat[5][0] *= a; mat[5][1] *= a; mat[5][2] *= a; mat[5][3] *= a; mat[5][4] *= a; mat[5][5] *= a;
  1662.     return *this;
  1663. }
  1664.  
  1665. ID_INLINE idMat6 &idMat6::operator*=( const idMat6 &a ) {
  1666.     *this = *this * a;
  1667.     return *this;
  1668. }
  1669.  
  1670. ID_INLINE idMat6 &idMat6::operator+=( const idMat6 &a ) {
  1671.     mat[0][0] += a[0][0]; mat[0][1] += a[0][1]; mat[0][2] += a[0][2]; mat[0][3] += a[0][3]; mat[0][4] += a[0][4]; mat[0][5] += a[0][5];
  1672.     mat[1][0] += a[1][0]; mat[1][1] += a[1][1]; mat[1][2] += a[1][2]; mat[1][3] += a[1][3]; mat[1][4] += a[1][4]; mat[1][5] += a[1][5];
  1673.     mat[2][0] += a[2][0]; mat[2][1] += a[2][1]; mat[2][2] += a[2][2]; mat[2][3] += a[2][3]; mat[2][4] += a[2][4]; mat[2][5] += a[2][5];
  1674.     mat[3][0] += a[3][0]; mat[3][1] += a[3][1]; mat[3][2] += a[3][2]; mat[3][3] += a[3][3]; mat[3][4] += a[3][4]; mat[3][5] += a[3][5];
  1675.     mat[4][0] += a[4][0]; mat[4][1] += a[4][1]; mat[4][2] += a[4][2]; mat[4][3] += a[4][3]; mat[4][4] += a[4][4]; mat[4][5] += a[4][5];
  1676.     mat[5][0] += a[5][0]; mat[5][1] += a[5][1]; mat[5][2] += a[5][2]; mat[5][3] += a[5][3]; mat[5][4] += a[5][4]; mat[5][5] += a[5][5];
  1677.     return *this;
  1678. }
  1679.  
  1680. ID_INLINE idMat6 &idMat6::operator-=( const idMat6 &a ) {
  1681.     mat[0][0] -= a[0][0]; mat[0][1] -= a[0][1]; mat[0][2] -= a[0][2]; mat[0][3] -= a[0][3]; mat[0][4] -= a[0][4]; mat[0][5] -= a[0][5];
  1682.     mat[1][0] -= a[1][0]; mat[1][1] -= a[1][1]; mat[1][2] -= a[1][2]; mat[1][3] -= a[1][3]; mat[1][4] -= a[1][4]; mat[1][5] -= a[1][5];
  1683.     mat[2][0] -= a[2][0]; mat[2][1] -= a[2][1]; mat[2][2] -= a[2][2]; mat[2][3] -= a[2][3]; mat[2][4] -= a[2][4]; mat[2][5] -= a[2][5];
  1684.     mat[3][0] -= a[3][0]; mat[3][1] -= a[3][1]; mat[3][2] -= a[3][2]; mat[3][3] -= a[3][3]; mat[3][4] -= a[3][4]; mat[3][5] -= a[3][5];
  1685.     mat[4][0] -= a[4][0]; mat[4][1] -= a[4][1]; mat[4][2] -= a[4][2]; mat[4][3] -= a[4][3]; mat[4][4] -= a[4][4]; mat[4][5] -= a[4][5];
  1686.     mat[5][0] -= a[5][0]; mat[5][1] -= a[5][1]; mat[5][2] -= a[5][2]; mat[5][3] -= a[5][3]; mat[5][4] -= a[5][4]; mat[5][5] -= a[5][5];
  1687.     return *this;
  1688. }
  1689.  
  1690. ID_INLINE idVec6 operator*( const idVec6 &vec, const idMat6 &mat ) {
  1691.     return mat * vec;
  1692. }
  1693.  
  1694. ID_INLINE idMat6 operator*( const float a, idMat6 const &mat ) {
  1695.     return mat * a;
  1696. }
  1697.  
  1698. ID_INLINE idVec6 &operator*=( idVec6 &vec, const idMat6 &mat ) {
  1699.     vec = mat * vec;
  1700.     return vec;
  1701. }
  1702.  
  1703. ID_INLINE bool idMat6::Compare( const idMat6 &a ) const {
  1704.     dword i;
  1705.     const float *ptr1, *ptr2;
  1706.  
  1707.     ptr1 = reinterpret_cast<const float *>(mat);
  1708.     ptr2 = reinterpret_cast<const float *>(a.mat);
  1709.     for ( i = 0; i < 6*6; i++ ) {
  1710.         if ( ptr1[i] != ptr2[i] ) {
  1711.             return false;
  1712.         }
  1713.     }
  1714.     return true;
  1715. }
  1716.  
  1717. ID_INLINE bool idMat6::Compare( const idMat6 &a, const float epsilon ) const {
  1718.     dword i;
  1719.     const float *ptr1, *ptr2;
  1720.  
  1721.     ptr1 = reinterpret_cast<const float *>(mat);
  1722.     ptr2 = reinterpret_cast<const float *>(a.mat);
  1723.     for ( i = 0; i < 6*6; i++ ) {
  1724.         if ( idMath::Fabs( ptr1[i] - ptr2[i] ) > epsilon ) {
  1725.             return false;
  1726.         }
  1727.     }
  1728.     return true;
  1729. }
  1730.  
  1731. ID_INLINE bool idMat6::operator==( const idMat6 &a ) const {
  1732.     return Compare( a );
  1733. }
  1734.  
  1735. ID_INLINE bool idMat6::operator!=( const idMat6 &a ) const {
  1736.     return !Compare( a );
  1737. }
  1738.  
  1739. ID_INLINE void idMat6::Zero( void ) {
  1740.     memset( mat, 0, sizeof( idMat6 ) );
  1741. }
  1742.  
  1743. ID_INLINE void idMat6::Identity( void ) {
  1744.     *this = mat6_identity;
  1745. }
  1746.  
  1747. ID_INLINE bool idMat6::IsIdentity( const float epsilon ) const {
  1748.     return Compare( mat6_identity, epsilon );
  1749. }
  1750.  
  1751. ID_INLINE bool idMat6::IsSymmetric( const float epsilon ) const {
  1752.     for ( int i = 1; i < 6; i++ ) {
  1753.         for ( int j = 0; j < i; j++ ) {
  1754.             if ( idMath::Fabs( mat[i][j] - mat[j][i] ) > epsilon ) {
  1755.                 return false;
  1756.             }
  1757.         }
  1758.     }
  1759.     return true;
  1760. }
  1761.  
  1762. ID_INLINE bool idMat6::IsDiagonal( const float epsilon ) const {
  1763.     for ( int i = 0; i < 6; i++ ) {
  1764.         for ( int j = 0; j < 6; j++ ) {
  1765.             if ( i != j && idMath::Fabs( mat[i][j] ) > epsilon ) {
  1766.                 return false;
  1767.             }
  1768.         }
  1769.     }
  1770.     return true;
  1771. }
  1772.  
  1773. ID_INLINE idMat3 idMat6::SubMat3( int n ) const {
  1774.     assert( n >= 0 && n < 4 );
  1775.     int b0 = ((n & 2) >> 1) * 3;
  1776.     int b1 = (n & 1) * 3;
  1777.     return idMat3(
  1778.         mat[b0 + 0][b1 + 0], mat[b0 + 0][b1 + 1], mat[b0 + 0][b1 + 2],
  1779.         mat[b0 + 1][b1 + 0], mat[b0 + 1][b1 + 1], mat[b0 + 1][b1 + 2],
  1780.         mat[b0 + 2][b1 + 0], mat[b0 + 2][b1 + 1], mat[b0 + 2][b1 + 2] );
  1781. }
  1782.  
  1783. ID_INLINE float idMat6::Trace( void ) const {
  1784.     return ( mat[0][0] + mat[1][1] + mat[2][2] + mat[3][3] + mat[4][4] + mat[5][5] );
  1785. }
  1786.  
  1787. ID_INLINE idMat6 idMat6::Inverse( void ) const {
  1788.     idMat6 invMat;
  1789.  
  1790.     invMat = *this;
  1791. #ifdef _DEBUG
  1792.     assert ( invMat.InverseSelf() );
  1793. #else
  1794.     invMat.InverseSelf();
  1795. #endif
  1796.     return invMat;
  1797. }
  1798.  
  1799. ID_INLINE idMat6 idMat6::InverseFast( void ) const {
  1800.     idMat6 invMat;
  1801.  
  1802.     invMat = *this;
  1803. #ifdef _DEBUG
  1804.     assert ( invMat.InverseFastSelf() );
  1805. #else
  1806.     invMat.InverseFastSelf();
  1807. #endif
  1808.     return invMat;
  1809. }
  1810.  
  1811. ID_INLINE int idMat6::GetDimension( void ) const {
  1812.     return 36;
  1813. }
  1814.  
  1815. ID_INLINE const float *idMat6::ToFloatPtr( void ) const {
  1816.     return mat[0].ToFloatPtr();
  1817. }
  1818.  
  1819. ID_INLINE float *idMat6::ToFloatPtr( void ) {
  1820.     return mat[0].ToFloatPtr();
  1821. }
  1822.  
  1823.  
  1824. //===============================================================
  1825. //
  1826. //    idMatX - arbitrary sized dense real matrix
  1827. //
  1828. //  The matrix lives on 16 byte aligned and 16 byte padded memory.
  1829. //
  1830. //    NOTE: due to the temporary memory pool idMatX cannot be used by multiple threads.
  1831. //
  1832. //===============================================================
  1833.  
  1834. #define MATX_MAX_TEMP        1024
  1835. #define MATX_QUAD( x )        ( ( ( ( x ) + 3 ) & ~3 ) * sizeof( float ) )
  1836. #define MATX_CLEAREND()        int s = numRows * numColumns; while( s < ( ( s + 3 ) & ~3 ) ) { mat[s++] = 0.0f; }
  1837. #define MATX_ALLOCA( n )    ( (float *) _alloca16( MATX_QUAD( n ) ) )
  1838. #define MATX_SIMD
  1839.  
  1840. // RAVEN BEGIN
  1841. // jsinger: this is broken at the moment because idSIMDProcessor is no longer virtual
  1842. #ifdef _XENON
  1843. #undef MATX_SIMD
  1844. #endif
  1845. // RAVEN END
  1846. class idMatX {
  1847. public:
  1848.                     idMatX( void );
  1849.                     explicit idMatX( int rows, int columns );
  1850.                     explicit idMatX( int rows, int columns, float *src );
  1851.                     ~idMatX( void );
  1852.  
  1853.     void            Set( int rows, int columns, const float *src );
  1854.     void            Set( const idMat3 &m1, const idMat3 &m2 );
  1855.     void            Set( const idMat3 &m1, const idMat3 &m2, const idMat3 &m3, const idMat3 &m4 );
  1856.  
  1857.     const float *    operator[]( int index ) const;
  1858.     float *            operator[]( int index );
  1859.     idMatX &        operator=( const idMatX &a );
  1860.     idMatX            operator*( const float a ) const;
  1861.     idVecX            operator*( const idVecX &vec ) const;
  1862.     idMatX            operator*( const idMatX &a ) const;
  1863.     idMatX            operator+( const idMatX &a ) const;
  1864.     idMatX            operator-( const idMatX &a ) const;
  1865.     idMatX &        operator*=( const float a );
  1866.     idMatX &        operator*=( const idMatX &a );
  1867.     idMatX &        operator+=( const idMatX &a );
  1868.     idMatX &        operator-=( const idMatX &a );
  1869.  
  1870.     friend idMatX    operator*( const float a, const idMatX &m );
  1871.     friend idVecX    operator*( const idVecX &vec, const idMatX &m );
  1872.     friend idVecX &    operator*=( idVecX &vec, const idMatX &m );
  1873.  
  1874.     bool            Compare( const idMatX &a ) const;                                // exact compare, no epsilon
  1875.     bool            Compare( const idMatX &a, const float epsilon ) const;            // compare with epsilon
  1876.     bool            operator==( const idMatX &a ) const;                            // exact compare, no epsilon
  1877.     bool            operator!=( const idMatX &a ) const;                            // exact compare, no epsilon
  1878.  
  1879.     void            SetSize( int rows, int columns );                                // set the number of rows/columns
  1880.     void            ChangeSize( int rows, int columns, bool makeZero = false );        // change the size keeping data intact where possible
  1881.     int                GetNumRows( void ) const { return numRows; }                    // get the number of rows
  1882.     int                GetNumColumns( void ) const { return numColumns; }                // get the number of columns
  1883.     void            SetData( int rows, int columns, float *data );                    // set float array pointer
  1884.     void            Zero( void );                                                    // clear matrix
  1885.     void            Zero( int rows, int columns );                                    // set size and clear matrix
  1886.     void            Identity( void );                                                // clear to identity matrix
  1887.     void            Identity( int rows, int columns );                                // set size and clear to identity matrix
  1888.     void            Diag( const idVecX &v );                                        // create diagonal matrix from vector
  1889.     void            Random( int seed, float l = 0.0f, float u = 1.0f );                // fill matrix with random values
  1890.     void            Random( int rows, int columns, int seed, float l = 0.0f, float u = 1.0f );
  1891.     void            Negate( void );                                                    // (*this) = - (*this)
  1892.     void            Clamp( float min, float max );                                    // clamp all values
  1893.     idMatX &        SwapRows( int r1, int r2 );                                        // swap rows
  1894.     idMatX &        SwapColumns( int r1, int r2 );                                    // swap columns
  1895.     idMatX &        SwapRowsColumns( int r1, int r2 );                                // swap rows and columns
  1896.     idMatX &        RemoveRow( int r );                                                // remove a row
  1897.     idMatX &        RemoveColumn( int r );                                            // remove a column
  1898.     idMatX &        RemoveRowColumn( int r );                                        // remove a row and column
  1899.     void            ClearUpperTriangle( void );                                        // clear the upper triangle
  1900.     void            ClearLowerTriangle( void );                                        // clear the lower triangle
  1901.     void            SquareSubMatrix( const idMatX &m, int size );                    // get square sub-matrix from 0,0 to size,size
  1902.     float            MaxDifference( const idMatX &m ) const;                            // return maximum element difference between this and m
  1903.  
  1904.     bool            IsSquare( void ) const { return ( numRows == numColumns ); }
  1905.     bool            IsZero( const float epsilon = MATRIX_EPSILON ) const;
  1906.     bool            IsIdentity( const float epsilon = MATRIX_EPSILON ) const;
  1907.     bool            IsDiagonal( const float epsilon = MATRIX_EPSILON ) const;
  1908.     bool            IsTriDiagonal( const float epsilon = MATRIX_EPSILON ) const;
  1909.     bool            IsSymmetric( const float epsilon = MATRIX_EPSILON ) const;
  1910.     bool            IsOrthogonal( const float epsilon = MATRIX_EPSILON ) const;
  1911.     bool            IsOrthonormal( const float epsilon = MATRIX_EPSILON ) const;
  1912.     bool            IsPMatrix( const float epsilon = MATRIX_EPSILON ) const;
  1913.     bool            IsZMatrix( const float epsilon = MATRIX_EPSILON ) const;
  1914.     bool            IsPositiveDefinite( const float epsilon = MATRIX_EPSILON ) const;
  1915.     bool            IsSymmetricPositiveDefinite( const float epsilon = MATRIX_EPSILON ) const;
  1916.     bool            IsPositiveSemiDefinite( const float epsilon = MATRIX_EPSILON ) const;
  1917.     bool            IsSymmetricPositiveSemiDefinite( const float epsilon = MATRIX_EPSILON ) const;
  1918.  
  1919.     float            Trace( void ) const;                                            // returns product of diagonal elements
  1920.     float            Determinant( void ) const;                                        // returns determinant of matrix
  1921.     idMatX            Transpose( void ) const;                                        // returns transpose
  1922.     idMatX &        TransposeSelf( void );                                            // transposes the matrix itself
  1923.     idMatX            Inverse( void ) const;                                            // returns the inverse ( m * m.Inverse() = identity )
  1924.     bool            InverseSelf( void );                                            // returns false if determinant is zero
  1925.     idMatX            InverseFast( void ) const;                                        // returns the inverse ( m * m.Inverse() = identity )
  1926.     bool            InverseFastSelf( void );                                        // returns false if determinant is zero
  1927.  
  1928.     bool            LowerTriangularInverse( void );                                    // in-place inversion, returns false if determinant is zero
  1929.     bool            UpperTriangularInverse( void );                                    // in-place inversion, returns false if determinant is zero
  1930.  
  1931.     idVecX            Multiply( const idVecX &vec ) const;                            // (*this) * vec
  1932.     idVecX            TransposeMultiply( const idVecX &vec ) const;                    // this->Transpose() * vec
  1933.  
  1934.     idMatX            Multiply( const idMatX &a ) const;                                // (*this) * a
  1935.     idMatX            TransposeMultiply( const idMatX &a ) const;                        // this->Transpose() * a
  1936.  
  1937.     void            Multiply( idVecX &dst, const idVecX &vec ) const;                // dst = (*this) * vec
  1938.     void            MultiplyAdd( idVecX &dst, const idVecX &vec ) const;            // dst += (*this) * vec
  1939.     void            MultiplySub( idVecX &dst, const idVecX &vec ) const;            // dst -= (*this) * vec
  1940.     void            TransposeMultiply( idVecX &dst, const idVecX &vec ) const;        // dst = this->Transpose() * vec
  1941.     void            TransposeMultiplyAdd( idVecX &dst, const idVecX &vec ) const;    // dst += this->Transpose() * vec
  1942.     void            TransposeMultiplySub( idVecX &dst, const idVecX &vec ) const;    // dst -= this->Transpose() * vec
  1943.  
  1944.     void            Multiply( idMatX &dst, const idMatX &a ) const;                    // dst = (*this) * a
  1945.     void            TransposeMultiply( idMatX &dst, const idMatX &a ) const;        // dst = this->Transpose() * a
  1946.  
  1947.     int                GetDimension( void ) const;                                        // returns total number of values in matrix
  1948.  
  1949.     const idVec6 &    SubVec6( int row ) const;                                        // interpret beginning of row as a const idVec6
  1950.     idVec6 &        SubVec6( int row );                                                // interpret beginning of row as an idVec6
  1951.     const idVecX    SubVecX( int row ) const;                                        // interpret complete row as a const idVecX
  1952.     idVecX            SubVecX( int row );                                                // interpret complete row as an idVecX
  1953.     const float *    ToFloatPtr( void ) const;                                        // pointer to const matrix float array
  1954.     float *            ToFloatPtr( void );                                                // pointer to matrix float array
  1955.     const char *    ToString( int precision = 2 ) const;
  1956.  
  1957.     void            Update_RankOne( const idVecX &v, const idVecX &w, float alpha );
  1958.     void            Update_RankOneSymmetric( const idVecX &v, float alpha );
  1959.     void            Update_RowColumn( const idVecX &v, const idVecX &w, int r );
  1960.     void            Update_RowColumnSymmetric( const idVecX &v, int r );
  1961.     void            Update_Increment( const idVecX &v, const idVecX &w );
  1962.     void            Update_IncrementSymmetric( const idVecX &v );
  1963.     void            Update_Decrement( int r );
  1964.  
  1965.     bool            Inverse_GaussJordan( void );                    // invert in-place with Gauss-Jordan elimination
  1966.     bool            Inverse_UpdateRankOne( const idVecX &v, const idVecX &w, float alpha );
  1967.     bool            Inverse_UpdateRowColumn( const idVecX &v, const idVecX &w, int r );
  1968.     bool            Inverse_UpdateIncrement( const idVecX &v, const idVecX &w );
  1969.     bool            Inverse_UpdateDecrement( const idVecX &v, const idVecX &w, int r );
  1970.     void            Inverse_Solve( idVecX &x, const idVecX &b ) const;
  1971.  
  1972.     bool            LU_Factor( int *index, float *det = NULL );        // factor in-place: L * U
  1973.     bool            LU_UpdateRankOne( const idVecX &v, const idVecX &w, float alpha, int *index );
  1974.     bool            LU_UpdateRowColumn( const idVecX &v, const idVecX &w, int r, int *index );
  1975.     bool            LU_UpdateIncrement( const idVecX &v, const idVecX &w, int *index );
  1976.     bool            LU_UpdateDecrement( const idVecX &v, const idVecX &w, const idVecX &u, int r, int *index );
  1977.     void            LU_Solve( idVecX &x, const idVecX &b, const int *index ) const;
  1978.     void            LU_Inverse( idMatX &inv, const int *index ) const;
  1979.     void            LU_UnpackFactors( idMatX &L, idMatX &U ) const;
  1980.     void            LU_MultiplyFactors( idMatX &m, const int *index ) const;
  1981.  
  1982.     bool            QR_Factor( idVecX &c, idVecX &d );                // factor in-place: Q * R
  1983.     bool            QR_UpdateRankOne( idMatX &R, const idVecX &v, const idVecX &w, float alpha );
  1984.     bool            QR_UpdateRowColumn( idMatX &R, const idVecX &v, const idVecX &w, int r );
  1985.     bool            QR_UpdateIncrement( idMatX &R, const idVecX &v, const idVecX &w );
  1986.     bool            QR_UpdateDecrement( idMatX &R, const idVecX &v, const idVecX &w, int r );
  1987.     void            QR_Solve( idVecX &x, const idVecX &b, const idVecX &c, const idVecX &d ) const;
  1988.     void            QR_Solve( idVecX &x, const idVecX &b, const idMatX &R ) const;
  1989.     void            QR_Inverse( idMatX &inv, const idVecX &c, const idVecX &d ) const;
  1990.     void            QR_UnpackFactors( idMatX &Q, idMatX &R, const idVecX &c, const idVecX &d ) const;
  1991.     void            QR_MultiplyFactors( idMatX &m, const idVecX &c, const idVecX &d ) const;
  1992.  
  1993.     bool            SVD_Factor( idVecX &w, idMatX &V );                // factor in-place: U * Diag(w) * V.Transpose()
  1994.     void            SVD_Solve( idVecX &x, const idVecX &b, const idVecX &w, const idMatX &V ) const;
  1995.     void            SVD_Inverse( idMatX &inv, const idVecX &w, const idMatX &V ) const;
  1996.     void            SVD_MultiplyFactors( idMatX &m, const idVecX &w, const idMatX &V ) const;
  1997.  
  1998.     bool            Cholesky_Factor( void );                        // factor in-place: L * L.Transpose()
  1999.     bool            Cholesky_UpdateRankOne( const idVecX &v, float alpha, int offset = 0 );
  2000.     bool            Cholesky_UpdateRowColumn( const idVecX &v, int r );
  2001.     bool            Cholesky_UpdateIncrement( const idVecX &v );
  2002.     bool            Cholesky_UpdateDecrement( const idVecX &v, int r );
  2003.     void            Cholesky_Solve( idVecX &x, const idVecX &b ) const;
  2004.     void            Cholesky_Inverse( idMatX &inv ) const;
  2005.     void            Cholesky_MultiplyFactors( idMatX &m ) const;
  2006.  
  2007.     bool            LDLT_Factor( void );                            // factor in-place: L * D * L.Transpose()
  2008.     bool            LDLT_UpdateRankOne( const idVecX &v, float alpha, int offset = 0 );
  2009.     bool            LDLT_UpdateRowColumn( const idVecX &v, int r );
  2010.     bool            LDLT_UpdateIncrement( const idVecX &v );
  2011.     bool            LDLT_UpdateDecrement( const idVecX &v, int r );
  2012.     void            LDLT_Solve( idVecX &x, const idVecX &b ) const;
  2013.     void            LDLT_Inverse( idMatX &inv ) const;
  2014.     void            LDLT_UnpackFactors( idMatX &L, idMatX &D ) const;
  2015.     void            LDLT_MultiplyFactors( idMatX &m ) const;
  2016.  
  2017.     void            TriDiagonal_ClearTriangles( void );
  2018.     bool            TriDiagonal_Solve( idVecX &x, const idVecX &b ) const;
  2019.     void            TriDiagonal_Inverse( idMatX &inv ) const;
  2020.  
  2021.     bool            Eigen_SolveSymmetricTriDiagonal( idVecX &eigenValues );
  2022.     bool            Eigen_SolveSymmetric( idVecX &eigenValues );
  2023.     bool            Eigen_Solve( idVecX &realEigenValues, idVecX &imaginaryEigenValues );
  2024.     void            Eigen_SortIncreasing( idVecX &eigenValues );
  2025.     void            Eigen_SortDecreasing( idVecX &eigenValues );
  2026.  
  2027.     static void        Test( void );
  2028.  
  2029. private:
  2030.     int                numRows;                // number of rows
  2031.     int                numColumns;                // number of columns
  2032.     int                alloced;                // floats allocated, if -1 then mat points to data set with SetData
  2033.     float *            mat;                    // memory the matrix is stored
  2034.  
  2035. // RAVEN BEGIN
  2036. // jscott: avoid pointer hackery pokery and use the compiler
  2037.     ALIGN16( static float tempPtr[MATX_MAX_TEMP]; )
  2038. //    static float *    tempPtr;                // pointer to 16 byte aligned temporary memory
  2039. // RAVEN END
  2040.     static int        tempIndex;                // index into memory pool, wraps around
  2041.  
  2042. private:
  2043.     void            SetTempSize( int rows, int columns );
  2044.     float            DeterminantGeneric( void ) const;
  2045.     bool            InverseSelfGeneric( void );
  2046.     void            QR_Rotate( idMatX &R, int i, float a, float b );
  2047.     float            Pythag( float a, float b ) const;
  2048.     void            SVD_BiDiag( idVecX &w, idVecX &rv1, float &anorm );
  2049.     void            SVD_InitialWV( idVecX &w, idMatX &V, idVecX &rv1 );
  2050.     void            HouseholderReduction( idVecX &diag, idVecX &subd );
  2051.     bool            QL( idVecX &diag, idVecX &subd );
  2052.     void            HessenbergReduction( idMatX &H );
  2053.     void            ComplexDivision( float xr, float xi, float yr, float yi, float &cdivr, float &cdivi );
  2054.     bool            HessenbergToRealSchur( idMatX &H, idVecX &realEigenValues, idVecX &imaginaryEigenValues );
  2055. };
  2056.  
  2057. ID_INLINE idMatX::idMatX( void ) {
  2058.     numRows = numColumns = alloced = 0;
  2059.     mat = NULL;
  2060. }
  2061.  
  2062. ID_INLINE idMatX::~idMatX( void ) {
  2063.     // if not temp memory
  2064.     if ( mat != NULL && ( mat < idMatX::tempPtr || mat > idMatX::tempPtr + MATX_MAX_TEMP ) && alloced != -1 ) {
  2065.         Mem_Free16( mat );
  2066.     }
  2067. }
  2068.  
  2069. ID_INLINE idMatX::idMatX( int rows, int columns ) {
  2070.     numRows = numColumns = alloced = 0;
  2071.     mat = NULL;
  2072.     SetSize( rows, columns );
  2073. }
  2074.  
  2075. ID_INLINE idMatX::idMatX( int rows, int columns, float *src ) {
  2076.     numRows = numColumns = alloced = 0;
  2077.     mat = NULL;
  2078.     SetData( rows, columns, src );
  2079. }
  2080.  
  2081. ID_INLINE void idMatX::Set( int rows, int columns, const float *src ) {
  2082.     SetSize( rows, columns );
  2083.     memcpy( this->mat, src, rows * columns * sizeof( float ) );
  2084. }
  2085.  
  2086. ID_INLINE void idMatX::Set( const idMat3 &m1, const idMat3 &m2 ) {
  2087.     int i, j;
  2088.  
  2089.     SetSize( 3, 6 );
  2090.     for ( i = 0; i < 3; i++ ) {
  2091.         for ( j = 0; j < 3; j++ ) {
  2092.             mat[(i+0) * numColumns + (j+0)] = m1[i][j];
  2093.             mat[(i+0) * numColumns + (j+3)] = m2[i][j];
  2094.         }
  2095.     }
  2096. }
  2097.  
  2098. ID_INLINE void idMatX::Set( const idMat3 &m1, const idMat3 &m2, const idMat3 &m3, const idMat3 &m4 ) {
  2099.     int i, j;
  2100.  
  2101.     SetSize( 6, 6 );
  2102.     for ( i = 0; i < 3; i++ ) {
  2103.         for ( j = 0; j < 3; j++ ) {
  2104.             mat[(i+0) * numColumns + (j+0)] = m1[i][j];
  2105.             mat[(i+0) * numColumns + (j+3)] = m2[i][j];
  2106.             mat[(i+3) * numColumns + (j+0)] = m3[i][j];
  2107.             mat[(i+3) * numColumns + (j+3)] = m4[i][j];
  2108.         }
  2109.     }
  2110. }
  2111.  
  2112. ID_INLINE const float *idMatX::operator[]( int index ) const {
  2113.     assert( ( index >= 0 ) && ( index < numRows ) );
  2114.     return mat + index * numColumns;
  2115. }
  2116.  
  2117. ID_INLINE float *idMatX::operator[]( int index ) {
  2118.     assert( ( index >= 0 ) && ( index < numRows ) );
  2119.     return mat + index * numColumns;
  2120. }
  2121.  
  2122. ID_INLINE idMatX &idMatX::operator=( const idMatX &a ) {
  2123.     SetSize( a.numRows, a.numColumns );
  2124. #ifdef MATX_SIMD
  2125.     SIMDProcessor->Copy16( mat, a.mat, a.numRows * a.numColumns );
  2126. #else
  2127.     memcpy( mat, a.mat, a.numRows * a.numColumns * sizeof( float ) );
  2128. #endif
  2129.     idMatX::tempIndex = 0;
  2130.     return *this;
  2131. }
  2132.  
  2133. ID_INLINE idMatX idMatX::operator*( const float a ) const {
  2134.     idMatX m;
  2135.  
  2136.     m.SetTempSize( numRows, numColumns );
  2137. #ifdef MATX_SIMD
  2138.     SIMDProcessor->Mul16( m.mat, mat, a, numRows * numColumns );
  2139. #else
  2140.     int i, s;
  2141.     s = numRows * numColumns;
  2142.     for ( i = 0; i < s; i++ ) {
  2143.         m.mat[i] = mat[i] * a;
  2144.     }
  2145. #endif
  2146.     return m;
  2147. }
  2148.  
  2149. ID_INLINE idVecX idMatX::operator*( const idVecX &vec ) const {
  2150.     idVecX dst;
  2151.  
  2152.     assert( numColumns == vec.GetSize() );
  2153.  
  2154.     dst.SetTempSize( numRows );
  2155. #ifdef MATX_SIMD
  2156.     SIMDProcessor->MatX_MultiplyVecX( dst, *this, vec );
  2157. #else
  2158.     Multiply( dst, vec );
  2159. #endif
  2160.     return dst;
  2161. }
  2162.  
  2163. ID_INLINE idMatX idMatX::operator*( const idMatX &a ) const {
  2164.     idMatX dst;
  2165.  
  2166.     assert( numColumns == a.numRows );
  2167.  
  2168.     dst.SetTempSize( numRows, a.numColumns );
  2169. #ifdef MATX_SIMD
  2170.     SIMDProcessor->MatX_MultiplyMatX( dst, *this, a );
  2171. #else
  2172.     Multiply( dst, a );
  2173. #endif
  2174.     return dst;
  2175. }
  2176.  
  2177. ID_INLINE idMatX idMatX::operator+( const idMatX &a ) const {
  2178.     idMatX m;
  2179.  
  2180.     assert( numRows == a.numRows && numColumns == a.numColumns );
  2181.     m.SetTempSize( numRows, numColumns );
  2182. #ifdef MATX_SIMD
  2183.     SIMDProcessor->Add16( m.mat, mat, a.mat, numRows * numColumns );
  2184. #else
  2185.     int i, s;
  2186.     s = numRows * numColumns;
  2187.     for ( i = 0; i < s; i++ ) {
  2188.         m.mat[i] = mat[i] + a.mat[i];
  2189.     }
  2190. #endif
  2191.     return m;
  2192. }
  2193.  
  2194. ID_INLINE idMatX idMatX::operator-( const idMatX &a ) const {
  2195.     idMatX m;
  2196.  
  2197.     assert( numRows == a.numRows && numColumns == a.numColumns );
  2198.     m.SetTempSize( numRows, numColumns );
  2199. #ifdef MATX_SIMD
  2200.     SIMDProcessor->Sub16( m.mat, mat, a.mat, numRows * numColumns );
  2201. #else
  2202.     int i, s;
  2203.     s = numRows * numColumns;
  2204.     for ( i = 0; i < s; i++ ) {
  2205.         m.mat[i] = mat[i] - a.mat[i];
  2206.     }
  2207. #endif
  2208.     return m;
  2209. }
  2210.  
  2211. ID_INLINE idMatX &idMatX::operator*=( const float a ) {
  2212. #ifdef MATX_SIMD
  2213.     SIMDProcessor->MulAssign16( mat, a, numRows * numColumns );
  2214. #else
  2215.     int i, s;
  2216.     s = numRows * numColumns;
  2217.     for ( i = 0; i < s; i++ ) {
  2218.         mat[i] *= a;
  2219.     }
  2220. #endif
  2221.     idMatX::tempIndex = 0;
  2222.     return *this;
  2223. }
  2224.  
  2225. ID_INLINE idMatX &idMatX::operator*=( const idMatX &a ) {
  2226.     *this = *this * a;
  2227.     idMatX::tempIndex = 0;
  2228.     return *this;
  2229. }
  2230.  
  2231. ID_INLINE idMatX &idMatX::operator+=( const idMatX &a ) {
  2232.     assert( numRows == a.numRows && numColumns == a.numColumns );
  2233. #ifdef MATX_SIMD
  2234.     SIMDProcessor->AddAssign16( mat, a.mat, numRows * numColumns );
  2235. #else
  2236.     int i, s;
  2237.     s = numRows * numColumns;
  2238.     for ( i = 0; i < s; i++ ) {
  2239.         mat[i] += a.mat[i];
  2240.     }
  2241. #endif
  2242.     idMatX::tempIndex = 0;
  2243.     return *this;
  2244. }
  2245.  
  2246. ID_INLINE idMatX &idMatX::operator-=( const idMatX &a ) {
  2247.     assert( numRows == a.numRows && numColumns == a.numColumns );
  2248. #ifdef MATX_SIMD
  2249.     SIMDProcessor->SubAssign16( mat, a.mat, numRows * numColumns );
  2250. #else
  2251.     int i, s;
  2252.     s = numRows * numColumns;
  2253.     for ( i = 0; i < s; i++ ) {
  2254.         mat[i] -= a.mat[i];
  2255.     }
  2256. #endif
  2257.     idMatX::tempIndex = 0;
  2258.     return *this;
  2259. }
  2260.  
  2261. ID_INLINE idMatX operator*( const float a, idMatX const &m ) {
  2262.     return m * a;
  2263. }
  2264.  
  2265. ID_INLINE idVecX operator*( const idVecX &vec, const idMatX &m ) {
  2266.     return m * vec;
  2267. }
  2268.  
  2269. ID_INLINE idVecX &operator*=( idVecX &vec, const idMatX &m ) {
  2270.     vec = m * vec;
  2271.     return vec;
  2272. }
  2273.  
  2274. ID_INLINE bool idMatX::Compare( const idMatX &a ) const {
  2275.     int i, s;
  2276.  
  2277.     assert( numRows == a.numRows && numColumns == a.numColumns );
  2278.  
  2279.     s = numRows * numColumns;
  2280.     for ( i = 0; i < s; i++ ) {
  2281.         if ( mat[i] != a.mat[i] ) {
  2282.             return false;
  2283.         }
  2284.     }
  2285.     return true;
  2286. }
  2287.  
  2288. ID_INLINE bool idMatX::Compare( const idMatX &a, const float epsilon ) const {
  2289.     int i, s;
  2290.  
  2291.     assert( numRows == a.numRows && numColumns == a.numColumns );
  2292.  
  2293.     s = numRows * numColumns;
  2294.     for ( i = 0; i < s; i++ ) {
  2295.         if ( idMath::Fabs( mat[i] - a.mat[i] ) > epsilon ) {
  2296.             return false;
  2297.         }
  2298.     }
  2299.     return true;
  2300. }
  2301.  
  2302. ID_INLINE bool idMatX::operator==( const idMatX &a ) const {
  2303.     return Compare( a );
  2304. }
  2305.  
  2306. ID_INLINE bool idMatX::operator!=( const idMatX &a ) const {
  2307.     return !Compare( a );
  2308. }
  2309.  
  2310. ID_INLINE void idMatX::SetSize( int rows, int columns ) {
  2311.     assert( mat < idMatX::tempPtr || mat > idMatX::tempPtr + MATX_MAX_TEMP );
  2312.     int alloc = ( rows * columns + 3 ) & ~3;
  2313.     if ( alloc > alloced && alloced != -1 ) {
  2314.         if ( mat != NULL ) {
  2315.             Mem_Free16( mat );
  2316.         }
  2317. //RAVEN BEGIN
  2318. //amccarthy: Added allocation tag
  2319.         mat = (float *) Mem_Alloc16( alloc * sizeof( float ), MA_MATH );
  2320. //RAVEN END
  2321.         alloced = alloc;
  2322.     }
  2323.     numRows = rows;
  2324.     numColumns = columns;
  2325.     MATX_CLEAREND();
  2326. }
  2327.  
  2328. ID_INLINE void idMatX::SetTempSize( int rows, int columns ) {
  2329.     int newSize;
  2330.  
  2331.     newSize = ( rows * columns + 3 ) & ~3;
  2332.     assert( newSize < MATX_MAX_TEMP );
  2333.     if ( idMatX::tempIndex + newSize > MATX_MAX_TEMP ) {
  2334.         idMatX::tempIndex = 0;
  2335.     }
  2336.     mat = idMatX::tempPtr + idMatX::tempIndex;
  2337.     idMatX::tempIndex += newSize;
  2338.     alloced = newSize;
  2339.     numRows = rows;
  2340.     numColumns = columns;
  2341.     MATX_CLEAREND();
  2342. }
  2343.  
  2344. ID_INLINE void idMatX::SetData( int rows, int columns, float *data ) {
  2345.     assert( mat < idMatX::tempPtr || mat > idMatX::tempPtr + MATX_MAX_TEMP );
  2346.     if ( mat != NULL && alloced != -1 ) {
  2347.         Mem_Free16( mat );
  2348.     }
  2349.     assert( ( ( (int) data ) & 15 ) == 0 ); // data must be 16 byte aligned
  2350.     mat = data;
  2351.     alloced = -1;
  2352.     numRows = rows;
  2353.     numColumns = columns;
  2354.     MATX_CLEAREND();
  2355. }
  2356.  
  2357. ID_INLINE void idMatX::Zero( void ) {
  2358. #ifdef MATX_SIMD
  2359.     SIMDProcessor->Zero16( mat, numRows * numColumns );
  2360. #else
  2361.     memset( mat, 0, numRows * numColumns * sizeof( float ) );
  2362. #endif
  2363. }
  2364.  
  2365. ID_INLINE void idMatX::Zero( int rows, int columns ) {
  2366.     SetSize( rows, columns );
  2367. #ifdef MATX_SIMD
  2368.     SIMDProcessor->Zero16( mat, numRows * numColumns );
  2369. #else
  2370.     memset( mat, 0, rows * columns * sizeof( float ) );
  2371. #endif
  2372. }
  2373.  
  2374. ID_INLINE void idMatX::Identity( void ) {
  2375.     assert( numRows == numColumns );
  2376. #ifdef MATX_SIMD
  2377.     SIMDProcessor->Zero16( mat, numRows * numColumns );
  2378. #else
  2379.     memset( mat, 0, numRows * numColumns * sizeof( float ) );
  2380. #endif
  2381.     for ( int i = 0; i < numRows; i++ ) {
  2382.         mat[i * numColumns + i] = 1.0f;
  2383.     }
  2384. }
  2385.  
  2386. ID_INLINE void idMatX::Identity( int rows, int columns ) {
  2387.     assert( rows == columns );
  2388.     SetSize( rows, columns );
  2389.     idMatX::Identity();
  2390. }
  2391.  
  2392. ID_INLINE void idMatX::Diag( const idVecX &v ) {
  2393.     Zero( v.GetSize(), v.GetSize() );
  2394.     for ( int i = 0; i < v.GetSize(); i++ ) {
  2395.         mat[i * numColumns + i] = v[i];
  2396.     }
  2397. }
  2398.  
  2399. ID_INLINE void idMatX::Random( int seed, float l, float u ) {
  2400.     int i, s;
  2401.     float c;
  2402.     idRandom rnd(seed);
  2403.  
  2404.     c = u - l;
  2405.     s = numRows * numColumns;
  2406.     for ( i = 0; i < s; i++ ) {
  2407.         mat[i] = l + rnd.RandomFloat() * c;
  2408.     }
  2409. }
  2410.  
  2411. ID_INLINE void idMatX::Random( int rows, int columns, int seed, float l, float u ) {
  2412.     int i, s;
  2413.     float c;
  2414.     idRandom rnd(seed);
  2415.  
  2416.     SetSize( rows, columns );
  2417.     c = u - l;
  2418.     s = numRows * numColumns;
  2419.     for ( i = 0; i < s; i++ ) {
  2420.         mat[i] = l + rnd.RandomFloat() * c;
  2421.     }
  2422. }
  2423.  
  2424. ID_INLINE void idMatX::Negate( void ) {
  2425. #ifdef MATX_SIMD
  2426.     SIMDProcessor->Negate16( mat, numRows * numColumns );
  2427. #else
  2428.     int i, s;
  2429.     s = numRows * numColumns;
  2430.     for ( i = 0; i < s; i++ ) {
  2431.         mat[i] = -mat[i];
  2432.     }
  2433. #endif
  2434. }
  2435.  
  2436. ID_INLINE void idMatX::Clamp( float min, float max ) {
  2437.     int i, s;
  2438.     s = numRows * numColumns;
  2439.     for ( i = 0; i < s; i++ ) {
  2440.         if ( mat[i] < min ) {
  2441.             mat[i] = min;
  2442.         } else if ( mat[i] > max ) {
  2443.             mat[i] = max;
  2444.         }
  2445.     }
  2446. }
  2447.  
  2448. ID_INLINE idMatX &idMatX::SwapRows( int r1, int r2 ) {
  2449.     float *ptr;
  2450.  
  2451.     ptr = (float *) _alloca16( numColumns * sizeof( float ) );
  2452.     memcpy( ptr, mat + r1 * numColumns, numColumns * sizeof( float ) );
  2453.     memcpy( mat + r1 * numColumns, mat + r2 * numColumns, numColumns * sizeof( float ) );
  2454.     memcpy( mat + r2 * numColumns, ptr, numColumns * sizeof( float ) );
  2455.  
  2456.     return *this;
  2457. }
  2458.  
  2459. ID_INLINE idMatX &idMatX::SwapColumns( int r1, int r2 ) {
  2460.     int i;
  2461.     float tmp, *ptr;
  2462.  
  2463.     for ( i = 0; i < numRows; i++ ) {
  2464.         ptr = mat + i * numColumns;
  2465.         tmp = ptr[r1];
  2466.         ptr[r1] = ptr[r2];
  2467.         ptr[r2] = tmp;
  2468.     }
  2469.  
  2470.     return *this;
  2471. }
  2472.  
  2473. ID_INLINE idMatX &idMatX::SwapRowsColumns( int r1, int r2 ) {
  2474.  
  2475.     SwapRows( r1, r2 );
  2476.     SwapColumns( r1, r2 );
  2477.     return *this;
  2478. }
  2479.  
  2480. ID_INLINE void idMatX::ClearUpperTriangle( void ) {
  2481.     assert( numRows == numColumns );
  2482.     for ( int i = numRows-2; i >= 0; i-- ) {
  2483.         memset( mat + i * numColumns + i + 1, 0, (numColumns - 1 - i) * sizeof(float) );
  2484.     }
  2485. }
  2486.  
  2487. ID_INLINE void idMatX::ClearLowerTriangle( void ) {
  2488.     assert( numRows == numColumns );
  2489.     for ( int i = 1; i < numRows; i++ ) {
  2490.         memset( mat + i * numColumns, 0, i * sizeof(float) );
  2491.     }
  2492. }
  2493.  
  2494. ID_INLINE void idMatX::SquareSubMatrix( const idMatX &m, int size ) {
  2495.     int i;
  2496.     assert( size <= m.numRows && size <= m.numColumns );
  2497.     SetSize( size, size );
  2498.     for ( i = 0; i < size; i++ ) {
  2499.         memcpy( mat + i * numColumns, m.mat + i * m.numColumns, size * sizeof( float ) );
  2500.     }
  2501. }
  2502.  
  2503. ID_INLINE float idMatX::MaxDifference( const idMatX &m ) const {
  2504.     int i, j;
  2505.     float diff, maxDiff;
  2506.  
  2507.     assert( numRows == m.numRows && numColumns == m.numColumns );
  2508.  
  2509.     maxDiff = -1.0f;
  2510.     for ( i = 0; i < numRows; i++ ) {
  2511.         for ( j = 0; j < numColumns; j++ ) {
  2512.             diff = idMath::Fabs( mat[ i * numColumns + j ] - m[i][j] );
  2513.             if ( maxDiff < 0.0f || diff > maxDiff ) {
  2514.                 maxDiff = diff;
  2515.             }
  2516.         }
  2517.     }
  2518.     return maxDiff;
  2519. }
  2520.  
  2521. ID_INLINE bool idMatX::IsZero( const float epsilon ) const {
  2522.     // returns true if (*this) == Zero
  2523.     for ( int i = 0; i < numRows; i++ ) {
  2524.         for ( int j = 0; j < numColumns; j++ ) {
  2525.             if ( idMath::Fabs( mat[i * numColumns + j] ) > epsilon ) {
  2526.                 return false;
  2527.             }
  2528.         }
  2529.     }
  2530.     return true;
  2531. }
  2532.  
  2533. ID_INLINE bool idMatX::IsIdentity( const float epsilon ) const {
  2534.     // returns true if (*this) == Identity
  2535.     assert( numRows == numColumns );
  2536.     for ( int i = 0; i < numRows; i++ ) {
  2537.         for ( int j = 0; j < numColumns; j++ ) {
  2538.             if ( idMath::Fabs( mat[i * numColumns + j] - (float)( i == j ) ) > epsilon ) {
  2539.                 return false;
  2540.             }
  2541.         }
  2542.     }
  2543.     return true;
  2544. }
  2545.  
  2546. ID_INLINE bool idMatX::IsDiagonal( const float epsilon ) const {
  2547.     // returns true if all elements are zero except for the elements on the diagonal
  2548.     assert( numRows == numColumns );
  2549.     for ( int i = 0; i < numRows; i++ ) {
  2550.         for ( int j = 0; j < numColumns; j++ ) {
  2551.             if ( i != j && idMath::Fabs( mat[i * numColumns + j] ) > epsilon ) {
  2552.                 return false;
  2553.             }
  2554.         }
  2555.     }
  2556.     return true;
  2557. }
  2558.  
  2559. ID_INLINE bool idMatX::IsTriDiagonal( const float epsilon ) const {
  2560.     // returns true if all elements are zero except for the elements on the diagonal plus or minus one column
  2561.  
  2562.     if ( numRows != numColumns ) {
  2563.         return false;
  2564.     }
  2565.     for ( int i = 0; i < numRows-2; i++ ) {
  2566.         for ( int j = i+2; j < numColumns; j++ ) {
  2567.             if ( idMath::Fabs( (*this)[i][j] ) > epsilon ) {
  2568.                 return false;
  2569.             }
  2570.             if ( idMath::Fabs( (*this)[j][i] ) > epsilon ) {
  2571.                 return false;
  2572.             }
  2573.         }
  2574.     }
  2575.     return true;
  2576. }
  2577.  
  2578. ID_INLINE bool idMatX::IsSymmetric( const float epsilon ) const {
  2579.     // (*this)[i][j] == (*this)[j][i]
  2580.     if ( numRows != numColumns ) {
  2581.         return false;
  2582.     }
  2583.     for ( int i = 0; i < numRows; i++ ) {
  2584.         for ( int j = 0; j < numColumns; j++ ) {
  2585.             if ( idMath::Fabs( mat[ i * numColumns + j ] - mat[ j * numColumns + i ] ) > epsilon ) {
  2586.                 return false;
  2587.             }
  2588.         }
  2589.     }
  2590.     return true;
  2591. }
  2592.  
  2593. ID_INLINE float idMatX::Trace( void ) const {
  2594.     float trace = 0.0f;
  2595.  
  2596.     assert( numRows == numColumns );
  2597.  
  2598.     // sum of elements on the diagonal
  2599.     for ( int i = 0; i < numRows; i++ ) {
  2600.         trace += mat[i * numRows + i];
  2601.     }
  2602.     return trace;
  2603. }
  2604.  
  2605. ID_INLINE float idMatX::Determinant( void ) const {
  2606.  
  2607.     assert( numRows == numColumns );
  2608.  
  2609.     switch( numRows ) {
  2610.         case 1:
  2611.             return mat[0];
  2612.         case 2:
  2613.             return reinterpret_cast<const idMat2 *>(mat)->Determinant();
  2614.         case 3:
  2615.             return reinterpret_cast<const idMat3 *>(mat)->Determinant();
  2616.         case 4:
  2617.             return reinterpret_cast<const idMat4 *>(mat)->Determinant();
  2618.         case 5:
  2619.             return reinterpret_cast<const idMat5 *>(mat)->Determinant();
  2620.         case 6:
  2621.             return reinterpret_cast<const idMat6 *>(mat)->Determinant();
  2622.         default:
  2623.             return DeterminantGeneric();
  2624.     }
  2625.     return 0.0f;
  2626. }
  2627.  
  2628. ID_INLINE idMatX idMatX::Transpose( void ) const {
  2629.     idMatX transpose;
  2630.     int i, j;
  2631.  
  2632.     transpose.SetTempSize( numColumns, numRows );
  2633.  
  2634.     for ( i = 0; i < numRows; i++ ) {
  2635.         for ( j = 0; j < numColumns; j++ ) {
  2636.             transpose.mat[j * transpose.numColumns + i] = mat[i * numColumns + j];
  2637.         }
  2638.     }
  2639.  
  2640.     return transpose;
  2641. }
  2642.  
  2643. ID_INLINE idMatX &idMatX::TransposeSelf( void ) {
  2644.     *this = Transpose();
  2645.     return *this;
  2646. }
  2647.  
  2648. ID_INLINE idMatX idMatX::Inverse( void ) const {
  2649.     idMatX invMat;
  2650.  
  2651.     invMat.SetTempSize( numRows, numColumns );
  2652.     memcpy( invMat.mat, mat, numRows * numColumns * sizeof( float ) );
  2653. #ifdef _DEBUG
  2654.     assert ( invMat.InverseSelf() );
  2655. #else
  2656.     invMat.InverseSelf();
  2657. #endif
  2658.     return invMat;
  2659. }
  2660.  
  2661. ID_INLINE bool idMatX::InverseSelf( void ) {
  2662.  
  2663.     assert( numRows == numColumns );
  2664.  
  2665.     switch( numRows ) {
  2666.         case 1:
  2667.             if ( idMath::Fabs( mat[0] ) < MATRIX_INVERSE_EPSILON ) {
  2668.                 return false;
  2669.             }
  2670.             mat[0] = 1.0f / mat[0];
  2671.             return true;
  2672.         case 2:
  2673.             return reinterpret_cast<idMat2 *>(mat)->InverseSelf();
  2674.         case 3:
  2675.             return reinterpret_cast<idMat3 *>(mat)->InverseSelf();
  2676.         case 4:
  2677.             return reinterpret_cast<idMat4 *>(mat)->InverseSelf();
  2678.         case 5:
  2679.             return reinterpret_cast<idMat5 *>(mat)->InverseSelf();
  2680.         case 6:
  2681.             return reinterpret_cast<idMat6 *>(mat)->InverseSelf();
  2682.         default:
  2683.             return InverseSelfGeneric();
  2684.     }
  2685. }
  2686.  
  2687. ID_INLINE idMatX idMatX::InverseFast( void ) const {
  2688.     idMatX invMat;
  2689.  
  2690.     invMat.SetTempSize( numRows, numColumns );
  2691.     memcpy( invMat.mat, mat, numRows * numColumns * sizeof( float ) );
  2692. #ifdef _DEBUG
  2693.     assert ( invMat.InverseFastSelf() );
  2694. #else
  2695.     invMat.InverseFastSelf();
  2696. #endif
  2697.     return invMat;
  2698. }
  2699.  
  2700. ID_INLINE bool idMatX::InverseFastSelf( void ) {
  2701.  
  2702.     assert( numRows == numColumns );
  2703.  
  2704.     switch( numRows ) {
  2705.         case 1:
  2706.             if ( idMath::Fabs( mat[0] ) < MATRIX_INVERSE_EPSILON ) {
  2707.                 return false;
  2708.             }
  2709.             mat[0] = 1.0f / mat[0];
  2710.             return true;
  2711.         case 2:
  2712.             return reinterpret_cast<idMat2 *>(mat)->InverseFastSelf();
  2713.         case 3:
  2714.             return reinterpret_cast<idMat3 *>(mat)->InverseFastSelf();
  2715.         case 4:
  2716.             return reinterpret_cast<idMat4 *>(mat)->InverseFastSelf();
  2717.         case 5:
  2718.             return reinterpret_cast<idMat5 *>(mat)->InverseFastSelf();
  2719.         case 6:
  2720.             return reinterpret_cast<idMat6 *>(mat)->InverseFastSelf();
  2721.         default:
  2722.             return InverseSelfGeneric();
  2723.     }
  2724. //unreachable
  2725. //return false;
  2726. }
  2727.  
  2728. ID_INLINE idVecX idMatX::Multiply( const idVecX &vec ) const {
  2729.     idVecX dst;
  2730.  
  2731.     assert( numColumns == vec.GetSize() );
  2732.  
  2733.     dst.SetTempSize( numRows );
  2734. #ifdef MATX_SIMD
  2735.     SIMDProcessor->MatX_MultiplyVecX( dst, *this, vec );
  2736. #else
  2737.     Multiply( dst, vec );
  2738. #endif
  2739.     return dst;
  2740. }
  2741.  
  2742. ID_INLINE idMatX idMatX::Multiply( const idMatX &a ) const {
  2743.     idMatX dst;
  2744.  
  2745.     assert( numColumns == a.numRows );
  2746.  
  2747.     dst.SetTempSize( numRows, a.numColumns );
  2748. #ifdef MATX_SIMD
  2749.     SIMDProcessor->MatX_MultiplyMatX( dst, *this, a );
  2750. #else
  2751.     Multiply( dst, a );
  2752. #endif
  2753.     return dst;
  2754. }
  2755.  
  2756. ID_INLINE idVecX idMatX::TransposeMultiply( const idVecX &vec ) const {
  2757.     idVecX dst;
  2758.  
  2759.     assert( numRows == vec.GetSize() );
  2760.  
  2761.     dst.SetTempSize( numColumns );
  2762. #ifdef MATX_SIMD
  2763.     SIMDProcessor->MatX_TransposeMultiplyVecX( dst, *this, vec );
  2764. #else
  2765.     TransposeMultiply( dst, vec );
  2766. #endif
  2767.     return dst;
  2768. }
  2769.  
  2770. ID_INLINE idMatX idMatX::TransposeMultiply( const idMatX &a ) const {
  2771.     idMatX dst;
  2772.  
  2773.     assert( numRows == a.numRows );
  2774.  
  2775.     dst.SetTempSize( numColumns, a.numColumns );
  2776. #ifdef MATX_SIMD
  2777.     SIMDProcessor->MatX_TransposeMultiplyMatX( dst, *this, a );
  2778. #else
  2779.     TransposeMultiply( dst, a );
  2780. #endif
  2781.     return dst;
  2782. }
  2783.  
  2784. ID_INLINE void idMatX::Multiply( idVecX &dst, const idVecX &vec ) const {
  2785. #ifdef MATX_SIMD
  2786.     SIMDProcessor->MatX_MultiplyVecX( dst, *this, vec );
  2787. #else
  2788.     int i, j;
  2789.     const float *mPtr, *vPtr;
  2790.     float *dstPtr;
  2791.  
  2792.     mPtr = mat;
  2793.     vPtr = vec.ToFloatPtr();
  2794.     dstPtr = dst.ToFloatPtr();
  2795.     for ( i = 0; i < numRows; i++ ) {
  2796.         float sum = mPtr[0] * vPtr[0];
  2797.         for ( j = 1; j < numColumns; j++ ) {
  2798.             sum += mPtr[j] * vPtr[j];
  2799.         }
  2800.         dstPtr[i] = sum;
  2801.         mPtr += numColumns;
  2802.     }
  2803. #endif
  2804. }
  2805.  
  2806. ID_INLINE void idMatX::MultiplyAdd( idVecX &dst, const idVecX &vec ) const {
  2807. #ifdef MATX_SIMD
  2808.     SIMDProcessor->MatX_MultiplyAddVecX( dst, *this, vec );
  2809. #else
  2810.     int i, j;
  2811.     const float *mPtr, *vPtr;
  2812.     float *dstPtr;
  2813.  
  2814.     mPtr = mat;
  2815.     vPtr = vec.ToFloatPtr();
  2816.     dstPtr = dst.ToFloatPtr();
  2817.     for ( i = 0; i < numRows; i++ ) {
  2818.         float sum = mPtr[0] * vPtr[0];
  2819.         for ( j = 1; j < numColumns; j++ ) {
  2820.             sum += mPtr[j] * vPtr[j];
  2821.         }
  2822.         dstPtr[i] += sum;
  2823.         mPtr += numColumns;
  2824.     }
  2825. #endif
  2826. }
  2827.  
  2828. ID_INLINE void idMatX::MultiplySub( idVecX &dst, const idVecX &vec ) const {
  2829. #ifdef MATX_SIMD
  2830.     SIMDProcessor->MatX_MultiplySubVecX( dst, *this, vec );
  2831. #else
  2832.     int i, j;
  2833.     const float *mPtr, *vPtr;
  2834.     float *dstPtr;
  2835.  
  2836.     mPtr = mat;
  2837.     vPtr = vec.ToFloatPtr();
  2838.     dstPtr = dst.ToFloatPtr();
  2839.     for ( i = 0; i < numRows; i++ ) {
  2840.         float sum = mPtr[0] * vPtr[0];
  2841.         for ( j = 1; j < numColumns; j++ ) {
  2842.             sum += mPtr[j] * vPtr[j];
  2843.         }
  2844.         dstPtr[i] -= sum;
  2845.         mPtr += numColumns;
  2846.     }
  2847. #endif
  2848. }
  2849.  
  2850. ID_INLINE void idMatX::TransposeMultiply( idVecX &dst, const idVecX &vec ) const {
  2851. #ifdef MATX_SIMD
  2852.     SIMDProcessor->MatX_TransposeMultiplyVecX( dst, *this, vec );
  2853. #else
  2854.     int i, j;
  2855.     const float *mPtr, *vPtr;
  2856.     float *dstPtr;
  2857.  
  2858.     vPtr = vec.ToFloatPtr();
  2859.     dstPtr = dst.ToFloatPtr();
  2860.     for ( i = 0; i < numColumns; i++ ) {
  2861.         mPtr = mat + i;
  2862.         float sum = mPtr[0] * vPtr[0];
  2863.         for ( j = 1; j < numRows; j++ ) {
  2864.             mPtr += numColumns;
  2865.             sum += mPtr[0] * vPtr[j];
  2866.         }
  2867.         dstPtr[i] = sum;
  2868.     }
  2869. #endif
  2870. }
  2871.  
  2872. ID_INLINE void idMatX::TransposeMultiplyAdd( idVecX &dst, const idVecX &vec ) const {
  2873. #ifdef MATX_SIMD
  2874.     SIMDProcessor->MatX_TransposeMultiplyAddVecX( dst, *this, vec );
  2875. #else
  2876.     int i, j;
  2877.     const float *mPtr, *vPtr;
  2878.     float *dstPtr;
  2879.  
  2880.     vPtr = vec.ToFloatPtr();
  2881.     dstPtr = dst.ToFloatPtr();
  2882.     for ( i = 0; i < numColumns; i++ ) {
  2883.         mPtr = mat + i;
  2884.         float sum = mPtr[0] * vPtr[0];
  2885.         for ( j = 1; j < numRows; j++ ) {
  2886.             mPtr += numColumns;
  2887.             sum += mPtr[0] * vPtr[j];
  2888.         }
  2889.         dstPtr[i] += sum;
  2890.     }
  2891. #endif
  2892. }
  2893.  
  2894. ID_INLINE void idMatX::TransposeMultiplySub( idVecX &dst, const idVecX &vec ) const {
  2895. #ifdef MATX_SIMD
  2896.     SIMDProcessor->MatX_TransposeMultiplySubVecX( dst, *this, vec );
  2897. #else
  2898.     int i, j;
  2899.     const float *mPtr, *vPtr;
  2900.     float *dstPtr;
  2901.  
  2902.     vPtr = vec.ToFloatPtr();
  2903.     dstPtr = dst.ToFloatPtr();
  2904.     for ( i = 0; i < numColumns; i++ ) {
  2905.         mPtr = mat + i;
  2906.         float sum = mPtr[0] * vPtr[0];
  2907.         for ( j = 1; j < numRows; j++ ) {
  2908.             mPtr += numColumns;
  2909.             sum += mPtr[0] * vPtr[j];
  2910.         }
  2911.         dstPtr[i] -= sum;
  2912.     }
  2913. #endif
  2914. }
  2915.  
  2916. ID_INLINE void idMatX::Multiply( idMatX &dst, const idMatX &a ) const {
  2917. #ifdef MATX_SIMD
  2918.     SIMDProcessor->MatX_MultiplyMatX( dst, *this, a );
  2919. #else
  2920.     int i, j, k, l, n;
  2921.     float *dstPtr;
  2922.     const float *m1Ptr, *m2Ptr;
  2923.     double sum;
  2924.  
  2925.     assert( numColumns == a.numRows );
  2926.  
  2927.     dstPtr = dst.ToFloatPtr();
  2928.     m1Ptr = ToFloatPtr();
  2929.     m2Ptr = a.ToFloatPtr();
  2930.     k = numRows;
  2931.     l = a.GetNumColumns();
  2932.  
  2933.     for ( i = 0; i < k; i++ ) {
  2934.         for ( j = 0; j < l; j++ ) {
  2935.             m2Ptr = a.ToFloatPtr() + j;
  2936.             sum = m1Ptr[0] * m2Ptr[0];
  2937.             for ( n = 1; n < numColumns; n++ ) {
  2938.                 m2Ptr += l;
  2939.                 sum += m1Ptr[n] * m2Ptr[0];
  2940.             }
  2941.             *dstPtr++ = sum;
  2942.         }
  2943.         m1Ptr += numColumns;
  2944.     }
  2945. #endif
  2946. }
  2947.  
  2948. ID_INLINE void idMatX::TransposeMultiply( idMatX &dst, const idMatX &a ) const {
  2949. #ifdef MATX_SIMD
  2950.     SIMDProcessor->MatX_TransposeMultiplyMatX( dst, *this, a );
  2951. #else
  2952.     int i, j, k, l, n;
  2953.     float *dstPtr;
  2954.     const float *m1Ptr, *m2Ptr;
  2955.     double sum;
  2956.  
  2957.     assert( numRows == a.numRows );
  2958.  
  2959.     dstPtr = dst.ToFloatPtr();
  2960.     m1Ptr = ToFloatPtr();
  2961.     k = numColumns;
  2962.     l = a.numColumns;
  2963.  
  2964.     for ( i = 0; i < k; i++ ) {
  2965.         for ( j = 0; j < l; j++ ) {
  2966.             m1Ptr = ToFloatPtr() + i;
  2967.             m2Ptr = a.ToFloatPtr() + j;
  2968.             sum = m1Ptr[0] * m2Ptr[0];
  2969.             for ( n = 1; n < numRows; n++ ) {
  2970.                 m1Ptr += numColumns;
  2971.                 m2Ptr += a.numColumns;
  2972.                 sum += m1Ptr[0] * m2Ptr[0];
  2973.             }
  2974.             *dstPtr++ = sum;
  2975.         }
  2976.     }
  2977. #endif
  2978. }
  2979.  
  2980. ID_INLINE int idMatX::GetDimension( void ) const {
  2981.     return numRows * numColumns;
  2982. }
  2983.  
  2984. ID_INLINE const idVec6 &idMatX::SubVec6( int row ) const {
  2985.     assert( numColumns >= 6 && row >= 0 && row < numRows );
  2986.     return *reinterpret_cast<const idVec6 *>(mat + row * numColumns);
  2987. }
  2988.  
  2989. ID_INLINE idVec6 &idMatX::SubVec6( int row ) {
  2990.     assert( numColumns >= 6 && row >= 0 && row < numRows );
  2991.     return *reinterpret_cast<idVec6 *>(mat + row * numColumns);
  2992. }
  2993.  
  2994. ID_INLINE const idVecX idMatX::SubVecX( int row ) const {
  2995.     idVecX v;
  2996.     assert( row >= 0 && row < numRows );
  2997.     v.SetData( numColumns, mat + row * numColumns );
  2998.     return v;
  2999. }
  3000.  
  3001. ID_INLINE idVecX idMatX::SubVecX( int row ) {
  3002.     idVecX v;
  3003.     assert( row >= 0 && row < numRows );
  3004.     v.SetData( numColumns, mat + row * numColumns );
  3005.     return v;
  3006. }
  3007.  
  3008. ID_INLINE const float *idMatX::ToFloatPtr( void ) const {
  3009.     return mat;
  3010. }
  3011.  
  3012. ID_INLINE float *idMatX::ToFloatPtr( void ) {
  3013.     return mat;
  3014. }
  3015.  
  3016. #endif /* !__MATH_MATRIX_H__ */
  3017.