Cinder

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

include/cinder/Matrix.h

Go to the documentation of this file.
00001 /*
00002  Copyright (c) 2010, The Barbarian Group
00003  All rights reserved.
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 #pragma once
00024 
00025 #include "cinder/Cinder.h"
00026 #include "cinder/CinderMath.h"
00027 #include "cinder/Vector.h"
00028 
00029 #include <iomanip>
00030 
00031 namespace cinder { 
00032 
00033 template<typename T>
00034 class Matrix22
00035 {
00036  public:
00037     //
00038     // m[i,j]:
00039     // | m[0,0] m[0,1] |
00040     // | m[1,0] m[1,1] |
00041     //
00042     // m[idx]
00043     // | m[0] m[2] |
00044     // | m[1] m[3] |
00045     //
00046     T m[4];
00047     
00048     Matrix22() 
00049     {
00050         for( size_t i = 0; i < 4; i++ ) {
00051             m[i] = (i % 3) ? static_cast<T>( 0.0 ) : static_cast<T>( 1.0 );
00052         }
00053     }
00054     
00055     Matrix22( const T *dt ) { std::memcpy( m, dt, sizeof(T) * 4 ); }
00056     Matrix22( const Matrix22<T>& src ) { std::memcpy( m, src.m, sizeof(T) * 4 ); }
00057     
00058     template<typename FromT>
00059     Matrix22( const Matrix22<FromT>& src ) 
00060     {
00061         for( int i = 0; i < 4; i++)
00062         {
00063             m[i] = static_cast<T>(src.m[i]);
00064         }   
00065     }
00066     
00067     void identity()
00068     {
00069         for( size_t i = 0; i < 4; i++ )
00070             m[i] = (i % 3) ? 0 : 1;
00071     }
00072 
00073     static Matrix22<T> createRotationAroundAxis( T a )
00074     {
00075         Matrix22<T> ma, mb, mc;
00076         float ac = cos(a);
00077         float as = sin(a);
00078 
00079         ma.at(0,0) =  ac;
00080         ma.at(1,0) = -as;
00081         ma.at(1,0) =  as;
00082         ma.at(1,1) =  ac;
00083 
00084         Matrix22<T> ret = ma * mb * mc;
00085         return ret;
00086     }
00087 
00088     bool operator==( const Matrix22<T>& rhs ) const
00089     {
00090         for( int i = 0; i < 4; i++)
00091         {
00092             if( std::fabs(m[i] - rhs.m[i]) >= EPSILON)
00093                 return false;
00094         }
00095         return true;
00096     }
00097 
00098     bool operator!=( const Matrix22<T>& rhs ) const
00099     {
00100         return !(*this == rhs);
00101     }
00102 
00103     T& at( int row, int col ) 
00104     {
00105         assert(row >= 0 && row < 2);
00106         assert(col >= 0 && col < 2);
00107         return m[col * 2 + row];
00108     }
00109 
00110     const T& at( int row, int col ) const 
00111     {
00112         assert(row >= 0 && row < 2);
00113         assert(col >= 0 && col < 2);
00114         return m[col * 2 + row];
00115     }
00116 
00117     Matrix22<T>& operator=( const Matrix22<T>& rhs ) 
00118     {
00119         std::memcpy( m, rhs.m, sizeof(T) * 4 );
00120         return * this;
00121     }
00122 
00123     template<class FromT>
00124     Matrix22<T>& operator=( const Matrix22<FromT>& rhs ) 
00125     {
00126         for( int i = 0; i < 4; i++)
00127         {
00128             m[i] = static_cast<T>( rhs.m[i] );
00129         }
00130         return * this;
00131     }
00132 
00133     Matrix22<T>& operator=( const T* rhs ) 
00134     {
00135         std::memcpy(m, rhs, sizeof(T) * 4);
00136         return * this;
00137     }
00138 
00139 
00140     const Matrix22<T> operator+( const Matrix22<T>& rhs ) const
00141     {
00142         Matrix22<T> ret;
00143         for( int i = 0; i < 4; i++ )
00144             ret.m[i] = m[i] + rhs.m[i];
00145         return ret;
00146     }
00147 
00148     const Matrix22<T> operator-( const Matrix22<T>& rhs ) const
00149     {
00150         Matrix22<T> ret;
00151         for( int i = 0; i < 4; i++ )
00152             ret.m[i] = m[i] - rhs.m[i];
00153         return ret;
00154     }
00155 
00156     const Matrix22<T> operator+( T rhs ) const
00157     {
00158         Matrix22<T> ret;
00159         for( int i = 0; i < 4; i++ )
00160             ret.m[i] = m[i] + rhs;
00161         return ret;
00162     }
00163 
00164     const Matrix22<T> operator-( T rhs ) const
00165     {
00166         Matrix22<T> ret;
00167         for( int i = 0; i < 4; i++ )
00168             ret.m[i] = m[i] - rhs;
00169         return ret;
00170     }
00171 
00172     const Matrix22<T> operator*( T rhs ) const
00173     {
00174         Matrix22<T> ret;
00175         for( int i = 0; i < 4; i++ )
00176             ret.m[i] = m[i] * rhs;
00177         return ret;
00178     }
00179 
00180     const Matrix22<T> operator/( T rhs ) const
00181     {
00182         Matrix22<T> ret;
00183         for( int i = 0; i < 4; i++ )
00184             ret.m[i] = m[i] / rhs;
00185         return ret;
00186     }
00187 
00188     const Vec2<T> operator*( const Vec3<T> &rhs ) const
00189     {
00190         return Vec2<T>(
00191             m[0] * rhs.x + m[2] * rhs.y,
00192             m[1] * rhs.x + m[3] * rhs.y
00193         );
00194     }
00195 
00196     const Matrix22<T> operator*( const Matrix22<T> &rhs ) const 
00197     {
00198         static Matrix22<T> w;
00199         for( int i = 0; i < 2; i++ ) {
00200             for( int j = 0; j < 2; j++ ) {
00201                 T n = 0;
00202 
00203                 for( int k = 0; k < 2; k++) n += rhs.at( k, i) * at( j, k );
00204                 w.at( j, i ) = n;
00205             }
00206         }
00207         
00208         return w;
00209     }
00210 
00211     Matrix22<T> transposed() const
00212     {
00213         Matrix22<T> ret;
00214         for( int i = 0; i < 2; i++) {
00215             for( int j = 0; j < 2; j++) {
00216                 ret.at( j, i ) = at( i, j );
00217             }
00218         }
00219         
00220         return ret;
00221     }
00222 
00223     operator T*() { return (T*) m; }
00224     operator const T*() const { return (const T*) m; }
00225 
00226     friend std::ostream& operator<<( std::ostream& lhs, const Matrix22<T>& rhs ) 
00227     {
00228         for( int i = 0; i < 2; i++ ) {
00229             lhs << "|\t";
00230             for( int j = 0; j < 2; j++ ) {
00231                 lhs << rhs.at( i, j ) << "\t";
00232             }
00233             lhs << "|" << std::endl;
00234         }
00235         
00236         return lhs;
00237     }
00238 
00239 };
00240 
00241 template<typename T>
00242 class Matrix44 
00243 {
00244 public:
00245     //
00246     // This class is OpenGL friendly and stores the m as how OpenGL would expect it.
00247     // m[i,j]:
00248     // | m[0,0] m[0,1] m[0,2] m[0,2] |
00249     // | m[1,0] m[1,1] m[1,2] m[1,2] |
00250     // | m[2,0] m[2,1] m[2,2] m[2,2] |
00251     // | m[3,0] m[3,1] m[3,2] m[3,2] |
00252     //
00253     // m[idx]
00254     // | m[0] m[4] m[8]  m[12] |
00255     // | m[1] m[5] m[9]  m[13] |
00256     // | m[2] m[6] m[10] m[14] |
00257     // | m[3] m[7] m[11] m[15] |
00258     //
00259 
00260     T m[16];
00261 
00262     Matrix44() { setToIdentity(); }
00263     Matrix44( const T * dt ) 
00264     {
00265         std::memcpy( m, dt, sizeof(T) * 16 ); 
00266     }
00267 
00268     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 )
00269     {
00270         m[0] = d0;  m[4] = d4;  m[8] = d8;  m[12] = d12;
00271         m[1] = d1;  m[5] = d5;  m[9] = d9;  m[13] = d13;
00272         m[2] = d2;  m[6] = d6;  m[10]= d10; m[14] = d14;
00273         m[3] = d3;  m[7] = d7;  m[11]= d11; m[15] = d15;        
00274     }
00275 
00276     Matrix44( const Vec3f &vecX, const Vec3f &vecY, const Vec3f &vecZ )
00277     {
00278         m[0] = vecX.x;  m[4] = vecX.y;  m[8] = vecX.z;  m[12] = 0;
00279         m[1] = vecY.x;  m[5] = vecY.y;  m[9] = vecY.z;  m[13] = 0;
00280         m[2] = vecZ.x;  m[6] = vecZ.y;  m[10]= vecZ.z;  m[14] = 0;
00281         m[3] = 0;       m[7] = 0;       m[11] = 0;      m[15] = 1;
00282     } 
00283 
00284     Matrix44( const Matrix44<T>& src )
00285     {
00286         std::memcpy( m, src.m, sizeof(T) * 16 );
00287     }
00288 
00289     template<typename FromT>
00290     Matrix44(const Matrix44<FromT>& src)
00291     {
00292         for( int i = 0; i < 16; i++ ) {
00293             m[i] = static_cast<T>( src.m[i] );
00294         }
00295     }
00296 
00297     void setToIdentity()
00298     {
00299         m[0] = 1;   m[4] = 0;   m[8] = 0;   m[12] = 0;
00300         m[1] = 0;   m[5] = 1;   m[9] = 0;   m[13] = 0;
00301         m[2] = 0;   m[6] = 0;   m[10] = 1;  m[14] = 0;
00302         m[3] = 0;   m[7] = 0;   m[11] = 0;  m[15] = 1;      
00303     }
00304 
00305     T determinant() const
00306     {
00307         return (m[ 0] * (   m[ 5] * (m[10] * m[15] - m[11] * m[14])  - 
00308                                 m[ 9] * (m[ 6] * m[15] - m[ 7] * m[14])  + 
00309                                 m[13] * (m[ 6] * m[11] - m[ 7] * m[10])) - 
00310                 m[ 4] * (   m[ 1] * (m[10] * m[15] - m[11] * m[14])  - 
00311                                 m[ 9] * (m[ 2] * m[15] - m[ 3] * m[14])  + 
00312                                 m[13] * (m[ 2] * m[11] - m[ 3] * m[10])) +
00313                 m[ 8] * (   m[ 1] * (m[ 6] * m[15] - m[ 7] * m[14])  -
00314                                 m[ 5] * (m[ 2] * m[15] - m[ 3] * m[14])  +
00315                                 m[13] * (m[ 2] * m[ 7] - m[ 3] * m[ 6])) -
00316                 m[12] * (   m[ 1] * (m[ 6] * m[11] - m[ 7] * m[10])  -
00317                                 m[ 5] * (m[ 2] * m[11] - m[ 3] * m[10])  +
00318                                 m[ 9] * (m[ 2] * m[ 7] - m[ 3] * m[ 6])) );
00319     }
00320     
00321     // post-multiplies column vector [rhs.x rhs.y rhs.z 1] and divides by w
00322     Vec3<T> transformPoint( const Vec3<T> &rhs ) const
00323     {
00324         T x =   m[0] * rhs.x + m[4] * rhs.y + m[8] * rhs.z + m[12];
00325         T y =   m[1] * rhs.x + m[5] * rhs.y + m[9] * rhs.z + m[13];
00326         T z =   m[2] * rhs.x + m[6] * rhs.y + m[10] * rhs.z + m[14];
00327         T w =   m[3] * rhs.x + m[7] * rhs.y + m[11] * rhs.z + m[15];
00328         
00329         return Vec3<T>( x / w, y / w, z / w );
00330     }
00331 
00332     // post-multiplies column vector [rhs.x rhs.y rhs.z 1] but omits divide by w
00333     Vec3<T> transformPointAffine( const Vec3<T> &rhs ) const
00334     {
00335         T x =   m[0] * rhs.x + m[4] * rhs.y + m[8] * rhs.z + m[12];
00336         T y =   m[1] * rhs.x + m[5] * rhs.y + m[9] * rhs.z + m[13];
00337         T z =   m[2] * rhs.x + m[6] * rhs.y + m[10] * rhs.z + m[14];
00338         
00339         return Vec3<T>( x, y, z );
00340     }
00341 
00342     // post-multiplies column vector [rhs.x rhs.y rhs.z 0]
00343     Vec3<T> transformVec( const Vec3<T> &rhs ) const
00344     {
00345         T x =   m[0] * rhs.x + m[4] * rhs.y + m[8] * rhs.z;
00346         T y =   m[1] * rhs.x + m[5] * rhs.y + m[9] * rhs.z;
00347         T z =   m[2] * rhs.x + m[6] * rhs.y + m[10] * rhs.z;
00348         
00349         return Vec3<T>( x, y, z );
00350     }
00351 
00352     bool operator==( const Matrix44<T> &rhs ) const
00353     {
00354         for( int i = 0; i < 16; i++ ) {
00355             if( math<T>::abs(m[i] - rhs.m[i]) >= (T)EPSILON )
00356                 return false;
00357         }
00358         
00359         return true;
00360     }
00361 
00362     bool operator!=( const Matrix44<T> &rhs ) const
00363     {
00364         return !(*this == rhs);
00365     }
00366 
00367     T& at( int row, int col ) 
00368     {
00369         assert(row >= 0 && row < 4);
00370         assert(col >= 0 && col < 4);
00371         return m[col * 4 + row];
00372     }
00373 
00374     const T& at( int row, int col ) const 
00375     {
00376         assert(row >= 0 && row < 4);
00377         assert(col >= 0 && col < 4);
00378         return m[col * 4 + row];
00379     }
00380 
00381     Matrix44<T>& operator=( const Matrix44<T>& rhs ) 
00382     {
00383         std::memcpy( m, rhs.m, sizeof(T) * 16 );
00384         return *this;
00385     }
00386 
00387     template<typename FromT>
00388     Matrix44<T>& operator=( const Matrix44<FromT>& rhs ) 
00389     {
00390         for( int i = 0; i < 16; i++ ) {
00391             m[i] = static_cast<T>(rhs.m[i]);
00392         }
00393         return *this;
00394     }
00395 
00396     const Matrix44<T> operator+( const Matrix44<T> &rhs ) const
00397     {
00398         Matrix44<T> ret;
00399         for( int i = 0; i < 16; i++ ) {
00400             ret.m[i] = m[i] + rhs.m[i];
00401         }
00402         return ret;
00403     }
00404 
00405     const Matrix44<T> operator-( const Matrix44<T> &rhs ) const
00406     {
00407         Matrix44<T> ret;
00408         for( int i = 0; i < 16; i++ ) {
00409             ret.m[i] = m[i] - rhs.m[i];
00410         }
00411         return ret;
00412     }
00413 
00414     const Matrix44<T> operator+( T rhs ) const
00415     {
00416         Matrix44<T> ret;
00417         for( int i = 0; i < 16; i++ ) {
00418             ret.m[i] = m[i] + rhs;
00419         }
00420         return ret;
00421     }
00422 
00423     const Matrix44<T> operator-( T rhs ) const
00424     {
00425         Matrix44<T> ret;
00426         for( int i = 0; i < 16; i++ )
00427             ret.m[i] = m[i] - rhs;
00428         return ret;
00429     }
00430 
00431     const Matrix44<T> operator*( T rhs ) const
00432     {
00433         Matrix44<T> ret;
00434         for( int i = 0; i < 16; i++ ) {
00435             ret.m[i] = m[i] * rhs;
00436         }
00437         return ret;
00438     }
00439 
00440     const Matrix44<T> operator/( T rhs ) const
00441     {
00442         Matrix44<T> ret;
00443         T invRhs = static_cast<T>( 1.0 ) / rhs;
00444         for( int i = 0; i < 16; i++ ) {
00445             ret.m[i] = m[i] * invRhs;
00446         }
00447         return ret;
00448     }
00449 
00450     const Vec4<T> operator*( const Vec4<T> &rhs ) const 
00451     {
00452         return Vec4<T>(
00453             m[0] * rhs.x + m[4] * rhs.y + m[8]  * rhs.z + m[12] * rhs.w,
00454             m[1] * rhs.x + m[5] * rhs.y + m[9]  * rhs.z + m[13] * rhs.w,
00455             m[2] * rhs.x + m[6] * rhs.y + m[10] * rhs.z + m[14] * rhs.w,
00456             m[3] * rhs.x + m[7] * rhs.y + m[11] * rhs.z + m[15] * rhs.w
00457             );
00458     }
00459 
00460     // post-multiplies column vector [rhs.x rhs.y rhs.z 1] and divides by w
00461     const Vec3<T> operator*( const Vec3<T> &rhs ) const
00462     {
00463         T x =   m[0] * rhs.x + m[4] * rhs.y + m[8] * rhs.z + m[12];
00464         T y =   m[1] * rhs.x + m[5] * rhs.y + m[9] * rhs.z + m[13];
00465         T z =   m[2] * rhs.x + m[6] * rhs.y + m[10] * rhs.z + m[14];
00466         T w =   m[3] * rhs.x + m[7] * rhs.y + m[11] * rhs.z + m[15];
00467         
00468         return Vec3<T>( x / w, y / w, z / w );
00469     }
00470 
00471     const Matrix44<T> operator*( const Matrix44<T> &rhs ) const 
00472     {
00473         Matrix44<T> result;
00474 
00475         result.m[0] = m[0]*rhs.m[0] + m[4]*rhs.m[1] + m[8]*rhs.m[2] + m[12]*rhs.m[3];
00476         result.m[1] = m[1]*rhs.m[0] + m[5]*rhs.m[1] + m[9]*rhs.m[2] + m[13]*rhs.m[3];
00477         result.m[2] = m[2]*rhs.m[0] + m[6]*rhs.m[1] + m[10]*rhs.m[2] + m[14]*rhs.m[3];
00478         result.m[3] = m[3]*rhs.m[0] + m[7]*rhs.m[1] + m[11]*rhs.m[2] + m[15]*rhs.m[3];
00479 
00480         result.m[4] = m[0]*rhs.m[4] + m[4]*rhs.m[5] + m[8]*rhs.m[6] + m[12]*rhs.m[7];
00481         result.m[5] = m[1]*rhs.m[4] + m[5]*rhs.m[5] + m[9]*rhs.m[6] + m[13]*rhs.m[7];
00482         result.m[6] = m[2]*rhs.m[4] + m[6]*rhs.m[5] + m[10]*rhs.m[6] + m[14]*rhs.m[7];
00483         result.m[7] = m[3]*rhs.m[4] + m[7]*rhs.m[5] + m[11]*rhs.m[6] + m[15]*rhs.m[7];
00484 
00485         result.m[8] = m[0]*rhs.m[8] + m[4]*rhs.m[9] + m[8]*rhs.m[10] + m[12]*rhs.m[11];
00486         result.m[9] = m[1]*rhs.m[8] + m[5]*rhs.m[9] + m[9]*rhs.m[10] + m[13]*rhs.m[11];
00487         result.m[10] = m[2]*rhs.m[8] + m[6]*rhs.m[9] + m[10]*rhs.m[10] + m[14]*rhs.m[11];
00488         result.m[11] = m[3]*rhs.m[8] + m[7]*rhs.m[9] + m[11]*rhs.m[10] + m[15]*rhs.m[11];
00489 
00490         result.m[12] = m[0]*rhs.m[12] + m[4]*rhs.m[13] + m[8]*rhs.m[14] + m[12]*rhs.m[15];
00491         result.m[13] = m[1]*rhs.m[12] + m[5]*rhs.m[13] + m[9]*rhs.m[14] + m[13]*rhs.m[15];
00492         result.m[14] = m[2]*rhs.m[12] + m[6]*rhs.m[13] + m[10]*rhs.m[14] + m[14]*rhs.m[15];
00493         result.m[15] = m[3]*rhs.m[12] + m[7]*rhs.m[13] + m[11]*rhs.m[14] + m[15]*rhs.m[15];
00494 
00495         return result;
00496     }
00497 
00498     Matrix44<T>& operator*=( T s ) 
00499     {
00500         for( size_t i = 0; i < 16; i++ ) {
00501             m[i] *= s;
00502         }
00503         return *this;
00504     }
00505 
00506     Matrix44<T>& operator*=( const Matrix44<T> &rhs )
00507     {
00508         Matrix44<T> result;
00509 
00510         result.m[0] = m[0]*rhs.m[0] + m[4]*rhs.m[1] + m[8]*rhs.m[2] + m[12]*rhs.m[3];
00511         result.m[1] = m[1]*rhs.m[0] + m[5]*rhs.m[1] + m[9]*rhs.m[2] + m[13]*rhs.m[3];
00512         result.m[2] = m[2]*rhs.m[0] + m[6]*rhs.m[1] + m[10]*rhs.m[2] + m[14]*rhs.m[3];
00513         result.m[3] = m[3]*rhs.m[0] + m[7]*rhs.m[1] + m[11]*rhs.m[2] + m[15]*rhs.m[3];
00514 
00515         result.m[4] = m[0]*rhs.m[4] + m[4]*rhs.m[5] + m[8]*rhs.m[6] + m[12]*rhs.m[7];
00516         result.m[5] = m[1]*rhs.m[4] + m[5]*rhs.m[5] + m[9]*rhs.m[6] + m[13]*rhs.m[7];
00517         result.m[6] = m[2]*rhs.m[4] + m[6]*rhs.m[5] + m[10]*rhs.m[6] + m[14]*rhs.m[7];
00518         result.m[7] = m[3]*rhs.m[4] + m[7]*rhs.m[5] + m[11]*rhs.m[6] + m[15]*rhs.m[7];
00519 
00520         result.m[8] = m[0]*rhs.m[8] + m[4]*rhs.m[9] + m[8]*rhs.m[10] + m[12]*rhs.m[11];
00521         result.m[9] = m[1]*rhs.m[8] + m[5]*rhs.m[9] + m[9]*rhs.m[10] + m[13]*rhs.m[11];
00522         result.m[10] = m[2]*rhs.m[8] + m[6]*rhs.m[9] + m[10]*rhs.m[10] + m[14]*rhs.m[11];
00523         result.m[11] = m[3]*rhs.m[8] + m[7]*rhs.m[9] + m[11]*rhs.m[10] + m[15]*rhs.m[11];
00524 
00525         result.m[12] = m[0]*rhs.m[12] + m[4]*rhs.m[13] + m[8]*rhs.m[14] + m[12]*rhs.m[15];
00526         result.m[13] = m[1]*rhs.m[12] + m[5]*rhs.m[13] + m[9]*rhs.m[14] + m[13]*rhs.m[15];
00527         result.m[14] = m[2]*rhs.m[12] + m[6]*rhs.m[13] + m[10]*rhs.m[14] + m[14]*rhs.m[15];
00528         result.m[15] = m[3]*rhs.m[12] + m[7]*rhs.m[13] + m[11]*rhs.m[14] + m[15]*rhs.m[15];
00529 
00530         for( unsigned int i = 0; i < 16; ++i ) {
00531             m[i] = result.m[i];
00532         }
00533 
00534         return *this;
00535     }
00536 
00537     Vec3<T> getTranslation() const
00538     {
00539         return Vec3<T>( at(0,3), at(1,3), at(2,3) );
00540     }
00541 
00542     // concatenate translation (conceptually, translation is before 'this')
00543     void translate( const Vec3<T> &t )
00544     {
00545         m[12] += t.x * m[0] + t.y * m[4] + t.z * m[8];
00546         m[13] += t.x * m[1] + t.y * m[5] + t.z * m[9];
00547         m[14] += t.x * m[2] + t.y * m[6] + t.z * m[10];
00548         m[15] += t.x * m[3] + t.y * m[7] + t.z * m[11];
00549     }
00550 
00551     // concatenate scale (conceptually, scale is before 'this')
00552     void scale( const Vec3<T> &s )
00553     {
00554         m[0] *= s.x;  m[4] *= s.y;  m[8] *= s.z; // m[12] *= 1
00555         m[1] *= s.x;  m[5] *= s.y;  m[9] *= s.z; // m[13] *= 1
00556         m[2] *= s.x;  m[6] *= s.y; m[10] *= s.z; // m[14] *= 1
00557         m[3] *= s.x;  m[7] *= s.y; m[11] *= s.z; // m[15] *= 1
00558     }
00559 
00560     void rotate( const Vec3<T> &axis, T radians )
00561     {
00562         *this *= Matrix44<T>::createRotation( axis, radians );
00563     }
00564 
00565     // rotate by 'r' euler angles in radians
00566     void rotate( const Vec3<T> &r )
00567     {
00568         *this *= Matrix44<T>::createRotation( r );
00569     }
00570     
00571     void rotate( const Vec3<T> &from, const Vec3<T> &to, const Vec3<T> &worldUp )
00572     {
00573         *this *= Matrix44<T>::createRotation( from, to, worldUp );
00574     }
00575 
00576     void transpose()
00577     {
00578         T temp;
00579         temp = m[1]; m[1] = m[4]; m[4] = temp;
00580         temp = m[2]; m[2] = m[8]; m[8] = temp;
00581         temp = m[3]; m[2] = m[12]; m[12] = temp;
00582         temp = m[6]; m[6] = m[9]; m[9] = temp;
00583         temp = m[7]; m[7] = m[13]; m[13] = temp;
00584         temp = m[11]; m[11] = m[14]; m[14] = temp;
00585     }
00586 
00587     Matrix44<T> transposed() const
00588     {
00589         return Matrix44<T>( m[0], m[4], m[8], m[12],
00590                             m[1], m[5], m[9], m[13],
00591                             m[2], m[6], m[10],m[14],
00592                             m[3], m[7], m[11],m[15] ); 
00593     }
00594 
00595     void invert()
00596     {
00597         *this = inverted();
00598     }
00599 
00600     Matrix44<T> inverted() const
00601     {
00602         Matrix44<T> inv;
00603     
00604         T d = determinant();
00605         if( fabs( d ) < (T)EPSILON ) {
00606             return Matrix44<T>(); // returns identity on error
00607         }
00608 
00609         // build adjoint (transpose of matrix of cofactors of this matrix (tricky!)
00610         T sub[3][3] ;
00611         int a,b;
00612         for( int i = 0; i < 4; i++ )  {
00613             for( int j = 0; j < 4; j++ ) {
00614                 a = 0;
00615                 for( int r = 0; r < 3; r++, a++ )  {
00616                     if( r == i ) {
00617                         a++;
00618                     }
00619 
00620                     b = 0;
00621                     for( int s = 0 ; s < 3; s++, b++ ) {
00622                         if( s == j ) {
00623                             b++;
00624                         }
00625                         sub[r][s] = m[a + 4 * b];
00626                     }
00627                 }
00628 
00629                 inv.m[j + 4 * i] =  sub[0][0] * ( sub[1][1] *sub[2][2] - sub[2][1] * sub[1][2] ) - 
00630                                         sub[0][1] * ( sub[1][0] *sub[2][2] - sub[2][0] * sub[1][2] ) + 
00631                                         sub[0][2] * ( sub[1][0] *sub[2][1] - sub[2][0] * sub[1][1] );
00632 
00633                 if( ( (i + j) % 2 != 0) && ( fabs(inv.m[j + 4 * i]) > (T)EPSILON ) ) {
00634                     inv.m[j + 4 * i] *= (T)-1.0;
00635                 }
00636             }
00637         }
00638 
00639         inv *= (T)(1.0 / d);
00640         return inv;
00641     }
00642 
00643     void affineInvert()
00644     {
00645         *this = affineInverted();
00646     }
00647     
00648     // assumes the matrix is affine, ie the bottom row is [0 0 0 1]
00649     Matrix44<T> affineInverted() const
00650     {
00651         Matrix44<T> result;
00652         
00653         // compute upper left 3x3 matrix determinant
00654         T cofactor0 = m[5]*m[10] - m[6]*m[9];
00655         T cofactor4 = m[2]*m[9] - m[1]*m[10];
00656         T cofactor8 = m[1]*m[6] - m[2]*m[5];
00657         T det = m[0]*cofactor0 + m[4]*cofactor4 + m[8]*cofactor8;
00658         if( fabs( det ) <= EPSILON ) {
00659             return result;
00660         }
00661 
00662         // create adjunct matrix and multiply by 1/det to get upper 3x3
00663         T invDet = ((T)1.0) / det;
00664         result.m[0] = invDet*cofactor0;
00665         result.m[1] = invDet*cofactor4;
00666         result.m[2] = invDet*cofactor8;
00667        
00668         result.m[4] = invDet*(m[6]*m[8] - m[4]*m[10]);
00669         result.m[5] = invDet*(m[0]*m[10] - m[2]*m[8]);
00670         result.m[6] = invDet*(m[2]*m[4] - m[0]*m[6]);
00671 
00672         result.m[8] = invDet*(m[4]*m[9] - m[5]*m[8]);
00673         result.m[9] = invDet*(m[1]*m[8] - m[0]*m[9]);
00674         result.m[10] = invDet*(m[0]*m[5] - m[1]*m[4]);
00675 
00676         // multiply -translation by inverted 3x3 to get its inverse     
00677         result.m[12] = -result.m[0]*m[12] - result.m[4]*m[13] - result.m[8]*m[14];
00678         result.m[13] = -result.m[1]*m[12] - result.m[5]*m[13] - result.m[9]*m[14];
00679         result.m[14] = -result.m[2]*m[12] - result.m[6]*m[13] - result.m[10]*m[14];
00680 
00681         return result;      
00682     }
00683 
00684     Matrix44<T> invertTransform() const
00685     {
00686         Matrix44<T> ret;
00687 
00688         // inverse translation
00689         ret.at( 0, 3 ) = -at( 0, 3 );
00690         ret.at( 1, 3 ) = -at( 1, 3 );
00691         ret.at( 2, 3 ) = -at( 2, 3 );
00692         ret.at( 3, 3 ) =  at( 3, 3 );
00693 
00694         // transpose rotation part
00695         for( int i = 0; i < 3; i++ ) {
00696             for( int j = 0; j < 3; j++ ) {
00697                 ret.at( j, i ) = at( i, j );
00698             }
00699         }
00700         return ret;
00701     }
00702     
00703     Vec4<T> getRow( uint8_t row ) const {
00704         return Vec4<T>( m[row + 0], m[row + 4], m[row + 8], m[row + 12] );
00705     }
00706 
00707     void    setRow( uint8_t row, const Vec4<T> &v ) {
00708         m[row + 0] = v.x; m[row + 4] = v.y; m[row + 8] = v.z; m[row + 12] = v.w;
00709     }
00710     
00711     static Matrix44<T> createTranslation( const Vec3<T> &v, T w = 1 );
00712     static Matrix44<T> createScale( const Vec3<T> &v );
00713     static Matrix44<T> createRotation( const Vec3<T> &axis, T angle );
00714     static Matrix44<T> createRotation( const Vec3<T> &from, const Vec3<T> &to, const Vec3<T> &worldUp );
00715     // equivalent to rotate( zAxis, z ), then rotate( yAxis, y ) then rotate( xAxis, x )
00716     static Matrix44<T> createRotation( const Vec3<T> &eulerRadians );
00717     
00718     static Matrix44<T> alignZAxisWithTarget( Vec3<T> targetDir, Vec3<T> upDir );
00719 
00720     operator T*() { return (T*) m; }
00721     operator const T*() const { return (const T*) m; }
00722 
00723     friend std::ostream& operator<<( std::ostream &lhs, const Matrix44<T> &rhs ) 
00724     {
00725         for( int i = 0; i < 4; i++) {
00726             lhs << " |";
00727             for( int j = 0; j < 4; j++) {
00728                 lhs << std::setw( 12 ) << std::setprecision( 5 ) << rhs.m[j*4+i];
00729             }
00730             lhs << "|" << std::endl;
00731         }
00732         
00733         return lhs;
00734     }
00735 };
00736 
00737 //  These methods compute a set of reference frames, defined by their
00738 //  transformation matrix, along a curve. It is designed so that the 
00739 //  array of points and the array of matrices used to fetch these routines 
00740 //  don't need to be ordered as the curve.
00741 //  
00742 //  A typical usage would be :
00743 //
00744 //      m[0] = Imath::firstFrame( p[0], p[1], p[2] );
00745 //      for( int i = 1; i < n - 1; i++ )
00746 //      {
00747 //          m[i] = Imath::nextFrame( m[i-1], p[i-1], p[i], t[i-1], t[i] );
00748 //      }
00749 //      m[n-1] = Imath::lastFrame( m[n-2], p[n-2], p[n-1] );
00750 //
00751 //  See Graphics Gems I for the underlying algorithm.
00752 //  These are also called Parallel Transport Frames
00753 //    see Game Programming Gems 2, Section 2.5
00754 
00755 template<typename T>
00756 Matrix44<T> firstFrame( const Vec3<T> &firstPoint, const Vec3<T> &secondPoint, const Vec3<T> &thirdPoint );
00757 template<typename T>
00758 Matrix44<T> nextFrame( const Matrix44<T> &prevMatrix, const Vec3<T> &prevPoint, const Vec3<T> &curPoint,
00759             Vec3<T> &prevTangent, Vec3<T> &curTangent );
00760 template<typename T>
00761 Matrix44<T> lastFrame( const Matrix44<T> &prevMatrix, const Vec3<T> &prevPoint, const Vec3<T> &lastPoint );
00762 
00763 typedef Matrix22<float> Matrix22f;
00764 typedef Matrix22<double> Matrix22d;
00765 typedef Matrix44<float> Matrix44f;
00766 typedef Matrix44<double> Matrix44d;
00767 
00768 } // namespace cinder