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