home *** CD-ROM | disk | FTP | other *** search
/ GameStar 2006 March / Gamestar_82_2006-03_dvd.iso / DVDStar / Editace / quake4_sdkv10.exe / source / idlib / math / Mat3x4.h < prev    next >
C/C++ Source or Header  |  2005-11-14  |  11KB  |  287 lines
  1.  
  2. #ifndef __MAT3X4_H__
  3. #define __MAT3X4_H__
  4.  
  5. /*
  6. ===============================================================================
  7.  
  8.   row-major 3x4 matrix
  9.  
  10.   idMat3 m;
  11.   idVec3 t;
  12.  
  13.   m[0][0], m[1][0], m[2][0], t[0]
  14.   m[0][1], m[1][1], m[2][1], t[1]
  15.   m[0][2], m[1][2], m[2][2], t[2]
  16.  
  17. ===============================================================================
  18. */
  19.  
  20. class idMat3x4 {
  21. public:
  22.  
  23.     void            SetRotation( const idMat3 &m );
  24.     void            SetTranslation( const idVec3 &t );
  25.  
  26.     bool            Compare( const idMat3x4 &a ) const;                            // exact compare, no epsilon
  27.     bool            Compare( const idMat3x4 &a, const float epsilon ) const;    // compare with epsilon
  28.     bool            operator==(    const idMat3x4 &a ) const;                        // exact compare, no epsilon
  29.     bool            operator!=(    const idMat3x4 &a ) const;                        // exact compare, no epsilon
  30.  
  31.     void            Identity( void );
  32.     void            Invert( void );
  33.  
  34.     void            LeftMultiply( const idMat3x4 &m );
  35.     void            LeftMultiply( const idMat3 &m );
  36.     void            RightMultiply( const idMat3x4 &m );
  37.     void            RightMultiply( const idMat3 &m );
  38.  
  39.     void            Transform( idVec3 &result, const idVec3 &v ) const;
  40.     void            Rotate( idVec3 &result, const idVec3 &v ) const;
  41.  
  42.     idMat3            ToMat3( void ) const;
  43.     idVec3            ToVec3( void ) const;
  44.     const float *    ToFloatPtr( void ) const;
  45.     float *            ToFloatPtr( void );
  46.     const char *    ToString( int precision = 2 ) const;
  47.  
  48. private:
  49.     float            mat[3*4];
  50. };
  51.  
  52.  
  53. ID_INLINE void idMat3x4::SetRotation( const idMat3 &m ) {
  54.     // NOTE: idMat3 is transposed because it is column-major
  55.     mat[0 * 4 + 0] = m[0][0];
  56.     mat[0 * 4 + 1] = m[1][0];
  57.     mat[0 * 4 + 2] = m[2][0];
  58.     mat[1 * 4 + 0] = m[0][1];
  59.     mat[1 * 4 + 1] = m[1][1];
  60.     mat[1 * 4 + 2] = m[2][1];
  61.     mat[2 * 4 + 0] = m[0][2];
  62.     mat[2 * 4 + 1] = m[1][2];
  63.     mat[2 * 4 + 2] = m[2][2];
  64. }
  65.  
  66. ID_INLINE void idMat3x4::SetTranslation( const idVec3 &t ) {
  67.     mat[0 * 4 + 3] = t[0];
  68.     mat[1 * 4 + 3] = t[1];
  69.     mat[2 * 4 + 3] = t[2];
  70. }
  71.  
  72. ID_INLINE bool idMat3x4::Compare( const idMat3x4 &a ) const {
  73.     int i;
  74.  
  75.     for ( i = 0; i < 12; i++ ) {
  76.         if ( mat[i] != a.mat[i] ) {
  77.             return false;
  78.         }
  79.     }
  80.     return true;
  81. }
  82.  
  83. ID_INLINE bool idMat3x4::Compare( const idMat3x4 &a, const float epsilon ) const {
  84.     int i;
  85.  
  86.     for ( i = 0; i < 12; i++ ) {
  87.         if ( idMath::Fabs( mat[i] - a.mat[i] ) > epsilon ) {
  88.             return false;
  89.         }
  90.     }
  91.     return true;
  92. }
  93.  
  94. ID_INLINE bool idMat3x4::operator==( const idMat3x4 &a ) const {
  95.     return Compare( a );
  96. }
  97.  
  98. ID_INLINE bool idMat3x4::operator!=( const idMat3x4 &a ) const {
  99.     return !Compare( a );
  100. }
  101.  
  102. ID_INLINE void idMat3x4::Identity( void ) {
  103.     mat[0 * 4 + 0] = 1.0f; mat[0 * 4 + 1] = 0.0f; mat[0 * 4 + 2] = 0.0f; mat[0 * 4 + 3] = 0.0f;
  104.     mat[0 * 4 + 0] = 0.0f; mat[0 * 4 + 1] = 1.0f; mat[0 * 4 + 2] = 0.0f; mat[0 * 4 + 3] = 0.0f;
  105.     mat[0 * 4 + 0] = 0.0f; mat[0 * 4 + 1] = 0.0f; mat[0 * 4 + 2] = 1.0f; mat[0 * 4 + 3] = 0.0f;
  106. }
  107.  
  108. ID_INLINE void idMat3x4::Invert( void ) {
  109.     float tmp[3];
  110.  
  111.     // negate inverse rotated translation part
  112.     tmp[0] = mat[0 * 4 + 0] * mat[0 * 4 + 3] + mat[1 * 4 + 0] * mat[1 * 4 + 3] + mat[2 * 4 + 0] * mat[2 * 4 + 3];
  113.     tmp[1] = mat[0 * 4 + 1] * mat[0 * 4 + 3] + mat[1 * 4 + 1] * mat[1 * 4 + 3] + mat[2 * 4 + 1] * mat[2 * 4 + 3];
  114.     tmp[2] = mat[0 * 4 + 2] * mat[0 * 4 + 3] + mat[1 * 4 + 2] * mat[1 * 4 + 3] + mat[2 * 4 + 2] * mat[2 * 4 + 3];
  115.     mat[0 * 4 + 3] = -tmp[0];
  116.     mat[1 * 4 + 3] = -tmp[1];
  117.     mat[2 * 4 + 3] = -tmp[2];
  118.  
  119.     // transpose rotation part
  120.     tmp[0] = mat[0 * 4 + 1];
  121.     mat[0 * 4 + 1] = mat[1 * 4 + 0];
  122.     mat[1 * 4 + 0] = tmp[0];
  123.     tmp[1] = mat[0 * 4 + 2];
  124.     mat[0 * 4 + 2] = mat[2 * 4 + 0];
  125.     mat[2 * 4 + 0] = tmp[1];
  126.     tmp[2] = mat[1 * 4 + 2];
  127.     mat[1 * 4 + 2] = mat[2 * 4 + 1];
  128.     mat[2 * 4 + 1] = tmp[2];
  129. }
  130.  
  131. ID_INLINE void idMat3x4::LeftMultiply( const idMat3x4 &m ) {
  132.     float t0, t1;
  133.  
  134.     t0                = m.mat[0 * 4 + 0] * mat[0 * 4 + 0] + m.mat[0 * 4 + 1] * mat[1 * 4 + 0] + m.mat[0 * 4 + 2] * mat[2 * 4 + 0];
  135.     t1                = m.mat[1 * 4 + 0] * mat[0 * 4 + 0] + m.mat[1 * 4 + 1] * mat[1 * 4 + 0] + m.mat[1 * 4 + 2] * mat[2 * 4 + 0];
  136.     mat[2 * 4 + 0]    = m.mat[2 * 4 + 0] * mat[0 * 4 + 0] + m.mat[2 * 4 + 1] * mat[1 * 4 + 0] + m.mat[2 * 4 + 2] * mat[2 * 4 + 0];
  137.  
  138.     mat[1 * 4 + 0] = t1;
  139.     mat[0 * 4 + 0] = t0;
  140.  
  141.     t0                = m.mat[0 * 4 + 0] * mat[0 * 4 + 1] + m.mat[0 * 4 + 1] * mat[1 * 4 + 1] + m.mat[0 * 4 + 2] * mat[2 * 4 + 1];
  142.     t1                = m.mat[1 * 4 + 0] * mat[0 * 4 + 1] + m.mat[1 * 4 + 1] * mat[1 * 4 + 1] + m.mat[1 * 4 + 2] * mat[2 * 4 + 1];
  143.     mat[2 * 4 + 1]    = m.mat[2 * 4 + 0] * mat[0 * 4 + 1] + m.mat[2 * 4 + 1] * mat[1 * 4 + 1] + m.mat[2 * 4 + 2] * mat[2 * 4 + 1];
  144.  
  145.     mat[1 * 4 + 1] = t1;
  146.     mat[0 * 4 + 1] = t0;
  147.  
  148.     t0                = m.mat[0 * 4 + 0] * mat[0 * 4 + 2] + m.mat[0 * 4 + 1] * mat[1 * 4 + 2] + m.mat[0 * 4 + 2] * mat[2 * 4 + 2];
  149.     t1                = m.mat[1 * 4 + 0] * mat[0 * 4 + 2] + m.mat[1 * 4 + 1] * mat[1 * 4 + 2] + m.mat[1 * 4 + 2] * mat[2 * 4 + 2];
  150.     mat[2 * 4 + 2]    = m.mat[2 * 4 + 0] * mat[0 * 4 + 2] + m.mat[2 * 4 + 1] * mat[1 * 4 + 2] + m.mat[2 * 4 + 2] * mat[2 * 4 + 2];
  151.  
  152.     mat[1 * 4 + 2] = t1;
  153.     mat[0 * 4 + 2] = t0;
  154.  
  155.     t0                = m.mat[0 * 4 + 0] * mat[0 * 4 + 3] + m.mat[0 * 4 + 1] * mat[1 * 4 + 3] + m.mat[0 * 4 + 2] * mat[2 * 4 + 3] + m.mat[0 * 4 + 3];
  156.     t1                = m.mat[1 * 4 + 0] * mat[0 * 4 + 3] + m.mat[1 * 4 + 1] * mat[1 * 4 + 3] + m.mat[1 * 4 + 2] * mat[2 * 4 + 3] + m.mat[1 * 4 + 3];
  157.     mat[2 * 4 + 3]    = m.mat[2 * 4 + 0] * mat[0 * 4 + 3] + m.mat[2 * 4 + 1] * mat[1 * 4 + 3] + m.mat[2 * 4 + 2] * mat[2 * 4 + 3] + m.mat[2 * 4 + 3];
  158.  
  159.     mat[1 * 4 + 3] = t1;
  160.     mat[0 * 4 + 3] = t0;
  161. }
  162.  
  163. ID_INLINE void idMat3x4::LeftMultiply( const idMat3 &m ) {
  164.     float t0, t1;
  165.  
  166.     // NOTE: idMat3 is column-major
  167.     t0                = m[0][0] * mat[0 * 4 + 0] + m[1][0] * mat[1 * 4 + 0] + m[2][0] * mat[2 * 4 + 0];
  168.     t1                = m[0][1] * mat[0 * 4 + 0] + m[1][1] * mat[1 * 4 + 0] + m[2][1] * mat[2 * 4 + 0];
  169.     mat[2 * 4 + 0]    = m[0][2] * mat[0 * 4 + 0] + m[1][2] * mat[1 * 4 + 0] + m[2][2] * mat[2 * 4 + 0];
  170.  
  171.     mat[1 * 4 + 0] = t1;
  172.     mat[0 * 4 + 0] = t0;
  173.  
  174.     t0                = m[0][0] * mat[0 * 4 + 1] + m[1][0] * mat[1 * 4 + 1] + m[2][0] * mat[2 * 4 + 1];
  175.     t1                = m[0][1] * mat[0 * 4 + 1] + m[1][1] * mat[1 * 4 + 1] + m[2][1] * mat[2 * 4 + 1];
  176.     mat[2 * 4 + 1]    = m[0][2] * mat[0 * 4 + 1] + m[1][2] * mat[1 * 4 + 1] + m[2][2] * mat[2 * 4 + 1];
  177.  
  178.     mat[1 * 4 + 1] = t1;
  179.     mat[0 * 4 + 1] = t0;
  180.  
  181.     t0                = m[0][0] * mat[0 * 4 + 2] + m[1][0] * mat[1 * 4 + 2] + m[2][0] * mat[2 * 4 + 2];
  182.     t1                = m[0][1] * mat[0 * 4 + 2] + m[1][1] * mat[1 * 4 + 2] + m[2][1] * mat[2 * 4 + 2];
  183.     mat[2 * 4 + 2]    = m[0][2] * mat[0 * 4 + 2] + m[1][2] * mat[1 * 4 + 2] + m[2][2] * mat[2 * 4 + 2];
  184.  
  185.     mat[1 * 4 + 2] = t1;
  186.     mat[0 * 4 + 2] = t0;
  187.  
  188.     t0                = m[0][0] * mat[0 * 4 + 3] + m[1][0] * mat[1 * 4 + 3] + m[2][0] * mat[2 * 4 + 3];
  189.     t1                = m[0][1] * mat[0 * 4 + 3] + m[1][1] * mat[1 * 4 + 3] + m[2][1] * mat[2 * 4 + 3];
  190.     mat[2 * 4 + 3]    = m[0][2] * mat[0 * 4 + 3] + m[1][2] * mat[1 * 4 + 3] + m[2][2] * mat[2 * 4 + 3];
  191.  
  192.     mat[1 * 4 + 3] = t1;
  193.     mat[0 * 4 + 3] = t0;
  194. }
  195.  
  196. ID_INLINE void idMat3x4::RightMultiply( const idMat3x4 &m ) {
  197.     float t0, t1, t2;
  198.  
  199.     t0                = mat[0 * 4 + 0] * m.mat[0 * 4 + 0] + mat[0 * 4 + 1] * m.mat[1 * 4 + 0] + mat[0 * 4 + 2] * m.mat[2 * 4 + 0];
  200.     t1                = mat[0 * 4 + 0] * m.mat[0 * 4 + 1] + mat[0 * 4 + 1] * m.mat[1 * 4 + 1] + mat[0 * 4 + 2] * m.mat[2 * 4 + 1];
  201.     t2                = mat[0 * 4 + 0] * m.mat[0 * 4 + 2] + mat[0 * 4 + 1] * m.mat[1 * 4 + 2] + mat[0 * 4 + 2] * m.mat[2 * 4 + 2];
  202.     mat[0 * 4 + 3]    = mat[0 * 4 + 0] * m.mat[0 * 4 + 3] + mat[0 * 4 + 1] * m.mat[1 * 4 + 3] + mat[0 * 4 + 2] * m.mat[2 * 4 + 3] + mat[0 * 4 + 3];
  203.  
  204.     mat[0 * 4 + 0] = t0;
  205.     mat[0 * 4 + 1] = t1;
  206.     mat[0 * 4 + 2] = t2;
  207.  
  208.     t0                = mat[1 * 4 + 0] * m.mat[0 * 4 + 0] + mat[1 * 4 + 1] * m.mat[1 * 4 + 0] + mat[1 * 4 + 2] * m.mat[2 * 4 + 0];
  209.     t1                = mat[1 * 4 + 0] * m.mat[0 * 4 + 1] + mat[1 * 4 + 1] * m.mat[1 * 4 + 1] + mat[1 * 4 + 2] * m.mat[2 * 4 + 1];
  210.     t2                = mat[1 * 4 + 0] * m.mat[0 * 4 + 2] + mat[1 * 4 + 1] * m.mat[1 * 4 + 2] + mat[1 * 4 + 2] * m.mat[2 * 4 + 2];
  211.     mat[1 * 4 + 3]    = mat[1 * 4 + 0] * m.mat[0 * 4 + 3] + mat[1 * 4 + 1] * m.mat[1 * 4 + 3] + mat[1 * 4 + 2] * m.mat[2 * 4 + 3] + mat[1 * 4 + 3];
  212.  
  213.     mat[1 * 4 + 0] = t0;
  214.     mat[1 * 4 + 1] = t1;
  215.     mat[1 * 4 + 2] = t2;
  216.  
  217.     t0                = mat[2 * 4 + 0] * m.mat[0 * 4 + 0] + mat[2 * 4 + 1] * m.mat[1 * 4 + 0] + mat[2 * 4 + 2] * m.mat[2 * 4 + 0];
  218.     t1                = mat[2 * 4 + 0] * m.mat[0 * 4 + 1] + mat[2 * 4 + 1] * m.mat[1 * 4 + 1] + mat[2 * 4 + 2] * m.mat[2 * 4 + 1];
  219.     t2                = mat[2 * 4 + 0] * m.mat[0 * 4 + 2] + mat[2 * 4 + 1] * m.mat[1 * 4 + 2] + mat[2 * 4 + 2] * m.mat[2 * 4 + 2];
  220.     mat[2 * 4 + 3]    = mat[2 * 4 + 0] * m.mat[0 * 4 + 3] + mat[2 * 4 + 1] * m.mat[1 * 4 + 3] + mat[2 * 4 + 2] * m.mat[2 * 4 + 3] + mat[2 * 4 + 3];
  221.  
  222.     mat[2 * 4 + 0] = t0;
  223.     mat[2 * 4 + 1] = t1;
  224.     mat[2 * 4 + 2] = t2;
  225. }
  226.  
  227. ID_INLINE void idMat3x4::RightMultiply( const idMat3 &m ) {
  228.     float t0, t1, t2;
  229.  
  230.     // NOTE: idMat3 is column-major
  231.     t0                = mat[0 * 4 + 0] * m[0][0] + mat[0 * 4 + 1] * m[0][1] + mat[0 * 4 + 2] * m[0][2];
  232.     t1                = mat[0 * 4 + 0] * m[1][0] + mat[0 * 4 + 1] * m[1][1] + mat[0 * 4 + 2] * m[1][2];
  233.     t2                = mat[0 * 4 + 0] * m[2][0] + mat[0 * 4 + 1] * m[2][1] + mat[0 * 4 + 2] * m[2][2];
  234.  
  235.     mat[0 * 4 + 0] = t0;
  236.     mat[0 * 4 + 1] = t1;
  237.     mat[0 * 4 + 2] = t2;
  238.  
  239.     t0                = mat[1 * 4 + 0] * m[0][0] + mat[1 * 4 + 1] * m[0][1] + mat[1 * 4 + 2] * m[0][2];
  240.     t1                = mat[1 * 4 + 0] * m[1][0] + mat[1 * 4 + 1] * m[1][1] + mat[1 * 4 + 2] * m[1][2];
  241.     t2                = mat[1 * 4 + 0] * m[2][0] + mat[1 * 4 + 1] * m[2][1] + mat[1 * 4 + 2] * m[2][2];
  242.  
  243.     mat[1 * 4 + 0] = t0;
  244.     mat[1 * 4 + 1] = t1;
  245.     mat[1 * 4 + 2] = t2;
  246.  
  247.     t0                = mat[2 * 4 + 0] * m[0][0] + mat[2 * 4 + 1] * m[0][1] + mat[2 * 4 + 2] * m[0][2];
  248.     t1                = mat[2 * 4 + 0] * m[1][0] + mat[2 * 4 + 1] * m[1][1] + mat[2 * 4 + 2] * m[1][2];
  249.     t2                = mat[2 * 4 + 0] * m[2][0] + mat[2 * 4 + 1] * m[2][1] + mat[2 * 4 + 2] * m[2][2];
  250.  
  251.     mat[2 * 4 + 0] = t0;
  252.     mat[2 * 4 + 1] = t1;
  253.     mat[2 * 4 + 2] = t2;
  254. }
  255.  
  256. ID_INLINE void idMat3x4::Transform( idVec3 &result, const idVec3 &v ) const {
  257.     result.x = mat[0 * 4 + 0] * v.x + mat[0 * 4 + 1] * v.y + mat[0 * 4 + 2] * v.z + mat[0 * 4 + 3];
  258.     result.y = mat[1 * 4 + 0] * v.x + mat[1 * 4 + 1] * v.y + mat[1 * 4 + 2] * v.z + mat[1 * 4 + 3];
  259.     result.z = mat[2 * 4 + 0] * v.x + mat[2 * 4 + 1] * v.y + mat[2 * 4 + 2] * v.z + mat[2 * 4 + 3];
  260. }
  261.  
  262. ID_INLINE void idMat3x4::Rotate( idVec3 &result, const idVec3 &v ) const {
  263.     result.x = mat[0 * 4 + 0] * v.x + mat[0 * 4 + 1] * v.y + mat[0 * 4 + 2] * v.z;
  264.     result.y = mat[1 * 4 + 0] * v.x + mat[1 * 4 + 1] * v.y + mat[1 * 4 + 2] * v.z;
  265.     result.z = mat[2 * 4 + 0] * v.x + mat[2 * 4 + 1] * v.y + mat[2 * 4 + 2] * v.z;
  266. }
  267.  
  268. ID_INLINE idMat3 idMat3x4::ToMat3( void ) const {
  269.     return idMat3(    mat[0 * 4 + 0], mat[1 * 4 + 0], mat[2 * 4 + 0],
  270.                     mat[0 * 4 + 1], mat[1 * 4 + 1], mat[2 * 4 + 1],
  271.                     mat[0 * 4 + 2], mat[1 * 4 + 2], mat[2 * 4 + 2] );
  272. }
  273.  
  274. ID_INLINE idVec3 idMat3x4::ToVec3( void ) const {
  275.     return idVec3( mat[0 * 4 + 3], mat[1 * 4 + 3], mat[2 * 4 + 3] );
  276. }
  277.  
  278. ID_INLINE const float *idMat3x4::ToFloatPtr( void ) const {
  279.     return mat;
  280. }
  281.  
  282. ID_INLINE float *idMat3x4::ToFloatPtr( void ) {
  283.     return mat;
  284. }
  285.  
  286. #endif /* !__MAT3X4_H__ */
  287.