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