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