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