neo/idlib/math/Matrix.h

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