Cinder

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

include/cinder/Matrix33.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 // Matrix33
00036 template< typename T >
00037 class Matrix33 
00038 {
00039 public:
00040     typedef T TYPE;
00041     //
00042     static const size_t DIM     = 3;
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] |
00050     // | m[1,0] m[1,1] m[1,2] |
00051     // | m[2,0] m[2,1] m[2,2] |
00052     //
00053     // m[idx]
00054     // | m[0] m[3] m[6] |
00055     // | m[1] m[4] m[7] |
00056     // | m[2] m[5] m[8] |
00057     //
00058     union {
00059         T m[9];
00060         struct {
00061             // This looks like it's transposed from the above, but it's really not.
00062             // It just has to be written this way so it follows the right ordering
00063             // in the memory layout as well as being mathematically correct.
00064             T m00, m10, m20;
00065             T m01, m11, m21;
00066             T m02, m12, m22;
00067         };
00068         // [Cols][Rows]
00069         T mcols[3][3];
00070     };
00071     
00072     Matrix33();
00073 
00074     Matrix33( T s );
00075 
00076     // OpenGL layout - unless srcIsRowMajor is true
00077     Matrix33( const T *dt, bool srcIsRowMajor = false );
00078 
00079     // OpenGL layout: m[0]=d0, m[1]=d1, m[2]=d2 ... m[6]=d6, m[7]=d7, m[8]=d8 - unless srcIsRowMajor is true
00080     Matrix33( T d0, T d1, T d2, T d3, T d4, T d5, T d6, T d7, T d8, bool srcIsRowMajor = false );
00081 
00082     // Creates matrix with column vectors vx, vy and z
00083     Matrix33( const Vec3<T> &vx, const Vec3<T> &vy, const Vec3<T> &vz ); 
00084 
00085     template< typename FromT >
00086     Matrix33( const Matrix33<FromT>& src );
00087 
00088     Matrix33( const Matrix22<T>& src );
00089 
00090     Matrix33( const Matrix33<T>& src );
00091 
00092                         operator T*() { return (T*)m; }
00093                         operator const T*() const { return (const T*)m; }
00094 
00095     Matrix33<T>&        operator=( const Matrix33<T>& rhs );
00096     Matrix33<T>&        operator=( T rhs );
00097 
00098     template< typename FromT >
00099     Matrix33<T>&        operator=( const Matrix33<FromT>& rhs );
00100 
00101     // remaining columns and rows will be filled with identity values
00102     Matrix33<T>&        operator=( const Matrix22<T>& rhs );
00103 
00104     bool                equalCompare( const Matrix33<T>& rhs, T epsilon ) const;
00105     bool                operator==( const Matrix33<T> &rhs ) const { return equalCompare( rhs, (T)EPSILON ); }
00106     bool                operator!=( const Matrix33<T> &rhs ) const { return ! ( *this == rhs ); }
00107 
00108     Matrix33<T>&        operator*=( const Matrix33<T> &rhs );
00109     Matrix33<T>&        operator+=( const Matrix33<T> &rhs );
00110     Matrix33<T>&        operator-=( const Matrix33<T> &rhs );
00111 
00112     Matrix33<T>&        operator*=( T s );
00113     Matrix33<T>&        operator/=( T s );
00114     Matrix33<T>&        operator+=( T s );
00115     Matrix33<T>&        operator-=( T s );
00116 
00117     const Matrix33<T>   operator*( const Matrix33<T> &rhs ) const;
00118     const Matrix33<T>   operator+( const Matrix33<T> &rhs ) const;
00119     const Matrix33<T>   operator-( const Matrix33<T> &rhs ) const;
00120 
00121     // post-multiplies column vector [rhs.x rhs.y rhs.z]
00122     const Vec3<T>       operator*( const Vec3<T> &rhs ) const;
00123 
00124     const Matrix33<T>   operator*( T rhs ) const;
00125     const Matrix33<T>   operator/( T rhs ) const;
00126     const Matrix33<T>   operator+( T rhs ) const;
00127     const Matrix33<T>   operator-( T rhs ) const;
00128 
00129     // Accessors
00130     T&                  at( int row, int col );
00131     const T&            at( int row, int col ) const;
00132 
00133     // OpenGL layout - unless srcIsRowMajor is true
00134     void                set( const T *dt, bool srcIsRowMajor = false );
00135     // OpenGL layout: m[0]=d0, m[1]=d1, m[2]=d2 ... m[6]=d6, m[7]=d7, m[8]=d8 - unless srcIsRowMajor is true
00136     void                set( T d0, T d1, T d2, T d3, T d4, T d5, T d6, T d7, T d8, bool srcIsRowMajor = false );
00137 
00138     Vec3<T>             getColumn( int col ) const;
00139     void                setColumn( int col, const Vec3<T> &v );
00140 
00141     Vec3<T>             getRow( int row ) const;
00142     void                setRow( int row, const Vec3<T> &v );
00143 
00144     void                getColumns( Vec3<T> *c0, Vec3<T> *c1, Vec3<T> *c2 ) const;
00145     void                setColumns( const Vec3<T> &c0, const Vec3<T> &c1, const Vec3<T> &c2 );
00146 
00147     void                getRows( Vec3<T> *r0, Vec3<T> *r1, Vec3<T> *r2 ) const;
00148     void                setRows( const Vec3<T> &r0, const Vec3<T> &r1, const Vec3<T> &r2 );
00149 
00150     // returns a sub-matrix starting at row, col
00151     Matrix22<T>         subMatrix22( int row, int col ) const;
00152 
00153     void                setToNull();
00154     void                setToIdentity();
00155 
00156     T                   determinant() const;
00157     T                   trace() const;
00158 
00159     Matrix33<T>         diagonal() const;
00160 
00161     Matrix33<T>         lowerTriangular() const;
00162     Matrix33<T>         upperTriangular() const;
00163 
00164     void                transpose();
00165     Matrix33<T>         transposed() const;
00166 
00167     void                invert (T epsilon = EPSILON ) { *this = inverted( epsilon ); }
00168     Matrix33<T>         inverted( T epsilon = EPSILON ) const;
00169 
00170     // pre-multiplies row vector v - no divide by w
00171     Vec3<T>             preMultiply( const Vec3<T> &v ) const;
00172 
00173     // post-multiplies column vector v - no divide by w
00174     Vec3<T>             postMultiply( const Vec3<T> &v ) const;
00175 
00176     // post-multiplies column vector [rhs.x rhs.y rhs.z]
00177     Vec3<T>             transformVec( const Vec3<T> &v ) const { return postMultiply( v ); }
00178 
00179     // rotate by radians on axis (conceptually, rotate is before 'this')
00180     template <template <typename> class VecT>
00181     void                rotate( const VecT<T> &axis, T radians ) { *this *= Matrix33<T>::createRotation( axis, radians ); }
00182     // rotate by eulerRadians - Euler angles in radians (conceptually, rotate is before 'this')
00183     template <template <typename> class VecT>
00184     void                rotate( const VecT<T> &eulerRadians ) { *this *= Matrix33<T>::createRotation( eulerRadians ); }
00185     // rotate by matrix derives rotation matrix using from, to, worldUp (conceptually, rotate is before 'this')
00186     template <template <typename> class VecT> 
00187     void                rotate( const VecT<T> &from, const VecT<T> &to, const VecT<T> &worldUp ) { *this *= Matrix33<T>::createRotation( from, to, worldUp ); }
00188 
00189     // concatenate scale (conceptually, scale is before 'this')
00190     void                scale( T s );
00191     void                scale( const Vec2<T> &v );
00192     void                scale( const Vec3<T> &v );
00193 
00194     // transposes rotation sub-matrix and inverts translation
00195     Matrix33<T>         invertTransform() const;
00196 
00197     // returns an identity matrix
00198     static Matrix33<T>  identity() { return Matrix33( 1, 0, 0, 0, 1, 0, 0, 0, 1 ); }
00199     // returns 1 filled matrix
00200     static Matrix33<T>  one() { return Matrix33( (T)1 ); }
00201     // returns 0 filled matrix
00202     static Matrix33<T>  zero() { return Matrix33( (T)0 ); }
00203 
00204     // creates rotation matrix
00205     static Matrix33<T>  createRotation( const Vec3<T> &axis, T radians );
00206     static Matrix33<T>  createRotation( const Vec3<T> &from, const Vec3<T> &to, const Vec3<T> &worldUp );
00207     // equivalent to rotate( zAxis, z ), then rotate( yAxis, y ) then rotate( xAxis, x )
00208     static Matrix33<T>  createRotation( const Vec3<T> &eulerRadians );
00209 
00210     // creates scale matrix
00211     static Matrix33<T>  createScale( T s );
00212     static Matrix33<T>  createScale( const Vec2<T> &v );
00213     static Matrix33<T>  createScale( const Vec3<T> &v );
00214 
00215     // creates a rotation matrix with z-axis aligned to targetDir   
00216     static Matrix33<T>  alignZAxisWithTarget( Vec3<T> targetDir, Vec3<T> upDir );
00217 
00218     friend std::ostream& operator<<( std::ostream &lhs, const Matrix33<T> &rhs ) 
00219     {
00220         for( int i = 0; i < 3; i++) {
00221             lhs << " |";
00222             for( int j = 0; j < 3; j++) {
00223                 lhs << std::setw( 12 ) << std::setprecision( 5 ) << rhs.m[j*3+i];
00224             }
00225             lhs << "|" << std::endl;
00226         }
00227         
00228         return lhs;
00229     }
00230 };
00231 
00232 template< typename T >
00233 Matrix33<T>::Matrix33()
00234 {
00235     setToIdentity();
00236 }
00237 
00238 template< typename T >
00239 Matrix33<T>::Matrix33( T s )
00240 {
00241     for( int i = 0; i < DIM_SQ; ++i ) {
00242         m[i] = s;
00243     }
00244 }
00245 
00246 template< typename T >
00247 Matrix33<T>::Matrix33( const T *dt, bool srcIsRowMajor )
00248 {
00249     set( dt, srcIsRowMajor );
00250 }
00251 
00252 template< typename T >
00253 Matrix33<T>::Matrix33( T d0, T d1, T d2, T d3, T d4, T d5, T d6, T d7, T d8, bool srcIsRowMajor )
00254 {
00255     set( d0, d1, d2, 
00256         d3, d4, d5, 
00257         d6, d7, d8, srcIsRowMajor );
00258 }
00259 
00260 template< typename T >
00261 Matrix33<T>::Matrix33( const Vec3<T> &vx, const Vec3<T> &vy, const Vec3<T> &vz )
00262 {
00263     m00 = vx.x; m01 = vy.x; m02 = vz.x;
00264     m10 = vx.y; m11 = vy.y; m12 = vz.y;
00265     m20 = vx.z; m21 = vy.z; m22 = vz.z;
00266 }
00267 
00268 template< typename T >
00269 template< typename FromT >
00270 Matrix33<T>::Matrix33( const Matrix33<FromT>& src )
00271 {
00272     for( int i = 0; i < DIM_SQ; ++i ) {
00273         m[i] = static_cast<T>( src.m[i] );
00274     }
00275 }
00276 
00277 template< typename T >
00278 Matrix33<T>::Matrix33( const Matrix22<T>& src )
00279 {
00280     setToIdentity();
00281     m00 = src.m00; m01 = src.m01;
00282     m10 = src.m10; m11 = src.m11;
00283 }
00284 
00285 template< typename T >
00286 Matrix33<T>::Matrix33( const Matrix33<T>& src )
00287 {
00288     std::memcpy( m, src.m, MEM_LEN );
00289 }
00290 
00291 template< typename T >
00292 Matrix33<T>& Matrix33<T>::operator=( const Matrix33<T>& rhs )
00293 {
00294     memcpy( m, rhs.m, MEM_LEN );
00295     return *this;
00296 }
00297 
00298 template< typename T >
00299 Matrix33<T>& Matrix33<T>::operator=( T rhs )
00300 {
00301     for( int i = 0; i < DIM_SQ; ++i ) {
00302         m[i] = rhs;
00303     }
00304     return *this;
00305 }
00306 
00307 template< typename T >
00308 template< typename FromT >
00309 Matrix33<T>& Matrix33<T>::operator=( const Matrix33<FromT>& rhs )
00310 {
00311     for( int i = 0; i < DIM_SQ; i++ ) {
00312         m[i] = static_cast<T>(rhs.m[i]);
00313     }
00314     return *this;
00315 }
00316 
00317 template< typename T >
00318 Matrix33<T>& Matrix33<T>::operator=( const Matrix22<T>& rhs )
00319 {
00320     setToIdentity();
00321     m00 = rhs.m00; m01 = rhs.m01;
00322     m10 = rhs.m10; m11 = rhs.m11;
00323     return *this;
00324 }
00325 
00326 template< typename T >
00327 bool Matrix33<T>::equalCompare( const Matrix33<T>& rhs, T epsilon ) const
00328 {
00329     for( int i = 0; i < DIM_SQ; ++i ) {
00330         if( math<T>::abs(m[i] - rhs.m[i]) >= epsilon )
00331             return false;
00332     }
00333     return true;
00334 }
00335 
00336 template< typename T >
00337 Matrix33<T>& Matrix33<T>::operator*=( const Matrix33<T> &rhs )
00338 {
00339     Matrix33<T> mat;
00340 
00341     mat.m[0] = m[0]*rhs.m[0] + m[3]*rhs.m[1] + m[6]*rhs.m[2];
00342     mat.m[1] = m[1]*rhs.m[0] + m[4]*rhs.m[1] + m[7]*rhs.m[2];
00343     mat.m[2] = m[2]*rhs.m[0] + m[5]*rhs.m[1] + m[8]*rhs.m[2];
00344 
00345     mat.m[3] = m[0]*rhs.m[3] + m[3]*rhs.m[4] + m[6]*rhs.m[5];
00346     mat.m[4] = m[1]*rhs.m[3] + m[4]*rhs.m[4] + m[7]*rhs.m[5];
00347     mat.m[5] = m[2]*rhs.m[3] + m[5]*rhs.m[4] + m[8]*rhs.m[5];
00348 
00349     mat.m[6] = m[0]*rhs.m[6] + m[3]*rhs.m[7] + m[6]*rhs.m[8];
00350     mat.m[7] = m[1]*rhs.m[6] + m[4]*rhs.m[7] + m[7]*rhs.m[8];
00351     mat.m[8] = m[2]*rhs.m[6] + m[5]*rhs.m[7] + m[8]*rhs.m[8];
00352 
00353     *this = mat;
00354 
00355     return *this;
00356 }
00357 
00358 template< typename T >
00359 Matrix33<T>& Matrix33<T>::operator+=( const Matrix33<T> &rhs )
00360 {
00361     for( int i = 0; i < DIM_SQ; ++i ) {
00362         m[i] += rhs.m[i];
00363     }
00364     return *this;
00365 }
00366 
00367 template< typename T >
00368 Matrix33<T>& Matrix33<T>::operator-=( const Matrix33<T> &rhs )
00369 {
00370     for( int i = 0; i < DIM_SQ; ++i ) {
00371         m[i] -= rhs.m[i];
00372     }
00373     return *this;
00374 }
00375 
00376 template< typename T >
00377 Matrix33<T>& Matrix33<T>::operator*=( T s )
00378 {
00379     for( int i = 0; i < DIM_SQ; ++i ) {
00380         m[i] *= s;
00381     }
00382     return *this;
00383 }
00384 
00385 template< typename T >
00386 Matrix33<T>& Matrix33<T>::operator/=( T s )
00387 {
00388     T invS = (T)1/s;
00389     for( int i = 0; i < DIM_SQ; ++i ) {
00390         m[i] *= invS;
00391     }
00392     return *this;
00393 }
00394 
00395 template< typename T >
00396 Matrix33<T>& Matrix33<T>::operator+=( T s )
00397 {
00398     for( int i = 0; i < DIM_SQ; ++i ) {
00399         m[i] += s;
00400     }
00401     return *this;
00402 }
00403 
00404 template< typename T >
00405 Matrix33<T>& Matrix33<T>::operator-=( T s )
00406 {
00407     for( int i = 0; i < DIM_SQ; ++i ) {
00408         m[i] -= s;
00409     }
00410     return *this;
00411 }
00412 
00413 template< typename T >
00414 const Matrix33<T> Matrix33<T>::operator*( const Matrix33<T> &rhs ) const
00415 {
00416     Matrix33<T> ret;
00417 
00418     ret.m[0] = m[0]*rhs.m[0] + m[3]*rhs.m[1] + m[6]*rhs.m[2];
00419     ret.m[1] = m[1]*rhs.m[0] + m[4]*rhs.m[1] + m[7]*rhs.m[2];
00420     ret.m[2] = m[2]*rhs.m[0] + m[5]*rhs.m[1] + m[8]*rhs.m[2];
00421 
00422     ret.m[3] = m[0]*rhs.m[3] + m[3]*rhs.m[4] + m[6]*rhs.m[5];
00423     ret.m[4] = m[1]*rhs.m[3] + m[4]*rhs.m[4] + m[7]*rhs.m[5];
00424     ret.m[5] = m[2]*rhs.m[3] + m[5]*rhs.m[4] + m[8]*rhs.m[5];
00425 
00426     ret.m[6] = m[0]*rhs.m[6] + m[3]*rhs.m[7] + m[6]*rhs.m[8];
00427     ret.m[7] = m[1]*rhs.m[6] + m[4]*rhs.m[7] + m[7]*rhs.m[8];
00428     ret.m[8] = m[2]*rhs.m[6] + m[5]*rhs.m[7] + m[8]*rhs.m[8];
00429 
00430     return ret;
00431 }
00432 
00433 template< typename T >
00434 const Matrix33<T> Matrix33<T>::operator+( const Matrix33<T> &rhs ) const
00435 {
00436     Matrix33<T> ret;
00437     for( int i = 0; i < DIM_SQ; ++i ) {
00438         ret.m[i] = m[i] + rhs.m[i];
00439     }
00440     return ret;
00441 }
00442 
00443 template< typename T >
00444 const Matrix33<T> Matrix33<T>::operator-( const Matrix33<T> &rhs ) const
00445 {
00446     Matrix33<T> ret;
00447     for( int i = 0; i < DIM_SQ; ++i ) {
00448         ret.m[i] = m[i] - rhs.m[i];
00449     }
00450     return ret;
00451 }
00452 
00453 template< typename T >
00454 const Vec3<T> Matrix33<T>::operator*( const Vec3<T> &rhs ) const
00455 {
00456     return Vec3<T>(
00457         m[0]*rhs.x + m[3]*rhs.y + m[6]*rhs.z,
00458         m[1]*rhs.x + m[4]*rhs.y + m[7]*rhs.z,
00459         m[2]*rhs.x + m[5]*rhs.y + m[8]*rhs.z
00460         );
00461 }
00462 
00463 template< typename T >
00464 const Matrix33<T> Matrix33<T>::operator*( T rhs ) const
00465 {
00466     Matrix33<T> ret;
00467     for( int i = 0; i < DIM_SQ; ++i ) {
00468         ret.m[i] = m[i]*rhs;
00469     }
00470     return ret;
00471 }
00472 
00473 template< typename T >
00474 const Matrix33<T> Matrix33<T>::operator/( T rhs ) const
00475 {
00476     Matrix33<T> ret;
00477     T s = (T)1/rhs;
00478     for( int i = 0; i < DIM_SQ; ++i ) {
00479         ret.m[i] = m[i]*s;
00480     }
00481     return ret;
00482 }
00483 
00484 template< typename T >
00485 const Matrix33<T> Matrix33<T>::operator+( T rhs ) const
00486 {
00487     Matrix33<T> ret;
00488     for( int i = 0; i < DIM_SQ; ++i ) {
00489         ret.m[i] = m[i] + rhs;
00490     }
00491     return ret;
00492 }
00493 
00494 template< typename T >
00495 const Matrix33<T> Matrix33<T>::operator-( T rhs ) const
00496 {
00497     Matrix33<T> ret;
00498     for( int i = 0; i < DIM_SQ; ++i ) {
00499         ret.m[i] = m[i] - rhs;
00500     }
00501     return ret;
00502 }
00503 
00504 template< typename T >
00505 T& Matrix33<T>::at( int row, int col ) 
00506 {
00507     assert(row >= 0 && row < DIM);
00508     assert(col >= 0 && col < DIM);
00509     return m[col*DIM + row];
00510 }
00511 
00512 template< typename T >
00513 const T& Matrix33<T>::at( int row, int col ) const 
00514 {
00515     assert(row >= 0 && row < DIM);
00516     assert(col >= 0 && col < DIM);
00517     return m[col*DIM + row];
00518 }
00519 
00520 template< typename T >
00521 void Matrix33<T>::set( const T *dt, bool srcIsRowMajor )
00522 {
00523     if( srcIsRowMajor ) {
00524         m[0] = dt[0]; m[3] = dt[1]; m[6] = dt[2];
00525         m[1] = dt[3]; m[4] = dt[4]; m[7] = dt[5];
00526         m[2] = dt[6]; m[5] = dt[7]; m[8] = dt[8];
00527     }
00528     else {
00529         std::memcpy( m, dt, MEM_LEN );
00530     }
00531 }
00532 
00533 template< typename T >
00534 void Matrix33<T>::set( T d0, T d1, T d2, T d3, T d4, T d5, T d6, T d7, T d8, bool srcIsRowMajor )
00535 {
00536     if( srcIsRowMajor ) {
00537         m[0] = d0; m[3] = d1; m[6] = d2;
00538         m[1] = d3; m[4] = d4; m[7] = d5;
00539         m[2] = d6; m[5] = d7; m[8] = d8;
00540     }
00541     else {
00542         m[0] = d0; m[3] = d3; m[6] = d6;
00543         m[1] = d1; m[4] = d4; m[7] = d7;
00544         m[2] = d2; m[5] = d5; m[8] = d8;
00545     }
00546 }
00547 
00548 template< typename T >
00549 Vec3<T> Matrix33<T>::getColumn( int col ) const
00550 {
00551     size_t i = col*DIM;
00552     return Vec3<T>( 
00553         m[i + 0], 
00554         m[i + 1], 
00555         m[i + 2]
00556     );
00557 }
00558 
00559 template< typename T >
00560 void Matrix33<T>::setColumn( int col, const Vec3<T> &v )
00561 {
00562     size_t i = col*DIM;
00563     m[i + 0] = v.x;
00564     m[i + 1] = v.y;
00565     m[i + 2] = v.z;
00566 }
00567 
00568 template< typename T >
00569 Vec3<T> Matrix33<T>::getRow( int row ) const 
00570 { 
00571     return Vec3<T>( 
00572         m[row +  0],
00573         m[row +  3],
00574         m[row +  6]
00575     ); 
00576 }
00577 
00578 template< typename T >
00579 void Matrix33<T>::setRow( int row, const Vec3<T> &v ) 
00580 { 
00581     m[row +  0] = v.x; 
00582     m[row +  3] = v.y; 
00583     m[row +  6] = v.z; 
00584 }
00585 
00586 template< typename T >
00587 void Matrix33<T>::getColumns( Vec3<T> *c0, Vec3<T> *c1, Vec3<T> *c2 ) const
00588 {
00589     *c0 = getColumn( 0 );
00590     *c1 = getColumn( 1 );
00591     *c2 = getColumn( 2 );
00592 }
00593 
00594 template< typename T >
00595 void Matrix33<T>::setColumns( const Vec3<T> &c0, const Vec3<T> &c1, const Vec3<T> &c2 )
00596 {
00597     setColumn( 0, c0 );
00598     setColumn( 1, c1 );
00599     setColumn( 2, c2 );
00600 }
00601 
00602 template< typename T >
00603 void Matrix33<T>::getRows( Vec3<T> *r0, Vec3<T> *r1, Vec3<T> *r2 ) const
00604 {
00605     *r0 = getRow( 0 );
00606     *r1 = getRow( 1 );
00607     *r2 = getRow( 2 );
00608 }
00609 
00610 template< typename T >
00611 void Matrix33<T>::setRows( const Vec3<T> &r0, const Vec3<T> &r1, const Vec3<T> &r2 )
00612 {
00613     setRow( 0, r0 );
00614     setRow( 1, r1 );
00615     setRow( 2, r2 );
00616 }
00617 
00618 template< typename T >
00619 Matrix22<T> Matrix33<T>::subMatrix22( int row, int col ) const
00620 {
00621     Matrix22<T> ret;
00622     ret.setToNull();
00623 
00624     for( int i = 0; i < 2; ++i ) {
00625         int r = row + i;
00626         if( r >= 3 ) {
00627             continue;
00628         }
00629         for( int j = 0; j < 2; ++j ) {
00630             int c = col + j;
00631             if( c >= 3 ) {
00632                 continue;
00633             }
00634             ret.at( i, j ) = at( r, c );
00635         }
00636     }
00637 
00638     return ret;
00639 }
00640 
00641 template< typename T >
00642 void Matrix33<T>::setToNull()
00643 {
00644     std::memset( m, 0, MEM_LEN );
00645 }
00646 
00647 template< typename T >
00648 void Matrix33<T>::setToIdentity()
00649 {
00650     m00 = 1; m01 = 0; m02 = 0;
00651     m10 = 0; m11 = 1; m12 = 0;
00652     m20 = 0; m21 = 0; m22 = 1;
00653 }
00654 
00655 template< typename T >
00656 T Matrix33<T>::determinant() const
00657 {
00658     T co00 = m[4]*m[8] - m[5]*m[7];
00659     T co10 = m[5]*m[6] - m[3]*m[8];
00660     T co20 = m[3]*m[7] - m[4]*m[6];
00661 
00662     T det  = m[0]*co00 + m[1]*co10 + m[2]*co20;
00663 
00664     return det;
00665 }
00666 
00667 template< typename T >
00668 T Matrix33<T>::trace() const
00669 {
00670     return m00 + m11 + m22;
00671 }
00672 
00673 template< typename T >
00674 Matrix33<T> Matrix33<T>::diagonal() const
00675 {
00676     Matrix33 ret;
00677     ret.m00 = m00; ret.m01 =   0; ret.m02 =   0;
00678     ret.m10 =   0; ret.m11 = m11; ret.m12 =   0;
00679     ret.m20 =   0; ret.m21 =   0; ret.m22 = m22;
00680     return ret;
00681 }
00682 
00683 template< typename T >
00684 Matrix33<T> Matrix33<T>::lowerTriangular() const
00685 {
00686     Matrix33 ret;
00687     ret.m00 = m00; ret.m01 =   0; ret.m02 =   0;
00688     ret.m10 = m10; ret.m11 = m11; ret.m12 =   0;
00689     ret.m20 = m20; ret.m21 = m21; ret.m22 = m22;
00690     return ret;
00691 }
00692 
00693 template< typename T >
00694 Matrix33<T> Matrix33<T>::upperTriangular() const
00695 {
00696     Matrix33 ret;
00697     ret.m00 = m00; ret.m01 = m01; ret.m02 = m02;
00698     ret.m10 =   0; ret.m11 = m11; ret.m12 = m12;
00699     ret.m20 =   0; ret.m21 =   0; ret.m22 = m22;
00700     return ret;
00701 }
00702 
00703 template< typename T >
00704 void Matrix33<T>::transpose()
00705 {
00706     T t;
00707     t = m01; m01 = m10; m10 = t;
00708     t = m02; m02 = m20; m20 = t;
00709     t = m12; m12 = m21; m21 = t;
00710 }
00711 
00712 template< typename T >
00713 Matrix33<T> Matrix33<T>::transposed() const
00714 {
00715     return Matrix33<T>( 
00716         m[ 0], m[ 3], m[6],
00717         m[ 1], m[ 4], m[7],
00718         m[ 2], m[ 5], m[8]
00719     );
00720 }
00721 
00722 template< typename T >
00723 Matrix33<T> Matrix33<T>::inverted( T epsilon ) const
00724 {
00725     Matrix33<T> inv( (T)0 );
00726 
00727     // Compute the adjoint.
00728     inv.m[0] = m[4]*m[8] - m[5]*m[7];
00729     inv.m[1] = m[2]*m[7] - m[1]*m[8];
00730     inv.m[2] = m[1]*m[5] - m[2]*m[4];
00731     inv.m[3] = m[5]*m[6] - m[3]*m[8];
00732     inv.m[4] = m[0]*m[8] - m[2]*m[6];
00733     inv.m[5] = m[2]*m[3] - m[0]*m[5];
00734     inv.m[6] = m[3]*m[7] - m[4]*m[6];
00735     inv.m[7] = m[1]*m[6] - m[0]*m[7];
00736     inv.m[8] = m[0]*m[4] - m[1]*m[3];
00737 
00738     T det = m[0]*inv.m[0] + m[1]*inv.m[3] + m[2]*inv.m[6];
00739 
00740     if( fabs( det ) < epsilon ) {
00741         T invDet = (T)1/det;
00742         inv.m[0] *= invDet;
00743         inv.m[1] *= invDet;
00744         inv.m[2] *= invDet;
00745         inv.m[3] *= invDet;
00746         inv.m[4] *= invDet;
00747         inv.m[5] *= invDet;
00748         inv.m[6] *= invDet;
00749         inv.m[7] *= invDet;
00750         inv.m[8] *= invDet;
00751     }
00752 
00753     return inv;
00754 }
00755 
00756 template< typename T >
00757 Vec3<T> Matrix33<T>::preMultiply( const Vec3<T> &v ) const
00758 {
00759     return Vec3<T>(
00760         v.x*m00 + v.y*m10 + v.z*m20,
00761         v.x*m01 + v.y*m11 + v.z*m21,
00762         v.x*m02 + v.y*m12 + v.z*m22
00763         );
00764 }
00765 
00766 template< typename T >
00767 Vec3<T> Matrix33<T>::postMultiply( const Vec3<T> &v ) const
00768 {
00769     return Vec3<T>(
00770         m00*v.x + m01*v.y + m02*v.z,
00771         m10*v.x + m11*v.y + m12*v.z,
00772         m20*v.x + m21*v.y + m22*v.z
00773         );
00774 }
00775 
00776 template< typename T >
00777 void Matrix33<T>::scale( T s )
00778 {
00779     for( int i = 0; i < DIM_SQ; ++i ) {
00780         m[i] *= s;
00781     }
00782 }
00783 
00784 template< typename T >
00785 void Matrix33<T>::scale( const Vec2<T> &s )
00786 {
00787     m[0] *= s.x; m[3] *= s.y;
00788     m[1] *= s.x; m[4] *= s.y;
00789     m[2] *= s.x; m[5] *= s.y;
00790 }
00791 
00792 template< typename T >
00793 void Matrix33<T>::scale( const Vec3<T> &s )
00794 {
00795     m[0] *= s.x; m[3] *= s.y; m[6] *= s.z;
00796     m[1] *= s.x; m[4] *= s.y; m[7] *= s.z;
00797     m[2] *= s.x; m[5] *= s.y; m[8] *= s.z;
00798 }
00799 
00800 template< typename T >
00801 Matrix33<T> Matrix33<T>::invertTransform() const
00802 {
00803     Matrix33<T> ret;
00804 
00805     // transpose rotation part
00806     for( int i = 0; i < DIM; i++ ) {
00807         for( int j = 0; j < DIM; j++ ) {
00808             ret.at( j, i ) = at( i, j );
00809         }
00810     }
00811 
00812     return ret;
00813 }
00814 
00815 template<typename T>
00816 Matrix33<T> Matrix33<T>::createRotation( const Vec3<T> &from, const Vec3<T> &to, const Vec3<T> &worldUp )
00817 {
00818     // The goal is to obtain a rotation matrix that takes 
00819     // "fromDir" to "toDir".  We do this in two steps and 
00820     // compose the resulting rotation matrices; 
00821     //    (a) rotate "fromDir" into the z-axis
00822     //    (b) rotate the z-axis into "toDir"
00823  
00824     // The from direction must be non-zero; but we allow zero to and up dirs.
00825     if( from.lengthSquared() == 0 ) {
00826         return Matrix33<T>();
00827     }
00828     else {
00829         Matrix33<T> zAxis2FromDir = alignZAxisWithTarget( from, Vec3<T>::yAxis() );
00830         Matrix33<T> fromDir2zAxis = zAxis2FromDir.transposed();
00831         Matrix33<T> zAxis2ToDir = alignZAxisWithTarget( to, worldUp );
00832         return fromDir2zAxis * zAxis2ToDir;
00833     }
00834 }
00835 
00836 template<typename T>
00837 Matrix33<T> Matrix33<T>::createRotation( const Vec3<T> &axis, T angle )
00838 {
00839     Vec3<T> unit( axis.normalized() );
00840     T sine   = math<T>::sin( angle );
00841     T cosine = math<T>::cos( angle );
00842 
00843     Matrix33<T> ret;
00844 
00845     ret.m[0] = unit.x * unit.x * (1 - cosine) + cosine;
00846     ret.m[1] = unit.x * unit.y * (1 - cosine) + unit.z * sine;
00847     ret.m[2] = unit.x * unit.z * (1 - cosine) - unit.y * sine;
00848 
00849     ret.m[3] = unit.x * unit.y * (1 - cosine) - unit.z * sine;
00850     ret.m[4] = unit.y * unit.y * (1 - cosine) + cosine;
00851     ret.m[5] = unit.y * unit.z * (1 - cosine) + unit.x * sine;
00852 
00853     ret.m[6] = unit.x * unit.z * (1 - cosine) + unit.y * sine;
00854     ret.m[7] = unit.y * unit.z * (1 - cosine) - unit.x * sine;
00855     ret.m[8] = unit.z * unit.z * (1 - cosine) + cosine;
00856 
00857     return ret;
00858 }
00859 
00860 template<typename T>
00861 Matrix33<T> Matrix33<T>::createRotation( const Vec3<T> &eulerRadians )
00862 {
00863     // The ordering for this is XYZ. In OpenGL, the ordering
00864     // is the same, but the operations needs to happen in
00865     // reverse: 
00866     //
00867     //     glRotatef( eulerRadians.z, 0.0f, 0.0f 1.0f );
00868     //     glRotatef( eulerRadians.y, 0.0f, 1.0f 0.0f );
00869     //     glRotatef( eulerRadians.x, 1.0f, 0.0f 0.0f );
00870     //
00871 
00872     Matrix33<T> ret;
00873     T cos_rz, sin_rz, cos_ry, sin_ry, cos_rx, sin_rx;
00874 
00875     cos_rx = math<T>::cos( eulerRadians.x );
00876     cos_ry = math<T>::cos( eulerRadians.y );
00877     cos_rz = math<T>::cos( eulerRadians.z );
00878 
00879     sin_rx = math<T>::sin( eulerRadians.x );
00880     sin_ry = math<T>::sin( eulerRadians.y );
00881     sin_rz = math<T>::sin( eulerRadians.z );
00882 
00883     ret.m[0] =  cos_rz*cos_ry;
00884     ret.m[1] =  sin_rz*cos_ry;
00885     ret.m[2] = -sin_ry;
00886 
00887     ret.m[3] = -sin_rz*cos_rx + cos_rz*sin_ry*sin_rx;
00888     ret.m[4] =  cos_rz*cos_rx + sin_rz*sin_ry*sin_rx;
00889     ret.m[5] =  cos_ry*sin_rx;
00890 
00891     ret.m[6] =  sin_rz*sin_rx + cos_rz*sin_ry*cos_rx;
00892     ret.m[7] = -cos_rz*sin_rx + sin_rz*sin_ry*cos_rx;
00893     ret.m[8] =  cos_ry*cos_rx;
00894 
00895     return ret;
00896 }
00897 
00898 template<typename T>
00899 Matrix33<T> Matrix33<T>::createScale( T s )
00900 {
00901     Matrix33<T> ret;
00902     ret.setToIdentity();
00903     ret.at(0,0) = s;
00904     ret.at(1,1) = s;
00905     ret.at(2,2) = s;
00906     return ret;
00907 }
00908 
00909 template<typename T>
00910 Matrix33<T> Matrix33<T>::createScale( const Vec2<T> &v )
00911 {
00912     Matrix33<T> ret;
00913     ret.setToIdentity();
00914     ret.at(0,0) = v.x;
00915     ret.at(1,1) = v.y;
00916     ret.at(2,2) = 1;
00917     return ret;
00918 }
00919 
00920 template<typename T>
00921 Matrix33<T> Matrix33<T>::createScale( const Vec3<T> &v )
00922 {
00923     Matrix33<T> ret;
00924     ret.setToIdentity();
00925     ret.at(0,0) = v.x;
00926     ret.at(1,1) = v.y;
00927     ret.at(2,2) = v.z;
00928     return ret;
00929 }
00930 
00931 template<typename T>
00932 Matrix33<T> Matrix33<T>::alignZAxisWithTarget( Vec3<T> targetDir, Vec3<T> upDir )
00933 {
00934 
00935     // Ensure that the target direction is non-zero.
00936     if( targetDir.lengthSquared() < EPSILON ) {
00937         // We want to look down the negative z-axis since to match OpenGL.
00938         targetDir = -Vec3<T>::zAxis();
00939     }
00940 
00941     // Ensure that the up direction is non-zero.
00942     if( upDir.lengthSquared() < EPSILON ) {
00943         upDir = Vec3<T>::yAxis();
00944     }
00945 
00946     // Check for degeneracies.  If the upDir and targetDir are parallel 
00947     // or opposite, then compute a new, arbitrary up direction that is
00948     // not parallel or opposite to the targetDir.
00949     if( upDir.cross( targetDir ).lengthSquared() == 0 ) {
00950         upDir = targetDir.cross( Vec3<T>::xAxis() );
00951         if( upDir.lengthSquared() == 0 ) {
00952             upDir = targetDir.cross( Vec3<T>::zAxis() );
00953         }
00954     }
00955 
00956     // Compute the x-, y-, and z-axis vectors of the new coordinate system.
00957     Vec3<T> targetPerpDir = targetDir.cross( upDir );
00958     Vec3<T> targetUpDir   = targetPerpDir.cross( targetDir );
00959 
00960 
00961     // Rotate the x-axis into targetPerpDir (row 0),
00962     // rotate the y-axis into targetUpDir   (row 1),
00963     // rotate the z-axis into targetDir     (row 2).
00964     Vec3<T> row[3];
00965     row[0] = targetPerpDir.normalized();
00966     row[1] = targetUpDir.normalized();
00967     row[2] = targetDir.normalized();
00968 
00969     Matrix33<T> mat( 
00970         row[0].x,  row[0].y,  row[0].z,
00971         row[1].x,  row[1].y,  row[1].z,
00972         row[2].x,  row[2].y,  row[2].z
00973     );
00974 
00975     return mat;
00976 }
00977 
00979 // Typedefs
00980 typedef Matrix33<float>  Matrix33f;
00981 typedef Matrix33<double> Matrix33d;
00982 
00983 } // namespace cinder