src/vectormath/scalar/mat_aos.h

   1 /*
   2    Copyright (C) 2009 Sony Computer Entertainment Inc.
   3    All rights reserved.
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14
  15 */
  16
  17 #ifndef _VECTORMATH_MAT_AOS_CPP_H
  18 #define _VECTORMATH_MAT_AOS_CPP_H
  19
  20 namespace Vectormath {
  21 namespace Aos {
  22
  23 //-----------------------------------------------------------------------------
  24 // Constants
  25
  26 #define _VECTORMATH_PI_OVER_2 1.570796327f
  27
  28 //-----------------------------------------------------------------------------
  29 // Definitions
  30
  31 inline Matrix3::Matrix3( const Matrix3 & mat )
  32 {
  33     mCol0 = mat.mCol0;
  34     mCol1 = mat.mCol1;
  35     mCol2 = mat.mCol2;
  36 }
  37
  38 inline Matrix3::Matrix3( float scalar )
  39 {
  40     mCol0 = Vector3( scalar );
  41     mCol1 = Vector3( scalar );
  42     mCol2 = Vector3( scalar );
  43 }
  44
  45 inline Matrix3::Matrix3( const Quat & unitQuat )
  46 {
  47     float qx, qy, qz, qw, qx2, qy2, qz2, qxqx2, qyqy2, qzqz2, qxqy2, qyqz2, qzqw2, qxqz2, qyqw2, qxqw2;
  48     qx = unitQuat.getX();
  49     qy = unitQuat.getY();
  50     qz = unitQuat.getZ();
  51     qw = unitQuat.getW();
  52     qx2 = ( qx + qx );
  53     qy2 = ( qy + qy );
  54     qz2 = ( qz + qz );
  55     qxqx2 = ( qx * qx2 );
  56     qxqy2 = ( qx * qy2 );
  57     qxqz2 = ( qx * qz2 );
  58     qxqw2 = ( qw * qx2 );
  59     qyqy2 = ( qy * qy2 );
  60     qyqz2 = ( qy * qz2 );
  61     qyqw2 = ( qw * qy2 );
  62     qzqz2 = ( qz * qz2 );
  63     qzqw2 = ( qw * qz2 );
  64     mCol0 = Vector3( ( ( 1.0f - qyqy2 ) - qzqz2 ), ( qxqy2 + qzqw2 ), ( qxqz2 - qyqw2 ) );
  65     mCol1 = Vector3( ( qxqy2 - qzqw2 ), ( ( 1.0f - qxqx2 ) - qzqz2 ), ( qyqz2 + qxqw2 ) );
  66     mCol2 = Vector3( ( qxqz2 + qyqw2 ), ( qyqz2 - qxqw2 ), ( ( 1.0f - qxqx2 ) - qyqy2 ) );
  67 }
  68
  69 inline Matrix3::Matrix3( const Vector3 & _col0, const Vector3 & _col1, const Vector3 & _col2 )
  70 {
  71     mCol0 = _col0;
  72     mCol1 = _col1;
  73     mCol2 = _col2;
  74 }
  75
  76 inline Matrix3 & Matrix3::setCol0( const Vector3 & _col0 )
  77 {
  78     mCol0 = _col0;
  79     return *this;
  80 }
  81
  82 inline Matrix3 & Matrix3::setCol1( const Vector3 & _col1 )
  83 {
  84     mCol1 = _col1;
  85     return *this;
  86 }
  87
  88 inline Matrix3 & Matrix3::setCol2( const Vector3 & _col2 )
  89 {
  90     mCol2 = _col2;
  91     return *this;
  92 }
  93
  94 inline Matrix3 & Matrix3::setCol( int col, const Vector3 & vec )
  95 {
  96     *(&mCol0 + col) = vec;
  97     return *this;
  98 }
  99
 100 inline Matrix3 & Matrix3::setRow( int row, const Vector3 & vec )
 101 {
 102     mCol0.setElem( row, vec.getElem( 0 ) );
 103     mCol1.setElem( row, vec.getElem( 1 ) );
 104     mCol2.setElem( row, vec.getElem( 2 ) );
 105     return *this;
 106 }
 107
 108 inline Matrix3 & Matrix3::setElem( int col, int row, float val )
 109 {
 110     Vector3 tmpV3_0;
 111     tmpV3_0 = this->getCol( col );
 112     tmpV3_0.setElem( row, val );
 113     this->setCol( col, tmpV3_0 );
 114     return *this;
 115 }
 116
 117 inline float Matrix3::getElem( int col, int row ) const
 118 {
 119     return this->getCol( col ).getElem( row );
 120 }
 121
 122 inline const Vector3 Matrix3::getCol0( ) const
 123 {
 124     return mCol0;
 125 }
 126
 127 inline const Vector3 Matrix3::getCol1( ) const
 128 {
 129     return mCol1;
 130 }
 131
 132 inline const Vector3 Matrix3::getCol2( ) const
 133 {
 134     return mCol2;
 135 }
 136
 137 inline const Vector3 Matrix3::getCol( int col ) const
 138 {
 139     return *(&mCol0 + col);
 140 }
 141
 142 inline const Vector3 Matrix3::getRow( int row ) const
 143 {
 144     return Vector3( mCol0.getElem( row ), mCol1.getElem( row ), mCol2.getElem( row ) );
 145 }
 146
 147 inline Vector3 & Matrix3::operator []( int col )
 148 {
 149     return *(&mCol0 + col);
 150 }
 151
 152 inline const Vector3 Matrix3::operator []( int col ) const
 153 {
 154     return *(&mCol0 + col);
 155 }
 156
 157 inline Matrix3 & Matrix3::operator =( const Matrix3 & mat )
 158 {
 159     mCol0 = mat.mCol0;
 160     mCol1 = mat.mCol1;
 161     mCol2 = mat.mCol2;
 162     return *this;
 163 }
 164
 165 inline const Matrix3 transpose( const Matrix3 & mat )
 166 {
 167     return Matrix3(
 168         Vector3( mat.getCol0().getX(), mat.getCol1().getX(), mat.getCol2().getX() ),
 169         Vector3( mat.getCol0().getY(), mat.getCol1().getY(), mat.getCol2().getY() ),
 170         Vector3( mat.getCol0().getZ(), mat.getCol1().getZ(), mat.getCol2().getZ() )
 171     );
 172 }
 173
 174 inline const Matrix3 inverse( const Matrix3 & mat )
 175 {
 176     Vector3 tmp0, tmp1, tmp2;
 177     float detinv;
 178     tmp0 = cross( mat.getCol1(), mat.getCol2() );
 179     tmp1 = cross( mat.getCol2(), mat.getCol0() );
 180     tmp2 = cross( mat.getCol0(), mat.getCol1() );
 181     detinv = ( 1.0f / dot( mat.getCol2(), tmp2 ) );
 182     return Matrix3(
 183         Vector3( ( tmp0.getX() * detinv ), ( tmp1.getX() * detinv ), ( tmp2.getX() * detinv ) ),
 184         Vector3( ( tmp0.getY() * detinv ), ( tmp1.getY() * detinv ), ( tmp2.getY() * detinv ) ),
 185         Vector3( ( tmp0.getZ() * detinv ), ( tmp1.getZ() * detinv ), ( tmp2.getZ() * detinv ) )
 186     );
 187 }
 188
 189 inline float determinant( const Matrix3 & mat )
 190 {
 191     return dot( mat.getCol2(), cross( mat.getCol0(), mat.getCol1() ) );
 192 }
 193
 194 inline const Matrix3 Matrix3::operator +( const Matrix3 & mat ) const
 195 {
 196     return Matrix3(
 197         ( mCol0 + mat.mCol0 ),
 198         ( mCol1 + mat.mCol1 ),
 199         ( mCol2 + mat.mCol2 )
 200     );
 201 }
 202
 203 inline const Matrix3 Matrix3::operator -( const Matrix3 & mat ) const
 204 {
 205     return Matrix3(
 206         ( mCol0 - mat.mCol0 ),
 207         ( mCol1 - mat.mCol1 ),
 208         ( mCol2 - mat.mCol2 )
 209     );
 210 }
 211
 212 inline Matrix3 & Matrix3::operator +=( const Matrix3 & mat )
 213 {
 214     *this = *this + mat;
 215     return *this;
 216 }
 217
 218 inline Matrix3 & Matrix3::operator -=( const Matrix3 & mat )
 219 {
 220     *this = *this - mat;
 221     return *this;
 222 }
 223
 224 inline const Matrix3 Matrix3::operator -( ) const
 225 {
 226     return Matrix3(
 227         ( -mCol0 ),
 228         ( -mCol1 ),
 229         ( -mCol2 )
 230     );
 231 }
 232
 233 inline const Matrix3 absPerElem( const Matrix3 & mat )
 234 {
 235     return Matrix3(
 236         absPerElem( mat.getCol0() ),
 237         absPerElem( mat.getCol1() ),
 238         absPerElem( mat.getCol2() )
 239     );
 240 }
 241
 242 inline const Matrix3 Matrix3::operator *( float scalar ) const
 243 {
 244     return Matrix3(
 245         ( mCol0 * scalar ),
 246         ( mCol1 * scalar ),
 247         ( mCol2 * scalar )
 248     );
 249 }
 250
 251 inline Matrix3 & Matrix3::operator *=( float scalar )
 252 {
 253     *this = *this * scalar;
 254     return *this;
 255 }
 256
 257 inline const Matrix3 operator *( float scalar, const Matrix3 & mat )
 258 {
 259     return mat * scalar;
 260 }
 261
 262 inline const Vector3 Matrix3::operator *( const Vector3 & vec ) const
 263 {
 264     return Vector3(
 265         ( ( ( mCol0.getX() * vec.getX() ) + ( mCol1.getX() * vec.getY() ) ) + ( mCol2.getX() * vec.getZ() ) ),
 266         ( ( ( mCol0.getY() * vec.getX() ) + ( mCol1.getY() * vec.getY() ) ) + ( mCol2.getY() * vec.getZ() ) ),
 267         ( ( ( mCol0.getZ() * vec.getX() ) + ( mCol1.getZ() * vec.getY() ) ) + ( mCol2.getZ() * vec.getZ() ) )
 268     );
 269 }
 270
 271 inline const Matrix3 Matrix3::operator *( const Matrix3 & mat ) const
 272 {
 273     return Matrix3(
 274         ( *this * mat.mCol0 ),
 275         ( *this * mat.mCol1 ),
 276         ( *this * mat.mCol2 )
 277     );
 278 }
 279
 280 inline Matrix3 & Matrix3::operator *=( const Matrix3 & mat )
 281 {
 282     *this = *this * mat;
 283     return *this;
 284 }
 285
 286 inline const Matrix3 mulPerElem( const Matrix3 & mat0, const Matrix3 & mat1 )
 287 {
 288     return Matrix3(
 289         mulPerElem( mat0.getCol0(), mat1.getCol0() ),
 290         mulPerElem( mat0.getCol1(), mat1.getCol1() ),
 291         mulPerElem( mat0.getCol2(), mat1.getCol2() )
 292     );
 293 }
 294
 295 inline const Matrix3 Matrix3::identity( )
 296 {
 297     return Matrix3(
 298         Vector3::xAxis( ),
 299         Vector3::yAxis( ),
 300         Vector3::zAxis( )
 301     );
 302 }
 303
 304 inline const Matrix3 Matrix3::rotationX( float radians )
 305 {
 306     float s, c;
 307     s = sinf( radians );
 308     c = cosf( radians );
 309     return Matrix3(
 310         Vector3::xAxis( ),
 311         Vector3( 0.0f, c, s ),
 312         Vector3( 0.0f, -s, c )
 313     );
 314 }
 315
 316 inline const Matrix3 Matrix3::rotationY( float radians )
 317 {
 318     float s, c;
 319     s = sinf( radians );
 320     c = cosf( radians );
 321     return Matrix3(
 322         Vector3( c, 0.0f, -s ),
 323         Vector3::yAxis( ),
 324         Vector3( s, 0.0f, c )
 325     );
 326 }
 327
 328 inline const Matrix3 Matrix3::rotationZ( float radians )
 329 {
 330     float s, c;
 331     s = sinf( radians );
 332     c = cosf( radians );
 333     return Matrix3(
 334         Vector3( c, s, 0.0f ),
 335         Vector3( -s, c, 0.0f ),
 336         Vector3::zAxis( )
 337     );
 338 }
 339
 340 inline const Matrix3 Matrix3::rotationZYX( const Vector3 & radiansXYZ )
 341 {
 342     float sX, cX, sY, cY, sZ, cZ, tmp0, tmp1;
 343     sX = sinf( radiansXYZ.getX() );
 344     cX = cosf( radiansXYZ.getX() );
 345     sY = sinf( radiansXYZ.getY() );
 346     cY = cosf( radiansXYZ.getY() );
 347     sZ = sinf( radiansXYZ.getZ() );
 348     cZ = cosf( radiansXYZ.getZ() );
 349     tmp0 = ( cZ * sY );
 350     tmp1 = ( sZ * sY );
 351     return Matrix3(
 352         Vector3( ( cZ * cY ), ( sZ * cY ), -sY ),
 353         Vector3( ( ( tmp0 * sX ) - ( sZ * cX ) ), ( ( tmp1 * sX ) + ( cZ * cX ) ), ( cY * sX ) ),
 354         Vector3( ( ( tmp0 * cX ) + ( sZ * sX ) ), ( ( tmp1 * cX ) - ( cZ * sX ) ), ( cY * cX ) )
 355     );
 356 }
 357
 358 inline const Matrix3 Matrix3::rotation( float radians, const Vector3 & unitVec )
 359 {
 360     float x, y, z, s, c, oneMinusC, xy, yz, zx;
 361     s = sinf( radians );
 362     c = cosf( radians );
 363     x = unitVec.getX();
 364     y = unitVec.getY();
 365     z = unitVec.getZ();
 366     xy = ( x * y );
 367     yz = ( y * z );
 368     zx = ( z * x );
 369     oneMinusC = ( 1.0f - c );
 370     return Matrix3(
 371         Vector3( ( ( ( x * x ) * oneMinusC ) + c ), ( ( xy * oneMinusC ) + ( z * s ) ), ( ( zx * oneMinusC ) - ( y * s ) ) ),
 372         Vector3( ( ( xy * oneMinusC ) - ( z * s ) ), ( ( ( y * y ) * oneMinusC ) + c ), ( ( yz * oneMinusC ) + ( x * s ) ) ),
 373         Vector3( ( ( zx * oneMinusC ) + ( y * s ) ), ( ( yz * oneMinusC ) - ( x * s ) ), ( ( ( z * z ) * oneMinusC ) + c ) )
 374     );
 375 }
 376
 377 inline const Matrix3 Matrix3::rotation( const Quat & unitQuat )
 378 {
 379     return Matrix3( unitQuat );
 380 }
 381
 382 inline const Matrix3 Matrix3::scale( const Vector3 & scaleVec )
 383 {
 384     return Matrix3(
 385         Vector3( scaleVec.getX(), 0.0f, 0.0f ),
 386         Vector3( 0.0f, scaleVec.getY(), 0.0f ),
 387         Vector3( 0.0f, 0.0f, scaleVec.getZ() )
 388     );
 389 }
 390
 391 inline const Matrix3 appendScale( const Matrix3 & mat, const Vector3 & scaleVec )
 392 {
 393     return Matrix3(
 394         ( mat.getCol0() * scaleVec.getX( ) ),
 395         ( mat.getCol1() * scaleVec.getY( ) ),
 396         ( mat.getCol2() * scaleVec.getZ( ) )
 397     );
 398 }
 399
 400 inline const Matrix3 prependScale( const Vector3 & scaleVec, const Matrix3 & mat )
 401 {
 402     return Matrix3(
 403         mulPerElem( mat.getCol0(), scaleVec ),
 404         mulPerElem( mat.getCol1(), scaleVec ),
 405         mulPerElem( mat.getCol2(), scaleVec )
 406     );
 407 }
 408
 409 inline const Matrix3 select( const Matrix3 & mat0, const Matrix3 & mat1, bool select1 )
 410 {
 411     return Matrix3(
 412         select( mat0.getCol0(), mat1.getCol0(), select1 ),
 413         select( mat0.getCol1(), mat1.getCol1(), select1 ),
 414         select( mat0.getCol2(), mat1.getCol2(), select1 )
 415     );
 416 }
 417
 418 #ifdef _VECTORMATH_DEBUG
 419
 420 inline void print( const Matrix3 & mat )
 421 {
 422     print( mat.getRow( 0 ) );
 423     print( mat.getRow( 1 ) );
 424     print( mat.getRow( 2 ) );
 425 }
 426
 427 inline void print( const Matrix3 & mat, const char * name )
 428 {
 429     printf("%s:\n", name);
 430     print( mat );
 431 }
 432
 433 #endif
 434
 435 inline Matrix4::Matrix4( const Matrix4 & mat )
 436 {
 437     mCol0 = mat.mCol0;
 438     mCol1 = mat.mCol1;
 439     mCol2 = mat.mCol2;
 440     mCol3 = mat.mCol3;
 441 }
 442
 443 inline Matrix4::Matrix4( float scalar )
 444 {
 445     mCol0 = Vector4( scalar );
 446     mCol1 = Vector4( scalar );
 447     mCol2 = Vector4( scalar );
 448     mCol3 = Vector4( scalar );
 449 }
 450
 451 inline Matrix4::Matrix4( const Transform3 & mat )
 452 {
 453     mCol0 = Vector4( mat.getCol0(), 0.0f );
 454     mCol1 = Vector4( mat.getCol1(), 0.0f );
 455     mCol2 = Vector4( mat.getCol2(), 0.0f );
 456     mCol3 = Vector4( mat.getCol3(), 1.0f );
 457 }
 458
 459 inline Matrix4::Matrix4( const Vector4 & _col0, const Vector4 & _col1, const Vector4 & _col2, const Vector4 & _col3 )
 460 {
 461     mCol0 = _col0;
 462     mCol1 = _col1;
 463     mCol2 = _col2;
 464     mCol3 = _col3;
 465 }
 466
 467 inline Matrix4::Matrix4( const Matrix3 & mat, const Vector3 & translateVec )
 468 {
 469     mCol0 = Vector4( mat.getCol0(), 0.0f );
 470     mCol1 = Vector4( mat.getCol1(), 0.0f );
 471     mCol2 = Vector4( mat.getCol2(), 0.0f );
 472     mCol3 = Vector4( translateVec, 1.0f );
 473 }
 474
 475 inline Matrix4::Matrix4( const Quat & unitQuat, const Vector3 & translateVec )
 476 {
 477     Matrix3 mat;
 478     mat = Matrix3( unitQuat );
 479     mCol0 = Vector4( mat.getCol0(), 0.0f );
 480     mCol1 = Vector4( mat.getCol1(), 0.0f );
 481     mCol2 = Vector4( mat.getCol2(), 0.0f );
 482     mCol3 = Vector4( translateVec, 1.0f );
 483 }
 484
 485 inline Matrix4 & Matrix4::setCol0( const Vector4 & _col0 )
 486 {
 487     mCol0 = _col0;
 488     return *this;
 489 }
 490
 491 inline Matrix4 & Matrix4::setCol1( const Vector4 & _col1 )
 492 {
 493     mCol1 = _col1;
 494     return *this;
 495 }
 496
 497 inline Matrix4 & Matrix4::setCol2( const Vector4 & _col2 )
 498 {
 499     mCol2 = _col2;
 500     return *this;
 501 }
 502
 503 inline Matrix4 & Matrix4::setCol3( const Vector4 & _col3 )
 504 {
 505     mCol3 = _col3;
 506     return *this;
 507 }
 508
 509 inline Matrix4 & Matrix4::setCol( int col, const Vector4 & vec )
 510 {
 511     *(&mCol0 + col) = vec;
 512     return *this;
 513 }
 514
 515 inline Matrix4 & Matrix4::setRow( int row, const Vector4 & vec )
 516 {
 517     mCol0.setElem( row, vec.getElem( 0 ) );
 518     mCol1.setElem( row, vec.getElem( 1 ) );
 519     mCol2.setElem( row, vec.getElem( 2 ) );
 520     mCol3.setElem( row, vec.getElem( 3 ) );
 521     return *this;
 522 }
 523
 524 inline Matrix4 & Matrix4::setElem( int col, int row, float val )
 525 {
 526     Vector4 tmpV3_0;
 527     tmpV3_0 = this->getCol( col );
 528     tmpV3_0.setElem( row, val );
 529     this->setCol( col, tmpV3_0 );
 530     return *this;
 531 }
 532
 533 inline float Matrix4::getElem( int col, int row ) const
 534 {
 535     return this->getCol( col ).getElem( row );
 536 }
 537
 538 inline const Vector4 Matrix4::getCol0( ) const
 539 {
 540     return mCol0;
 541 }
 542
 543 inline const Vector4 Matrix4::getCol1( ) const
 544 {
 545     return mCol1;
 546 }
 547
 548 inline const Vector4 Matrix4::getCol2( ) const
 549 {
 550     return mCol2;
 551 }
 552
 553 inline const Vector4 Matrix4::getCol3( ) const
 554 {
 555     return mCol3;
 556 }
 557
 558 inline const Vector4 Matrix4::getCol( int col ) const
 559 {
 560     return *(&mCol0 + col);
 561 }
 562
 563 inline const Vector4 Matrix4::getRow( int row ) const
 564 {
 565     return Vector4( mCol0.getElem( row ), mCol1.getElem( row ), mCol2.getElem( row ), mCol3.getElem( row ) );
 566 }
 567
 568 inline Vector4 & Matrix4::operator []( int col )
 569 {
 570     return *(&mCol0 + col);
 571 }
 572
 573 inline const Vector4 Matrix4::operator []( int col ) const
 574 {
 575     return *(&mCol0 + col);
 576 }
 577
 578 inline Matrix4 & Matrix4::operator =( const Matrix4 & mat )
 579 {
 580     mCol0 = mat.mCol0;
 581     mCol1 = mat.mCol1;
 582     mCol2 = mat.mCol2;
 583     mCol3 = mat.mCol3;
 584     return *this;
 585 }
 586
 587 inline const Matrix4 transpose( const Matrix4 & mat )
 588 {
 589     return Matrix4(
 590         Vector4( mat.getCol0().getX(), mat.getCol1().getX(), mat.getCol2().getX(), mat.getCol3().getX() ),
 591         Vector4( mat.getCol0().getY(), mat.getCol1().getY(), mat.getCol2().getY(), mat.getCol3().getY() ),
 592         Vector4( mat.getCol0().getZ(), mat.getCol1().getZ(), mat.getCol2().getZ(), mat.getCol3().getZ() ),
 593         Vector4( mat.getCol0().getW(), mat.getCol1().getW(), mat.getCol2().getW(), mat.getCol3().getW() )
 594     );
 595 }
 596
 597 inline const Matrix4 inverse( const Matrix4 & mat )
 598 {
 599     Vector4 res0, res1, res2, res3;
 600     float mA, mB, mC, mD, mE, mF, mG, mH, mI, mJ, mK, mL, mM, mN, mO, mP, tmp0, tmp1, tmp2, tmp3, tmp4, tmp5, detInv;
 601     mA = mat.getCol0().getX();
 602     mB = mat.getCol0().getY();
 603     mC = mat.getCol0().getZ();
 604     mD = mat.getCol0().getW();
 605     mE = mat.getCol1().getX();
 606     mF = mat.getCol1().getY();
 607     mG = mat.getCol1().getZ();
 608     mH = mat.getCol1().getW();
 609     mI = mat.getCol2().getX();
 610     mJ = mat.getCol2().getY();
 611     mK = mat.getCol2().getZ();
 612     mL = mat.getCol2().getW();
 613     mM = mat.getCol3().getX();
 614     mN = mat.getCol3().getY();
 615     mO = mat.getCol3().getZ();
 616     mP = mat.getCol3().getW();
 617     tmp0 = ( ( mK * mD ) - ( mC * mL ) );
 618     tmp1 = ( ( mO * mH ) - ( mG * mP ) );
 619     tmp2 = ( ( mB * mK ) - ( mJ * mC ) );
 620     tmp3 = ( ( mF * mO ) - ( mN * mG ) );
 621     tmp4 = ( ( mJ * mD ) - ( mB * mL ) );
 622     tmp5 = ( ( mN * mH ) - ( mF * mP ) );
 623     res0.setX( ( ( ( mJ * tmp1 ) - ( mL * tmp3 ) ) - ( mK * tmp5 ) ) );
 624     res0.setY( ( ( ( mN * tmp0 ) - ( mP * tmp2 ) ) - ( mO * tmp4 ) ) );
 625     res0.setZ( ( ( ( mD * tmp3 ) + ( mC * tmp5 ) ) - ( mB * tmp1 ) ) );
 626     res0.setW( ( ( ( mH * tmp2 ) + ( mG * tmp4 ) ) - ( mF * tmp0 ) ) );
 627     detInv = ( 1.0f / ( ( ( ( mA * res0.getX() ) + ( mE * res0.getY() ) ) + ( mI * res0.getZ() ) ) + ( mM * res0.getW() ) ) );
 628     res1.setX( ( mI * tmp1 ) );
 629     res1.setY( ( mM * tmp0 ) );
 630     res1.setZ( ( mA * tmp1 ) );
 631     res1.setW( ( mE * tmp0 ) );
 632     res3.setX( ( mI * tmp3 ) );
 633     res3.setY( ( mM * tmp2 ) );
 634     res3.setZ( ( mA * tmp3 ) );
 635     res3.setW( ( mE * tmp2 ) );
 636     res2.setX( ( mI * tmp5 ) );
 637     res2.setY( ( mM * tmp4 ) );
 638     res2.setZ( ( mA * tmp5 ) );
 639     res2.setW( ( mE * tmp4 ) );
 640     tmp0 = ( ( mI * mB ) - ( mA * mJ ) );
 641     tmp1 = ( ( mM * mF ) - ( mE * mN ) );
 642     tmp2 = ( ( mI * mD ) - ( mA * mL ) );
 643     tmp3 = ( ( mM * mH ) - ( mE * mP ) );
 644     tmp4 = ( ( mI * mC ) - ( mA * mK ) );
 645     tmp5 = ( ( mM * mG ) - ( mE * mO ) );
 646     res2.setX( ( ( ( mL * tmp1 ) - ( mJ * tmp3 ) ) + res2.getX() ) );
 647     res2.setY( ( ( ( mP * tmp0 ) - ( mN * tmp2 ) ) + res2.getY() ) );
 648     res2.setZ( ( ( ( mB * tmp3 ) - ( mD * tmp1 ) ) - res2.getZ() ) );
 649     res2.setW( ( ( ( mF * tmp2 ) - ( mH * tmp0 ) ) - res2.getW() ) );
 650     res3.setX( ( ( ( mJ * tmp5 ) - ( mK * tmp1 ) ) + res3.getX() ) );
 651     res3.setY( ( ( ( mN * tmp4 ) - ( mO * tmp0 ) ) + res3.getY() ) );
 652     res3.setZ( ( ( ( mC * tmp1 ) - ( mB * tmp5 ) ) - res3.getZ() ) );
 653     res3.setW( ( ( ( mG * tmp0 ) - ( mF * tmp4 ) ) - res3.getW() ) );
 654     res1.setX( ( ( ( mK * tmp3 ) - ( mL * tmp5 ) ) - res1.getX() ) );
 655     res1.setY( ( ( ( mO * tmp2 ) - ( mP * tmp4 ) ) - res1.getY() ) );
 656     res1.setZ( ( ( ( mD * tmp5 ) - ( mC * tmp3 ) ) + res1.getZ() ) );
 657     res1.setW( ( ( ( mH * tmp4 ) - ( mG * tmp2 ) ) + res1.getW() ) );
 658     return Matrix4(
 659         ( res0 * detInv ),
 660         ( res1 * detInv ),
 661         ( res2 * detInv ),
 662         ( res3 * detInv )
 663     );
 664 }
 665
 666 inline const Matrix4 affineInverse( const Matrix4 & mat )
 667 {
 668     Transform3 affineMat;
 669     affineMat.setCol0( mat.getCol0().getXYZ( ) );
 670     affineMat.setCol1( mat.getCol1().getXYZ( ) );
 671     affineMat.setCol2( mat.getCol2().getXYZ( ) );
 672     affineMat.setCol3( mat.getCol3().getXYZ( ) );
 673     return Matrix4( inverse( affineMat ) );
 674 }
 675
 676 inline const Matrix4 orthoInverse( const Matrix4 & mat )
 677 {
 678     Transform3 affineMat;
 679     affineMat.setCol0( mat.getCol0().getXYZ( ) );
 680     affineMat.setCol1( mat.getCol1().getXYZ( ) );
 681     affineMat.setCol2( mat.getCol2().getXYZ( ) );
 682     affineMat.setCol3( mat.getCol3().getXYZ( ) );
 683     return Matrix4( orthoInverse( affineMat ) );
 684 }
 685
 686 inline float determinant( const Matrix4 & mat )
 687 {
 688     float dx, dy, dz, dw, mA, mB, mC, mD, mE, mF, mG, mH, mI, mJ, mK, mL, mM, mN, mO, mP, tmp0, tmp1, tmp2, tmp3, tmp4, tmp5;
 689     mA = mat.getCol0().getX();
 690     mB = mat.getCol0().getY();
 691     mC = mat.getCol0().getZ();
 692     mD = mat.getCol0().getW();
 693     mE = mat.getCol1().getX();
 694     mF = mat.getCol1().getY();
 695     mG = mat.getCol1().getZ();
 696     mH = mat.getCol1().getW();
 697     mI = mat.getCol2().getX();
 698     mJ = mat.getCol2().getY();
 699     mK = mat.getCol2().getZ();
 700     mL = mat.getCol2().getW();
 701     mM = mat.getCol3().getX();
 702     mN = mat.getCol3().getY();
 703     mO = mat.getCol3().getZ();
 704     mP = mat.getCol3().getW();
 705     tmp0 = ( ( mK * mD ) - ( mC * mL ) );
 706     tmp1 = ( ( mO * mH ) - ( mG * mP ) );
 707     tmp2 = ( ( mB * mK ) - ( mJ * mC ) );
 708     tmp3 = ( ( mF * mO ) - ( mN * mG ) );
 709     tmp4 = ( ( mJ * mD ) - ( mB * mL ) );
 710     tmp5 = ( ( mN * mH ) - ( mF * mP ) );
 711     dx = ( ( ( mJ * tmp1 ) - ( mL * tmp3 ) ) - ( mK * tmp5 ) );
 712     dy = ( ( ( mN * tmp0 ) - ( mP * tmp2 ) ) - ( mO * tmp4 ) );
 713     dz = ( ( ( mD * tmp3 ) + ( mC * tmp5 ) ) - ( mB * tmp1 ) );
 714     dw = ( ( ( mH * tmp2 ) + ( mG * tmp4 ) ) - ( mF * tmp0 ) );
 715     return ( ( ( ( mA * dx ) + ( mE * dy ) ) + ( mI * dz ) ) + ( mM * dw ) );
 716 }
 717
 718 inline const Matrix4 Matrix4::operator +( const Matrix4 & mat ) const
 719 {
 720     return Matrix4(
 721         ( mCol0 + mat.mCol0 ),
 722         ( mCol1 + mat.mCol1 ),
 723         ( mCol2 + mat.mCol2 ),
 724         ( mCol3 + mat.mCol3 )
 725     );
 726 }
 727
 728 inline const Matrix4 Matrix4::operator -( const Matrix4 & mat ) const
 729 {
 730     return Matrix4(
 731         ( mCol0 - mat.mCol0 ),
 732         ( mCol1 - mat.mCol1 ),
 733         ( mCol2 - mat.mCol2 ),
 734         ( mCol3 - mat.mCol3 )
 735     );
 736 }
 737
 738 inline Matrix4 & Matrix4::operator +=( const Matrix4 & mat )
 739 {
 740     *this = *this + mat;
 741     return *this;
 742 }
 743
 744 inline Matrix4 & Matrix4::operator -=( const Matrix4 & mat )
 745 {
 746     *this = *this - mat;
 747     return *this;
 748 }
 749
 750 inline const Matrix4 Matrix4::operator -( ) const
 751 {
 752     return Matrix4(
 753         ( -mCol0 ),
 754         ( -mCol1 ),
 755         ( -mCol2 ),
 756         ( -mCol3 )
 757     );
 758 }
 759
 760 inline const Matrix4 absPerElem( const Matrix4 & mat )
 761 {
 762     return Matrix4(
 763         absPerElem( mat.getCol0() ),
 764         absPerElem( mat.getCol1() ),
 765         absPerElem( mat.getCol2() ),
 766         absPerElem( mat.getCol3() )
 767     );
 768 }
 769
 770 inline const Matrix4 Matrix4::operator *( float scalar ) const
 771 {
 772     return Matrix4(
 773         ( mCol0 * scalar ),
 774         ( mCol1 * scalar ),
 775         ( mCol2 * scalar ),
 776         ( mCol3 * scalar )
 777     );
 778 }
 779
 780 inline Matrix4 & Matrix4::operator *=( float scalar )
 781 {
 782     *this = *this * scalar;
 783     return *this;
 784 }
 785
 786 inline const Matrix4 operator *( float scalar, const Matrix4 & mat )
 787 {
 788     return mat * scalar;
 789 }
 790
 791 inline const Vector4 Matrix4::operator *( const Vector4 & vec ) const
 792 {
 793     return Vector4(
 794         ( ( ( ( mCol0.getX() * vec.getX() ) + ( mCol1.getX() * vec.getY() ) ) + ( mCol2.getX() * vec.getZ() ) ) + ( mCol3.getX() * vec.getW() ) ),
 795         ( ( ( ( mCol0.getY() * vec.getX() ) + ( mCol1.getY() * vec.getY() ) ) + ( mCol2.getY() * vec.getZ() ) ) + ( mCol3.getY() * vec.getW() ) ),
 796         ( ( ( ( mCol0.getZ() * vec.getX() ) + ( mCol1.getZ() * vec.getY() ) ) + ( mCol2.getZ() * vec.getZ() ) ) + ( mCol3.getZ() * vec.getW() ) ),
 797         ( ( ( ( mCol0.getW() * vec.getX() ) + ( mCol1.getW() * vec.getY() ) ) + ( mCol2.getW() * vec.getZ() ) ) + ( mCol3.getW() * vec.getW() ) )
 798     );
 799 }
 800
 801 inline const Vector4 Matrix4::operator *( const Vector3 & vec ) const
 802 {
 803     return Vector4(
 804         ( ( ( mCol0.getX() * vec.getX() ) + ( mCol1.getX() * vec.getY() ) ) + ( mCol2.getX() * vec.getZ() ) ),
 805         ( ( ( mCol0.getY() * vec.getX() ) + ( mCol1.getY() * vec.getY() ) ) + ( mCol2.getY() * vec.getZ() ) ),
 806         ( ( ( mCol0.getZ() * vec.getX() ) + ( mCol1.getZ() * vec.getY() ) ) + ( mCol2.getZ() * vec.getZ() ) ),
 807         ( ( ( mCol0.getW() * vec.getX() ) + ( mCol1.getW() * vec.getY() ) ) + ( mCol2.getW() * vec.getZ() ) )
 808     );
 809 }
 810
 811 inline const Vector4 Matrix4::operator *( const Point3 & pnt ) const
 812 {
 813     return Vector4(
 814         ( ( ( ( mCol0.getX() * pnt.getX() ) + ( mCol1.getX() * pnt.getY() ) ) + ( mCol2.getX() * pnt.getZ() ) ) + mCol3.getX() ),
 815         ( ( ( ( mCol0.getY() * pnt.getX() ) + ( mCol1.getY() * pnt.getY() ) ) + ( mCol2.getY() * pnt.getZ() ) ) + mCol3.getY() ),
 816         ( ( ( ( mCol0.getZ() * pnt.getX() ) + ( mCol1.getZ() * pnt.getY() ) ) + ( mCol2.getZ() * pnt.getZ() ) ) + mCol3.getZ() ),
 817         ( ( ( ( mCol0.getW() * pnt.getX() ) + ( mCol1.getW() * pnt.getY() ) ) + ( mCol2.getW() * pnt.getZ() ) ) + mCol3.getW() )
 818     );
 819 }
 820
 821 inline const Matrix4 Matrix4::operator *( const Matrix4 & mat ) const
 822 {
 823     return Matrix4(
 824         ( *this * mat.mCol0 ),
 825         ( *this * mat.mCol1 ),
 826         ( *this * mat.mCol2 ),
 827         ( *this * mat.mCol3 )
 828     );
 829 }
 830
 831 inline Matrix4 & Matrix4::operator *=( const Matrix4 & mat )
 832 {
 833     *this = *this * mat;
 834     return *this;
 835 }
 836
 837 inline const Matrix4 Matrix4::operator *( const Transform3 & tfrm ) const
 838 {
 839     return Matrix4(
 840         ( *this * tfrm.getCol0() ),
 841         ( *this * tfrm.getCol1() ),
 842         ( *this * tfrm.getCol2() ),
 843         ( *this * Point3( tfrm.getCol3() ) )
 844     );
 845 }
 846
 847 inline Matrix4 & Matrix4::operator *=( const Transform3 & tfrm )
 848 {
 849     *this = *this * tfrm;
 850     return *this;
 851 }
 852
 853 inline const Matrix4 mulPerElem( const Matrix4 & mat0, const Matrix4 & mat1 )
 854 {
 855     return Matrix4(
 856         mulPerElem( mat0.getCol0(), mat1.getCol0() ),
 857         mulPerElem( mat0.getCol1(), mat1.getCol1() ),
 858         mulPerElem( mat0.getCol2(), mat1.getCol2() ),
 859         mulPerElem( mat0.getCol3(), mat1.getCol3() )
 860     );
 861 }
 862
 863 inline const Matrix4 Matrix4::identity( )
 864 {
 865     return Matrix4(
 866         Vector4::xAxis( ),
 867         Vector4::yAxis( ),
 868         Vector4::zAxis( ),
 869         Vector4::wAxis( )
 870     );
 871 }
 872
 873 inline Matrix4 & Matrix4::setUpper3x3( const Matrix3 & mat3 )
 874 {
 875     mCol0.setXYZ( mat3.getCol0() );
 876     mCol1.setXYZ( mat3.getCol1() );
 877     mCol2.setXYZ( mat3.getCol2() );
 878     return *this;
 879 }
 880
 881 inline const Matrix3 Matrix4::getUpper3x3( ) const
 882 {
 883     return Matrix3(
 884         mCol0.getXYZ( ),
 885         mCol1.getXYZ( ),
 886         mCol2.getXYZ( )
 887     );
 888 }
 889
 890 inline Matrix4 & Matrix4::setTranslation( const Vector3 & translateVec )
 891 {
 892     mCol3.setXYZ( translateVec );
 893     return *this;
 894 }
 895
 896 inline const Vector3 Matrix4::getTranslation( ) const
 897 {
 898     return mCol3.getXYZ( );
 899 }
 900
 901 inline const Matrix4 Matrix4::rotationX( float radians )
 902 {
 903     float s, c;
 904     s = sinf( radians );
 905     c = cosf( radians );
 906     return Matrix4(
 907         Vector4::xAxis( ),
 908         Vector4( 0.0f, c, s, 0.0f ),
 909         Vector4( 0.0f, -s, c, 0.0f ),
 910         Vector4::wAxis( )
 911     );
 912 }
 913
 914 inline const Matrix4 Matrix4::rotationY( float radians )
 915 {
 916     float s, c;
 917     s = sinf( radians );
 918     c = cosf( radians );
 919     return Matrix4(
 920         Vector4( c, 0.0f, -s, 0.0f ),
 921         Vector4::yAxis( ),
 922         Vector4( s, 0.0f, c, 0.0f ),
 923         Vector4::wAxis( )
 924     );
 925 }
 926
 927 inline const Matrix4 Matrix4::rotationZ( float radians )
 928 {
 929     float s, c;
 930     s = sinf( radians );
 931     c = cosf( radians );
 932     return Matrix4(
 933         Vector4( c, s, 0.0f, 0.0f ),
 934         Vector4( -s, c, 0.0f, 0.0f ),
 935         Vector4::zAxis( ),
 936         Vector4::wAxis( )
 937     );
 938 }
 939
 940 inline const Matrix4 Matrix4::rotationZYX( const Vector3 & radiansXYZ )
 941 {
 942     float sX, cX, sY, cY, sZ, cZ, tmp0, tmp1;
 943     sX = sinf( radiansXYZ.getX() );
 944     cX = cosf( radiansXYZ.getX() );
 945     sY = sinf( radiansXYZ.getY() );
 946     cY = cosf( radiansXYZ.getY() );
 947     sZ = sinf( radiansXYZ.getZ() );
 948     cZ = cosf( radiansXYZ.getZ() );
 949     tmp0 = ( cZ * sY );
 950     tmp1 = ( sZ * sY );
 951     return Matrix4(
 952         Vector4( ( cZ * cY ), ( sZ * cY ), -sY, 0.0f ),
 953         Vector4( ( ( tmp0 * sX ) - ( sZ * cX ) ), ( ( tmp1 * sX ) + ( cZ * cX ) ), ( cY * sX ), 0.0f ),
 954         Vector4( ( ( tmp0 * cX ) + ( sZ * sX ) ), ( ( tmp1 * cX ) - ( cZ * sX ) ), ( cY * cX ), 0.0f ),
 955         Vector4::wAxis( )
 956     );
 957 }
 958
 959 inline const Matrix4 Matrix4::rotation( float radians, const Vector3 & unitVec )
 960 {
 961     float x, y, z, s, c, oneMinusC, xy, yz, zx;
 962     s = sinf( radians );
 963     c = cosf( radians );
 964     x = unitVec.getX();
 965     y = unitVec.getY();
 966     z = unitVec.getZ();
 967     xy = ( x * y );
 968     yz = ( y * z );
 969     zx = ( z * x );
 970     oneMinusC = ( 1.0f - c );
 971     return Matrix4(
 972         Vector4( ( ( ( x * x ) * oneMinusC ) + c ), ( ( xy * oneMinusC ) + ( z * s ) ), ( ( zx * oneMinusC ) - ( y * s ) ), 0.0f ),
 973         Vector4( ( ( xy * oneMinusC ) - ( z * s ) ), ( ( ( y * y ) * oneMinusC ) + c ), ( ( yz * oneMinusC ) + ( x * s ) ), 0.0f ),
 974         Vector4( ( ( zx * oneMinusC ) + ( y * s ) ), ( ( yz * oneMinusC ) - ( x * s ) ), ( ( ( z * z ) * oneMinusC ) + c ), 0.0f ),
 975         Vector4::wAxis( )
 976     );
 977 }
 978
 979 inline const Matrix4 Matrix4::rotation( const Quat & unitQuat )
 980 {
 981     return Matrix4( Transform3::rotation( unitQuat ) );
 982 }
 983
 984 inline const Matrix4 Matrix4::scale( const Vector3 & scaleVec )
 985 {
 986     return Matrix4(
 987         Vector4( scaleVec.getX(), 0.0f, 0.0f, 0.0f ),
 988         Vector4( 0.0f, scaleVec.getY(), 0.0f, 0.0f ),
 989         Vector4( 0.0f, 0.0f, scaleVec.getZ(), 0.0f ),
 990         Vector4::wAxis( )
 991     );
 992 }
 993
 994 inline const Matrix4 appendScale( const Matrix4 & mat, const Vector3 & scaleVec )
 995 {
 996     return Matrix4(
 997         ( mat.getCol0() * scaleVec.getX( ) ),
 998         ( mat.getCol1() * scaleVec.getY( ) ),
 999         ( mat.getCol2() * scaleVec.getZ( ) ),
1000         mat.getCol3()
1001     );
1002 }
1003
1004 inline const Matrix4 prependScale( const Vector3 & scaleVec, const Matrix4 & mat )
1005 {
1006     Vector4 scale4;
1007     scale4 = Vector4( scaleVec, 1.0f );
1008     return Matrix4(
1009         mulPerElem( mat.getCol0(), scale4 ),
1010         mulPerElem( mat.getCol1(), scale4 ),
1011         mulPerElem( mat.getCol2(), scale4 ),
1012         mulPerElem( mat.getCol3(), scale4 )
1013     );
1014 }
1015
1016 inline const Matrix4 Matrix4::translation( const Vector3 & translateVec )
1017 {
1018     return Matrix4(
1019         Vector4::xAxis( ),
1020         Vector4::yAxis( ),
1021         Vector4::zAxis( ),
1022         Vector4( translateVec, 1.0f )
1023     );
1024 }
1025
1026 inline const Matrix4 Matrix4::lookAt( const Point3 & eyePos, const Point3 & lookAtPos, const Vector3 & upVec )
1027 {
1028     Matrix4 m4EyeFrame;
1029     Vector3 v3X, v3Y, v3Z;
1030     v3Y = normalize( upVec );
1031     v3Z = normalize( ( eyePos - lookAtPos ) );
1032     v3X = normalize( cross( v3Y, v3Z ) );
1033     v3Y = cross( v3Z, v3X );
1034     m4EyeFrame = Matrix4( Vector4( v3X ), Vector4( v3Y ), Vector4( v3Z ), Vector4( eyePos ) );
1035     return orthoInverse( m4EyeFrame );
1036 }
1037
1038 inline const Matrix4 Matrix4::perspective( float fovyRadians, float aspect, float zNear, float zFar )
1039 {
1040     float f, rangeInv;
1041     f = tanf( ( (float)( _VECTORMATH_PI_OVER_2 ) - ( 0.5f * fovyRadians ) ) );
1042     rangeInv = ( 1.0f / ( zNear - zFar ) );
1043     return Matrix4(
1044         Vector4( ( f / aspect ), 0.0f, 0.0f, 0.0f ),
1045         Vector4( 0.0f, f, 0.0f, 0.0f ),
1046         Vector4( 0.0f, 0.0f, ( ( zNear + zFar ) * rangeInv ), -1.0f ),
1047         Vector4( 0.0f, 0.0f, ( ( ( zNear * zFar ) * rangeInv ) * 2.0f ), 0.0f )
1048     );
1049 }
1050
1051 inline const Matrix4 Matrix4::frustum( float left, float right, float bottom, float top, float zNear, float zFar )
1052 {
1053     float sum_rl, sum_tb, sum_nf, inv_rl, inv_tb, inv_nf, n2;
1054     sum_rl = ( right + left );
1055     sum_tb = ( top + bottom );
1056     sum_nf = ( zNear + zFar );
1057     inv_rl = ( 1.0f / ( right - left ) );
1058     inv_tb = ( 1.0f / ( top - bottom ) );
1059     inv_nf = ( 1.0f / ( zNear - zFar ) );
1060     n2 = ( zNear + zNear );
1061     return Matrix4(
1062         Vector4( ( n2 * inv_rl ), 0.0f, 0.0f, 0.0f ),
1063         Vector4( 0.0f, ( n2 * inv_tb ), 0.0f, 0.0f ),
1064         Vector4( ( sum_rl * inv_rl ), ( sum_tb * inv_tb ), ( sum_nf * inv_nf ), -1.0f ),
1065         Vector4( 0.0f, 0.0f, ( ( n2 * inv_nf ) * zFar ), 0.0f )
1066     );
1067 }
1068
1069 inline const Matrix4 Matrix4::orthographic( float left, float right, float bottom, float top, float zNear, float zFar )
1070 {
1071     float sum_rl, sum_tb, sum_nf, inv_rl, inv_tb, inv_nf;
1072     sum_rl = ( right + left );
1073     sum_tb = ( top + bottom );
1074     sum_nf = ( zNear + zFar );
1075     inv_rl = ( 1.0f / ( right - left ) );
1076     inv_tb = ( 1.0f / ( top - bottom ) );
1077     inv_nf = ( 1.0f / ( zNear - zFar ) );
1078     return Matrix4(
1079         Vector4( ( inv_rl + inv_rl ), 0.0f, 0.0f, 0.0f ),
1080         Vector4( 0.0f, ( inv_tb + inv_tb ), 0.0f, 0.0f ),
1081         Vector4( 0.0f, 0.0f, ( inv_nf + inv_nf ), 0.0f ),
1082         Vector4( ( -sum_rl * inv_rl ), ( -sum_tb * inv_tb ), ( sum_nf * inv_nf ), 1.0f )
1083     );
1084 }
1085
1086 inline const Matrix4 select( const Matrix4 & mat0, const Matrix4 & mat1, bool select1 )
1087 {
1088     return Matrix4(
1089         select( mat0.getCol0(), mat1.getCol0(), select1 ),
1090         select( mat0.getCol1(), mat1.getCol1(), select1 ),
1091         select( mat0.getCol2(), mat1.getCol2(), select1 ),
1092         select( mat0.getCol3(), mat1.getCol3(), select1 )
1093     );
1094 }
1095
1096 #ifdef _VECTORMATH_DEBUG
1097
1098 inline void print( const Matrix4 & mat )
1099 {
1100     print( mat.getRow( 0 ) );
1101     print( mat.getRow( 1 ) );
1102     print( mat.getRow( 2 ) );
1103     print( mat.getRow( 3 ) );
1104 }
1105
1106 inline void print( const Matrix4 & mat, const char * name )
1107 {
1108     printf("%s:\n", name);
1109     print( mat );
1110 }
1111
1112 #endif
1113
1114 inline Transform3::Transform3( const Transform3 & tfrm )
1115 {
1116     mCol0 = tfrm.mCol0;
1117     mCol1 = tfrm.mCol1;
1118     mCol2 = tfrm.mCol2;
1119     mCol3 = tfrm.mCol3;
1120 }
1121
1122 inline Transform3::Transform3( float scalar )
1123 {
1124     mCol0 = Vector3( scalar );
1125     mCol1 = Vector3( scalar );
1126     mCol2 = Vector3( scalar );
1127     mCol3 = Vector3( scalar );
1128 }
1129
1130 inline Transform3::Transform3( const Vector3 & _col0, const Vector3 & _col1, const Vector3 & _col2, const Vector3 & _col3 )
1131 {
1132     mCol0 = _col0;
1133     mCol1 = _col1;
1134     mCol2 = _col2;
1135     mCol3 = _col3;
1136 }
1137
1138 inline Transform3::Transform3( const Matrix3 & tfrm, const Vector3 & translateVec )
1139 {
1140     this->setUpper3x3( tfrm );
1141     this->setTranslation( translateVec );
1142 }
1143
1144 inline Transform3::Transform3( const Quat & unitQuat, const Vector3 & translateVec )
1145 {
1146     this->setUpper3x3( Matrix3( unitQuat ) );
1147     this->setTranslation( translateVec );
1148 }
1149
1150 inline Transform3 & Transform3::setCol0( const Vector3 & _col0 )
1151 {
1152     mCol0 = _col0;
1153     return *this;
1154 }
1155
1156 inline Transform3 & Transform3::setCol1( const Vector3 & _col1 )
1157 {
1158     mCol1 = _col1;
1159     return *this;
1160 }
1161
1162 inline Transform3 & Transform3::setCol2( const Vector3 & _col2 )
1163 {
1164     mCol2 = _col2;
1165     return *this;
1166 }
1167
1168 inline Transform3 & Transform3::setCol3( const Vector3 & _col3 )
1169 {
1170     mCol3 = _col3;
1171     return *this;
1172 }
1173
1174 inline Transform3 & Transform3::setCol( int col, const Vector3 & vec )
1175 {
1176     *(&mCol0 + col) = vec;
1177     return *this;
1178 }
1179
1180 inline Transform3 & Transform3::setRow( int row, const Vector4 & vec )
1181 {
1182     mCol0.setElem( row, vec.getElem( 0 ) );
1183     mCol1.setElem( row, vec.getElem( 1 ) );
1184     mCol2.setElem( row, vec.getElem( 2 ) );
1185     mCol3.setElem( row, vec.getElem( 3 ) );
1186     return *this;
1187 }
1188
1189 inline Transform3 & Transform3::setElem( int col, int row, float val )
1190 {
1191     Vector3 tmpV3_0;
1192     tmpV3_0 = this->getCol( col );
1193     tmpV3_0.setElem( row, val );
1194     this->setCol( col, tmpV3_0 );
1195     return *this;
1196 }
1197
1198 inline float Transform3::getElem( int col, int row ) const
1199 {
1200     return this->getCol( col ).getElem( row );
1201 }
1202
1203 inline const Vector3 Transform3::getCol0( ) const
1204 {
1205     return mCol0;
1206 }
1207
1208 inline const Vector3 Transform3::getCol1( ) const
1209 {
1210     return mCol1;
1211 }
1212
1213 inline const Vector3 Transform3::getCol2( ) const
1214 {
1215     return mCol2;
1216 }
1217
1218 inline const Vector3 Transform3::getCol3( ) const
1219 {
1220     return mCol3;
1221 }
1222
1223 inline const Vector3 Transform3::getCol( int col ) const
1224 {
1225     return *(&mCol0 + col);
1226 }
1227
1228 inline const Vector4 Transform3::getRow( int row ) const
1229 {
1230     return Vector4( mCol0.getElem( row ), mCol1.getElem( row ), mCol2.getElem( row ), mCol3.getElem( row ) );
1231 }
1232
1233 inline Vector3 & Transform3::operator []( int col )
1234 {
1235     return *(&mCol0 + col);
1236 }
1237
1238 inline const Vector3 Transform3::operator []( int col ) const
1239 {
1240     return *(&mCol0 + col);
1241 }
1242
1243 inline Transform3 & Transform3::operator =( const Transform3 & tfrm )
1244 {
1245     mCol0 = tfrm.mCol0;
1246     mCol1 = tfrm.mCol1;
1247     mCol2 = tfrm.mCol2;
1248     mCol3 = tfrm.mCol3;
1249     return *this;
1250 }
1251
1252 inline const Transform3 inverse( const Transform3 & tfrm )
1253 {
1254     Vector3 tmp0, tmp1, tmp2, inv0, inv1, inv2;
1255     float detinv;
1256     tmp0 = cross( tfrm.getCol1(), tfrm.getCol2() );
1257     tmp1 = cross( tfrm.getCol2(), tfrm.getCol0() );
1258     tmp2 = cross( tfrm.getCol0(), tfrm.getCol1() );
1259     detinv = ( 1.0f / dot( tfrm.getCol2(), tmp2 ) );
1260     inv0 = Vector3( ( tmp0.getX() * detinv ), ( tmp1.getX() * detinv ), ( tmp2.getX() * detinv ) );
1261     inv1 = Vector3( ( tmp0.getY() * detinv ), ( tmp1.getY() * detinv ), ( tmp2.getY() * detinv ) );
1262     inv2 = Vector3( ( tmp0.getZ() * detinv ), ( tmp1.getZ() * detinv ), ( tmp2.getZ() * detinv ) );
1263     return Transform3(
1264         inv0,
1265         inv1,
1266         inv2,
1267         Vector3( ( -( ( inv0 * tfrm.getCol3().getX() ) + ( ( inv1 * tfrm.getCol3().getY() ) + ( inv2 * tfrm.getCol3().getZ() ) ) ) ) )
1268     );
1269 }
1270
1271 inline const Transform3 orthoInverse( const Transform3 & tfrm )
1272 {
1273     Vector3 inv0, inv1, inv2;
1274     inv0 = Vector3( tfrm.getCol0().getX(), tfrm.getCol1().getX(), tfrm.getCol2().getX() );
1275     inv1 = Vector3( tfrm.getCol0().getY(), tfrm.getCol1().getY(), tfrm.getCol2().getY() );
1276     inv2 = Vector3( tfrm.getCol0().getZ(), tfrm.getCol1().getZ(), tfrm.getCol2().getZ() );
1277     return Transform3(
1278         inv0,
1279         inv1,
1280         inv2,
1281         Vector3( ( -( ( inv0 * tfrm.getCol3().getX() ) + ( ( inv1 * tfrm.getCol3().getY() ) + ( inv2 * tfrm.getCol3().getZ() ) ) ) ) )
1282     );
1283 }
1284
1285 inline const Transform3 absPerElem( const Transform3 & tfrm )
1286 {
1287     return Transform3(
1288         absPerElem( tfrm.getCol0() ),
1289         absPerElem( tfrm.getCol1() ),
1290         absPerElem( tfrm.getCol2() ),
1291         absPerElem( tfrm.getCol3() )
1292     );
1293 }
1294
1295 inline const Vector3 Transform3::operator *( const Vector3 & vec ) const
1296 {
1297     return Vector3(
1298         ( ( ( mCol0.getX() * vec.getX() ) + ( mCol1.getX() * vec.getY() ) ) + ( mCol2.getX() * vec.getZ() ) ),
1299         ( ( ( mCol0.getY() * vec.getX() ) + ( mCol1.getY() * vec.getY() ) ) + ( mCol2.getY() * vec.getZ() ) ),
1300         ( ( ( mCol0.getZ() * vec.getX() ) + ( mCol1.getZ() * vec.getY() ) ) + ( mCol2.getZ() * vec.getZ() ) )
1301     );
1302 }
1303
1304 inline const Point3 Transform3::operator *( const Point3 & pnt ) const
1305 {
1306     return Point3(
1307         ( ( ( ( mCol0.getX() * pnt.getX() ) + ( mCol1.getX() * pnt.getY() ) ) + ( mCol2.getX() * pnt.getZ() ) ) + mCol3.getX() ),
1308         ( ( ( ( mCol0.getY() * pnt.getX() ) + ( mCol1.getY() * pnt.getY() ) ) + ( mCol2.getY() * pnt.getZ() ) ) + mCol3.getY() ),
1309         ( ( ( ( mCol0.getZ() * pnt.getX() ) + ( mCol1.getZ() * pnt.getY() ) ) + ( mCol2.getZ() * pnt.getZ() ) ) + mCol3.getZ() )
1310     );
1311 }
1312
1313 inline const Transform3 Transform3::operator *( const Transform3 & tfrm ) const
1314 {
1315     return Transform3(
1316         ( *this * tfrm.mCol0 ),
1317         ( *this * tfrm.mCol1 ),
1318         ( *this * tfrm.mCol2 ),
1319         Vector3( ( *this * Point3( tfrm.mCol3 ) ) )
1320     );
1321 }
1322
1323 inline Transform3 & Transform3::operator *=( const Transform3 & tfrm )
1324 {
1325     *this = *this * tfrm;
1326     return *this;
1327 }
1328
1329 inline const Transform3 mulPerElem( const Transform3 & tfrm0, const Transform3 & tfrm1 )
1330 {
1331     return Transform3(
1332         mulPerElem( tfrm0.getCol0(), tfrm1.getCol0() ),
1333         mulPerElem( tfrm0.getCol1(), tfrm1.getCol1() ),
1334         mulPerElem( tfrm0.getCol2(), tfrm1.getCol2() ),
1335         mulPerElem( tfrm0.getCol3(), tfrm1.getCol3() )
1336     );
1337 }
1338
1339 inline const Transform3 Transform3::identity( )
1340 {
1341     return Transform3(
1342         Vector3::xAxis( ),
1343         Vector3::yAxis( ),
1344         Vector3::zAxis( ),
1345         Vector3( 0.0f )
1346     );
1347 }
1348
1349 inline Transform3 & Transform3::setUpper3x3( const Matrix3 & tfrm )
1350 {
1351     mCol0 = tfrm.getCol0();
1352     mCol1 = tfrm.getCol1();
1353     mCol2 = tfrm.getCol2();
1354     return *this;
1355 }
1356
1357 inline const Matrix3 Transform3::getUpper3x3( ) const
1358 {
1359     return Matrix3( mCol0, mCol1, mCol2 );
1360 }
1361
1362 inline Transform3 & Transform3::setTranslation( const Vector3 & translateVec )
1363 {
1364     mCol3 = translateVec;
1365     return *this;
1366 }
1367
1368 inline const Vector3 Transform3::getTranslation( ) const
1369 {
1370     return mCol3;
1371 }
1372
1373 inline const Transform3 Transform3::rotationX( float radians )
1374 {
1375     float s, c;
1376     s = sinf( radians );
1377     c = cosf( radians );
1378     return Transform3(
1379         Vector3::xAxis( ),
1380         Vector3( 0.0f, c, s ),
1381         Vector3( 0.0f, -s, c ),
1382         Vector3( 0.0f )
1383     );
1384 }
1385
1386 inline const Transform3 Transform3::rotationY( float radians )
1387 {
1388     float s, c;
1389     s = sinf( radians );
1390     c = cosf( radians );
1391     return Transform3(
1392         Vector3( c, 0.0f, -s ),
1393         Vector3::yAxis( ),
1394         Vector3( s, 0.0f, c ),
1395         Vector3( 0.0f )
1396     );
1397 }
1398
1399 inline const Transform3 Transform3::rotationZ( float radians )
1400 {
1401     float s, c;
1402     s = sinf( radians );
1403     c = cosf( radians );
1404     return Transform3(
1405         Vector3( c, s, 0.0f ),
1406         Vector3( -s, c, 0.0f ),
1407         Vector3::zAxis( ),
1408         Vector3( 0.0f )
1409     );
1410 }
1411
1412 inline const Transform3 Transform3::rotationZYX( const Vector3 & radiansXYZ )
1413 {
1414     float sX, cX, sY, cY, sZ, cZ, tmp0, tmp1;
1415     sX = sinf( radiansXYZ.getX() );
1416     cX = cosf( radiansXYZ.getX() );
1417     sY = sinf( radiansXYZ.getY() );
1418     cY = cosf( radiansXYZ.getY() );
1419     sZ = sinf( radiansXYZ.getZ() );
1420     cZ = cosf( radiansXYZ.getZ() );
1421     tmp0 = ( cZ * sY );
1422     tmp1 = ( sZ * sY );
1423     return Transform3(
1424         Vector3( ( cZ * cY ), ( sZ * cY ), -sY ),
1425         Vector3( ( ( tmp0 * sX ) - ( sZ * cX ) ), ( ( tmp1 * sX ) + ( cZ * cX ) ), ( cY * sX ) ),
1426         Vector3( ( ( tmp0 * cX ) + ( sZ * sX ) ), ( ( tmp1 * cX ) - ( cZ * sX ) ), ( cY * cX ) ),
1427         Vector3( 0.0f )
1428     );
1429 }
1430
1431 inline const Transform3 Transform3::rotation( float radians, const Vector3 & unitVec )
1432 {
1433     return Transform3( Matrix3::rotation( radians, unitVec ), Vector3( 0.0f ) );
1434 }
1435
1436 inline const Transform3 Transform3::rotation( const Quat & unitQuat )
1437 {
1438     return Transform3( Matrix3( unitQuat ), Vector3( 0.0f ) );
1439 }
1440
1441 inline const Transform3 Transform3::scale( const Vector3 & scaleVec )
1442 {
1443     return Transform3(
1444         Vector3( scaleVec.getX(), 0.0f, 0.0f ),
1445         Vector3( 0.0f, scaleVec.getY(), 0.0f ),
1446         Vector3( 0.0f, 0.0f, scaleVec.getZ() ),
1447         Vector3( 0.0f )
1448     );
1449 }
1450
1451 inline const Transform3 appendScale( const Transform3 & tfrm, const Vector3 & scaleVec )
1452 {
1453     return Transform3(
1454         ( tfrm.getCol0() * scaleVec.getX( ) ),
1455         ( tfrm.getCol1() * scaleVec.getY( ) ),
1456         ( tfrm.getCol2() * scaleVec.getZ( ) ),
1457         tfrm.getCol3()
1458     );
1459 }
1460
1461 inline const Transform3 prependScale( const Vector3 & scaleVec, const Transform3 & tfrm )
1462 {
1463     return Transform3(
1464         mulPerElem( tfrm.getCol0(), scaleVec ),
1465         mulPerElem( tfrm.getCol1(), scaleVec ),
1466         mulPerElem( tfrm.getCol2(), scaleVec ),
1467         mulPerElem( tfrm.getCol3(), scaleVec )
1468     );
1469 }
1470
1471 inline const Transform3 Transform3::translation( const Vector3 & translateVec )
1472 {
1473     return Transform3(
1474         Vector3::xAxis( ),
1475         Vector3::yAxis( ),
1476         Vector3::zAxis( ),
1477         translateVec
1478     );
1479 }
1480
1481 inline const Transform3 select( const Transform3 & tfrm0, const Transform3 & tfrm1, bool select1 )
1482 {
1483     return Transform3(
1484         select( tfrm0.getCol0(), tfrm1.getCol0(), select1 ),
1485         select( tfrm0.getCol1(), tfrm1.getCol1(), select1 ),
1486         select( tfrm0.getCol2(), tfrm1.getCol2(), select1 ),
1487         select( tfrm0.getCol3(), tfrm1.getCol3(), select1 )
1488     );
1489 }
1490
1491 #ifdef _VECTORMATH_DEBUG
1492
1493 inline void print( const Transform3 & tfrm )
1494 {
1495     print( tfrm.getRow( 0 ) );
1496     print( tfrm.getRow( 1 ) );
1497     print( tfrm.getRow( 2 ) );
1498 }
1499
1500 inline void print( const Transform3 & tfrm, const char * name )
1501 {
1502     printf("%s:\n", name);
1503     print( tfrm );
1504 }
1505
1506 #endif
1507
1508 inline Quat::Quat( const Matrix3 & tfrm )
1509 {
1510     float trace, radicand, scale, xx, yx, zx, xy, yy, zy, xz, yz, zz, tmpx, tmpy, tmpz, tmpw, qx, qy, qz, qw;
1511     int negTrace, ZgtX, ZgtY, YgtX;
1512     int largestXorY, largestYorZ, largestZorX;
1513
1514     xx = tfrm.getCol0().getX();
1515     yx = tfrm.getCol0().getY();
1516     zx = tfrm.getCol0().getZ();
1517     xy = tfrm.getCol1().getX();
1518     yy = tfrm.getCol1().getY();
1519     zy = tfrm.getCol1().getZ();
1520     xz = tfrm.getCol2().getX();
1521     yz = tfrm.getCol2().getY();
1522     zz = tfrm.getCol2().getZ();
1523
1524     trace = ( ( xx + yy ) + zz );
1525
1526     negTrace = ( trace < 0.0f );
1527     ZgtX = zz > xx;
1528     ZgtY = zz > yy;
1529     YgtX = yy > xx;
1530     largestXorY = ( !ZgtX || !ZgtY ) && negTrace;
1531     largestYorZ = ( YgtX || ZgtX ) && negTrace;
1532     largestZorX = ( ZgtY || !YgtX ) && negTrace;
1533
1534     if ( largestXorY )
1535     {
1536         zz = -zz;
1537         xy = -xy;
1538     }
1539     if ( largestYorZ )
1540     {
1541         xx = -xx;
1542         yz = -yz;
1543     }
1544     if ( largestZorX )
1545     {
1546         yy = -yy;
1547         zx = -zx;
1548     }
1549
1550     radicand = ( ( ( xx + yy ) + zz ) + 1.0f );
1551     scale = ( 0.5f * ( 1.0f / sqrtf( radicand ) ) );
1552
1553     tmpx = ( ( zy - yz ) * scale );
1554     tmpy = ( ( xz - zx ) * scale );
1555     tmpz = ( ( yx - xy ) * scale );
1556     tmpw = ( radicand * scale );
1557     qx = tmpx;
1558     qy = tmpy;
1559     qz = tmpz;
1560     qw = tmpw;
1561
1562     if ( largestXorY )
1563     {
1564         qx = tmpw;
1565         qy = tmpz;
1566         qz = tmpy;
1567         qw = tmpx;
1568     }
1569     if ( largestYorZ )
1570     {
1571         tmpx = qx;
1572         tmpz = qz;
1573         qx = qy;
1574         qy = tmpx;
1575         qz = qw;
1576         qw = tmpz;
1577     }
1578
1579     mX = qx;
1580     mY = qy;
1581     mZ = qz;
1582     mW = qw;
1583 }
1584
1585 inline const Matrix3 outer( const Vector3 & tfrm0, const Vector3 & tfrm1 )
1586 {
1587     return Matrix3(
1588         ( tfrm0 * tfrm1.getX( ) ),
1589         ( tfrm0 * tfrm1.getY( ) ),
1590         ( tfrm0 * tfrm1.getZ( ) )
1591     );
1592 }
1593
1594 inline const Matrix4 outer( const Vector4 & tfrm0, const Vector4 & tfrm1 )
1595 {
1596     return Matrix4(
1597         ( tfrm0 * tfrm1.getX( ) ),
1598         ( tfrm0 * tfrm1.getY( ) ),
1599         ( tfrm0 * tfrm1.getZ( ) ),
1600         ( tfrm0 * tfrm1.getW( ) )
1601     );
1602 }
1603
1604 inline const Vector3 rowMul( const Vector3 & vec, const Matrix3 & mat )
1605 {
1606     return Vector3(
1607         ( ( ( vec.getX() * mat.getCol0().getX() ) + ( vec.getY() * mat.getCol0().getY() ) ) + ( vec.getZ() * mat.getCol0().getZ() ) ),
1608         ( ( ( vec.getX() * mat.getCol1().getX() ) + ( vec.getY() * mat.getCol1().getY() ) ) + ( vec.getZ() * mat.getCol1().getZ() ) ),
1609         ( ( ( vec.getX() * mat.getCol2().getX() ) + ( vec.getY() * mat.getCol2().getY() ) ) + ( vec.getZ() * mat.getCol2().getZ() ) )
1610     );
1611 }
1612
1613 inline const Matrix3 crossMatrix( const Vector3 & vec )
1614 {
1615     return Matrix3(
1616         Vector3( 0.0f, vec.getZ(), -vec.getY() ),
1617         Vector3( -vec.getZ(), 0.0f, vec.getX() ),
1618         Vector3( vec.getY(), -vec.getX(), 0.0f )
1619     );
1620 }
1621
1622 inline const Matrix3 crossMatrixMul( const Vector3 & vec, const Matrix3 & mat )
1623 {
1624     return Matrix3( cross( vec, mat.getCol0() ), cross( vec, mat.getCol1() ), cross( vec, mat.getCol2() ) );
1625 }
1626
1627 } // namespace Aos
1628 } // namespace Vectormath
1629
1630 #endif