Extras/PhysicsEffects/include/vecmath/std/mat_aos.h

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