Cinder

  • Main Page
  • Related Pages
  • Namespaces
  • Classes
  • Files
  • Examples
  • File List
  • File Members

include/cinder/Matrix44.h

Go to the documentation of this file.
00001 /*
00002  Copyright (c) 2011, The Cinder Project: http://libcinder.org All rights reserved.
00003  This code is intended for use with the Cinder C++ library: http://libcinder.org
00004 
00005  Redistribution and use in source and binary forms, with or without modification, are permitted provided that
00006  the following conditions are met:
00007 
00008     * Redistributions of source code must retain the above copyright notice, this list of conditions and
00009     the following disclaimer.
00010     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
00011     the following disclaimer in the documentation and/or other materials provided with the distribution.
00012 
00013  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
00014  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
00015  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
00016  ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
00017  TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
00018  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
00019  NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00020  POSSIBILITY OF SUCH DAMAGE.
00021 */
00022 
00023 
00024 #pragma once
00025 
00026 #include "cinder/Cinder.h"
00027 #include "cinder/CinderMath.h"
00028 #include "cinder/Vector.h"
00029 
00030 #include <iomanip>
00031 
00032 namespace cinder { 
00033 
00035 // Matrix44
00036 template< typename T >
00037 class Matrix44 
00038 {
00039 public:
00040     typedef T   TYPE;
00041     typedef T   value_type;
00042     //
00043     static const size_t DIM     = 4;
00044     static const size_t DIM_SQ  = DIM*DIM;
00045     static const size_t MEM_LEN = sizeof(T)*DIM_SQ;
00046 
00047     //
00048     // This class is OpenGL friendly and stores the m as how OpenGL would expect it.
00049     // m[i,j]:
00050     // | m[0,0] m[0,1] m[0,2] m[0,2] |
00051     // | m[1,0] m[1,1] m[1,2] m[1,2] |
00052     // | m[2,0] m[2,1] m[2,2] m[2,2] |
00053     // | m[3,0] m[3,1] m[3,2] m[3,2] |
00054     //
00055     // m[idx]
00056     // | m[ 0] m[ 4] m[ 8] m[12] |
00057     // | m[ 1] m[ 5] m[ 9] m[13] |
00058     // | m[ 2] m[ 6] m[10] m[14] |
00059     // | m[ 3] m[ 7] m[11] m[15] |
00060     //
00061     union {
00062         T m[16];
00063         struct {
00064             // This looks like it's transposed from the above, but it's really not.
00065             // It just has to be written this way so it follows the right ordering
00066             // in the memory layout as well as being mathematically correct.
00067             T m00, m10, m20, m30;
00068             T m01, m11, m21, m31;
00069             T m02, m12, m22, m32;
00070             T m03, m13, m23, m33;
00071         };
00072         // [Cols][Rows]
00073         T mcols[4][4];
00074     };
00075 
00076     Matrix44();
00077     
00078     Matrix44( T s );
00079 
00080     // OpenGL layout - unless srcIsRowMajor is true
00081     Matrix44( const T *dt, bool srcIsRowMajor = false );
00082 
00083     // OpenGL layout: m[0]=d0, m[1]=d1, m[2]=d2 ... m[13]=d13, m[14]=d14, m[15]=d15 - unless srcIsRowMajor is true
00084     Matrix44( T d0, T d1, T d2, T d3, T d4, T d5, T d6, T d7, T d8, T d9, T d10, T d11, T d12, T d13, T d14, T d15, bool srcIsRowMajor = false );
00085 
00086     // Creates matrix with column vectors vx, vy, vz and vw
00087     Matrix44( const Vec3<T> &vx, const Vec3<T> &vy, const Vec3<T> &vz ); 
00088     Matrix44( const Vec4<T> &vx, const Vec4<T> &vy, const Vec4<T> &vz, const Vec4<T> &vw = Vec4<T>( 0, 0, 0, 1 ) ); 
00089 
00090     template< typename FromT >
00091     Matrix44( const Matrix44<FromT>& src );
00092 
00093     Matrix44( const Matrix22<T>& src );
00094     explicit Matrix44( const MatrixAffine2<T> &src );
00095     Matrix44( const Matrix33<T>& src );
00096 
00097     Matrix44( const Matrix44<T>& src );
00098 
00099                         operator T*() { return (T*)m; }
00100                         operator const T*() const { return (const T*)m; }
00101 
00102     Matrix44<T>&        operator=( const Matrix44<T>& rhs );
00103     Matrix44<T>&        operator=( T rhs );
00104     
00105     template< typename FromT >
00106     Matrix44<T>&        operator=( const Matrix44<FromT>& rhs );
00107     
00108     // remaining columns and rows will be filled with identity values
00109     Matrix44<T>&        operator=( const Matrix22<T>& rhs );
00110     Matrix44<T>&        operator=( const MatrixAffine2<T>& rhs );
00111     Matrix44<T>&        operator=( const Matrix33<T>& rhs );
00112 
00113     bool                equalCompare( const Matrix44<T>& rhs, T epsilon ) const;
00114     bool                operator==( const Matrix44<T> &rhs ) const { return equalCompare( rhs, (T)EPSILON ); }
00115     bool                operator!=( const Matrix44<T> &rhs ) const { return ! ( *this == rhs ); }
00116 
00117     Matrix44<T>&        operator*=( const Matrix44<T> &rhs );
00118     Matrix44<T>&        operator+=( const Matrix44<T> &rhs );
00119     Matrix44<T>&        operator-=( const Matrix44<T> &rhs );
00120 
00121     Matrix44<T>&        operator*=( T rhs );
00122     Matrix44<T>&        operator/=( T rhs );
00123     Matrix44<T>&        operator+=( T rhs );
00124     Matrix44<T>&        operator-=( T rhs );
00125 
00126     const Matrix44<T>   operator*( const Matrix44<T> &rhs ) const;
00127     const Matrix44<T>   operator+( const Matrix44<T> &rhs ) const;
00128     const Matrix44<T>   operator-( const Matrix44<T> &rhs ) const;
00129 
00130     // post-multiplies column vector [rhs.x rhs.y rhs.z 1] and divides by w
00131     const Vec3<T>       operator*( const Vec3<T> &rhs ) const;
00132 
00133     // post-multiplies column vector [rhs.x rhs.y rhs.z rhs.w]
00134     const Vec4<T>       operator*( const Vec4<T> &rhs ) const;
00135 
00136     const Matrix44<T>   operator*( T rhs ) const;
00137     const Matrix44<T>   operator/( T rhs ) const;
00138     const Matrix44<T>   operator+( T rhs ) const;
00139     const Matrix44<T>   operator-( T rhs ) const;
00140 
00141     // Accessors
00142     T&                  at( int row, int col );
00143     const T&            at( int row, int col ) const;
00144 
00145     // OpenGL layout - unless srcIsRowMajor is true
00146     void                set( const T *dt, bool srcIsRowMajor = false );
00147     // OpenGL layout: m[0]=d0, m[1]=d1, m[2]=d2 ... m[13]=d13, m[14]=d14, m[15]=d15 - unless srcIsRowMajor is true
00148     void                set( T d0, T d1, T d2, T d3, T d4, T d5, T d6, T d7, T d8, T d9, T d10, T d11, T d12, T d13, T d14, T d15, bool srcIsRowMajor = false );
00149 
00150     Vec4<T>             getColumn( int col ) const;
00151     void                setColumn( int col, const Vec4<T> &v );
00152     
00153     Vec4<T>             getRow( int row ) const;
00154     void                setRow( int row, const Vec4<T> &v );
00155 
00156     void                getColumns( Vec4<T> *c0, Vec4<T> *c1, Vec4<T> *c2, Vec4<T> *c3 ) const;
00157     void                setColumns( const Vec4<T> &c0, const Vec4<T> &c1, const Vec4<T> &c2, const Vec4<T> &c3 );
00158 
00159     void                getRows( Vec4<T> *r0, Vec4<T> *r1, Vec4<T> *r2, Vec4<T> *r3 ) const;
00160     void                setRows( const Vec4<T> &r0, const Vec4<T> &r1, const Vec4<T> &r2, const Vec4<T> &r3 );
00161 
00162     // returns a sub-matrix starting at row, col
00163     Matrix22<T>         subMatrix22( int row, int col ) const;
00164     Matrix33<T>         subMatrix33( int row, int col ) const;
00165 
00166     void                setToNull();
00167     void                setToIdentity();
00168 
00169     T                   determinant() const;
00170     // returns trace of 3x3 submatrix if fullTrace == false, otherwise returns trace of full 4x4 matrix
00171     T                   trace( bool fullTrace = false ) const;
00172 
00173     Matrix44<T>         diagonal() const;
00174 
00175     Matrix44<T>         lowerTriangular() const;
00176     Matrix44<T>         upperTriangular() const;
00177 
00178     void                transpose();
00179     Matrix44<T>         transposed() const;
00180 
00181     void                invert (T epsilon = EPSILON ) { *this = inverted( epsilon ); }
00182     Matrix44<T>         inverted( T epsilon = EPSILON ) const;
00183 
00184     // pre-multiplies row vector v - no divide by w
00185     Vec3<T>             preMultiply( const Vec3<T> &v ) const;
00186     Vec4<T>             preMultiply( const Vec4<T> &v ) const;
00187 
00188     // post-multiplies column vector v - no divide by w
00189     Vec3<T>             postMultiply( const Vec3<T> &v ) const;
00190     Vec4<T>             postMultiply( const Vec4<T> &v ) const;
00191 
00193     void                affineInvert(){ *this = affineInverted(); } 
00194     Matrix44<T>         affineInverted() const;
00195     
00197     void                orthonormalInvert();
00198     Matrix44<T>         orthonormalInverted() const { Matrix44<T> result( *this ); result.orthonormalInvert(); return result; }
00199     
00200     // post-multiplies column vector [rhs.x rhs.y rhs.z 1] and divides by w - same as operator*( const Vec3<T>& )
00201     Vec3<T>             transformPoint( const Vec3<T> &rhs ) const;
00202     
00203     // post-multiplies column vector [rhs.x rhs.y rhs.z 1] but omits divide by w
00204     Vec3<T>             transformPointAffine( const Vec3<T> &rhs ) const;
00205     
00206     // post-multiplies column vector [rhs.x rhs.y rhs.z 0]
00207     Vec3<T>             transformVec( const Vec3<T> &rhs ) const;
00208     Vec4<T>             transformVec( const Vec4<T> &rhs ) const { return transformVec( rhs.xyz() ); }
00209 
00210     // returns the translation values from the last column
00211     Vec4<T>             getTranslate() const { return Vec4<T>( m03, m13, m23, m33 ); }
00212     // sets the translation values in the last column
00213     void                setTranslate( const Vec3<T>& v ) { m03 = v.x; m13 = v.y; m23 = v.z; }
00214     void                setTranslate( const Vec4<T>& v ) { setTranslate( v.xyz() ); }
00215 
00216     // multiplies the current matrix by a translation matrix derived from tr
00217     void                translate( const Vec3<T> &tr ) { *this *= createTranslation( tr ); }
00218     void                translate( const Vec4<T> &tr ) { *this *= createTranslation( tr ); }
00219 
00220     // multiplies the current matrix by the rotation matrix derived using axis and radians
00221     void                rotate( const Vec3<T> &axis, T radians ) { *this *= createRotation( axis, radians ); }
00222     void                rotate( const Vec4<T> &axis, T radians ) { *this *= createRotation( axis, radians ); }
00223     // multiplies the current matrix by the rotation matrix derived using eulerRadians
00224     void                rotate( const Vec3<T> &eulerRadians ) { *this *= createRotation( eulerRadians ); }
00225     void                rotate( const Vec4<T> &eulerRadians ) { *this *= createRotation( eulerRadians ); }
00226     // multiplies the current matrix by the rotation matrix derived using from, to, worldUp
00227     void                rotate( const Vec3<T> &from, const Vec3<T> &to, const Vec3<T> &worldUp ) { *this *= createRotation( from, to, worldUp ); }
00228     void                rotate( const Vec4<T> &from, const Vec4<T> &to, const Vec4<T> &worldUp ) { *this *= createRotation( from, to, worldUp ); }
00229 
00230     // multiplies the current matrix by the scale matrix derived from supplies parameters
00231     void                scale( T s ) { Matrix44 op = createScale( s ); Matrix44 mat = *this; *this = op*mat; }
00232     void                scale( const Vec2<T> &v ) { *this *= createScale( v ); }
00233     void                scale( const Vec3<T> &v ) { *this *= createScale( v ); }
00234     void                scale( const Vec4<T> &v ) { *this *= createScale( v ); }
00235 
00236     // transposes rotation sub-matrix and inverts translation
00237     Matrix44<T>         invertTransform() const;
00238         
00239     // returns an identity matrix
00240     static Matrix44<T>  identity() { return Matrix44( 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 ); }
00241     // returns 1 filled matrix
00242     static Matrix44<T>  one() { return Matrix44( (T)1 ); }
00243     // returns 0 filled matrix
00244     static Matrix44<T>  zero() { return Matrix44( (T)0 ); }
00245 
00246     // creates translation matrix
00247     static Matrix44<T>  createTranslation( const Vec3<T> &v, T w = 1 );
00248     static Matrix44<T>  createTranslation( const Vec4<T> &v ) { return createTranslation( v.xyz(), v.w );}
00249 
00250     // creates rotation matrix
00251     static Matrix44<T>  createRotation( const Vec3<T> &axis, T radians );
00252     static Matrix44<T>  createRotation( const Vec4<T> &axis, T radians ) { return createRotation( axis.xyz(), radians ); }
00253     static Matrix44<T>  createRotation( const Vec3<T> &from, const Vec3<T> &to, const Vec3<T> &worldUp );
00254     static Matrix44<T>  createRotation( const Vec4<T> &from, const Vec4<T> &to, const Vec4<T> &worldUp ) { return createRotation( from.xyz(), to.xyz(), worldUp.xyz() ); }
00255     // equivalent to rotate( zAxis, z ), then rotate( yAxis, y ) then rotate( xAxis, x )
00256     static Matrix44<T>  createRotation( const Vec3<T> &eulerRadians );
00257     static Matrix44<T>  createRotation( const Vec4<T> &eulerRadians ) { return createRotation( eulerRadians.xyz() ); }
00258     // creates rotation matrix from ortho normal basis (u, v, n)
00259     static Matrix44<T>  createRotationOnb( const Vec3<T>& u, const Vec3<T>& v, const Vec3<T>& w );
00260     static Matrix44<T>  createRotationOnb( const Vec4<T>& u, const Vec4<T>& v, const Vec4<T>& w ) { return createRotationOnb( u.xyz(), v.xyz(), w.xyz() ); }
00261 
00262     // creates scale matrix
00263     static Matrix44<T>  createScale( T s );
00264     static Matrix44<T>  createScale( const Vec2<T> &v );
00265     static Matrix44<T>  createScale( const Vec3<T> &v );
00266     static Matrix44<T>  createScale( const Vec4<T> &v );
00267 
00268     // creates a rotation matrix with z-axis aligned to targetDir   
00269     static Matrix44<T>  alignZAxisWithTarget( Vec3<T> targetDir, Vec3<T> upDir );
00270     static Matrix44<T>  alignZAxisWithTarget( Vec4<T> targetDir, Vec4<T> upDir ) { return alignZAxisWithTarget( targetDir.xyz(), upDir.xyz() ); }
00271 
00272     friend std::ostream& operator<<( std::ostream &lhs, const Matrix44<T> &rhs ) {
00273         for( int i = 0; i < 4; i++) {
00274             lhs << " |";
00275             for( int j = 0; j < 4; j++) {
00276                 lhs << std::setw( 12 ) << std::setprecision( 6 ) << rhs.m[j*4+i];
00277             }
00278             lhs << "|" << std::endl;
00279         }
00280         
00281         return lhs;
00282     }
00283 };
00284 
00285 template< typename T >
00286 Matrix44<T>::Matrix44()
00287 {
00288     setToIdentity();
00289 }
00290 
00291 template< typename T >
00292 Matrix44<T>::Matrix44( T s )
00293 {
00294     for( int i = 0; i < DIM_SQ; ++i ) {
00295         m[i] = s;
00296     }
00297 }
00298 
00299 template< typename T >
00300 Matrix44<T>::Matrix44( const T *dt, bool srcIsRowMajor )
00301 {
00302     set( dt, srcIsRowMajor );
00303 }
00304 
00305 template< typename T >
00306 Matrix44<T>::Matrix44( T d0, T d1, T d2, T d3, T d4, T d5, T d6, T d7, T d8, T d9, T d10, T d11, T d12, T d13, T d14, T d15, bool srcIsRowMajor )
00307 {
00308     set(  d0,  d1,  d2,  d3, 
00309         d4,  d5,  d6,  d7, 
00310         d8,  d9, d10, d11, 
00311         d12, d13, d14, d15, srcIsRowMajor );
00312 }
00313 
00314 template< typename T >
00315 Matrix44<T>::Matrix44( const Vec3<T> &vx, const Vec3<T> &vy, const Vec3<T> &vz )
00316 {
00317     m[0] = vx.x; m[4] = vy.x; m[ 8] = vz.x; m[12] = 0;
00318     m[1] = vx.y; m[5] = vy.y; m[ 9] = vz.y; m[13] = 0;
00319     m[2] = vx.z; m[6] = vy.z; m[10] = vz.z; m[14] = 0;
00320     m[3] =    0; m[7] =    0; m[11] =    0; m[15] = 1;
00321 }
00322 
00323 template< typename T >
00324 Matrix44<T>::Matrix44( const Vec4<T> &vx, const Vec4<T> &vy, const Vec4<T> &vz, const Vec4<T> &vw )
00325 {
00326     m[0] = vx.x; m[4] = vy.x; m[ 8] = vz.x; m[12] = vw.x;
00327     m[1] = vx.y; m[5] = vy.y; m[ 9] = vz.y; m[13] = vw.y;
00328     m[2] = vx.z; m[6] = vy.z; m[10] = vz.z; m[14] = vw.z;
00329     m[3] = vx.w; m[7] = vy.w; m[11] = vz.w; m[15] = vw.w;
00330 }
00331 
00332 template< typename T >
00333 template< typename FromT >
00334 Matrix44<T>::Matrix44( const Matrix44<FromT>& src )
00335 {
00336     for( int i = 0; i < DIM_SQ; ++i ) {
00337         m[i] = static_cast<T>( src.m[i] );
00338     }
00339 }
00340 
00341 template< typename T >
00342 Matrix44<T>::Matrix44( const Matrix22<T>& src )
00343 {
00344     setToIdentity();
00345     m00 = src.m00; m01 = src.m01;
00346     m10 = src.m10; m11 = src.m11;
00347 }
00348 
00349 template< typename T >
00350 Matrix44<T>::Matrix44( const MatrixAffine2<T>& src )
00351 {
00352     m[ 0] = src.m[0]; m[ 4] = src.m[2]; m[ 8] = 0; m[12] = src.m[4];
00353     m[ 1] = src.m[1]; m[ 5] = src.m[3]; m[ 9] = 0; m[13] = src.m[5];
00354     m[ 2] = 0;        m[ 6] = 0;        m[10] = 1; m[14] = 0;
00355     m[ 3] = 0;        m[ 7] = 0;        m[11] = 0; m[15] = 1;
00356 }
00357 
00358 template< typename T >
00359 Matrix44<T>::Matrix44( const Matrix33<T>& src )
00360 {
00361     setToIdentity();
00362     m00 = src.m00; m01 = src.m01; m02 = src.m02;
00363     m10 = src.m10; m11 = src.m11; m12 = src.m12;
00364     m20 = src.m20; m21 = src.m21; m22 = src.m22;
00365 }
00366 
00367 template< typename T >
00368 Matrix44<T>::Matrix44( const Matrix44<T>& src )
00369 {
00370     std::memcpy( m, src.m, MEM_LEN );
00371 }
00372 
00373 template< typename T >
00374 Matrix44<T>& Matrix44<T>::operator=( const Matrix44<T>& rhs )
00375 {
00376     std::memcpy( m, rhs.m, MEM_LEN );
00377     return *this;
00378 }
00379 
00380 template< typename T >
00381 Matrix44<T>& Matrix44<T>::operator=( T rhs )
00382 {
00383     for( int i = 0; i < DIM_SQ; i++ ) {
00384         m[i] = rhs;
00385     }
00386     return *this;
00387 }
00388 
00389 template< typename T >
00390 template< typename FromT >
00391 Matrix44<T>& Matrix44<T>::operator=( const Matrix44<FromT>& rhs )
00392 {
00393     for( int i = 0; i < DIM_SQ; i++ ) {
00394         m[i] = static_cast<T>(rhs.m[i]);
00395     }
00396     return *this;
00397 }
00398 
00399 template< typename T >
00400 Matrix44<T>& Matrix44<T>::operator=( const Matrix22<T>& rhs )
00401 {
00402     setToIdentity();
00403     m00 = rhs.m00; m01 = rhs.m01;
00404     m10 = rhs.m10; m11 = rhs.m11;
00405     return *this;
00406 }
00407 
00408 template< typename T >
00409 Matrix44<T>& Matrix44<T>::operator=( const MatrixAffine2<T>& rhs )
00410 {
00411     m[ 0] = rhs.m[0]; m[ 4] = rhs.m[2]; m[ 8] = 0; m[12] = rhs.m[4];
00412     m[ 1] = rhs.m[1]; m[ 5] = rhs.m[3]; m[ 9] = 0; m[13] = rhs.m[5];
00413     m[ 2] = 0;        m[ 6] = 0;        m[10] = 1; m[14] = 0;
00414     m[ 3] = 0;        m[ 7] = 0;        m[11] = 0; m[15] = 1;
00415     return *this;
00416 }
00417 
00418 
00419 
00420 template< typename T >
00421 Matrix44<T>& Matrix44<T>::operator=( const Matrix33<T>& rhs )
00422 {
00423     setToIdentity();
00424     m00 = rhs.m00; m01 = rhs.m01; m02 = rhs.m02;
00425     m10 = rhs.m10; m11 = rhs.m11; m12 = rhs.m12;
00426     m20 = rhs.m20; m21 = rhs.m21; m22 = rhs.m22;
00427     return *this;
00428 }
00429 
00430 template< typename T >
00431 bool Matrix44<T>::equalCompare( const Matrix44<T>& rhs, T epsilon ) const
00432 {
00433     for( int i = 0; i < DIM_SQ; ++i ) {
00434         if( math<T>::abs(m[i] - rhs.m[i]) >= epsilon )
00435             return false;
00436     }
00437     return true;
00438 }
00439 
00440 template< typename T >
00441 inline Matrix44<T>& Matrix44<T>::operator*=( const Matrix44<T> &rhs )
00442 {
00443     Matrix44<T> ret;
00444 
00445     ret.m[ 0] = m[ 0]*rhs.m[ 0] + m[ 4]*rhs.m[ 1] + m[ 8]*rhs.m[ 2] + m[12]*rhs.m[ 3];
00446     ret.m[ 1] = m[ 1]*rhs.m[ 0] + m[ 5]*rhs.m[ 1] + m[ 9]*rhs.m[ 2] + m[13]*rhs.m[ 3];
00447     ret.m[ 2] = m[ 2]*rhs.m[ 0] + m[ 6]*rhs.m[ 1] + m[10]*rhs.m[ 2] + m[14]*rhs.m[ 3];
00448     ret.m[ 3] = m[ 3]*rhs.m[ 0] + m[ 7]*rhs.m[ 1] + m[11]*rhs.m[ 2] + m[15]*rhs.m[ 3];
00449 
00450     ret.m[ 4] = m[ 0]*rhs.m[ 4] + m[ 4]*rhs.m[ 5] + m[ 8]*rhs.m[ 6] + m[12]*rhs.m[ 7];
00451     ret.m[ 5] = m[ 1]*rhs.m[ 4] + m[ 5]*rhs.m[ 5] + m[ 9]*rhs.m[ 6] + m[13]*rhs.m[ 7];
00452     ret.m[ 6] = m[ 2]*rhs.m[ 4] + m[ 6]*rhs.m[ 5] + m[10]*rhs.m[ 6] + m[14]*rhs.m[ 7];
00453     ret.m[ 7] = m[ 3]*rhs.m[ 4] + m[ 7]*rhs.m[ 5] + m[11]*rhs.m[ 6] + m[15]*rhs.m[ 7];
00454 
00455     ret.m[ 8] = m[ 0]*rhs.m[ 8] + m[ 4]*rhs.m[ 9] + m[ 8]*rhs.m[10] + m[12]*rhs.m[11];
00456     ret.m[ 9] = m[ 1]*rhs.m[ 8] + m[ 5]*rhs.m[ 9] + m[ 9]*rhs.m[10] + m[13]*rhs.m[11];
00457     ret.m[10] = m[ 2]*rhs.m[ 8] + m[ 6]*rhs.m[ 9] + m[10]*rhs.m[10] + m[14]*rhs.m[11];
00458     ret.m[11] = m[ 3]*rhs.m[ 8] + m[ 7]*rhs.m[ 9] + m[11]*rhs.m[10] + m[15]*rhs.m[11];
00459 
00460     ret.m[12] = m[ 0]*rhs.m[12] + m[ 4]*rhs.m[13] + m[ 8]*rhs.m[14] + m[12]*rhs.m[15];
00461     ret.m[13] = m[ 1]*rhs.m[12] + m[ 5]*rhs.m[13] + m[ 9]*rhs.m[14] + m[13]*rhs.m[15];
00462     ret.m[14] = m[ 2]*rhs.m[12] + m[ 6]*rhs.m[13] + m[10]*rhs.m[14] + m[14]*rhs.m[15];
00463     ret.m[15] = m[ 3]*rhs.m[12] + m[ 7]*rhs.m[13] + m[11]*rhs.m[14] + m[15]*rhs.m[15];
00464 
00465     for( int i = 0; i < DIM_SQ; ++i ) {
00466         m[i] = ret.m[i];
00467     }
00468 
00469     return *this;
00470 }
00471 
00472 template< typename T >
00473 Matrix44<T>& Matrix44<T>::operator+=( const Matrix44<T> &rhs )
00474 {
00475     for( int i = 0; i < DIM_SQ; ++i ) {
00476         m[i] += rhs.m[i];
00477     }
00478     return *this;
00479 }
00480 
00481 template< typename T >
00482 Matrix44<T>& Matrix44<T>::operator-=( const Matrix44<T> &rhs )
00483 {
00484     for( int i = 0; i < DIM_SQ; ++i ) {
00485         m[i] -= rhs.m[i];
00486     }
00487     return *this;
00488 }
00489 
00490 template< typename T >
00491 Matrix44<T>& Matrix44<T>::operator*=( T rhs )
00492 {
00493     for( int i = 0; i < DIM_SQ; ++i ) {
00494         m[i] *= rhs;
00495     }
00496     return *this;
00497 }
00498 
00499 template< typename T >
00500 Matrix44<T>& Matrix44<T>::operator/=( T rhs )
00501 {
00502     T invS = (T)1/rhs;
00503     for( int i = 0; i < DIM_SQ; ++i ) {
00504         m[i] *= invS;
00505     }
00506     return *this;
00507 }
00508 
00509 template< typename T >
00510 Matrix44<T>& Matrix44<T>::operator+=( T rhs )
00511 {
00512     for( int i = 0; i < DIM_SQ; ++i ) {
00513         m[i] += rhs;
00514     }
00515     return *this;
00516 }
00517 
00518 template< typename T >
00519 Matrix44<T>& Matrix44<T>::operator-=( T rhs )
00520 {
00521     for( int i = 0; i < DIM_SQ; ++i ) {
00522         m[i] -= rhs;
00523     }
00524     return *this;
00525 }
00526 
00527 template< typename T >
00528 inline const Matrix44<T> Matrix44<T>::operator*( const Matrix44<T> &rhs ) const
00529 {
00530     Matrix44<T> ret;
00531 
00532     ret.m[ 0] = m[ 0]*rhs.m[ 0] + m[ 4]*rhs.m[ 1] + m[ 8]*rhs.m[ 2] + m[12]*rhs.m[ 3];
00533     ret.m[ 1] = m[ 1]*rhs.m[ 0] + m[ 5]*rhs.m[ 1] + m[ 9]*rhs.m[ 2] + m[13]*rhs.m[ 3];
00534     ret.m[ 2] = m[ 2]*rhs.m[ 0] + m[ 6]*rhs.m[ 1] + m[10]*rhs.m[ 2] + m[14]*rhs.m[ 3];
00535     ret.m[ 3] = m[ 3]*rhs.m[ 0] + m[ 7]*rhs.m[ 1] + m[11]*rhs.m[ 2] + m[15]*rhs.m[ 3];
00536 
00537     ret.m[ 4] = m[ 0]*rhs.m[ 4] + m[ 4]*rhs.m[ 5] + m[ 8]*rhs.m[ 6] + m[12]*rhs.m[ 7];
00538     ret.m[ 5] = m[ 1]*rhs.m[ 4] + m[ 5]*rhs.m[ 5] + m[ 9]*rhs.m[ 6] + m[13]*rhs.m[ 7];
00539     ret.m[ 6] = m[ 2]*rhs.m[ 4] + m[ 6]*rhs.m[ 5] + m[10]*rhs.m[ 6] + m[14]*rhs.m[ 7];
00540     ret.m[ 7] = m[ 3]*rhs.m[ 4] + m[ 7]*rhs.m[ 5] + m[11]*rhs.m[ 6] + m[15]*rhs.m[ 7];
00541 
00542     ret.m[ 8] = m[ 0]*rhs.m[ 8] + m[ 4]*rhs.m[ 9] + m[ 8]*rhs.m[10] + m[12]*rhs.m[11];
00543     ret.m[ 9] = m[ 1]*rhs.m[ 8] + m[ 5]*rhs.m[ 9] + m[ 9]*rhs.m[10] + m[13]*rhs.m[11];
00544     ret.m[10] = m[ 2]*rhs.m[ 8] + m[ 6]*rhs.m[ 9] + m[10]*rhs.m[10] + m[14]*rhs.m[11];
00545     ret.m[11] = m[ 3]*rhs.m[ 8] + m[ 7]*rhs.m[ 9] + m[11]*rhs.m[10] + m[15]*rhs.m[11];
00546 
00547     ret.m[12] = m[ 0]*rhs.m[12] + m[ 4]*rhs.m[13] + m[ 8]*rhs.m[14] + m[12]*rhs.m[15];
00548     ret.m[13] = m[ 1]*rhs.m[12] + m[ 5]*rhs.m[13] + m[ 9]*rhs.m[14] + m[13]*rhs.m[15];
00549     ret.m[14] = m[ 2]*rhs.m[12] + m[ 6]*rhs.m[13] + m[10]*rhs.m[14] + m[14]*rhs.m[15];
00550     ret.m[15] = m[ 3]*rhs.m[12] + m[ 7]*rhs.m[13] + m[11]*rhs.m[14] + m[15]*rhs.m[15];
00551 
00552     return ret;
00553 }
00554 
00555 template< typename T >
00556 const Matrix44<T> Matrix44<T>::operator+( const Matrix44<T> &rhs ) const
00557 {
00558     Matrix44<T> ret;
00559     for( int i = 0; i < DIM_SQ; ++i ) {
00560         ret.m[i] = m[i] + rhs.m[i];
00561     }
00562     return ret;
00563 }
00564 
00565 template< typename T >
00566 const Matrix44<T> Matrix44<T>::operator-( const Matrix44<T> &rhs ) const
00567 {
00568     Matrix44<T> ret;
00569     for( int i = 0; i < DIM_SQ; ++i ) {
00570         ret.m[i] = m[i] - rhs.m[i];
00571     }
00572     return ret;
00573 }
00574 
00575 template< typename T >
00576 const Vec3<T> Matrix44<T>::operator*( const Vec3<T> &rhs ) const
00577 {
00578     T x = m[ 0]*rhs.x + m[ 4]*rhs.y + m[ 8]*rhs.z + m[12];
00579     T y = m[ 1]*rhs.x + m[ 5]*rhs.y + m[ 9]*rhs.z + m[13];
00580     T z = m[ 2]*rhs.x + m[ 6]*rhs.y + m[10]*rhs.z + m[14];
00581     T w = m[ 3]*rhs.x + m[ 7]*rhs.y + m[11]*rhs.z + m[15];
00582 
00583     return Vec3<T>( x/w, y/w, z/w );
00584 }
00585 
00586 template< typename T >
00587 const Vec4<T> Matrix44<T>::operator*( const Vec4<T> &rhs ) const
00588 {
00589     return Vec4<T>(
00590         m[ 0]*rhs.x + m[ 4]*rhs.y + m[ 8]*rhs.z + m[12]*rhs.w,
00591         m[ 1]*rhs.x + m[ 5]*rhs.y + m[ 9]*rhs.z + m[13]*rhs.w,
00592         m[ 2]*rhs.x + m[ 6]*rhs.y + m[10]*rhs.z + m[14]*rhs.w,
00593         m[ 3]*rhs.x + m[ 7]*rhs.y + m[11]*rhs.z + m[15]*rhs.w
00594         );
00595 }
00596 
00597 template< typename T >
00598 const Matrix44<T> Matrix44<T>::operator*( T rhs ) const
00599 {
00600     Matrix44<T> ret;
00601     for( int i = 0; i < DIM_SQ; ++i ) {
00602         ret.m[i] = m[i]*rhs;
00603     }
00604     return ret;
00605 }
00606 
00607 template< typename T >
00608 const Matrix44<T> Matrix44<T>::operator/( T rhs ) const
00609 {
00610     Matrix44<T> ret;
00611     T s = (T)1/rhs;
00612     for( int i = 0; i < DIM_SQ; ++i ) {
00613         ret.m[i] = m[i]*s;
00614     }
00615     return ret;
00616 }
00617 
00618 template< typename T >
00619 const Matrix44<T> Matrix44<T>::operator+( T rhs ) const
00620 {
00621     Matrix44<T> ret;
00622     for( int i = 0; i < DIM_SQ; ++i ) {
00623         ret.m[i] = m[i] + rhs;
00624     }
00625     return ret;
00626 }
00627 
00628 template< typename T >
00629 const Matrix44<T> Matrix44<T>::operator-( T rhs ) const
00630 {
00631     Matrix44<T> ret;
00632     for( int i = 0; i < DIM_SQ; ++i ) {
00633         ret.m[i] = m[i] - rhs;
00634     }
00635     return ret;
00636 }
00637 
00638 template< typename T >
00639 T& Matrix44<T>::at( int row, int col ) 
00640 {
00641     assert(row >= 0 && row < DIM);
00642     assert(col >= 0 && col < DIM);
00643     return m[col*DIM + row];
00644 }
00645 
00646 template< typename T >
00647 const T& Matrix44<T>::at( int row, int col ) const 
00648 {
00649     assert(row >= 0 && row < DIM);
00650     assert(col >= 0 && col < DIM);
00651     return m[col*DIM + row];
00652 }
00653 
00654 template< typename T >
00655 void Matrix44<T>::set( const T *dt, bool srcIsRowMajor )
00656 {
00657     if( srcIsRowMajor ) {
00658         m[0] = dt[ 0]; m[4] = dt[ 1]; m[ 8] = dt[ 2]; m[12] = dt[ 3];
00659         m[1] = dt[ 4]; m[5] = dt[ 5]; m[ 9] = dt[ 6]; m[13] = dt[ 7];
00660         m[2] = dt[ 8]; m[6] = dt[ 9]; m[10] = dt[10]; m[14] = dt[11];
00661         m[3] = dt[12]; m[7] = dt[13]; m[11] = dt[14]; m[15] = dt[15];
00662     }
00663     else {
00664         std::memcpy( m, dt, MEM_LEN );
00665     }
00666 }
00667 
00668 template< typename T >
00669 void Matrix44<T>::set( T d0, T d1, T d2, T d3, T d4, T d5, T d6, T d7, T d8, T d9, T d10, T d11, T d12, T d13, T d14, T d15, bool srcIsRowMajor )
00670 {
00671     if( srcIsRowMajor ) {
00672         m[0] =  d0; m[4] =  d1; m[ 8] =  d2; m[12] =  d3;
00673         m[1] =  d4; m[5] =  d5; m[ 9] =  d6; m[13] =  d7;
00674         m[2] =  d8; m[6] =  d9; m[10] = d10; m[14] = d11;
00675         m[3] = d12; m[7] = d13; m[11] = d14; m[15] = d15;
00676     }
00677     else {
00678         m[0] =  d0; m[4] =  d4; m[ 8] =  d8; m[12] = d12;
00679         m[1] =  d1; m[5] =  d5; m[ 9] =  d9; m[13] = d13;
00680         m[2] =  d2; m[6] =  d6; m[10] = d10; m[14] = d14;
00681         m[3] =  d3; m[7] =  d7; m[11] = d11; m[15] = d15;
00682     }
00683 }
00684 
00685 template< typename T >
00686 Vec4<T> Matrix44<T>::getColumn( int col ) const
00687 {
00688     size_t i = col*DIM;
00689     return Vec4<T>( 
00690         m[i + 0], 
00691         m[i + 1], 
00692         m[i + 2], 
00693         m[i + 3]
00694     );
00695 }
00696 
00697 template< typename T >
00698 void Matrix44<T>::setColumn( int col, const Vec4<T> &v )
00699 {
00700     size_t i = col*DIM;
00701     m[i + 0] = v.x;
00702     m[i + 1] = v.y;
00703     m[i + 2] = v.z;
00704     m[i + 3] = v.w;
00705 }
00706 
00707 template< typename T >
00708 Vec4<T> Matrix44<T>::getRow( int row ) const 
00709 { 
00710     return Vec4<T>( 
00711         m[row +  0], 
00712         m[row +  4], 
00713         m[row +  8], 
00714         m[row + 12] 
00715     ); 
00716 }
00717 
00718 template< typename T >
00719 void Matrix44<T>::setRow( int row, const Vec4<T> &v ) 
00720 { 
00721     m[row +  0] = v.x; 
00722     m[row +  4] = v.y; 
00723     m[row +  8] = v.z; 
00724     m[row + 12] = v.w; 
00725 }
00726 
00727 template< typename T >
00728 void Matrix44<T>::getColumns( Vec4<T> *c0, Vec4<T> *c1, Vec4<T> *c2, Vec4<T> *c3 ) const
00729 {
00730     *c0 = getColumn( 0 );
00731     *c1 = getColumn( 1 );
00732     *c2 = getColumn( 2 );
00733     *c3 = getColumn( 3 );
00734 }
00735 
00736 template< typename T >
00737 void Matrix44<T>::setColumns( const Vec4<T> &c0, const Vec4<T> &c1, const Vec4<T> &c2, const Vec4<T> &c3 )
00738 {
00739     setColumn( 0, c0 );
00740     setColumn( 1, c1 );
00741     setColumn( 2, c2 );
00742     setColumn( 3, c3 );
00743 }
00744 
00745 template< typename T >
00746 void Matrix44<T>::getRows( Vec4<T> *r0, Vec4<T> *r1, Vec4<T> *r2, Vec4<T> *r3 ) const
00747 {
00748     *r0 = getRow( 0 );
00749     *r1 = getRow( 1 );
00750     *r2 = getRow( 2 );
00751     *r3 = getRow( 3 );
00752 }
00753 
00754 template< typename T >
00755 void Matrix44<T>::setRows( const Vec4<T> &r0, const Vec4<T> &r1, const Vec4<T> &r2, const Vec4<T> &r3 )
00756 {
00757     setRow( 0, r0 );
00758     setRow( 1, r1 );
00759     setRow( 2, r2 );
00760     setRow( 3, r3 );
00761 }
00762 
00763 template< typename T >
00764 Matrix22<T> Matrix44<T>::subMatrix22( int row, int col ) const
00765 {
00766     Matrix22<T> ret;
00767     ret.setToNull();
00768 
00769     for( int i = 0; i < 2; ++i ) {
00770         int r = row + i;
00771         if( r >= 4 ) {
00772             continue;
00773         }
00774         for( int j = 0; j < 2; ++j ) {
00775             int c = col + j;
00776             if( c >= 4 ) {
00777                 continue;
00778             }
00779             ret.at( i, j ) = at( r, c );
00780         }
00781     }
00782 
00783     return ret;
00784 }
00785 
00786 template< typename T >
00787 Matrix33<T> Matrix44<T>::subMatrix33( int row, int col ) const
00788 {
00789     Matrix33<T> ret;
00790     ret.setToNull();
00791 
00792     for( int i = 0; i < 3; ++i ) {
00793         int r = row + i;
00794         if( r >= 4 ) {
00795             continue;
00796         }
00797         for( int j = 0; j < 3; ++j ) {
00798             int c = col + j;
00799             if( c >= 4 ) {
00800                 continue;
00801             }
00802             ret.at( i, j ) = at( r, c );
00803         }
00804     }
00805 
00806     return ret;
00807 }
00808 
00809 template< typename T >
00810 void Matrix44<T>::setToNull()
00811 {
00812     std::memset( m, 0, MEM_LEN );
00813 }
00814 
00815 template< typename T >
00816 void Matrix44<T>::setToIdentity()
00817 {
00818     m[ 0] = 1; m[ 4] = 0; m[ 8] = 0; m[12] = 0;
00819     m[ 1] = 0; m[ 5] = 1; m[ 9] = 0; m[13] = 0;
00820     m[ 2] = 0; m[ 6] = 0; m[10] = 1; m[14] = 0;
00821     m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
00822 }
00823 
00824 template< typename T >
00825 T Matrix44<T>::determinant() const
00826 {
00827     T a0 = m[ 0]*m[ 5] - m[ 1]*m[ 4];
00828     T a1 = m[ 0]*m[ 6] - m[ 2]*m[ 4];
00829     T a2 = m[ 0]*m[ 7] - m[ 3]*m[ 4];
00830     T a3 = m[ 1]*m[ 6] - m[ 2]*m[ 5];
00831     T a4 = m[ 1]*m[ 7] - m[ 3]*m[ 5];
00832     T a5 = m[ 2]*m[ 7] - m[ 3]*m[ 6];
00833     T b0 = m[ 8]*m[13] - m[ 9]*m[12];
00834     T b1 = m[ 8]*m[14] - m[10]*m[12];
00835     T b2 = m[ 8]*m[15] - m[11]*m[12];
00836     T b3 = m[ 9]*m[14] - m[10]*m[13];
00837     T b4 = m[ 9]*m[15] - m[11]*m[13];
00838     T b5 = m[10]*m[15] - m[11]*m[14];
00839 
00840     T det = a0*b5 - a1*b4 + a2*b3 + a3*b2 - a4*b1 + a5*b0;
00841 
00842     return det;
00843 }
00844 
00845 template< typename T >
00846 T Matrix44<T>::trace( bool fullTrace ) const
00847 {
00848     if( fullTrace ) {
00849         return m00 + m11 + m22 + m33;
00850     }
00851 
00852     return m00 + m11 + m22;
00853 }
00854 
00855 template< typename T >
00856 Matrix44<T> Matrix44<T>::diagonal() const
00857 {
00858     Matrix44 ret;
00859     ret.m00 = m00; ret.m01 =   0; ret.m02 =   0; ret.m03 =   0;
00860     ret.m10 =   0; ret.m11 = m11; ret.m12 =   0; ret.m13 =   0;
00861     ret.m20 =   0; ret.m21 =   0; ret.m22 = m22; ret.m23 =   0;
00862     ret.m30 =   0; ret.m31 =   0; ret.m32 =   0; ret.m33 = m33;
00863     return ret;
00864 }
00865 
00866 template< typename T >
00867 Matrix44<T> Matrix44<T>::lowerTriangular() const
00868 {
00869     Matrix44 ret;
00870     ret.m00 = m00; ret.m01 =   0; ret.m02 =   0; ret.m03 =   0;
00871     ret.m10 = m10; ret.m11 = m11; ret.m12 =   0; ret.m13 =   0;
00872     ret.m20 = m20; ret.m21 = m21; ret.m22 = m22; ret.m23 =   0;
00873     ret.m30 = m30; ret.m31 = m31; ret.m32 = m32; ret.m33 = m33;
00874     return ret;
00875 }
00876 
00877 template< typename T >
00878 Matrix44<T> Matrix44<T>::upperTriangular() const
00879 {
00880     Matrix44 ret;
00881     ret.m00 = m00; ret.m01 = m01; ret.m02 = m02; ret.m03 = m03;
00882     ret.m10 =   0; ret.m11 = m11; ret.m12 = m12; ret.m13 = m13;
00883     ret.m20 =   0; ret.m21 =   0; ret.m22 = m22; ret.m23 = m23;
00884     ret.m30 =   0; ret.m31 =   0; ret.m32 =   0; ret.m33 = m33;
00885     return ret;
00886 }
00887 
00888 template< typename T >
00889 void Matrix44<T>::transpose()
00890 {
00891     T t;
00892     t = m01; m01 = m10; m10 = t;
00893     t = m02; m02 = m20; m20 = t;
00894     t = m03; m03 = m30; m30 = t;
00895     t = m12; m12 = m21; m21 = t;
00896     t = m13; m13 = m31; m31 = t;
00897     t = m23; m23 = m32; m32 = t;
00898 }
00899 
00900 template< typename T >
00901 Matrix44<T> Matrix44<T>::transposed() const
00902 {
00903     return Matrix44<T>( 
00904         m[ 0], m[ 4], m[ 8], m[12],
00905         m[ 1], m[ 5], m[ 9], m[13],
00906         m[ 2], m[ 6], m[10], m[14],
00907         m[ 3], m[ 7], m[11], m[15] 
00908     );
00909 }
00910 
00911 template< typename T >
00912 inline Matrix44<T> Matrix44<T>::inverted( T epsilon ) const
00913 {
00914     Matrix44<T> inv( (T)0 );
00915 
00916     T a0 = m[ 0]*m[ 5] - m[ 1]*m[ 4];
00917     T a1 = m[ 0]*m[ 6] - m[ 2]*m[ 4];
00918     T a2 = m[ 0]*m[ 7] - m[ 3]*m[ 4];
00919     T a3 = m[ 1]*m[ 6] - m[ 2]*m[ 5];
00920     T a4 = m[ 1]*m[ 7] - m[ 3]*m[ 5];
00921     T a5 = m[ 2]*m[ 7] - m[ 3]*m[ 6];
00922     T b0 = m[ 8]*m[13] - m[ 9]*m[12];
00923     T b1 = m[ 8]*m[14] - m[10]*m[12];
00924     T b2 = m[ 8]*m[15] - m[11]*m[12];
00925     T b3 = m[ 9]*m[14] - m[10]*m[13];
00926     T b4 = m[ 9]*m[15] - m[11]*m[13];
00927     T b5 = m[10]*m[15] - m[11]*m[14];
00928 
00929     T det = a0*b5 - a1*b4 + a2*b3 + a3*b2 - a4*b1 + a5*b0;
00930 
00931     if( fabs( det ) > epsilon ) {
00932         inv.m[ 0] = + m[ 5]*b5 - m[ 6]*b4 + m[ 7]*b3;
00933         inv.m[ 4] = - m[ 4]*b5 + m[ 6]*b2 - m[ 7]*b1;
00934         inv.m[ 8] = + m[ 4]*b4 - m[ 5]*b2 + m[ 7]*b0;
00935         inv.m[12] = - m[ 4]*b3 + m[ 5]*b1 - m[ 6]*b0;
00936         inv.m[ 1] = - m[ 1]*b5 + m[ 2]*b4 - m[ 3]*b3;
00937         inv.m[ 5] = + m[ 0]*b5 - m[ 2]*b2 + m[ 3]*b1;
00938         inv.m[ 9] = - m[ 0]*b4 + m[ 1]*b2 - m[ 3]*b0;
00939         inv.m[13] = + m[ 0]*b3 - m[ 1]*b1 + m[ 2]*b0;
00940         inv.m[ 2] = + m[13]*a5 - m[14]*a4 + m[15]*a3;
00941         inv.m[ 6] = - m[12]*a5 + m[14]*a2 - m[15]*a1;
00942         inv.m[10] = + m[12]*a4 - m[13]*a2 + m[15]*a0;
00943         inv.m[14] = - m[12]*a3 + m[13]*a1 - m[14]*a0;
00944         inv.m[ 3] = - m[ 9]*a5 + m[10]*a4 - m[11]*a3;
00945         inv.m[ 7] = + m[ 8]*a5 - m[10]*a2 + m[11]*a1;
00946         inv.m[11] = - m[ 8]*a4 + m[ 9]*a2 - m[11]*a0;
00947         inv.m[15] = + m[ 8]*a3 - m[ 9]*a1 + m[10]*a0;
00948 
00949         T invDet = ((T)1)/det;
00950         inv.m[ 0] *= invDet;
00951         inv.m[ 1] *= invDet;
00952         inv.m[ 2] *= invDet;
00953         inv.m[ 3] *= invDet;
00954         inv.m[ 4] *= invDet;
00955         inv.m[ 5] *= invDet;
00956         inv.m[ 6] *= invDet;
00957         inv.m[ 7] *= invDet;
00958         inv.m[ 8] *= invDet;
00959         inv.m[ 9] *= invDet;
00960         inv.m[10] *= invDet;
00961         inv.m[11] *= invDet;
00962         inv.m[12] *= invDet;
00963         inv.m[13] *= invDet;
00964         inv.m[14] *= invDet;
00965         inv.m[15] *= invDet;
00966     }
00967 
00968     return inv;
00969 }
00970 
00971 template< typename T >
00972 Vec3<T> Matrix44<T>::preMultiply( const Vec3<T> &v ) const
00973 {
00974     return Vec3<T>(
00975         v.x*m00 + v.y*m10 + v.z*m20,
00976         v.x*m01 + v.y*m11 + v.z*m21,
00977         v.x*m02 + v.y*m12 + v.z*m22
00978         );
00979 }
00980 
00981 template< typename T >
00982 Vec4<T> Matrix44<T>::preMultiply( const Vec4<T> &v ) const
00983 {
00984     return Vec4<T>(
00985         v.x*m00 + v.y*m10 + v.z*m20 + v.w*m30,
00986         v.x*m01 + v.y*m11 + v.z*m21 + v.w*m31,
00987         v.x*m02 + v.y*m12 + v.z*m22 + v.w*m32,
00988         v.x*m03 + v.y*m13 + v.z*m23 + v.w*m33
00989         );
00990 }
00991 
00992 template< typename T >
00993 Vec3<T> Matrix44<T>::postMultiply( const Vec3<T> &v ) const
00994 {
00995     return Vec3<T>(
00996         m00*v.x + m01*v.y + m02*v.z,
00997         m10*v.x + m11*v.y + m12*v.z,
00998         m20*v.x + m21*v.y + m22*v.z
00999         );
01000 }
01001 
01002 template< typename T >
01003 Vec4<T> Matrix44<T>::postMultiply( const Vec4<T> &v ) const
01004 {
01005     return Vec4<T>(
01006         m00*v.x + m01*v.y + m02*v.z + m03*v.w,
01007         m10*v.x + m11*v.y + m12*v.z + m13*v.w,
01008         m20*v.x + m21*v.y + m22*v.z + m23*v.w,
01009         m30*v.x + m31*v.y + m32*v.z + m33*v.w
01010         );
01011 }
01012 
01013 template< typename T >
01014 Matrix44<T> Matrix44<T>::affineInverted() const
01015 {
01016     Matrix44<T> ret;
01017 
01018     // compute upper left 3x3 matrix determinant
01019     T cofactor0 = m[ 5]*m[10] - m[6]*m[ 9];
01020     T cofactor4 = m[ 2]*m[ 9] - m[1]*m[10];
01021     T cofactor8 = m[ 1]*m[ 6] - m[2]*m[ 5];
01022     T det = m[0]*cofactor0 + m[4]*cofactor4 + m[8]*cofactor8;
01023     if( fabs( det ) <= EPSILON ) {
01024         return ret;
01025     }
01026 
01027     // create adjunct matrix and multiply by 1/det to get upper 3x3
01028     T invDet = ((T)1.0) / det;
01029     ret.m[ 0] = invDet*cofactor0;
01030     ret.m[ 1] = invDet*cofactor4;
01031     ret.m[ 2] = invDet*cofactor8;
01032 
01033     ret.m[ 4] = invDet*(m[ 6]*m[ 8] - m[ 4]*m[10]);
01034     ret.m[ 5] = invDet*(m[ 0]*m[10] - m[ 2]*m[ 8]);
01035     ret.m[ 6] = invDet*(m[ 2]*m[ 4] - m[ 0]*m[ 6]);
01036 
01037     ret.m[ 8] = invDet*(m[ 4]*m[ 9] - m[ 5]*m[ 8]);
01038     ret.m[ 9] = invDet*(m[ 1]*m[ 8] - m[ 0]*m[ 9]);
01039     ret.m[10] = invDet*(m[ 0]*m[ 5] - m[ 1]*m[ 4]);
01040 
01041     // multiply -translation by inverted 3x3 to get its inverse     
01042     ret.m[12] = -ret.m[ 0]*m[12] - ret.m[ 4]*m[13] - ret.m[ 8]*m[14];
01043     ret.m[13] = -ret.m[ 1]*m[12] - ret.m[ 5]*m[13] - ret.m[ 9]*m[14];
01044     ret.m[14] = -ret.m[ 2]*m[12] - ret.m[ 6]*m[13] - ret.m[10]*m[14];
01045 
01046     return ret;
01047 }
01048 
01049 template< typename T >
01050 Vec3<T> Matrix44<T>::transformPoint( const Vec3<T> &rhs ) const
01051 {
01052     T x = m00*rhs.x + m01*rhs.y + m02*rhs.z + m03;
01053     T y = m10*rhs.x + m11*rhs.y + m12*rhs.z + m13;
01054     T z = m20*rhs.x + m21*rhs.y + m22*rhs.z + m23;
01055     T w = m30*rhs.x + m31*rhs.y + m32*rhs.z + m33;
01056 
01057     return Vec3<T>( x / w, y / w, z / w );
01058 }
01059 
01060 template< typename T >
01061 Vec3<T> Matrix44<T>::transformPointAffine( const Vec3<T> &rhs ) const
01062 {
01063     T x = m00*rhs.x + m01*rhs.y + m02*rhs.z + m03;
01064     T y = m10*rhs.x + m11*rhs.y + m12*rhs.z + m13;
01065     T z = m20*rhs.x + m21*rhs.y + m22*rhs.z + m23;
01066 
01067     return Vec3<T>( x, y, z );
01068 }
01069 
01070 template< typename T >
01071 Vec3<T> Matrix44<T>::transformVec( const Vec3<T> &rhs ) const
01072 {
01073     T x = m00*rhs.x + m01*rhs.y + m02*rhs.z;
01074     T y = m10*rhs.x + m11*rhs.y + m12*rhs.z;
01075     T z = m20*rhs.x + m21*rhs.y + m22*rhs.z;
01076 
01077     return Vec3<T>( x, y, z );
01078 }
01079 
01080 template< typename T > // thanks to @juj/MathGeoLib for fix
01081 void Matrix44<T>::orthonormalInvert()
01082 {
01083     // transpose upper 3x3 (R->R^t)
01084     std::swap( at(0,1), at(1,0) );
01085     std::swap( at(0,2), at(2,0) );
01086     std::swap( at(1,2), at(2,1) );
01087 
01088     // replace translation (T) with R^t(-T).
01089     Vec3f newT( transformVec( Vec3f(-at(0,3),-at(1,3),-at(2,3)) ) );
01090     at(0,3) = newT.x;
01091     at(1,3) = newT.y;
01092     at(2,3) = newT.z;
01093 }
01094 
01095 template<typename T>
01096 Matrix44<T> Matrix44<T>::createTranslation( const Vec3<T> &v, T w )
01097 {
01098     Matrix44 ret;
01099     ret.m[12] = v.x;
01100     ret.m[13] = v.y;
01101     ret.m[14] = v.z;
01102     ret.m[15] = w;
01103 
01104     return ret;
01105 }
01106 
01108 // Matrix44
01109 template<typename T>
01110 Matrix44<T> Matrix44<T>::createRotation( const Vec3<T> &from, const Vec3<T> &to, const Vec3<T> &worldUp )
01111 {
01112     // The goal is to obtain a rotation matrix that takes 
01113     // "fromDir" to "toDir".  We do this in two steps and 
01114     // compose the resulting rotation matrices; 
01115     //    (a) rotate "fromDir" into the z-axis
01116     //    (b) rotate the z-axis into "toDir"
01117  
01118     // The from direction must be non-zero; but we allow zero to and up dirs.
01119     if( from.lengthSquared() == 0 ) {
01120         return Matrix44<T>();
01121     }
01122     else {
01123         Matrix44<T> zAxis2FromDir = alignZAxisWithTarget( from, Vec3<T>::yAxis() );
01124         Matrix44<T> fromDir2zAxis = zAxis2FromDir.transposed();
01125         Matrix44<T> zAxis2ToDir = alignZAxisWithTarget( to, worldUp );
01126         return fromDir2zAxis * zAxis2ToDir;
01127     }
01128 }
01129 
01130 template<typename T>
01131 Matrix44<T> Matrix44<T>::createRotation( const Vec3<T> &axis, T angle )
01132 {
01133     Vec3<T> unit( axis.normalized() );
01134     T sine   = math<T>::sin( angle );
01135     T cosine = math<T>::cos( angle );
01136 
01137     Matrix44<T> ret;
01138 
01139     ret.m[ 0] = unit.x * unit.x * (1 - cosine) + cosine;
01140     ret.m[ 1] = unit.x * unit.y * (1 - cosine) + unit.z * sine;
01141     ret.m[ 2] = unit.x * unit.z * (1 - cosine) - unit.y * sine;
01142     ret.m[ 3] = 0;
01143 
01144     ret.m[ 4] = unit.x * unit.y * (1 - cosine) - unit.z * sine;
01145     ret.m[ 5] = unit.y * unit.y * (1 - cosine) + cosine;
01146     ret.m[ 6] = unit.y * unit.z * (1 - cosine) + unit.x * sine;
01147     ret.m[ 7] = 0;
01148 
01149     ret.m[ 8] = unit.x * unit.z * (1 - cosine) + unit.y * sine;
01150     ret.m[ 9] = unit.y * unit.z * (1 - cosine) - unit.x * sine;
01151     ret.m[10] = unit.z * unit.z * (1 - cosine) + cosine;
01152     ret.m[11] = 0;
01153 
01154     ret.m[12] = 0;
01155     ret.m[13] = 0;
01156     ret.m[14] = 0;
01157     ret.m[15] = 1;
01158 
01159     return ret;
01160 }
01161 
01162 template<typename T>
01163 Matrix44<T> Matrix44<T>::createRotation( const Vec3<T> &eulerRadians )
01164 {
01165     // The ordering for this is XYZ. In OpenGL, the ordering
01166     // is the same, but the operations needs to happen in
01167     // reverse: 
01168     //
01169     //     glRotatef( eulerRadians.z, 0.0f, 0.0f 1.0f );
01170     //     glRotatef( eulerRadians.y, 0.0f, 1.0f 0.0f );
01171     //     glRotatef( eulerRadians.x, 1.0f, 0.0f 0.0f );
01172     //
01173 
01174     Matrix44<T> ret;
01175     T cos_rz, sin_rz, cos_ry, sin_ry, cos_rx, sin_rx;
01176 
01177     cos_rx = math<T>::cos( eulerRadians.x );
01178     cos_ry = math<T>::cos( eulerRadians.y );
01179     cos_rz = math<T>::cos( eulerRadians.z );
01180 
01181     sin_rx = math<T>::sin( eulerRadians.x );
01182     sin_ry = math<T>::sin( eulerRadians.y );
01183     sin_rz = math<T>::sin( eulerRadians.z );
01184 
01185     ret.m[ 0] =  cos_rz*cos_ry;
01186     ret.m[ 1] =  sin_rz*cos_ry;
01187     ret.m[ 2] = -sin_ry;
01188     ret.m[ 3] =  0;
01189 
01190     ret.m[ 4] = -sin_rz*cos_rx + cos_rz*sin_ry*sin_rx;
01191     ret.m[ 5] =  cos_rz*cos_rx + sin_rz*sin_ry*sin_rx;
01192     ret.m[ 6] =  cos_ry*sin_rx;
01193     ret.m[ 7] =  0;
01194 
01195     ret.m[ 8] =  sin_rz*sin_rx + cos_rz*sin_ry*cos_rx;
01196     ret.m[ 9] = -cos_rz*sin_rx + sin_rz*sin_ry*cos_rx;
01197     ret.m[10] =  cos_ry*cos_rx;
01198     ret.m[11] =  0;
01199 
01200     ret.m[12] =  0;
01201     ret.m[13] =  0;
01202     ret.m[14] =  0;
01203     ret.m[15] =  1;
01204 
01205     return ret;
01206 }
01207 
01208 template<typename T>
01209 Matrix44<T> Matrix44<T>::createRotationOnb( const Vec3<T>& u, const Vec3<T>& v, const Vec3<T>& w )
01210 {
01211     return Matrix44<T>(
01212         u.x,  u.y, u.z, 0,
01213         v.x,  v.y, v.z, 0,
01214         w.x,  w.y, w.z, 0,
01215         0,    0,   0, 1
01216         );
01217 }
01218 
01219 template<typename T>
01220 Matrix44<T> Matrix44<T>::createScale( T s )
01221 {
01222     Matrix44<T> ret;
01223     ret.setToIdentity();
01224     ret.at(0,0) = s;
01225     ret.at(1,1) = s;
01226     ret.at(2,2) = s;
01227     ret.at(3,3) = s;
01228     return ret;
01229 }
01230 
01231 template<typename T>
01232 Matrix44<T> Matrix44<T>::createScale( const Vec2<T> &v )
01233 {
01234     Matrix44<T> ret;
01235     ret.setToIdentity();
01236     ret.at(0,0) = v.x;
01237     ret.at(1,1) = v.y;
01238     ret.at(2,2) = 1;
01239     ret.at(3,3) = 1;
01240     return ret;
01241 }
01242 
01243 template<typename T>
01244 Matrix44<T> Matrix44<T>::createScale( const Vec3<T> &v )
01245 {
01246     Matrix44<T> ret;
01247     ret.setToIdentity();
01248     ret.at(0,0) = v.x;
01249     ret.at(1,1) = v.y;
01250     ret.at(2,2) = v.z;
01251     ret.at(3,3) = 1;
01252     return ret;
01253 }
01254 
01255 template<typename T>
01256 Matrix44<T> Matrix44<T>::createScale( const Vec4<T> &v )
01257 {
01258     Matrix44<T> ret;
01259     ret.setToIdentity();
01260     ret.at(0,0) = v.x;
01261     ret.at(1,1) = v.y;
01262     ret.at(2,2) = v.z;
01263     ret.at(3,3) = v.w;
01264     return ret;
01265 }
01266 
01267 template<typename T>
01268 Matrix44<T> Matrix44<T>::alignZAxisWithTarget( Vec3<T> targetDir, Vec3<T> upDir )
01269 {
01270     // Ensure that the target direction is non-zero.
01271     if( targetDir.lengthSquared() == 0 )
01272         targetDir = Vec3<T>::zAxis();
01273 
01274     // Ensure that the up direction is non-zero.
01275     if( upDir.lengthSquared() == 0 )
01276         upDir = Vec3<T>::yAxis();
01277 
01278     // Check for degeneracies.  If the upDir and targetDir are parallel 
01279     // or opposite, then compute a new, arbitrary up direction that is
01280     // not parallel or opposite to the targetDir.
01281     if( upDir.cross( targetDir ).lengthSquared() == 0 ) {
01282         upDir = targetDir.cross( Vec3<T>::xAxis() );
01283     if( upDir.lengthSquared() == 0 )
01284         upDir = targetDir.cross( Vec3<T>::zAxis() );
01285     }
01286 
01287     // Compute the x-, y-, and z-axis vectors of the new coordinate system.
01288     Vec3<T> targetPerpDir = upDir.cross( targetDir );    
01289     Vec3<T> targetUpDir = targetDir.cross( targetPerpDir );
01290     
01291 
01292     // Rotate the x-axis into targetPerpDir (row 0),
01293     // rotate the y-axis into targetUpDir   (row 1),
01294     // rotate the z-axis into targetDir     (row 2).
01295     Vec3<T> row[3];
01296     row[0] = targetPerpDir.normalized();
01297     row[1] = targetUpDir.normalized();
01298     row[2] = targetDir.normalized();
01299     
01300     Matrix44<T> mat( row[0].x,  row[0].y,  row[0].z,  0,
01301                      row[1].x,  row[1].y,  row[1].z,  0,
01302                      row[2].x,  row[2].y,  row[2].z,  0,
01303                             0,          0,        0,  1 );
01304     
01305     return mat;
01306 }
01307 
01309 // Typedefs
01310 typedef Matrix44<float>  Matrix44f;
01311 typedef Matrix44<double> Matrix44d;
01312 
01313 } // namespace cinder