scalar/mat_aos.h

Go to the documentation of this file.
00001 /*
00002    Copyright (C) 2009 Sony Computer Entertainment Inc.
00003    All rights reserved.
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 
00015 */
00016 
00017 #ifndef _VECTORMATH_MAT_AOS_CPP_H
00018 #define _VECTORMATH_MAT_AOS_CPP_H
00019 
00020 namespace Vectormath {
00021 namespace Aos {
00022 
00023 //-----------------------------------------------------------------------------
00024 // Constants
00025 
00026 #define _VECTORMATH_PI_OVER_2 1.570796327f
00027 
00028 //-----------------------------------------------------------------------------
00029 // Definitions
00030 
00031 inline Matrix3::Matrix3( const Matrix3 & mat )
00032 {
00033     mCol0 = mat.mCol0;
00034     mCol1 = mat.mCol1;
00035     mCol2 = mat.mCol2;
00036 }
00037 
00038 inline Matrix3::Matrix3( float scalar )
00039 {
00040     mCol0 = Vector3( scalar );
00041     mCol1 = Vector3( scalar );
00042     mCol2 = Vector3( scalar );
00043 }
00044 
00045 inline Matrix3::Matrix3( const Quat & unitQuat )
00046 {
00047     float qx, qy, qz, qw, qx2, qy2, qz2, qxqx2, qyqy2, qzqz2, qxqy2, qyqz2, qzqw2, qxqz2, qyqw2, qxqw2;
00048     qx = unitQuat.getX();
00049     qy = unitQuat.getY();
00050     qz = unitQuat.getZ();
00051     qw = unitQuat.getW();
00052     qx2 = ( qx + qx );
00053     qy2 = ( qy + qy );
00054     qz2 = ( qz + qz );
00055     qxqx2 = ( qx * qx2 );
00056     qxqy2 = ( qx * qy2 );
00057     qxqz2 = ( qx * qz2 );
00058     qxqw2 = ( qw * qx2 );
00059     qyqy2 = ( qy * qy2 );
00060     qyqz2 = ( qy * qz2 );
00061     qyqw2 = ( qw * qy2 );
00062     qzqz2 = ( qz * qz2 );
00063     qzqw2 = ( qw * qz2 );
00064     mCol0 = Vector3( ( ( 1.0f - qyqy2 ) - qzqz2 ), ( qxqy2 + qzqw2 ), ( qxqz2 - qyqw2 ) );
00065     mCol1 = Vector3( ( qxqy2 - qzqw2 ), ( ( 1.0f - qxqx2 ) - qzqz2 ), ( qyqz2 + qxqw2 ) );
00066     mCol2 = Vector3( ( qxqz2 + qyqw2 ), ( qyqz2 - qxqw2 ), ( ( 1.0f - qxqx2 ) - qyqy2 ) );
00067 }
00068 
00069 inline Matrix3::Matrix3( const Vector3 & _col0, const Vector3 & _col1, const Vector3 & _col2 )
00070 {
00071     mCol0 = _col0;
00072     mCol1 = _col1;
00073     mCol2 = _col2;
00074 }
00075 
00076 inline Matrix3 & Matrix3::setCol0( const Vector3 & _col0 )
00077 {
00078     mCol0 = _col0;
00079     return *this;
00080 }
00081 
00082 inline Matrix3 & Matrix3::setCol1( const Vector3 & _col1 )
00083 {
00084     mCol1 = _col1;
00085     return *this;
00086 }
00087 
00088 inline Matrix3 & Matrix3::setCol2( const Vector3 & _col2 )
00089 {
00090     mCol2 = _col2;
00091     return *this;
00092 }
00093 
00094 inline Matrix3 & Matrix3::setCol( int col, const Vector3 & vec )
00095 {
00096     *(&mCol0 + col) = vec;
00097     return *this;
00098 }
00099 
00100 inline Matrix3 & Matrix3::setRow( int row, const Vector3 & vec )
00101 {
00102     mCol0.setElem( row, vec.getElem( 0 ) );
00103     mCol1.setElem( row, vec.getElem( 1 ) );
00104     mCol2.setElem( row, vec.getElem( 2 ) );
00105     return *this;
00106 }
00107 
00108 inline Matrix3 & Matrix3::setElem( int col, int row, float val )
00109 {
00110     Vector3 tmpV3_0;
00111     tmpV3_0 = this->getCol( col );
00112     tmpV3_0.setElem( row, val );
00113     this->setCol( col, tmpV3_0 );
00114     return *this;
00115 }
00116 
00117 inline float Matrix3::getElem( int col, int row ) const
00118 {
00119     return this->getCol( col ).getElem( row );
00120 }
00121 
00122 inline const Vector3 Matrix3::getCol0( ) const
00123 {
00124     return mCol0;
00125 }
00126 
00127 inline const Vector3 Matrix3::getCol1( ) const
00128 {
00129     return mCol1;
00130 }
00131 
00132 inline const Vector3 Matrix3::getCol2( ) const
00133 {
00134     return mCol2;
00135 }
00136 
00137 inline const Vector3 Matrix3::getCol( int col ) const
00138 {
00139     return *(&mCol0 + col);
00140 }
00141 
00142 inline const Vector3 Matrix3::getRow( int row ) const
00143 {
00144     return Vector3( mCol0.getElem( row ), mCol1.getElem( row ), mCol2.getElem( row ) );
00145 }
00146 
00147 inline Vector3 & Matrix3::operator []( int col )
00148 {
00149     return *(&mCol0 + col);
00150 }
00151 
00152 inline const Vector3 Matrix3::operator []( int col ) const
00153 {
00154     return *(&mCol0 + col);
00155 }
00156 
00157 inline Matrix3 & Matrix3::operator =( const Matrix3 & mat )
00158 {
00159     mCol0 = mat.mCol0;
00160     mCol1 = mat.mCol1;
00161     mCol2 = mat.mCol2;
00162     return *this;
00163 }
00164 
00165 inline const Matrix3 transpose( const Matrix3 & mat )
00166 {
00167     return Matrix3(
00168         Vector3( mat.getCol0().getX(), mat.getCol1().getX(), mat.getCol2().getX() ),
00169         Vector3( mat.getCol0().getY(), mat.getCol1().getY(), mat.getCol2().getY() ),
00170         Vector3( mat.getCol0().getZ(), mat.getCol1().getZ(), mat.getCol2().getZ() )
00171     );
00172 }
00173 
00174 inline const Matrix3 inverse( const Matrix3 & mat )
00175 {
00176     Vector3 tmp0, tmp1, tmp2;
00177     float detinv;
00178     tmp0 = cross( mat.getCol1(), mat.getCol2() );
00179     tmp1 = cross( mat.getCol2(), mat.getCol0() );
00180     tmp2 = cross( mat.getCol0(), mat.getCol1() );
00181     detinv = ( 1.0f / dot( mat.getCol2(), tmp2 ) );
00182     return Matrix3(
00183         Vector3( ( tmp0.getX() * detinv ), ( tmp1.getX() * detinv ), ( tmp2.getX() * detinv ) ),
00184         Vector3( ( tmp0.getY() * detinv ), ( tmp1.getY() * detinv ), ( tmp2.getY() * detinv ) ),
00185         Vector3( ( tmp0.getZ() * detinv ), ( tmp1.getZ() * detinv ), ( tmp2.getZ() * detinv ) )
00186     );
00187 }
00188 
00189 inline float determinant( const Matrix3 & mat )
00190 {
00191     return dot( mat.getCol2(), cross( mat.getCol0(), mat.getCol1() ) );
00192 }
00193 
00194 inline const Matrix3 Matrix3::operator +( const Matrix3 & mat ) const
00195 {
00196     return Matrix3(
00197         ( mCol0 + mat.mCol0 ),
00198         ( mCol1 + mat.mCol1 ),
00199         ( mCol2 + mat.mCol2 )
00200     );
00201 }
00202 
00203 inline const Matrix3 Matrix3::operator -( const Matrix3 & mat ) const
00204 {
00205     return Matrix3(
00206         ( mCol0 - mat.mCol0 ),
00207         ( mCol1 - mat.mCol1 ),
00208         ( mCol2 - mat.mCol2 )
00209     );
00210 }
00211 
00212 inline Matrix3 & Matrix3::operator +=( const Matrix3 & mat )
00213 {
00214     *this = *this + mat;
00215     return *this;
00216 }
00217 
00218 inline Matrix3 & Matrix3::operator -=( const Matrix3 & mat )
00219 {
00220     *this = *this - mat;
00221     return *this;
00222 }
00223 
00224 inline const Matrix3 Matrix3::operator -( ) const
00225 {
00226     return Matrix3(
00227         ( -mCol0 ),
00228         ( -mCol1 ),
00229         ( -mCol2 )
00230     );
00231 }
00232 
00233 inline const Matrix3 absPerElem( const Matrix3 & mat )
00234 {
00235     return Matrix3(
00236         absPerElem( mat.getCol0() ),
00237         absPerElem( mat.getCol1() ),
00238         absPerElem( mat.getCol2() )
00239     );
00240 }
00241 
00242 inline const Matrix3 Matrix3::operator *( float scalar ) const
00243 {
00244     return Matrix3(
00245         ( mCol0 * scalar ),
00246         ( mCol1 * scalar ),
00247         ( mCol2 * scalar )
00248     );
00249 }
00250 
00251 inline Matrix3 & Matrix3::operator *=( float scalar )
00252 {
00253     *this = *this * scalar;
00254     return *this;
00255 }
00256 
00257 inline const Matrix3 operator *( float scalar, const Matrix3 & mat )
00258 {
00259     return mat * scalar;
00260 }
00261 
00262 inline const Vector3 Matrix3::operator *( const Vector3 & vec ) const
00263 {
00264     return Vector3(
00265         ( ( ( mCol0.getX() * vec.getX() ) + ( mCol1.getX() * vec.getY() ) ) + ( mCol2.getX() * vec.getZ() ) ),
00266         ( ( ( mCol0.getY() * vec.getX() ) + ( mCol1.getY() * vec.getY() ) ) + ( mCol2.getY() * vec.getZ() ) ),
00267         ( ( ( mCol0.getZ() * vec.getX() ) + ( mCol1.getZ() * vec.getY() ) ) + ( mCol2.getZ() * vec.getZ() ) )
00268     );
00269 }
00270 
00271 inline const Matrix3 Matrix3::operator *( const Matrix3 & mat ) const
00272 {
00273     return Matrix3(
00274         ( *this * mat.mCol0 ),
00275         ( *this * mat.mCol1 ),
00276         ( *this * mat.mCol2 )
00277     );
00278 }
00279 
00280 inline Matrix3 & Matrix3::operator *=( const Matrix3 & mat )
00281 {
00282     *this = *this * mat;
00283     return *this;
00284 }
00285 
00286 inline const Matrix3 mulPerElem( const Matrix3 & mat0, const Matrix3 & mat1 )
00287 {
00288     return Matrix3(
00289         mulPerElem( mat0.getCol0(), mat1.getCol0() ),
00290         mulPerElem( mat0.getCol1(), mat1.getCol1() ),
00291         mulPerElem( mat0.getCol2(), mat1.getCol2() )
00292     );
00293 }
00294 
00295 inline const Matrix3 Matrix3::identity( )
00296 {
00297     return Matrix3(
00298         Vector3::xAxis( ),
00299         Vector3::yAxis( ),
00300         Vector3::zAxis( )
00301     );
00302 }
00303 
00304 inline const Matrix3 Matrix3::rotationX( float radians )
00305 {
00306     float s, c;
00307     s = sinf( radians );
00308     c = cosf( radians );
00309     return Matrix3(
00310         Vector3::xAxis( ),
00311         Vector3( 0.0f, c, s ),
00312         Vector3( 0.0f, -s, c )
00313     );
00314 }
00315 
00316 inline const Matrix3 Matrix3::rotationY( float radians )
00317 {
00318     float s, c;
00319     s = sinf( radians );
00320     c = cosf( radians );
00321     return Matrix3(
00322         Vector3( c, 0.0f, -s ),
00323         Vector3::yAxis( ),
00324         Vector3( s, 0.0f, c )
00325     );
00326 }
00327 
00328 inline const Matrix3 Matrix3::rotationZ( float radians )
00329 {
00330     float s, c;
00331     s = sinf( radians );
00332     c = cosf( radians );
00333     return Matrix3(
00334         Vector3( c, s, 0.0f ),
00335         Vector3( -s, c, 0.0f ),
00336         Vector3::zAxis( )
00337     );
00338 }
00339 
00340 inline const Matrix3 Matrix3::rotationZYX( const Vector3 & radiansXYZ )
00341 {
00342     float sX, cX, sY, cY, sZ, cZ, tmp0, tmp1;
00343     sX = sinf( radiansXYZ.getX() );
00344     cX = cosf( radiansXYZ.getX() );
00345     sY = sinf( radiansXYZ.getY() );
00346     cY = cosf( radiansXYZ.getY() );
00347     sZ = sinf( radiansXYZ.getZ() );
00348     cZ = cosf( radiansXYZ.getZ() );
00349     tmp0 = ( cZ * sY );
00350     tmp1 = ( sZ * sY );
00351     return Matrix3(
00352         Vector3( ( cZ * cY ), ( sZ * cY ), -sY ),
00353         Vector3( ( ( tmp0 * sX ) - ( sZ * cX ) ), ( ( tmp1 * sX ) + ( cZ * cX ) ), ( cY * sX ) ),
00354         Vector3( ( ( tmp0 * cX ) + ( sZ * sX ) ), ( ( tmp1 * cX ) - ( cZ * sX ) ), ( cY * cX ) )
00355     );
00356 }
00357 
00358 inline const Matrix3 Matrix3::rotation( float radians, const Vector3 & unitVec )
00359 {
00360     float x, y, z, s, c, oneMinusC, xy, yz, zx;
00361     s = sinf( radians );
00362     c = cosf( radians );
00363     x = unitVec.getX();
00364     y = unitVec.getY();
00365     z = unitVec.getZ();
00366     xy = ( x * y );
00367     yz = ( y * z );
00368     zx = ( z * x );
00369     oneMinusC = ( 1.0f - c );
00370     return Matrix3(
00371         Vector3( ( ( ( x * x ) * oneMinusC ) + c ), ( ( xy * oneMinusC ) + ( z * s ) ), ( ( zx * oneMinusC ) - ( y * s ) ) ),
00372         Vector3( ( ( xy * oneMinusC ) - ( z * s ) ), ( ( ( y * y ) * oneMinusC ) + c ), ( ( yz * oneMinusC ) + ( x * s ) ) ),
00373         Vector3( ( ( zx * oneMinusC ) + ( y * s ) ), ( ( yz * oneMinusC ) - ( x * s ) ), ( ( ( z * z ) * oneMinusC ) + c ) )
00374     );
00375 }
00376 
00377 inline const Matrix3 Matrix3::rotation( const Quat & unitQuat )
00378 {
00379     return Matrix3( unitQuat );
00380 }
00381 
00382 inline const Matrix3 Matrix3::scale( const Vector3 & scaleVec )
00383 {
00384     return Matrix3(
00385         Vector3( scaleVec.getX(), 0.0f, 0.0f ),
00386         Vector3( 0.0f, scaleVec.getY(), 0.0f ),
00387         Vector3( 0.0f, 0.0f, scaleVec.getZ() )
00388     );
00389 }
00390 
00391 inline const Matrix3 appendScale( const Matrix3 & mat, const Vector3 & scaleVec )
00392 {
00393     return Matrix3(
00394         ( mat.getCol0() * scaleVec.getX( ) ),
00395         ( mat.getCol1() * scaleVec.getY( ) ),
00396         ( mat.getCol2() * scaleVec.getZ( ) )
00397     );
00398 }
00399 
00400 inline const Matrix3 prependScale( const Vector3 & scaleVec, const Matrix3 & mat )
00401 {
00402     return Matrix3(
00403         mulPerElem( mat.getCol0(), scaleVec ),
00404         mulPerElem( mat.getCol1(), scaleVec ),
00405         mulPerElem( mat.getCol2(), scaleVec )
00406     );
00407 }
00408 
00409 inline const Matrix3 select( const Matrix3 & mat0, const Matrix3 & mat1, bool select1 )
00410 {
00411     return Matrix3(
00412         select( mat0.getCol0(), mat1.getCol0(), select1 ),
00413         select( mat0.getCol1(), mat1.getCol1(), select1 ),
00414         select( mat0.getCol2(), mat1.getCol2(), select1 )
00415     );
00416 }
00417 
00418 #ifdef _VECTORMATH_DEBUG
00419 
00420 inline void print( const Matrix3 & mat )
00421 {
00422     print( mat.getRow( 0 ) );
00423     print( mat.getRow( 1 ) );
00424     print( mat.getRow( 2 ) );
00425 }
00426 
00427 inline void print( const Matrix3 & mat, const char * name )
00428 {
00429     printf("%s:\n", name);
00430     print( mat );
00431 }
00432 
00433 #endif
00434 
00435 inline Matrix4::Matrix4( const Matrix4 & mat )
00436 {
00437     mCol0 = mat.mCol0;
00438     mCol1 = mat.mCol1;
00439     mCol2 = mat.mCol2;
00440     mCol3 = mat.mCol3;
00441 }
00442 
00443 inline Matrix4::Matrix4( float scalar )
00444 {
00445     mCol0 = Vector4( scalar );
00446     mCol1 = Vector4( scalar );
00447     mCol2 = Vector4( scalar );
00448     mCol3 = Vector4( scalar );
00449 }
00450 
00451 inline Matrix4::Matrix4( const Transform3 & mat )
00452 {
00453     mCol0 = Vector4( mat.getCol0(), 0.0f );
00454     mCol1 = Vector4( mat.getCol1(), 0.0f );
00455     mCol2 = Vector4( mat.getCol2(), 0.0f );
00456     mCol3 = Vector4( mat.getCol3(), 1.0f );
00457 }
00458 
00459 inline Matrix4::Matrix4( const Vector4 & _col0, const Vector4 & _col1, const Vector4 & _col2, const Vector4 & _col3 )
00460 {
00461     mCol0 = _col0;
00462     mCol1 = _col1;
00463     mCol2 = _col2;
00464     mCol3 = _col3;
00465 }
00466 
00467 inline Matrix4::Matrix4( const Matrix3 & mat, const Vector3 & translateVec )
00468 {
00469     mCol0 = Vector4( mat.getCol0(), 0.0f );
00470     mCol1 = Vector4( mat.getCol1(), 0.0f );
00471     mCol2 = Vector4( mat.getCol2(), 0.0f );
00472     mCol3 = Vector4( translateVec, 1.0f );
00473 }
00474 
00475 inline Matrix4::Matrix4( const Quat & unitQuat, const Vector3 & translateVec )
00476 {
00477     Matrix3 mat;
00478     mat = Matrix3( unitQuat );
00479     mCol0 = Vector4( mat.getCol0(), 0.0f );
00480     mCol1 = Vector4( mat.getCol1(), 0.0f );
00481     mCol2 = Vector4( mat.getCol2(), 0.0f );
00482     mCol3 = Vector4( translateVec, 1.0f );
00483 }
00484 
00485 inline Matrix4 & Matrix4::setCol0( const Vector4 & _col0 )
00486 {
00487     mCol0 = _col0;
00488     return *this;
00489 }
00490 
00491 inline Matrix4 & Matrix4::setCol1( const Vector4 & _col1 )
00492 {
00493     mCol1 = _col1;
00494     return *this;
00495 }
00496 
00497 inline Matrix4 & Matrix4::setCol2( const Vector4 & _col2 )
00498 {
00499     mCol2 = _col2;
00500     return *this;
00501 }
00502 
00503 inline Matrix4 & Matrix4::setCol3( const Vector4 & _col3 )
00504 {
00505     mCol3 = _col3;
00506     return *this;
00507 }
00508 
00509 inline Matrix4 & Matrix4::setCol( int col, const Vector4 & vec )
00510 {
00511     *(&mCol0 + col) = vec;
00512     return *this;
00513 }
00514 
00515 inline Matrix4 & Matrix4::setRow( int row, const Vector4 & vec )
00516 {
00517     mCol0.setElem( row, vec.getElem( 0 ) );
00518     mCol1.setElem( row, vec.getElem( 1 ) );
00519     mCol2.setElem( row, vec.getElem( 2 ) );
00520     mCol3.setElem( row, vec.getElem( 3 ) );
00521     return *this;
00522 }
00523 
00524 inline Matrix4 & Matrix4::setElem( int col, int row, float val )
00525 {
00526     Vector4 tmpV3_0;
00527     tmpV3_0 = this->getCol( col );
00528     tmpV3_0.setElem( row, val );
00529     this->setCol( col, tmpV3_0 );
00530     return *this;
00531 }
00532 
00533 inline float Matrix4::getElem( int col, int row ) const
00534 {
00535     return this->getCol( col ).getElem( row );
00536 }
00537 
00538 inline const Vector4 Matrix4::getCol0( ) const
00539 {
00540     return mCol0;
00541 }
00542 
00543 inline const Vector4 Matrix4::getCol1( ) const
00544 {
00545     return mCol1;
00546 }
00547 
00548 inline const Vector4 Matrix4::getCol2( ) const
00549 {
00550     return mCol2;
00551 }
00552 
00553 inline const Vector4 Matrix4::getCol3( ) const
00554 {
00555     return mCol3;
00556 }
00557 
00558 inline const Vector4 Matrix4::getCol( int col ) const
00559 {
00560     return *(&mCol0 + col);
00561 }
00562 
00563 inline const Vector4 Matrix4::getRow( int row ) const
00564 {
00565     return Vector4( mCol0.getElem( row ), mCol1.getElem( row ), mCol2.getElem( row ), mCol3.getElem( row ) );
00566 }
00567 
00568 inline Vector4 & Matrix4::operator []( int col )
00569 {
00570     return *(&mCol0 + col);
00571 }
00572 
00573 inline const Vector4 Matrix4::operator []( int col ) const
00574 {
00575     return *(&mCol0 + col);
00576 }
00577 
00578 inline Matrix4 & Matrix4::operator =( const Matrix4 & mat )
00579 {
00580     mCol0 = mat.mCol0;
00581     mCol1 = mat.mCol1;
00582     mCol2 = mat.mCol2;
00583     mCol3 = mat.mCol3;
00584     return *this;
00585 }
00586 
00587 inline const Matrix4 transpose( const Matrix4 & mat )
00588 {
00589     return Matrix4(
00590         Vector4( mat.getCol0().getX(), mat.getCol1().getX(), mat.getCol2().getX(), mat.getCol3().getX() ),
00591         Vector4( mat.getCol0().getY(), mat.getCol1().getY(), mat.getCol2().getY(), mat.getCol3().getY() ),
00592         Vector4( mat.getCol0().getZ(), mat.getCol1().getZ(), mat.getCol2().getZ(), mat.getCol3().getZ() ),
00593         Vector4( mat.getCol0().getW(), mat.getCol1().getW(), mat.getCol2().getW(), mat.getCol3().getW() )
00594     );
00595 }
00596 
00597 inline const Matrix4 inverse( const Matrix4 & mat )
00598 {
00599     Vector4 res0, res1, res2, res3;
00600     float mA, mB, mC, mD, mE, mF, mG, mH, mI, mJ, mK, mL, mM, mN, mO, mP, tmp0, tmp1, tmp2, tmp3, tmp4, tmp5, detInv;
00601     mA = mat.getCol0().getX();
00602     mB = mat.getCol0().getY();
00603     mC = mat.getCol0().getZ();
00604     mD = mat.getCol0().getW();
00605     mE = mat.getCol1().getX();
00606     mF = mat.getCol1().getY();
00607     mG = mat.getCol1().getZ();
00608     mH = mat.getCol1().getW();
00609     mI = mat.getCol2().getX();
00610     mJ = mat.getCol2().getY();
00611     mK = mat.getCol2().getZ();
00612     mL = mat.getCol2().getW();
00613     mM = mat.getCol3().getX();
00614     mN = mat.getCol3().getY();
00615     mO = mat.getCol3().getZ();
00616     mP = mat.getCol3().getW();
00617     tmp0 = ( ( mK * mD ) - ( mC * mL ) );
00618     tmp1 = ( ( mO * mH ) - ( mG * mP ) );
00619     tmp2 = ( ( mB * mK ) - ( mJ * mC ) );
00620     tmp3 = ( ( mF * mO ) - ( mN * mG ) );
00621     tmp4 = ( ( mJ * mD ) - ( mB * mL ) );
00622     tmp5 = ( ( mN * mH ) - ( mF * mP ) );
00623     res0.setX( ( ( ( mJ * tmp1 ) - ( mL * tmp3 ) ) - ( mK * tmp5 ) ) );
00624     res0.setY( ( ( ( mN * tmp0 ) - ( mP * tmp2 ) ) - ( mO * tmp4 ) ) );
00625     res0.setZ( ( ( ( mD * tmp3 ) + ( mC * tmp5 ) ) - ( mB * tmp1 ) ) );
00626     res0.setW( ( ( ( mH * tmp2 ) + ( mG * tmp4 ) ) - ( mF * tmp0 ) ) );
00627     detInv = ( 1.0f / ( ( ( ( mA * res0.getX() ) + ( mE * res0.getY() ) ) + ( mI * res0.getZ() ) ) + ( mM * res0.getW() ) ) );
00628     res1.setX( ( mI * tmp1 ) );
00629     res1.setY( ( mM * tmp0 ) );
00630     res1.setZ( ( mA * tmp1 ) );
00631     res1.setW( ( mE * tmp0 ) );
00632     res3.setX( ( mI * tmp3 ) );
00633     res3.setY( ( mM * tmp2 ) );
00634     res3.setZ( ( mA * tmp3 ) );
00635     res3.setW( ( mE * tmp2 ) );
00636     res2.setX( ( mI * tmp5 ) );
00637     res2.setY( ( mM * tmp4 ) );
00638     res2.setZ( ( mA * tmp5 ) );
00639     res2.setW( ( mE * tmp4 ) );
00640     tmp0 = ( ( mI * mB ) - ( mA * mJ ) );
00641     tmp1 = ( ( mM * mF ) - ( mE * mN ) );
00642     tmp2 = ( ( mI * mD ) - ( mA * mL ) );
00643     tmp3 = ( ( mM * mH ) - ( mE * mP ) );
00644     tmp4 = ( ( mI * mC ) - ( mA * mK ) );
00645     tmp5 = ( ( mM * mG ) - ( mE * mO ) );
00646     res2.setX( ( ( ( mL * tmp1 ) - ( mJ * tmp3 ) ) + res2.getX() ) );
00647     res2.setY( ( ( ( mP * tmp0 ) - ( mN * tmp2 ) ) + res2.getY() ) );
00648     res2.setZ( ( ( ( mB * tmp3 ) - ( mD * tmp1 ) ) - res2.getZ() ) );
00649     res2.setW( ( ( ( mF * tmp2 ) - ( mH * tmp0 ) ) - res2.getW() ) );
00650     res3.setX( ( ( ( mJ * tmp5 ) - ( mK * tmp1 ) ) + res3.getX() ) );
00651     res3.setY( ( ( ( mN * tmp4 ) - ( mO * tmp0 ) ) + res3.getY() ) );
00652     res3.setZ( ( ( ( mC * tmp1 ) - ( mB * tmp5 ) ) - res3.getZ() ) );
00653     res3.setW( ( ( ( mG * tmp0 ) - ( mF * tmp4 ) ) - res3.getW() ) );
00654     res1.setX( ( ( ( mK * tmp3 ) - ( mL * tmp5 ) ) - res1.getX() ) );
00655     res1.setY( ( ( ( mO * tmp2 ) - ( mP * tmp4 ) ) - res1.getY() ) );
00656     res1.setZ( ( ( ( mD * tmp5 ) - ( mC * tmp3 ) ) + res1.getZ() ) );
00657     res1.setW( ( ( ( mH * tmp4 ) - ( mG * tmp2 ) ) + res1.getW() ) );
00658     return Matrix4(
00659         ( res0 * detInv ),
00660         ( res1 * detInv ),
00661         ( res2 * detInv ),
00662         ( res3 * detInv )
00663     );
00664 }
00665 
00666 inline const Matrix4 affineInverse( const Matrix4 & mat )
00667 {
00668     Transform3 affineMat;
00669     affineMat.setCol0( mat.getCol0().getXYZ( ) );
00670     affineMat.setCol1( mat.getCol1().getXYZ( ) );
00671     affineMat.setCol2( mat.getCol2().getXYZ( ) );
00672     affineMat.setCol3( mat.getCol3().getXYZ( ) );
00673     return Matrix4( inverse( affineMat ) );
00674 }
00675 
00676 inline const Matrix4 orthoInverse( const Matrix4 & mat )
00677 {
00678     Transform3 affineMat;
00679     affineMat.setCol0( mat.getCol0().getXYZ( ) );
00680     affineMat.setCol1( mat.getCol1().getXYZ( ) );
00681     affineMat.setCol2( mat.getCol2().getXYZ( ) );
00682     affineMat.setCol3( mat.getCol3().getXYZ( ) );
00683     return Matrix4( orthoInverse( affineMat ) );
00684 }
00685 
00686 inline float determinant( const Matrix4 & mat )
00687 {
00688     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;
00689     mA = mat.getCol0().getX();
00690     mB = mat.getCol0().getY();
00691     mC = mat.getCol0().getZ();
00692     mD = mat.getCol0().getW();
00693     mE = mat.getCol1().getX();
00694     mF = mat.getCol1().getY();
00695     mG = mat.getCol1().getZ();
00696     mH = mat.getCol1().getW();
00697     mI = mat.getCol2().getX();
00698     mJ = mat.getCol2().getY();
00699     mK = mat.getCol2().getZ();
00700     mL = mat.getCol2().getW();
00701     mM = mat.getCol3().getX();
00702     mN = mat.getCol3().getY();
00703     mO = mat.getCol3().getZ();
00704     mP = mat.getCol3().getW();
00705     tmp0 = ( ( mK * mD ) - ( mC * mL ) );
00706     tmp1 = ( ( mO * mH ) - ( mG * mP ) );
00707     tmp2 = ( ( mB * mK ) - ( mJ * mC ) );
00708     tmp3 = ( ( mF * mO ) - ( mN * mG ) );
00709     tmp4 = ( ( mJ * mD ) - ( mB * mL ) );
00710     tmp5 = ( ( mN * mH ) - ( mF * mP ) );
00711     dx = ( ( ( mJ * tmp1 ) - ( mL * tmp3 ) ) - ( mK * tmp5 ) );
00712     dy = ( ( ( mN * tmp0 ) - ( mP * tmp2 ) ) - ( mO * tmp4 ) );
00713     dz = ( ( ( mD * tmp3 ) + ( mC * tmp5 ) ) - ( mB * tmp1 ) );
00714     dw = ( ( ( mH * tmp2 ) + ( mG * tmp4 ) ) - ( mF * tmp0 ) );
00715     return ( ( ( ( mA * dx ) + ( mE * dy ) ) + ( mI * dz ) ) + ( mM * dw ) );
00716 }
00717 
00718 inline const Matrix4 Matrix4::operator +( const Matrix4 & mat ) const
00719 {
00720     return Matrix4(
00721         ( mCol0 + mat.mCol0 ),
00722         ( mCol1 + mat.mCol1 ),
00723         ( mCol2 + mat.mCol2 ),
00724         ( mCol3 + mat.mCol3 )
00725     );
00726 }
00727 
00728 inline const Matrix4 Matrix4::operator -( const Matrix4 & mat ) const
00729 {
00730     return Matrix4(
00731         ( mCol0 - mat.mCol0 ),
00732         ( mCol1 - mat.mCol1 ),
00733         ( mCol2 - mat.mCol2 ),
00734         ( mCol3 - mat.mCol3 )
00735     );
00736 }
00737 
00738 inline Matrix4 & Matrix4::operator +=( const Matrix4 & mat )
00739 {
00740     *this = *this + mat;
00741     return *this;
00742 }
00743 
00744 inline Matrix4 & Matrix4::operator -=( const Matrix4 & mat )
00745 {
00746     *this = *this - mat;
00747     return *this;
00748 }
00749 
00750 inline const Matrix4 Matrix4::operator -( ) const
00751 {
00752     return Matrix4(
00753         ( -mCol0 ),
00754         ( -mCol1 ),
00755         ( -mCol2 ),
00756         ( -mCol3 )
00757     );
00758 }
00759 
00760 inline const Matrix4 absPerElem( const Matrix4 & mat )
00761 {
00762     return Matrix4(
00763         absPerElem( mat.getCol0() ),
00764         absPerElem( mat.getCol1() ),
00765         absPerElem( mat.getCol2() ),
00766         absPerElem( mat.getCol3() )
00767     );
00768 }
00769 
00770 inline const Matrix4 Matrix4::operator *( float scalar ) const
00771 {
00772     return Matrix4(
00773         ( mCol0 * scalar ),
00774         ( mCol1 * scalar ),
00775         ( mCol2 * scalar ),
00776         ( mCol3 * scalar )
00777     );
00778 }
00779 
00780 inline Matrix4 & Matrix4::operator *=( float scalar )
00781 {
00782     *this = *this * scalar;
00783     return *this;
00784 }
00785 
00786 inline const Matrix4 operator *( float scalar, const Matrix4 & mat )
00787 {
00788     return mat * scalar;
00789 }
00790 
00791 inline const Vector4 Matrix4::operator *( const Vector4 & vec ) const
00792 {
00793     return Vector4(
00794         ( ( ( ( mCol0.getX() * vec.getX() ) + ( mCol1.getX() * vec.getY() ) ) + ( mCol2.getX() * vec.getZ() ) ) + ( mCol3.getX() * vec.getW() ) ),
00795         ( ( ( ( mCol0.getY() * vec.getX() ) + ( mCol1.getY() * vec.getY() ) ) + ( mCol2.getY() * vec.getZ() ) ) + ( mCol3.getY() * vec.getW() ) ),
00796         ( ( ( ( mCol0.getZ() * vec.getX() ) + ( mCol1.getZ() * vec.getY() ) ) + ( mCol2.getZ() * vec.getZ() ) ) + ( mCol3.getZ() * vec.getW() ) ),
00797         ( ( ( ( mCol0.getW() * vec.getX() ) + ( mCol1.getW() * vec.getY() ) ) + ( mCol2.getW() * vec.getZ() ) ) + ( mCol3.getW() * vec.getW() ) )
00798     );
00799 }
00800 
00801 inline const Vector4 Matrix4::operator *( const Vector3 & vec ) const
00802 {
00803     return Vector4(
00804         ( ( ( mCol0.getX() * vec.getX() ) + ( mCol1.getX() * vec.getY() ) ) + ( mCol2.getX() * vec.getZ() ) ),
00805         ( ( ( mCol0.getY() * vec.getX() ) + ( mCol1.getY() * vec.getY() ) ) + ( mCol2.getY() * vec.getZ() ) ),
00806         ( ( ( mCol0.getZ() * vec.getX() ) + ( mCol1.getZ() * vec.getY() ) ) + ( mCol2.getZ() * vec.getZ() ) ),
00807         ( ( ( mCol0.getW() * vec.getX() ) + ( mCol1.getW() * vec.getY() ) ) + ( mCol2.getW() * vec.getZ() ) )
00808     );
00809 }
00810 
00811 inline const Vector4 Matrix4::operator *( const Point3 & pnt ) const
00812 {
00813     return Vector4(
00814         ( ( ( ( mCol0.getX() * pnt.getX() ) + ( mCol1.getX() * pnt.getY() ) ) + ( mCol2.getX() * pnt.getZ() ) ) + mCol3.getX() ),
00815         ( ( ( ( mCol0.getY() * pnt.getX() ) + ( mCol1.getY() * pnt.getY() ) ) + ( mCol2.getY() * pnt.getZ() ) ) + mCol3.getY() ),
00816         ( ( ( ( mCol0.getZ() * pnt.getX() ) + ( mCol1.getZ() * pnt.getY() ) ) + ( mCol2.getZ() * pnt.getZ() ) ) + mCol3.getZ() ),
00817         ( ( ( ( mCol0.getW() * pnt.getX() ) + ( mCol1.getW() * pnt.getY() ) ) + ( mCol2.getW() * pnt.getZ() ) ) + mCol3.getW() )
00818     );
00819 }
00820 
00821 inline const Matrix4 Matrix4::operator *( const Matrix4 & mat ) const
00822 {
00823     return Matrix4(
00824         ( *this * mat.mCol0 ),
00825         ( *this * mat.mCol1 ),
00826         ( *this * mat.mCol2 ),
00827         ( *this * mat.mCol3 )
00828     );
00829 }
00830 
00831 inline Matrix4 & Matrix4::operator *=( const Matrix4 & mat )
00832 {
00833     *this = *this * mat;
00834     return *this;
00835 }
00836 
00837 inline const Matrix4 Matrix4::operator *( const Transform3 & tfrm ) const
00838 {
00839     return Matrix4(
00840         ( *this * tfrm.getCol0() ),
00841         ( *this * tfrm.getCol1() ),
00842         ( *this * tfrm.getCol2() ),
00843         ( *this * Point3( tfrm.getCol3() ) )
00844     );
00845 }
00846 
00847 inline Matrix4 & Matrix4::operator *=( const Transform3 & tfrm )
00848 {
00849     *this = *this * tfrm;
00850     return *this;
00851 }
00852 
00853 inline const Matrix4 mulPerElem( const Matrix4 & mat0, const Matrix4 & mat1 )
00854 {
00855     return Matrix4(
00856         mulPerElem( mat0.getCol0(), mat1.getCol0() ),
00857         mulPerElem( mat0.getCol1(), mat1.getCol1() ),
00858         mulPerElem( mat0.getCol2(), mat1.getCol2() ),
00859         mulPerElem( mat0.getCol3(), mat1.getCol3() )
00860     );
00861 }
00862 
00863 inline const Matrix4 Matrix4::identity( )
00864 {
00865     return Matrix4(
00866         Vector4::xAxis( ),
00867         Vector4::yAxis( ),
00868         Vector4::zAxis( ),
00869         Vector4::wAxis( )
00870     );
00871 }
00872 
00873 inline Matrix4 & Matrix4::setUpper3x3( const Matrix3 & mat3 )
00874 {
00875     mCol0.setXYZ( mat3.getCol0() );
00876     mCol1.setXYZ( mat3.getCol1() );
00877     mCol2.setXYZ( mat3.getCol2() );
00878     return *this;
00879 }
00880 
00881 inline const Matrix3 Matrix4::getUpper3x3( ) const
00882 {
00883     return Matrix3(
00884         mCol0.getXYZ( ),
00885         mCol1.getXYZ( ),
00886         mCol2.getXYZ( )
00887     );
00888 }
00889 
00890 inline Matrix4 & Matrix4::setTranslation( const Vector3 & translateVec )
00891 {
00892     mCol3.setXYZ( translateVec );
00893     return *this;
00894 }
00895 
00896 inline const Vector3 Matrix4::getTranslation( ) const
00897 {
00898     return mCol3.getXYZ( );
00899 }
00900 
00901 inline const Matrix4 Matrix4::rotationX( float radians )
00902 {
00903     float s, c;
00904     s = sinf( radians );
00905     c = cosf( radians );
00906     return Matrix4(
00907         Vector4::xAxis( ),
00908         Vector4( 0.0f, c, s, 0.0f ),
00909         Vector4( 0.0f, -s, c, 0.0f ),
00910         Vector4::wAxis( )
00911     );
00912 }
00913 
00914 inline const Matrix4 Matrix4::rotationY( float radians )
00915 {
00916     float s, c;
00917     s = sinf( radians );
00918     c = cosf( radians );
00919     return Matrix4(
00920         Vector4( c, 0.0f, -s, 0.0f ),
00921         Vector4::yAxis( ),
00922         Vector4( s, 0.0f, c, 0.0f ),
00923         Vector4::wAxis( )
00924     );
00925 }
00926 
00927 inline const Matrix4 Matrix4::rotationZ( float radians )
00928 {
00929     float s, c;
00930     s = sinf( radians );
00931     c = cosf( radians );
00932     return Matrix4(
00933         Vector4( c, s, 0.0f, 0.0f ),
00934         Vector4( -s, c, 0.0f, 0.0f ),
00935         Vector4::zAxis( ),
00936         Vector4::wAxis( )
00937     );
00938 }
00939 
00940 inline const Matrix4 Matrix4::rotationZYX( const Vector3 & radiansXYZ )
00941 {
00942     float sX, cX, sY, cY, sZ, cZ, tmp0, tmp1;
00943     sX = sinf( radiansXYZ.getX() );
00944     cX = cosf( radiansXYZ.getX() );
00945     sY = sinf( radiansXYZ.getY() );
00946     cY = cosf( radiansXYZ.getY() );
00947     sZ = sinf( radiansXYZ.getZ() );
00948     cZ = cosf( radiansXYZ.getZ() );
00949     tmp0 = ( cZ * sY );
00950     tmp1 = ( sZ * sY );
00951     return Matrix4(
00952         Vector4( ( cZ * cY ), ( sZ * cY ), -sY, 0.0f ),
00953         Vector4( ( ( tmp0 * sX ) - ( sZ * cX ) ), ( ( tmp1 * sX ) + ( cZ * cX ) ), ( cY * sX ), 0.0f ),
00954         Vector4( ( ( tmp0 * cX ) + ( sZ * sX ) ), ( ( tmp1 * cX ) - ( cZ * sX ) ), ( cY * cX ), 0.0f ),
00955         Vector4::wAxis( )
00956     );
00957 }
00958 
00959 inline const Matrix4 Matrix4::rotation( float radians, const Vector3 & unitVec )
00960 {
00961     float x, y, z, s, c, oneMinusC, xy, yz, zx;
00962     s = sinf( radians );
00963     c = cosf( radians );
00964     x = unitVec.getX();
00965     y = unitVec.getY();
00966     z = unitVec.getZ();
00967     xy = ( x * y );
00968     yz = ( y * z );
00969     zx = ( z * x );
00970     oneMinusC = ( 1.0f - c );
00971     return Matrix4(
00972         Vector4( ( ( ( x * x ) * oneMinusC ) + c ), ( ( xy * oneMinusC ) + ( z * s ) ), ( ( zx * oneMinusC ) - ( y * s ) ), 0.0f ),
00973         Vector4( ( ( xy * oneMinusC ) - ( z * s ) ), ( ( ( y * y ) * oneMinusC ) + c ), ( ( yz * oneMinusC ) + ( x * s ) ), 0.0f ),
00974         Vector4( ( ( zx * oneMinusC ) + ( y * s ) ), ( ( yz * oneMinusC ) - ( x * s ) ), ( ( ( z * z ) * oneMinusC ) + c ), 0.0f ),
00975         Vector4::wAxis( )
00976     );
00977 }
00978 
00979 inline const Matrix4 Matrix4::rotation( const Quat & unitQuat )
00980 {
00981     return Matrix4( Transform3::rotation( unitQuat ) );
00982 }
00983 
00984 inline const Matrix4 Matrix4::scale( const Vector3 & scaleVec )
00985 {
00986     return Matrix4(
00987         Vector4( scaleVec.getX(), 0.0f, 0.0f, 0.0f ),
00988         Vector4( 0.0f, scaleVec.getY(), 0.0f, 0.0f ),
00989         Vector4( 0.0f, 0.0f, scaleVec.getZ(), 0.0f ),
00990         Vector4::wAxis( )
00991     );
00992 }
00993 
00994 inline const Matrix4 appendScale( const Matrix4 & mat, const Vector3 & scaleVec )
00995 {
00996     return Matrix4(
00997         ( mat.getCol0() * scaleVec.getX( ) ),
00998         ( mat.getCol1() * scaleVec.getY( ) ),
00999         ( mat.getCol2() * scaleVec.getZ( ) ),
01000         mat.getCol3()
01001     );
01002 }
01003 
01004 inline const Matrix4 prependScale( const Vector3 & scaleVec, const Matrix4 & mat )
01005 {
01006     Vector4 scale4;
01007     scale4 = Vector4( scaleVec, 1.0f );
01008     return Matrix4(
01009         mulPerElem( mat.getCol0(), scale4 ),
01010         mulPerElem( mat.getCol1(), scale4 ),
01011         mulPerElem( mat.getCol2(), scale4 ),
01012         mulPerElem( mat.getCol3(), scale4 )
01013     );
01014 }
01015 
01016 inline const Matrix4 Matrix4::translation( const Vector3 & translateVec )
01017 {
01018     return Matrix4(
01019         Vector4::xAxis( ),
01020         Vector4::yAxis( ),
01021         Vector4::zAxis( ),
01022         Vector4( translateVec, 1.0f )
01023     );
01024 }
01025 
01026 inline const Matrix4 Matrix4::lookAt( const Point3 & eyePos, const Point3 & lookAtPos, const Vector3 & upVec )
01027 {
01028     Matrix4 m4EyeFrame;
01029     Vector3 v3X, v3Y, v3Z;
01030     v3Y = normalize( upVec );
01031     v3Z = normalize( ( eyePos - lookAtPos ) );
01032     v3X = normalize( cross( v3Y, v3Z ) );
01033     v3Y = cross( v3Z, v3X );
01034     m4EyeFrame = Matrix4( Vector4( v3X ), Vector4( v3Y ), Vector4( v3Z ), Vector4( eyePos ) );
01035     return orthoInverse( m4EyeFrame );
01036 }
01037 
01038 inline const Matrix4 Matrix4::perspective( float fovyRadians, float aspect, float zNear, float zFar )
01039 {
01040     float f, rangeInv;
01041     f = tanf( ( (float)( _VECTORMATH_PI_OVER_2 ) - ( 0.5f * fovyRadians ) ) );
01042     rangeInv = ( 1.0f / ( zNear - zFar ) );
01043     return Matrix4(
01044         Vector4( ( f / aspect ), 0.0f, 0.0f, 0.0f ),
01045         Vector4( 0.0f, f, 0.0f, 0.0f ),
01046         Vector4( 0.0f, 0.0f, ( ( zNear + zFar ) * rangeInv ), -1.0f ),
01047         Vector4( 0.0f, 0.0f, ( ( ( zNear * zFar ) * rangeInv ) * 2.0f ), 0.0f )
01048     );
01049 }
01050 
01051 inline const Matrix4 Matrix4::frustum( float left, float right, float bottom, float top, float zNear, float zFar )
01052 {
01053     float sum_rl, sum_tb, sum_nf, inv_rl, inv_tb, inv_nf, n2;
01054     sum_rl = ( right + left );
01055     sum_tb = ( top + bottom );
01056     sum_nf = ( zNear + zFar );
01057     inv_rl = ( 1.0f / ( right - left ) );
01058     inv_tb = ( 1.0f / ( top - bottom ) );
01059     inv_nf = ( 1.0f / ( zNear - zFar ) );
01060     n2 = ( zNear + zNear );
01061     return Matrix4(
01062         Vector4( ( n2 * inv_rl ), 0.0f, 0.0f, 0.0f ),
01063         Vector4( 0.0f, ( n2 * inv_tb ), 0.0f, 0.0f ),
01064         Vector4( ( sum_rl * inv_rl ), ( sum_tb * inv_tb ), ( sum_nf * inv_nf ), -1.0f ),
01065         Vector4( 0.0f, 0.0f, ( ( n2 * inv_nf ) * zFar ), 0.0f )
01066     );
01067 }
01068 
01069 inline const Matrix4 Matrix4::orthographic( float left, float right, float bottom, float top, float zNear, float zFar )
01070 {
01071     float sum_rl, sum_tb, sum_nf, inv_rl, inv_tb, inv_nf;
01072     sum_rl = ( right + left );
01073     sum_tb = ( top + bottom );
01074     sum_nf = ( zNear + zFar );
01075     inv_rl = ( 1.0f / ( right - left ) );
01076     inv_tb = ( 1.0f / ( top - bottom ) );
01077     inv_nf = ( 1.0f / ( zNear - zFar ) );
01078     return Matrix4(
01079         Vector4( ( inv_rl + inv_rl ), 0.0f, 0.0f, 0.0f ),
01080         Vector4( 0.0f, ( inv_tb + inv_tb ), 0.0f, 0.0f ),
01081         Vector4( 0.0f, 0.0f, ( inv_nf + inv_nf ), 0.0f ),
01082         Vector4( ( -sum_rl * inv_rl ), ( -sum_tb * inv_tb ), ( sum_nf * inv_nf ), 1.0f )
01083     );
01084 }
01085 
01086 inline const Matrix4 select( const Matrix4 & mat0, const Matrix4 & mat1, bool select1 )
01087 {
01088     return Matrix4(
01089         select( mat0.getCol0(), mat1.getCol0(), select1 ),
01090         select( mat0.getCol1(), mat1.getCol1(), select1 ),
01091         select( mat0.getCol2(), mat1.getCol2(), select1 ),
01092         select( mat0.getCol3(), mat1.getCol3(), select1 )
01093     );
01094 }
01095 
01096 #ifdef _VECTORMATH_DEBUG
01097 
01098 inline void print( const Matrix4 & mat )
01099 {
01100     print( mat.getRow( 0 ) );
01101     print( mat.getRow( 1 ) );
01102     print( mat.getRow( 2 ) );
01103     print( mat.getRow( 3 ) );
01104 }
01105 
01106 inline void print( const Matrix4 & mat, const char * name )
01107 {
01108     printf("%s:\n", name);
01109     print( mat );
01110 }
01111 
01112 #endif
01113 
01114 inline Transform3::Transform3( const Transform3 & tfrm )
01115 {
01116     mCol0 = tfrm.mCol0;
01117     mCol1 = tfrm.mCol1;
01118     mCol2 = tfrm.mCol2;
01119     mCol3 = tfrm.mCol3;
01120 }
01121 
01122 inline Transform3::Transform3( float scalar )
01123 {
01124     mCol0 = Vector3( scalar );
01125     mCol1 = Vector3( scalar );
01126     mCol2 = Vector3( scalar );
01127     mCol3 = Vector3( scalar );
01128 }
01129 
01130 inline Transform3::Transform3( const Vector3 & _col0, const Vector3 & _col1, const Vector3 & _col2, const Vector3 & _col3 )
01131 {
01132     mCol0 = _col0;
01133     mCol1 = _col1;
01134     mCol2 = _col2;
01135     mCol3 = _col3;
01136 }
01137 
01138 inline Transform3::Transform3( const Matrix3 & tfrm, const Vector3 & translateVec )
01139 {
01140     this->setUpper3x3( tfrm );
01141     this->setTranslation( translateVec );
01142 }
01143 
01144 inline Transform3::Transform3( const Quat & unitQuat, const Vector3 & translateVec )
01145 {
01146     this->setUpper3x3( Matrix3( unitQuat ) );
01147     this->setTranslation( translateVec );
01148 }
01149 
01150 inline Transform3 & Transform3::setCol0( const Vector3 & _col0 )
01151 {
01152     mCol0 = _col0;
01153     return *this;
01154 }
01155 
01156 inline Transform3 & Transform3::setCol1( const Vector3 & _col1 )
01157 {
01158     mCol1 = _col1;
01159     return *this;
01160 }
01161 
01162 inline Transform3 & Transform3::setCol2( const Vector3 & _col2 )
01163 {
01164     mCol2 = _col2;
01165     return *this;
01166 }
01167 
01168 inline Transform3 & Transform3::setCol3( const Vector3 & _col3 )
01169 {
01170     mCol3 = _col3;
01171     return *this;
01172 }
01173 
01174 inline Transform3 & Transform3::setCol( int col, const Vector3 & vec )
01175 {
01176     *(&mCol0 + col) = vec;
01177     return *this;
01178 }
01179 
01180 inline Transform3 & Transform3::setRow( int row, const Vector4 & vec )
01181 {
01182     mCol0.setElem( row, vec.getElem( 0 ) );
01183     mCol1.setElem( row, vec.getElem( 1 ) );
01184     mCol2.setElem( row, vec.getElem( 2 ) );
01185     mCol3.setElem( row, vec.getElem( 3 ) );
01186     return *this;
01187 }
01188 
01189 inline Transform3 & Transform3::setElem( int col, int row, float val )
01190 {
01191     Vector3 tmpV3_0;
01192     tmpV3_0 = this->getCol( col );
01193     tmpV3_0.setElem( row, val );
01194     this->setCol( col, tmpV3_0 );
01195     return *this;
01196 }
01197 
01198 inline float Transform3::getElem( int col, int row ) const
01199 {
01200     return this->getCol( col ).getElem( row );
01201 }
01202 
01203 inline const Vector3 Transform3::getCol0( ) const
01204 {
01205     return mCol0;
01206 }
01207 
01208 inline const Vector3 Transform3::getCol1( ) const
01209 {
01210     return mCol1;
01211 }
01212 
01213 inline const Vector3 Transform3::getCol2( ) const
01214 {
01215     return mCol2;
01216 }
01217 
01218 inline const Vector3 Transform3::getCol3( ) const
01219 {
01220     return mCol3;
01221 }
01222 
01223 inline const Vector3 Transform3::getCol( int col ) const
01224 {
01225     return *(&mCol0 + col);
01226 }
01227 
01228 inline const Vector4 Transform3::getRow( int row ) const
01229 {
01230     return Vector4( mCol0.getElem( row ), mCol1.getElem( row ), mCol2.getElem( row ), mCol3.getElem( row ) );
01231 }
01232 
01233 inline Vector3 & Transform3::operator []( int col )
01234 {
01235     return *(&mCol0 + col);
01236 }
01237 
01238 inline const Vector3 Transform3::operator []( int col ) const
01239 {
01240     return *(&mCol0 + col);
01241 }
01242 
01243 inline Transform3 & Transform3::operator =( const Transform3 & tfrm )
01244 {
01245     mCol0 = tfrm.mCol0;
01246     mCol1 = tfrm.mCol1;
01247     mCol2 = tfrm.mCol2;
01248     mCol3 = tfrm.mCol3;
01249     return *this;
01250 }
01251 
01252 inline const Transform3 inverse( const Transform3 & tfrm )
01253 {
01254     Vector3 tmp0, tmp1, tmp2, inv0, inv1, inv2;
01255     float detinv;
01256     tmp0 = cross( tfrm.getCol1(), tfrm.getCol2() );
01257     tmp1 = cross( tfrm.getCol2(), tfrm.getCol0() );
01258     tmp2 = cross( tfrm.getCol0(), tfrm.getCol1() );
01259     detinv = ( 1.0f / dot( tfrm.getCol2(), tmp2 ) );
01260     inv0 = Vector3( ( tmp0.getX() * detinv ), ( tmp1.getX() * detinv ), ( tmp2.getX() * detinv ) );
01261     inv1 = Vector3( ( tmp0.getY() * detinv ), ( tmp1.getY() * detinv ), ( tmp2.getY() * detinv ) );
01262     inv2 = Vector3( ( tmp0.getZ() * detinv ), ( tmp1.getZ() * detinv ), ( tmp2.getZ() * detinv ) );
01263     return Transform3(
01264         inv0,
01265         inv1,
01266         inv2,
01267         Vector3( ( -( ( inv0 * tfrm.getCol3().getX() ) + ( ( inv1 * tfrm.getCol3().getY() ) + ( inv2 * tfrm.getCol3().getZ() ) ) ) ) )
01268     );
01269 }
01270 
01271 inline const Transform3 orthoInverse( const Transform3 & tfrm )
01272 {
01273     Vector3 inv0, inv1, inv2;
01274     inv0 = Vector3( tfrm.getCol0().getX(), tfrm.getCol1().getX(), tfrm.getCol2().getX() );
01275     inv1 = Vector3( tfrm.getCol0().getY(), tfrm.getCol1().getY(), tfrm.getCol2().getY() );
01276     inv2 = Vector3( tfrm.getCol0().getZ(), tfrm.getCol1().getZ(), tfrm.getCol2().getZ() );
01277     return Transform3(
01278         inv0,
01279         inv1,
01280         inv2,
01281         Vector3( ( -( ( inv0 * tfrm.getCol3().getX() ) + ( ( inv1 * tfrm.getCol3().getY() ) + ( inv2 * tfrm.getCol3().getZ() ) ) ) ) )
01282     );
01283 }
01284 
01285 inline const Transform3 absPerElem( const Transform3 & tfrm )
01286 {
01287     return Transform3(
01288         absPerElem( tfrm.getCol0() ),
01289         absPerElem( tfrm.getCol1() ),
01290         absPerElem( tfrm.getCol2() ),
01291         absPerElem( tfrm.getCol3() )
01292     );
01293 }
01294 
01295 inline const Vector3 Transform3::operator *( const Vector3 & vec ) const
01296 {
01297     return Vector3(
01298         ( ( ( mCol0.getX() * vec.getX() ) + ( mCol1.getX() * vec.getY() ) ) + ( mCol2.getX() * vec.getZ() ) ),
01299         ( ( ( mCol0.getY() * vec.getX() ) + ( mCol1.getY() * vec.getY() ) ) + ( mCol2.getY() * vec.getZ() ) ),
01300         ( ( ( mCol0.getZ() * vec.getX() ) + ( mCol1.getZ() * vec.getY() ) ) + ( mCol2.getZ() * vec.getZ() ) )
01301     );
01302 }
01303 
01304 inline const Point3 Transform3::operator *( const Point3 & pnt ) const
01305 {
01306     return Point3(
01307         ( ( ( ( mCol0.getX() * pnt.getX() ) + ( mCol1.getX() * pnt.getY() ) ) + ( mCol2.getX() * pnt.getZ() ) ) + mCol3.getX() ),
01308         ( ( ( ( mCol0.getY() * pnt.getX() ) + ( mCol1.getY() * pnt.getY() ) ) + ( mCol2.getY() * pnt.getZ() ) ) + mCol3.getY() ),
01309         ( ( ( ( mCol0.getZ() * pnt.getX() ) + ( mCol1.getZ() * pnt.getY() ) ) + ( mCol2.getZ() * pnt.getZ() ) ) + mCol3.getZ() )
01310     );
01311 }
01312 
01313 inline const Transform3 Transform3::operator *( const Transform3 & tfrm ) const
01314 {
01315     return Transform3(
01316         ( *this * tfrm.mCol0 ),
01317         ( *this * tfrm.mCol1 ),
01318         ( *this * tfrm.mCol2 ),
01319         Vector3( ( *this * Point3( tfrm.mCol3 ) ) )
01320     );
01321 }
01322 
01323 inline Transform3 & Transform3::operator *=( const Transform3 & tfrm )
01324 {
01325     *this = *this * tfrm;
01326     return *this;
01327 }
01328 
01329 inline const Transform3 mulPerElem( const Transform3 & tfrm0, const Transform3 & tfrm1 )
01330 {
01331     return Transform3(
01332         mulPerElem( tfrm0.getCol0(), tfrm1.getCol0() ),
01333         mulPerElem( tfrm0.getCol1(), tfrm1.getCol1() ),
01334         mulPerElem( tfrm0.getCol2(), tfrm1.getCol2() ),
01335         mulPerElem( tfrm0.getCol3(), tfrm1.getCol3() )
01336     );
01337 }
01338 
01339 inline const Transform3 Transform3::identity( )
01340 {
01341     return Transform3(
01342         Vector3::xAxis( ),
01343         Vector3::yAxis( ),
01344         Vector3::zAxis( ),
01345         Vector3( 0.0f )
01346     );
01347 }
01348 
01349 inline Transform3 & Transform3::setUpper3x3( const Matrix3 & tfrm )
01350 {
01351     mCol0 = tfrm.getCol0();
01352     mCol1 = tfrm.getCol1();
01353     mCol2 = tfrm.getCol2();
01354     return *this;
01355 }
01356 
01357 inline const Matrix3 Transform3::getUpper3x3( ) const
01358 {
01359     return Matrix3( mCol0, mCol1, mCol2 );
01360 }
01361 
01362 inline Transform3 & Transform3::setTranslation( const Vector3 & translateVec )
01363 {
01364     mCol3 = translateVec;
01365     return *this;
01366 }
01367 
01368 inline const Vector3 Transform3::getTranslation( ) const
01369 {
01370     return mCol3;
01371 }
01372 
01373 inline const Transform3 Transform3::rotationX( float radians )
01374 {
01375     float s, c;
01376     s = sinf( radians );
01377     c = cosf( radians );
01378     return Transform3(
01379         Vector3::xAxis( ),
01380         Vector3( 0.0f, c, s ),
01381         Vector3( 0.0f, -s, c ),
01382         Vector3( 0.0f )
01383     );
01384 }
01385 
01386 inline const Transform3 Transform3::rotationY( float radians )
01387 {
01388     float s, c;
01389     s = sinf( radians );
01390     c = cosf( radians );
01391     return Transform3(
01392         Vector3( c, 0.0f, -s ),
01393         Vector3::yAxis( ),
01394         Vector3( s, 0.0f, c ),
01395         Vector3( 0.0f )
01396     );
01397 }
01398 
01399 inline const Transform3 Transform3::rotationZ( float radians )
01400 {
01401     float s, c;
01402     s = sinf( radians );
01403     c = cosf( radians );
01404     return Transform3(
01405         Vector3( c, s, 0.0f ),
01406         Vector3( -s, c, 0.0f ),
01407         Vector3::zAxis( ),
01408         Vector3( 0.0f )
01409     );
01410 }
01411 
01412 inline const Transform3 Transform3::rotationZYX( const Vector3 & radiansXYZ )
01413 {
01414     float sX, cX, sY, cY, sZ, cZ, tmp0, tmp1;
01415     sX = sinf( radiansXYZ.getX() );
01416     cX = cosf( radiansXYZ.getX() );
01417     sY = sinf( radiansXYZ.getY() );
01418     cY = cosf( radiansXYZ.getY() );
01419     sZ = sinf( radiansXYZ.getZ() );
01420     cZ = cosf( radiansXYZ.getZ() );
01421     tmp0 = ( cZ * sY );
01422     tmp1 = ( sZ * sY );
01423     return Transform3(
01424         Vector3( ( cZ * cY ), ( sZ * cY ), -sY ),
01425         Vector3( ( ( tmp0 * sX ) - ( sZ * cX ) ), ( ( tmp1 * sX ) + ( cZ * cX ) ), ( cY * sX ) ),
01426         Vector3( ( ( tmp0 * cX ) + ( sZ * sX ) ), ( ( tmp1 * cX ) - ( cZ * sX ) ), ( cY * cX ) ),
01427         Vector3( 0.0f )
01428     );
01429 }
01430 
01431 inline const Transform3 Transform3::rotation( float radians, const Vector3 & unitVec )
01432 {
01433     return Transform3( Matrix3::rotation( radians, unitVec ), Vector3( 0.0f ) );
01434 }
01435 
01436 inline const Transform3 Transform3::rotation( const Quat & unitQuat )
01437 {
01438     return Transform3( Matrix3( unitQuat ), Vector3( 0.0f ) );
01439 }
01440 
01441 inline const Transform3 Transform3::scale( const Vector3 & scaleVec )
01442 {
01443     return Transform3(
01444         Vector3( scaleVec.getX(), 0.0f, 0.0f ),
01445         Vector3( 0.0f, scaleVec.getY(), 0.0f ),
01446         Vector3( 0.0f, 0.0f, scaleVec.getZ() ),
01447         Vector3( 0.0f )
01448     );
01449 }
01450 
01451 inline const Transform3 appendScale( const Transform3 & tfrm, const Vector3 & scaleVec )
01452 {
01453     return Transform3(
01454         ( tfrm.getCol0() * scaleVec.getX( ) ),
01455         ( tfrm.getCol1() * scaleVec.getY( ) ),
01456         ( tfrm.getCol2() * scaleVec.getZ( ) ),
01457         tfrm.getCol3()
01458     );
01459 }
01460 
01461 inline const Transform3 prependScale( const Vector3 & scaleVec, const Transform3 & tfrm )
01462 {
01463     return Transform3(
01464         mulPerElem( tfrm.getCol0(), scaleVec ),
01465         mulPerElem( tfrm.getCol1(), scaleVec ),
01466         mulPerElem( tfrm.getCol2(), scaleVec ),
01467         mulPerElem( tfrm.getCol3(), scaleVec )
01468     );
01469 }
01470 
01471 inline const Transform3 Transform3::translation( const Vector3 & translateVec )
01472 {
01473     return Transform3(
01474         Vector3::xAxis( ),
01475         Vector3::yAxis( ),
01476         Vector3::zAxis( ),
01477         translateVec
01478     );
01479 }
01480 
01481 inline const Transform3 select( const Transform3 & tfrm0, const Transform3 & tfrm1, bool select1 )
01482 {
01483     return Transform3(
01484         select( tfrm0.getCol0(), tfrm1.getCol0(), select1 ),
01485         select( tfrm0.getCol1(), tfrm1.getCol1(), select1 ),
01486         select( tfrm0.getCol2(), tfrm1.getCol2(), select1 ),
01487         select( tfrm0.getCol3(), tfrm1.getCol3(), select1 )
01488     );
01489 }
01490 
01491 #ifdef _VECTORMATH_DEBUG
01492 
01493 inline void print( const Transform3 & tfrm )
01494 {
01495     print( tfrm.getRow( 0 ) );
01496     print( tfrm.getRow( 1 ) );
01497     print( tfrm.getRow( 2 ) );
01498 }
01499 
01500 inline void print( const Transform3 & tfrm, const char * name )
01501 {
01502     printf("%s:\n", name);
01503     print( tfrm );
01504 }
01505 
01506 #endif
01507 
01508 inline Quat::Quat( const Matrix3 & tfrm )
01509 {
01510     float trace, radicand, scale, xx, yx, zx, xy, yy, zy, xz, yz, zz, tmpx, tmpy, tmpz, tmpw, qx, qy, qz, qw;
01511     int negTrace, ZgtX, ZgtY, YgtX;
01512     int largestXorY, largestYorZ, largestZorX;
01513 
01514     xx = tfrm.getCol0().getX();
01515     yx = tfrm.getCol0().getY();
01516     zx = tfrm.getCol0().getZ();
01517     xy = tfrm.getCol1().getX();
01518     yy = tfrm.getCol1().getY();
01519     zy = tfrm.getCol1().getZ();
01520     xz = tfrm.getCol2().getX();
01521     yz = tfrm.getCol2().getY();
01522     zz = tfrm.getCol2().getZ();
01523 
01524     trace = ( ( xx + yy ) + zz );
01525 
01526     negTrace = ( trace < 0.0f );
01527     ZgtX = zz > xx;
01528     ZgtY = zz > yy;
01529     YgtX = yy > xx;
01530     largestXorY = ( !ZgtX || !ZgtY ) && negTrace;
01531     largestYorZ = ( YgtX || ZgtX ) && negTrace;
01532     largestZorX = ( ZgtY || !YgtX ) && negTrace;
01533     
01534     if ( largestXorY )
01535     {
01536         zz = -zz;
01537         xy = -xy;
01538     }
01539     if ( largestYorZ )
01540     {
01541         xx = -xx;
01542         yz = -yz;
01543     }
01544     if ( largestZorX )
01545     {
01546         yy = -yy;
01547         zx = -zx;
01548     }
01549 
01550     radicand = ( ( ( xx + yy ) + zz ) + 1.0f );
01551     scale = ( 0.5f * ( 1.0f / sqrtf( radicand ) ) );
01552 
01553     tmpx = ( ( zy - yz ) * scale );
01554     tmpy = ( ( xz - zx ) * scale );
01555     tmpz = ( ( yx - xy ) * scale );
01556     tmpw = ( radicand * scale );
01557     qx = tmpx;
01558     qy = tmpy;
01559     qz = tmpz;
01560     qw = tmpw;
01561 
01562     if ( largestXorY )
01563     {
01564         qx = tmpw;
01565         qy = tmpz;
01566         qz = tmpy;
01567         qw = tmpx;
01568     }
01569     if ( largestYorZ )
01570     {
01571         tmpx = qx;
01572         tmpz = qz;
01573         qx = qy;
01574         qy = tmpx;
01575         qz = qw;
01576         qw = tmpz;
01577     }
01578 
01579     mX = qx;
01580     mY = qy;
01581     mZ = qz;
01582     mW = qw;
01583 }
01584 
01585 inline const Matrix3 outer( const Vector3 & tfrm0, const Vector3 & tfrm1 )
01586 {
01587     return Matrix3(
01588         ( tfrm0 * tfrm1.getX( ) ),
01589         ( tfrm0 * tfrm1.getY( ) ),
01590         ( tfrm0 * tfrm1.getZ( ) )
01591     );
01592 }
01593 
01594 inline const Matrix4 outer( const Vector4 & tfrm0, const Vector4 & tfrm1 )
01595 {
01596     return Matrix4(
01597         ( tfrm0 * tfrm1.getX( ) ),
01598         ( tfrm0 * tfrm1.getY( ) ),
01599         ( tfrm0 * tfrm1.getZ( ) ),
01600         ( tfrm0 * tfrm1.getW( ) )
01601     );
01602 }
01603 
01604 inline const Vector3 rowMul( const Vector3 & vec, const Matrix3 & mat )
01605 {
01606     return Vector3(
01607         ( ( ( vec.getX() * mat.getCol0().getX() ) + ( vec.getY() * mat.getCol0().getY() ) ) + ( vec.getZ() * mat.getCol0().getZ() ) ),
01608         ( ( ( vec.getX() * mat.getCol1().getX() ) + ( vec.getY() * mat.getCol1().getY() ) ) + ( vec.getZ() * mat.getCol1().getZ() ) ),
01609         ( ( ( vec.getX() * mat.getCol2().getX() ) + ( vec.getY() * mat.getCol2().getY() ) ) + ( vec.getZ() * mat.getCol2().getZ() ) )
01610     );
01611 }
01612 
01613 inline const Matrix3 crossMatrix( const Vector3 & vec )
01614 {
01615     return Matrix3(
01616         Vector3( 0.0f, vec.getZ(), -vec.getY() ),
01617         Vector3( -vec.getZ(), 0.0f, vec.getX() ),
01618         Vector3( vec.getY(), -vec.getX(), 0.0f )
01619     );
01620 }
01621 
01622 inline const Matrix3 crossMatrixMul( const Vector3 & vec, const Matrix3 & mat )
01623 {
01624     return Matrix3( cross( vec, mat.getCol0() ), cross( vec, mat.getCol1() ), cross( vec, mat.getCol2() ) );
01625 }
01626 
01627 } // namespace Aos
01628 } // namespace Vectormath
01629 
01630 #endif