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