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 // Matrix44 00036 template< typename T > 00037 class Matrix44 00038 { 00039 public: 00040 typedef T TYPE; 00041 // 00042 static const size_t DIM = 4; 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] m[0,2] | 00050 // | m[1,0] m[1,1] m[1,2] m[1,2] | 00051 // | m[2,0] m[2,1] m[2,2] m[2,2] | 00052 // | m[3,0] m[3,1] m[3,2] m[3,2] | 00053 // 00054 // m[idx] 00055 // | m[ 0] m[ 4] m[ 8] m[12] | 00056 // | m[ 1] m[ 5] m[ 9] m[13] | 00057 // | m[ 2] m[ 6] m[10] m[14] | 00058 // | m[ 3] m[ 7] m[11] m[15] | 00059 // 00060 union { 00061 T m[16]; 00062 struct { 00063 // This looks like it's transposed from the above, but it's really not. 00064 // It just has to be written this way so it follows the right ordering 00065 // in the memory layout as well as being mathematically correct. 00066 T m00, m10, m20, m30; 00067 T m01, m11, m21, m31; 00068 T m02, m12, m22, m32; 00069 T m03, m13, m23, m33; 00070 }; 00071 // [Cols][Rows] 00072 T mcols[4][4]; 00073 }; 00074 00075 Matrix44(); 00076 00077 Matrix44( T s ); 00078 00079 // OpenGL layout - unless srcIsRowMajor is true 00080 Matrix44( const T *dt, bool srcIsRowMajor = false ); 00081 00082 // OpenGL layout: m[0]=d0, m[1]=d1, m[2]=d2 ... m[13]=d13, m[14]=d14, m[15]=d15 - unless srcIsRowMajor is true 00083 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, bool srcIsRowMajor = false ); 00084 00085 // Creates matrix with column vectors vx, vy, vz and vw 00086 Matrix44( const Vec3<T> &vx, const Vec3<T> &vy, const Vec3<T> &vz ); 00087 Matrix44( const Vec4<T> &vx, const Vec4<T> &vy, const Vec4<T> &vz, const Vec4<T> &vw = Vec4<T>( 0, 0, 0, 1 ) ); 00088 00089 template< typename FromT > 00090 Matrix44( const Matrix44<FromT>& src ); 00091 00092 Matrix44( const Matrix22<T>& src ); 00093 Matrix44( const Matrix33<T>& src ); 00094 00095 Matrix44( const Matrix44<T>& src ); 00096 00097 operator T*() { return (T*)m; } 00098 operator const T*() const { return (const T*)m; } 00099 00100 Matrix44<T>& operator=( const Matrix44<T>& rhs ); 00101 Matrix44<T>& operator=( T rhs ); 00102 00103 template< typename FromT > 00104 Matrix44<T>& operator=( const Matrix44<FromT>& rhs ); 00105 00106 // remaining columns and rows will be filled with identity values 00107 Matrix44<T>& operator=( const Matrix22<T>& rhs ); 00108 Matrix44<T>& operator=( const Matrix33<T>& rhs ); 00109 00110 bool equalCompare( const Matrix44<T>& rhs, T epsilon ) const; 00111 bool operator==( const Matrix44<T> &rhs ) const { return equalCompare( rhs, (T)EPSILON ); } 00112 bool operator!=( const Matrix44<T> &rhs ) const { return ! ( *this == rhs ); } 00113 00114 Matrix44<T>& operator*=( const Matrix44<T> &rhs ); 00115 Matrix44<T>& operator+=( const Matrix44<T> &rhs ); 00116 Matrix44<T>& operator-=( const Matrix44<T> &rhs ); 00117 00118 Matrix44<T>& operator*=( T rhs ); 00119 Matrix44<T>& operator/=( T rhs ); 00120 Matrix44<T>& operator+=( T rhs ); 00121 Matrix44<T>& operator-=( T rhs ); 00122 00123 const Matrix44<T> operator*( const Matrix44<T> &rhs ) const; 00124 const Matrix44<T> operator+( const Matrix44<T> &rhs ) const; 00125 const Matrix44<T> operator-( const Matrix44<T> &rhs ) const; 00126 00127 // post-multiplies column vector [rhs.x rhs.y rhs.z 1] and divides by w 00128 const Vec3<T> operator*( const Vec3<T> &rhs ) const; 00129 00130 // post-multiplies column vector [rhs.x rhs.y rhs.z rhs.w] 00131 const Vec4<T> operator*( const Vec4<T> &rhs ) const; 00132 00133 const Matrix44<T> operator*( T rhs ) const; 00134 const Matrix44<T> operator/( T rhs ) const; 00135 const Matrix44<T> operator+( T rhs ) const; 00136 const Matrix44<T> operator-( T rhs ) const; 00137 00138 // Accessors 00139 T& at( int row, int col ); 00140 const T& at( int row, int col ) const; 00141 00142 // OpenGL layout - unless srcIsRowMajor is true 00143 void set( const T *dt, bool srcIsRowMajor = false ); 00144 // OpenGL layout: m[0]=d0, m[1]=d1, m[2]=d2 ... m[13]=d13, m[14]=d14, m[15]=d15 - unless srcIsRowMajor is true 00145 void set( 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, bool srcIsRowMajor = false ); 00146 00147 Vec4<T> getColumn( int col ) const; 00148 void setColumn( int col, const Vec4<T> &v ); 00149 00150 Vec4<T> getRow( int row ) const; 00151 void setRow( int row, const Vec4<T> &v ); 00152 00153 void getColumns( Vec4<T> *c0, Vec4<T> *c1, Vec4<T> *c2, Vec4<T> *c3 ) const; 00154 void setColumns( const Vec4<T> &c0, const Vec4<T> &c1, const Vec4<T> &c2, const Vec4<T> &c3 ); 00155 00156 void getRows( Vec4<T> *r0, Vec4<T> *r1, Vec4<T> *r2, Vec4<T> *r3 ) const; 00157 void setRows( const Vec4<T> &r0, const Vec4<T> &r1, const Vec4<T> &r2, const Vec4<T> &r3 ); 00158 00159 // returns a sub-matrix starting at row, col 00160 Matrix22<T> subMatrix22( int row, int col ) const; 00161 Matrix33<T> subMatrix33( int row, int col ) const; 00162 00163 void setToNull(); 00164 void setToIdentity(); 00165 00166 T determinant() const; 00167 // returns trace of 3x3 submatrix if fullTrace == false, otherwise returns trace of full 4x4 matrix 00168 T trace( bool fullTrace = false ) const; 00169 00170 Matrix44<T> diagonal() const; 00171 00172 Matrix44<T> lowerTriangular() const; 00173 Matrix44<T> upperTriangular() const; 00174 00175 void transpose(); 00176 Matrix44<T> transposed() const; 00177 00178 void invert (T epsilon = EPSILON ) { *this = inverted( epsilon ); } 00179 Matrix44<T> inverted( T epsilon = EPSILON ) const; 00180 00181 // pre-multiplies row vector v - no divide by w 00182 Vec3<T> preMultiply( const Vec3<T> &v ) const; 00183 Vec4<T> preMultiply( const Vec4<T> &v ) const; 00184 00185 // post-multiplies column vector v - no divide by w 00186 Vec3<T> postMultiply( const Vec3<T> &v ) const; 00187 Vec4<T> postMultiply( const Vec4<T> &v ) const; 00188 00189 // assumes the matrix is affine, i.e. the bottom row is [0 0 0 1] 00190 void affineInvert(){ *this = affineInverted(); } 00191 Matrix44<T> affineInverted() const; 00192 00193 // post-multiplies column vector [rhs.x rhs.y rhs.z 1] and divides by w - same as operator*( const Vec3<T>& ) 00194 Vec3<T> transformPoint( const Vec3<T> &rhs ) const; 00195 00196 // post-multiplies column vector [rhs.x rhs.y rhs.z 1] but omits divide by w 00197 Vec3<T> transformPointAffine( const Vec3<T> &rhs ) const; 00198 00199 // post-multiplies column vector [rhs.x rhs.y rhs.z 0] 00200 Vec3<T> transformVec( const Vec3<T> &rhs ) const; 00201 Vec4<T> transformVec( const Vec4<T> &rhs ) const { return transformVec( rhs.xyz() ); } 00202 00203 // returns the translation values from the last column 00204 Vec4<T> getTranslate() const { return Vec4<T>( m03, m13, m23, m33 ); } 00205 // sets the translation values in the last column 00206 void setTranslate( const Vec3<T>& v ) { m03 = v.x; m13 = v.y; m23 = v.z; } 00207 void setTranslate( const Vec4<T>& v ) { setTranslate( v.xyz() ); } 00208 00209 // multiplies the current matrix by a translation matrix derived from tr 00210 void translate( const Vec3<T> &tr ) { *this *= createTranslation( tr ); } 00211 void translate( const Vec4<T> &tr ) { *this *= createTranslation( tr ); } 00212 00213 // multiplies the current matrix by the rotation matrix derived using axis and radians 00214 void rotate( const Vec3<T> &axis, T radians ) { *this *= createRotation( axis, radians ); } 00215 void rotate( const Vec4<T> &axis, T radians ) { *this *= createRotation( axis, radians ); } 00216 // multiplies the current matrix by the rotation matrix derived using eulerRadians 00217 void rotate( const Vec3<T> &eulerRadians ) { *this *= createRotation( eulerRadians ); } 00218 void rotate( const Vec4<T> &eulerRadians ) { *this *= createRotation( eulerRadians ); } 00219 // multiplies the current matrix by the rotation matrix derived using from, to, worldUp 00220 void rotate( const Vec3<T> &from, const Vec3<T> &to, const Vec3<T> &worldUp ) { *this *= createRotation( from, to, worldUp ); } 00221 void rotate( const Vec4<T> &from, const Vec4<T> &to, const Vec4<T> &worldUp ) { *this *= createRotation( from, to, worldUp ); } 00222 00223 // multiplies the current matrix by the scale matrix derived from supplies parameters 00224 void scale( T s ) { Matrix44 op = createScale( s ); Matrix44 mat = *this; *this = op*mat; } 00225 void scale( const Vec2<T> &v ) { *this *= createScale( v ); } 00226 void scale( const Vec3<T> &v ) { *this *= createScale( v ); } 00227 void scale( const Vec4<T> &v ) { *this *= createScale( v ); } 00228 00229 // transposes rotation sub-matrix and inverts translation 00230 Matrix44<T> invertTransform() const; 00231 00232 // returns an identity matrix 00233 static Matrix44<T> identity() { return Matrix44( 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 ); } 00234 // returns 1 filled matrix 00235 static Matrix44<T> one() { return Matrix44( (T)1 ); } 00236 // returns 0 filled matrix 00237 static Matrix44<T> zero() { return Matrix44( (T)0 ); } 00238 00239 // creates translation matrix 00240 static Matrix44<T> createTranslation( const Vec3<T> &v, T w = 1 ); 00241 static Matrix44<T> createTranslation( const Vec4<T> &v ) { return createTranslation( v.xyz(), v.w );} 00242 00243 // creates rotation matrix 00244 static Matrix44<T> createRotation( const Vec3<T> &axis, T radians ); 00245 static Matrix44<T> createRotation( const Vec4<T> &axis, T radians ) { return createRotation( axis.xyz(), radians ); } 00246 static Matrix44<T> createRotation( const Vec3<T> &from, const Vec3<T> &to, const Vec3<T> &worldUp ); 00247 static Matrix44<T> createRotation( const Vec4<T> &from, const Vec4<T> &to, const Vec4<T> &worldUp ) { return createRotation( from.xyz(), to.xyz(), worldUp.xyz() ); } 00248 // equivalent to rotate( zAxis, z ), then rotate( yAxis, y ) then rotate( xAxis, x ) 00249 static Matrix44<T> createRotation( const Vec3<T> &eulerRadians ); 00250 static Matrix44<T> createRotation( const Vec4<T> &eulerRadians ) { return createRotation( eulerRadians.xyz() ); } 00251 00252 // creates scale matrix 00253 static Matrix44<T> createScale( T s ); 00254 static Matrix44<T> createScale( const Vec2<T> &v ); 00255 static Matrix44<T> createScale( const Vec3<T> &v ); 00256 static Matrix44<T> createScale( const Vec4<T> &v ); 00257 00258 // creates a rotation matrix with z-axis aligned to targetDir 00259 static Matrix44<T> alignZAxisWithTarget( Vec3<T> targetDir, Vec3<T> upDir ); 00260 static Matrix44<T> alignZAxisWithTarget( Vec4<T> targetDir, Vec4<T> upDir ) { return alignZAxisWithTarget( targetDir.xyz(), upDir.xyz() ); } 00261 00262 friend std::ostream& operator<<( std::ostream &lhs, const Matrix44<T> &rhs ) { 00263 for( int i = 0; i < 4; i++) { 00264 lhs << " |"; 00265 for( int j = 0; j < 4; j++) { 00266 lhs << std::setw( 12 ) << std::setprecision( 6 ) << rhs.m[j*4+i]; 00267 } 00268 lhs << "|" << std::endl; 00269 } 00270 00271 return lhs; 00272 } 00273 }; 00274 00275 template< typename T > 00276 Matrix44<T>::Matrix44() 00277 { 00278 setToIdentity(); 00279 } 00280 00281 template< typename T > 00282 Matrix44<T>::Matrix44( T s ) 00283 { 00284 for( int i = 0; i < DIM_SQ; ++i ) { 00285 m[i] = s; 00286 } 00287 } 00288 00289 template< typename T > 00290 Matrix44<T>::Matrix44( const T *dt, bool srcIsRowMajor ) 00291 { 00292 set( dt, srcIsRowMajor ); 00293 } 00294 00295 template< typename T > 00296 Matrix44<T>::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, bool srcIsRowMajor ) 00297 { 00298 set( d0, d1, d2, d3, 00299 d4, d5, d6, d7, 00300 d8, d9, d10, d11, 00301 d12, d13, d14, d15, srcIsRowMajor ); 00302 } 00303 00304 template< typename T > 00305 Matrix44<T>::Matrix44( const Vec3<T> &vx, const Vec3<T> &vy, const Vec3<T> &vz ) 00306 { 00307 m[0] = vx.x; m[4] = vy.x; m[ 8] = vz.x; m[12] = 0; 00308 m[1] = vx.y; m[5] = vy.y; m[ 9] = vz.y; m[13] = 0; 00309 m[2] = vx.z; m[6] = vy.z; m[10] = vz.z; m[14] = 0; 00310 m[3] = 0; m[7] = 0; m[11] = 0; m[15] = 1; 00311 } 00312 00313 template< typename T > 00314 Matrix44<T>::Matrix44( const Vec4<T> &vx, const Vec4<T> &vy, const Vec4<T> &vz, const Vec4<T> &vw ) 00315 { 00316 m[0] = vx.x; m[4] = vy.x; m[ 8] = vz.x; m[12] = vw.x; 00317 m[1] = vx.y; m[5] = vy.y; m[ 9] = vz.y; m[13] = vw.y; 00318 m[2] = vx.z; m[6] = vy.z; m[10] = vz.z; m[14] = vw.z; 00319 m[3] = vx.w; m[7] = vy.w; m[11] = vz.w; m[15] = vw.w; 00320 } 00321 00322 template< typename T > 00323 template< typename FromT > 00324 Matrix44<T>::Matrix44( const Matrix44<FromT>& src ) 00325 { 00326 for( int i = 0; i < DIM_SQ; ++i ) { 00327 m[i] = static_cast<T>( src.m[i] ); 00328 } 00329 } 00330 00331 template< typename T > 00332 Matrix44<T>::Matrix44( const Matrix22<T>& src ) 00333 { 00334 setToIdentity(); 00335 m00 = src.m00; m01 = src.m01; 00336 m10 = src.m10; m11 = src.m11; 00337 } 00338 00339 template< typename T > 00340 Matrix44<T>::Matrix44( const Matrix33<T>& src ) 00341 { 00342 setToIdentity(); 00343 m00 = src.m00; m01 = src.m01; m02 = src.m02; 00344 m10 = src.m10; m11 = src.m11; m12 = src.m12; 00345 m20 = src.m20; m21 = src.m21; m22 = src.m22; 00346 } 00347 00348 template< typename T > 00349 Matrix44<T>::Matrix44( const Matrix44<T>& src ) 00350 { 00351 std::memcpy( m, src.m, MEM_LEN ); 00352 } 00353 00354 template< typename T > 00355 Matrix44<T>& Matrix44<T>::operator=( const Matrix44<T>& rhs ) 00356 { 00357 std::memcpy( m, rhs.m, MEM_LEN ); 00358 return *this; 00359 } 00360 00361 template< typename T > 00362 Matrix44<T>& Matrix44<T>::operator=( T rhs ) 00363 { 00364 for( int i = 0; i < DIM_SQ; i++ ) { 00365 m[i] = rhs; 00366 } 00367 return *this; 00368 } 00369 00370 template< typename T > 00371 template< typename FromT > 00372 Matrix44<T>& Matrix44<T>::operator=( const Matrix44<FromT>& rhs ) 00373 { 00374 for( int i = 0; i < DIM_SQ; i++ ) { 00375 m[i] = static_cast<T>(rhs.m[i]); 00376 } 00377 return *this; 00378 } 00379 00380 template< typename T > 00381 Matrix44<T>& Matrix44<T>::operator=( const Matrix22<T>& rhs ) 00382 { 00383 setToIdentity(); 00384 m00 = rhs.m00; m01 = rhs.m01; 00385 m10 = rhs.m10; m11 = rhs.m11; 00386 return *this; 00387 } 00388 00389 template< typename T > 00390 Matrix44<T>& Matrix44<T>::operator=( const Matrix33<T>& rhs ) 00391 { 00392 setToIdentity(); 00393 m00 = rhs.m00; m01 = rhs.m01; m02 = rhs.m02; 00394 m10 = rhs.m10; m11 = rhs.m11; m12 = rhs.m12; 00395 m20 = rhs.m20; m21 = rhs.m21; m22 = rhs.m22; 00396 return *this; 00397 } 00398 00399 template< typename T > 00400 bool Matrix44<T>::equalCompare( const Matrix44<T>& rhs, T epsilon ) const 00401 { 00402 for( int i = 0; i < DIM_SQ; ++i ) { 00403 if( math<T>::abs(m[i] - rhs.m[i]) >= epsilon ) 00404 return false; 00405 } 00406 return true; 00407 } 00408 00409 template< typename T > 00410 inline Matrix44<T>& Matrix44<T>::operator*=( const Matrix44<T> &rhs ) 00411 { 00412 Matrix44<T> ret; 00413 00414 ret.m[ 0] = m[ 0]*rhs.m[ 0] + m[ 4]*rhs.m[ 1] + m[ 8]*rhs.m[ 2] + m[12]*rhs.m[ 3]; 00415 ret.m[ 1] = m[ 1]*rhs.m[ 0] + m[ 5]*rhs.m[ 1] + m[ 9]*rhs.m[ 2] + m[13]*rhs.m[ 3]; 00416 ret.m[ 2] = m[ 2]*rhs.m[ 0] + m[ 6]*rhs.m[ 1] + m[10]*rhs.m[ 2] + m[14]*rhs.m[ 3]; 00417 ret.m[ 3] = m[ 3]*rhs.m[ 0] + m[ 7]*rhs.m[ 1] + m[11]*rhs.m[ 2] + m[15]*rhs.m[ 3]; 00418 00419 ret.m[ 4] = m[ 0]*rhs.m[ 4] + m[ 4]*rhs.m[ 5] + m[ 8]*rhs.m[ 6] + m[12]*rhs.m[ 7]; 00420 ret.m[ 5] = m[ 1]*rhs.m[ 4] + m[ 5]*rhs.m[ 5] + m[ 9]*rhs.m[ 6] + m[13]*rhs.m[ 7]; 00421 ret.m[ 6] = m[ 2]*rhs.m[ 4] + m[ 6]*rhs.m[ 5] + m[10]*rhs.m[ 6] + m[14]*rhs.m[ 7]; 00422 ret.m[ 7] = m[ 3]*rhs.m[ 4] + m[ 7]*rhs.m[ 5] + m[11]*rhs.m[ 6] + m[15]*rhs.m[ 7]; 00423 00424 ret.m[ 8] = m[ 0]*rhs.m[ 8] + m[ 4]*rhs.m[ 9] + m[ 8]*rhs.m[10] + m[12]*rhs.m[11]; 00425 ret.m[ 9] = m[ 1]*rhs.m[ 8] + m[ 5]*rhs.m[ 9] + m[ 9]*rhs.m[10] + m[13]*rhs.m[11]; 00426 ret.m[10] = m[ 2]*rhs.m[ 8] + m[ 6]*rhs.m[ 9] + m[10]*rhs.m[10] + m[14]*rhs.m[11]; 00427 ret.m[11] = m[ 3]*rhs.m[ 8] + m[ 7]*rhs.m[ 9] + m[11]*rhs.m[10] + m[15]*rhs.m[11]; 00428 00429 ret.m[12] = m[ 0]*rhs.m[12] + m[ 4]*rhs.m[13] + m[ 8]*rhs.m[14] + m[12]*rhs.m[15]; 00430 ret.m[13] = m[ 1]*rhs.m[12] + m[ 5]*rhs.m[13] + m[ 9]*rhs.m[14] + m[13]*rhs.m[15]; 00431 ret.m[14] = m[ 2]*rhs.m[12] + m[ 6]*rhs.m[13] + m[10]*rhs.m[14] + m[14]*rhs.m[15]; 00432 ret.m[15] = m[ 3]*rhs.m[12] + m[ 7]*rhs.m[13] + m[11]*rhs.m[14] + m[15]*rhs.m[15]; 00433 00434 for( int i = 0; i < DIM_SQ; ++i ) { 00435 m[i] = ret.m[i]; 00436 } 00437 00438 return *this; 00439 } 00440 00441 template< typename T > 00442 Matrix44<T>& Matrix44<T>::operator+=( const Matrix44<T> &rhs ) 00443 { 00444 for( int i = 0; i < DIM_SQ; ++i ) { 00445 m[i] += rhs.m[i]; 00446 } 00447 return *this; 00448 } 00449 00450 template< typename T > 00451 Matrix44<T>& Matrix44<T>::operator-=( const Matrix44<T> &rhs ) 00452 { 00453 for( int i = 0; i < DIM_SQ; ++i ) { 00454 m[i] -= rhs.m[i]; 00455 } 00456 return *this; 00457 } 00458 00459 template< typename T > 00460 Matrix44<T>& Matrix44<T>::operator*=( T rhs ) 00461 { 00462 for( int i = 0; i < DIM_SQ; ++i ) { 00463 m[i] *= rhs; 00464 } 00465 return *this; 00466 } 00467 00468 template< typename T > 00469 Matrix44<T>& Matrix44<T>::operator/=( T rhs ) 00470 { 00471 T invS = (T)1/rhs; 00472 for( int i = 0; i < DIM_SQ; ++i ) { 00473 m[i] *= invS; 00474 } 00475 return *this; 00476 } 00477 00478 template< typename T > 00479 Matrix44<T>& Matrix44<T>::operator+=( T rhs ) 00480 { 00481 for( int i = 0; i < DIM_SQ; ++i ) { 00482 m[i] += rhs; 00483 } 00484 return *this; 00485 } 00486 00487 template< typename T > 00488 Matrix44<T>& Matrix44<T>::operator-=( T rhs ) 00489 { 00490 for( int i = 0; i < DIM_SQ; ++i ) { 00491 m[i] -= rhs; 00492 } 00493 return *this; 00494 } 00495 00496 template< typename T > 00497 inline const Matrix44<T> Matrix44<T>::operator*( const Matrix44<T> &rhs ) const 00498 { 00499 Matrix44<T> ret; 00500 00501 ret.m[ 0] = m[ 0]*rhs.m[ 0] + m[ 4]*rhs.m[ 1] + m[ 8]*rhs.m[ 2] + m[12]*rhs.m[ 3]; 00502 ret.m[ 1] = m[ 1]*rhs.m[ 0] + m[ 5]*rhs.m[ 1] + m[ 9]*rhs.m[ 2] + m[13]*rhs.m[ 3]; 00503 ret.m[ 2] = m[ 2]*rhs.m[ 0] + m[ 6]*rhs.m[ 1] + m[10]*rhs.m[ 2] + m[14]*rhs.m[ 3]; 00504 ret.m[ 3] = m[ 3]*rhs.m[ 0] + m[ 7]*rhs.m[ 1] + m[11]*rhs.m[ 2] + m[15]*rhs.m[ 3]; 00505 00506 ret.m[ 4] = m[ 0]*rhs.m[ 4] + m[ 4]*rhs.m[ 5] + m[ 8]*rhs.m[ 6] + m[12]*rhs.m[ 7]; 00507 ret.m[ 5] = m[ 1]*rhs.m[ 4] + m[ 5]*rhs.m[ 5] + m[ 9]*rhs.m[ 6] + m[13]*rhs.m[ 7]; 00508 ret.m[ 6] = m[ 2]*rhs.m[ 4] + m[ 6]*rhs.m[ 5] + m[10]*rhs.m[ 6] + m[14]*rhs.m[ 7]; 00509 ret.m[ 7] = m[ 3]*rhs.m[ 4] + m[ 7]*rhs.m[ 5] + m[11]*rhs.m[ 6] + m[15]*rhs.m[ 7]; 00510 00511 ret.m[ 8] = m[ 0]*rhs.m[ 8] + m[ 4]*rhs.m[ 9] + m[ 8]*rhs.m[10] + m[12]*rhs.m[11]; 00512 ret.m[ 9] = m[ 1]*rhs.m[ 8] + m[ 5]*rhs.m[ 9] + m[ 9]*rhs.m[10] + m[13]*rhs.m[11]; 00513 ret.m[10] = m[ 2]*rhs.m[ 8] + m[ 6]*rhs.m[ 9] + m[10]*rhs.m[10] + m[14]*rhs.m[11]; 00514 ret.m[11] = m[ 3]*rhs.m[ 8] + m[ 7]*rhs.m[ 9] + m[11]*rhs.m[10] + m[15]*rhs.m[11]; 00515 00516 ret.m[12] = m[ 0]*rhs.m[12] + m[ 4]*rhs.m[13] + m[ 8]*rhs.m[14] + m[12]*rhs.m[15]; 00517 ret.m[13] = m[ 1]*rhs.m[12] + m[ 5]*rhs.m[13] + m[ 9]*rhs.m[14] + m[13]*rhs.m[15]; 00518 ret.m[14] = m[ 2]*rhs.m[12] + m[ 6]*rhs.m[13] + m[10]*rhs.m[14] + m[14]*rhs.m[15]; 00519 ret.m[15] = m[ 3]*rhs.m[12] + m[ 7]*rhs.m[13] + m[11]*rhs.m[14] + m[15]*rhs.m[15]; 00520 00521 return ret; 00522 } 00523 00524 template< typename T > 00525 const Matrix44<T> Matrix44<T>::operator+( const Matrix44<T> &rhs ) const 00526 { 00527 Matrix44<T> ret; 00528 for( int i = 0; i < DIM_SQ; ++i ) { 00529 ret.m[i] = m[i] + rhs.m[i]; 00530 } 00531 return ret; 00532 } 00533 00534 template< typename T > 00535 const Matrix44<T> Matrix44<T>::operator-( const Matrix44<T> &rhs ) const 00536 { 00537 Matrix44<T> ret; 00538 for( int i = 0; i < DIM_SQ; ++i ) { 00539 ret.m[i] = m[i] - rhs.m[i]; 00540 } 00541 return ret; 00542 } 00543 00544 template< typename T > 00545 const Vec3<T> Matrix44<T>::operator*( const Vec3<T> &rhs ) const 00546 { 00547 T x = m[ 0]*rhs.x + m[ 4]*rhs.y + m[ 8]*rhs.z + m[12]; 00548 T y = m[ 1]*rhs.x + m[ 5]*rhs.y + m[ 9]*rhs.z + m[13]; 00549 T z = m[ 2]*rhs.x + m[ 6]*rhs.y + m[10]*rhs.z + m[14]; 00550 T w = m[ 3]*rhs.x + m[ 7]*rhs.y + m[11]*rhs.z + m[15]; 00551 00552 return Vec3<T>( x/w, y/w, z/w ); 00553 } 00554 00555 template< typename T > 00556 const Vec4<T> Matrix44<T>::operator*( const Vec4<T> &rhs ) const 00557 { 00558 return Vec4<T>( 00559 m[ 0]*rhs.x + m[ 4]*rhs.y + m[ 8]*rhs.z + m[12]*rhs.w, 00560 m[ 1]*rhs.x + m[ 5]*rhs.y + m[ 9]*rhs.z + m[13]*rhs.w, 00561 m[ 2]*rhs.x + m[ 6]*rhs.y + m[10]*rhs.z + m[14]*rhs.w, 00562 m[ 3]*rhs.x + m[ 7]*rhs.y + m[11]*rhs.z + m[15]*rhs.w 00563 ); 00564 } 00565 00566 template< typename T > 00567 const Matrix44<T> Matrix44<T>::operator*( T rhs ) const 00568 { 00569 Matrix44<T> ret; 00570 for( int i = 0; i < DIM_SQ; ++i ) { 00571 ret.m[i] = m[i]*rhs; 00572 } 00573 return ret; 00574 } 00575 00576 template< typename T > 00577 const Matrix44<T> Matrix44<T>::operator/( T rhs ) const 00578 { 00579 Matrix44<T> ret; 00580 T s = (T)1/rhs; 00581 for( int i = 0; i < DIM_SQ; ++i ) { 00582 ret.m[i] = m[i]*s; 00583 } 00584 return ret; 00585 } 00586 00587 template< typename T > 00588 const Matrix44<T> Matrix44<T>::operator+( T rhs ) const 00589 { 00590 Matrix44<T> ret; 00591 for( int i = 0; i < DIM_SQ; ++i ) { 00592 ret.m[i] = m[i] + rhs; 00593 } 00594 return ret; 00595 } 00596 00597 template< typename T > 00598 const Matrix44<T> Matrix44<T>::operator-( T rhs ) const 00599 { 00600 Matrix44<T> ret; 00601 for( int i = 0; i < DIM_SQ; ++i ) { 00602 ret.m[i] = m[i] - rhs; 00603 } 00604 return ret; 00605 } 00606 00607 template< typename T > 00608 T& Matrix44<T>::at( int row, int col ) 00609 { 00610 assert(row >= 0 && row < DIM); 00611 assert(col >= 0 && col < DIM); 00612 return m[col*DIM + row]; 00613 } 00614 00615 template< typename T > 00616 const T& Matrix44<T>::at( int row, int col ) const 00617 { 00618 assert(row >= 0 && row < DIM); 00619 assert(col >= 0 && col < DIM); 00620 return m[col*DIM + row]; 00621 } 00622 00623 template< typename T > 00624 void Matrix44<T>::set( const T *dt, bool srcIsRowMajor ) 00625 { 00626 if( srcIsRowMajor ) { 00627 m[0] = dt[ 0]; m[4] = dt[ 1]; m[ 8] = dt[ 2]; m[12] = dt[ 3]; 00628 m[1] = dt[ 4]; m[5] = dt[ 5]; m[ 9] = dt[ 6]; m[13] = dt[ 7]; 00629 m[2] = dt[ 8]; m[6] = dt[ 9]; m[10] = dt[10]; m[14] = dt[11]; 00630 m[3] = dt[12]; m[7] = dt[13]; m[11] = dt[14]; m[15] = dt[15]; 00631 } 00632 else { 00633 std::memcpy( m, dt, MEM_LEN ); 00634 } 00635 } 00636 00637 template< typename T > 00638 void Matrix44<T>::set( 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, bool srcIsRowMajor ) 00639 { 00640 if( srcIsRowMajor ) { 00641 m[0] = d0; m[4] = d1; m[ 8] = d2; m[12] = d3; 00642 m[1] = d4; m[5] = d5; m[ 9] = d6; m[13] = d7; 00643 m[2] = d8; m[6] = d9; m[10] = d10; m[14] = d11; 00644 m[3] = d12; m[7] = d13; m[11] = d14; m[15] = d15; 00645 } 00646 else { 00647 m[0] = d0; m[4] = d4; m[ 8] = d8; m[12] = d12; 00648 m[1] = d1; m[5] = d5; m[ 9] = d9; m[13] = d13; 00649 m[2] = d2; m[6] = d6; m[10] = d10; m[14] = d14; 00650 m[3] = d3; m[7] = d7; m[11] = d11; m[15] = d15; 00651 } 00652 } 00653 00654 template< typename T > 00655 Vec4<T> Matrix44<T>::getColumn( int col ) const 00656 { 00657 size_t i = col*DIM; 00658 return Vec4<T>( 00659 m[i + 0], 00660 m[i + 1], 00661 m[i + 2], 00662 m[i + 3] 00663 ); 00664 } 00665 00666 template< typename T > 00667 void Matrix44<T>::setColumn( int col, const Vec4<T> &v ) 00668 { 00669 size_t i = col*DIM; 00670 m[i + 0] = v.x; 00671 m[i + 1] = v.y; 00672 m[i + 2] = v.z; 00673 m[i + 3] = v.w; 00674 } 00675 00676 template< typename T > 00677 Vec4<T> Matrix44<T>::getRow( int row ) const 00678 { 00679 return Vec4<T>( 00680 m[row + 0], 00681 m[row + 4], 00682 m[row + 8], 00683 m[row + 12] 00684 ); 00685 } 00686 00687 template< typename T > 00688 void Matrix44<T>::setRow( int row, const Vec4<T> &v ) 00689 { 00690 m[row + 0] = v.x; 00691 m[row + 4] = v.y; 00692 m[row + 8] = v.z; 00693 m[row + 12] = v.w; 00694 } 00695 00696 template< typename T > 00697 void Matrix44<T>::getColumns( Vec4<T> *c0, Vec4<T> *c1, Vec4<T> *c2, Vec4<T> *c3 ) const 00698 { 00699 *c0 = getColumn( 0 ); 00700 *c1 = getColumn( 1 ); 00701 *c2 = getColumn( 2 ); 00702 *c3 = getColumn( 3 ); 00703 } 00704 00705 template< typename T > 00706 void Matrix44<T>::setColumns( const Vec4<T> &c0, const Vec4<T> &c1, const Vec4<T> &c2, const Vec4<T> &c3 ) 00707 { 00708 setColumn( 0, c0 ); 00709 setColumn( 1, c1 ); 00710 setColumn( 2, c2 ); 00711 setColumn( 3, c3 ); 00712 } 00713 00714 template< typename T > 00715 void Matrix44<T>::getRows( Vec4<T> *r0, Vec4<T> *r1, Vec4<T> *r2, Vec4<T> *r3 ) const 00716 { 00717 *r0 = getRow( 0 ); 00718 *r1 = getRow( 1 ); 00719 *r2 = getRow( 2 ); 00720 *r3 = getRow( 3 ); 00721 } 00722 00723 template< typename T > 00724 void Matrix44<T>::setRows( const Vec4<T> &r0, const Vec4<T> &r1, const Vec4<T> &r2, const Vec4<T> &r3 ) 00725 { 00726 setRow( 0, r0 ); 00727 setRow( 1, r1 ); 00728 setRow( 2, r2 ); 00729 setRow( 3, r3 ); 00730 } 00731 00732 template< typename T > 00733 Matrix22<T> Matrix44<T>::subMatrix22( int row, int col ) const 00734 { 00735 Matrix22<T> ret; 00736 ret.setToNull(); 00737 00738 for( int i = 0; i < 2; ++i ) { 00739 int r = row + i; 00740 if( r >= 4 ) { 00741 continue; 00742 } 00743 for( int j = 0; j < 2; ++j ) { 00744 int c = col + j; 00745 if( c >= 4 ) { 00746 continue; 00747 } 00748 ret.at( i, j ) = at( r, c ); 00749 } 00750 } 00751 00752 return ret; 00753 } 00754 00755 template< typename T > 00756 Matrix33<T> Matrix44<T>::subMatrix33( int row, int col ) const 00757 { 00758 Matrix33<T> ret; 00759 ret.setToNull(); 00760 00761 for( int i = 0; i < 3; ++i ) { 00762 int r = row + i; 00763 if( r >= 4 ) { 00764 continue; 00765 } 00766 for( int j = 0; j < 3; ++j ) { 00767 int c = col + j; 00768 if( c >= 4 ) { 00769 continue; 00770 } 00771 ret.at( i, j ) = at( r, c ); 00772 } 00773 } 00774 00775 return ret; 00776 } 00777 00778 template< typename T > 00779 void Matrix44<T>::setToNull() 00780 { 00781 std::memset( m, 0, MEM_LEN ); 00782 } 00783 00784 template< typename T > 00785 void Matrix44<T>::setToIdentity() 00786 { 00787 m[ 0] = 1; m[ 4] = 0; m[ 8] = 0; m[12] = 0; 00788 m[ 1] = 0; m[ 5] = 1; m[ 9] = 0; m[13] = 0; 00789 m[ 2] = 0; m[ 6] = 0; m[10] = 1; m[14] = 0; 00790 m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1; 00791 } 00792 00793 template< typename T > 00794 T Matrix44<T>::determinant() const 00795 { 00796 T a0 = m[ 0]*m[ 5] - m[ 1]*m[ 4]; 00797 T a1 = m[ 0]*m[ 6] - m[ 2]*m[ 4]; 00798 T a2 = m[ 0]*m[ 7] - m[ 3]*m[ 4]; 00799 T a3 = m[ 1]*m[ 6] - m[ 2]*m[ 5]; 00800 T a4 = m[ 1]*m[ 7] - m[ 3]*m[ 5]; 00801 T a5 = m[ 2]*m[ 7] - m[ 3]*m[ 6]; 00802 T b0 = m[ 8]*m[13] - m[ 9]*m[12]; 00803 T b1 = m[ 8]*m[14] - m[10]*m[12]; 00804 T b2 = m[ 8]*m[15] - m[11]*m[12]; 00805 T b3 = m[ 9]*m[14] - m[10]*m[13]; 00806 T b4 = m[ 9]*m[15] - m[11]*m[13]; 00807 T b5 = m[10]*m[15] - m[11]*m[14]; 00808 00809 T det = a0*b5 - a1*b4 + a2*b3 + a3*b2 - a4*b1 + a5*b0; 00810 00811 return det; 00812 } 00813 00814 template< typename T > 00815 T Matrix44<T>::trace( bool fullTrace ) const 00816 { 00817 if( fullTrace ) { 00818 return m00 + m11 + m22 + m33; 00819 } 00820 00821 return m00 + m11 + m22; 00822 } 00823 00824 template< typename T > 00825 Matrix44<T> Matrix44<T>::diagonal() const 00826 { 00827 Matrix44 ret; 00828 ret.m00 = m00; ret.m01 = 0; ret.m02 = 0; ret.m03 = 0; 00829 ret.m10 = 0; ret.m11 = m11; ret.m12 = 0; ret.m13 = 0; 00830 ret.m20 = 0; ret.m21 = 0; ret.m22 = m22; ret.m23 = 0; 00831 ret.m30 = 0; ret.m31 = 0; ret.m32 = 0; ret.m33 = m33; 00832 return ret; 00833 } 00834 00835 template< typename T > 00836 Matrix44<T> Matrix44<T>::lowerTriangular() const 00837 { 00838 Matrix44 ret; 00839 ret.m00 = m00; ret.m01 = 0; ret.m02 = 0; ret.m03 = 0; 00840 ret.m10 = m10; ret.m11 = m11; ret.m12 = 0; ret.m13 = 0; 00841 ret.m20 = m20; ret.m21 = m21; ret.m22 = m22; ret.m23 = 0; 00842 ret.m30 = m30; ret.m31 = m31; ret.m32 = m32; ret.m33 = m33; 00843 return ret; 00844 } 00845 00846 template< typename T > 00847 Matrix44<T> Matrix44<T>::upperTriangular() const 00848 { 00849 Matrix44 ret; 00850 ret.m00 = m00; ret.m01 = m01; ret.m02 = m02; ret.m03 = m03; 00851 ret.m10 = 0; ret.m11 = m11; ret.m12 = m12; ret.m13 = m13; 00852 ret.m20 = 0; ret.m21 = 0; ret.m22 = m22; ret.m23 = m23; 00853 ret.m30 = 0; ret.m31 = 0; ret.m32 = 0; ret.m33 = m33; 00854 return ret; 00855 } 00856 00857 template< typename T > 00858 void Matrix44<T>::transpose() 00859 { 00860 T t; 00861 t = m01; m01 = m10; m10 = t; 00862 t = m02; m02 = m20; m20 = t; 00863 t = m03; m03 = m30; m30 = t; 00864 t = m12; m12 = m21; m21 = t; 00865 t = m13; m13 = m31; m31 = t; 00866 t = m23; m23 = m32; m32 = t; 00867 } 00868 00869 template< typename T > 00870 Matrix44<T> Matrix44<T>::transposed() const 00871 { 00872 return Matrix44<T>( 00873 m[ 0], m[ 4], m[ 8], m[12], 00874 m[ 1], m[ 5], m[ 9], m[13], 00875 m[ 2], m[ 6], m[10], m[14], 00876 m[ 3], m[ 7], m[11], m[15] 00877 ); 00878 } 00879 00880 template< typename T > 00881 inline Matrix44<T> Matrix44<T>::inverted( T epsilon ) const 00882 { 00883 Matrix44<T> inv( (T)0 ); 00884 00885 T a0 = m[ 0]*m[ 5] - m[ 1]*m[ 4]; 00886 T a1 = m[ 0]*m[ 6] - m[ 2]*m[ 4]; 00887 T a2 = m[ 0]*m[ 7] - m[ 3]*m[ 4]; 00888 T a3 = m[ 1]*m[ 6] - m[ 2]*m[ 5]; 00889 T a4 = m[ 1]*m[ 7] - m[ 3]*m[ 5]; 00890 T a5 = m[ 2]*m[ 7] - m[ 3]*m[ 6]; 00891 T b0 = m[ 8]*m[13] - m[ 9]*m[12]; 00892 T b1 = m[ 8]*m[14] - m[10]*m[12]; 00893 T b2 = m[ 8]*m[15] - m[11]*m[12]; 00894 T b3 = m[ 9]*m[14] - m[10]*m[13]; 00895 T b4 = m[ 9]*m[15] - m[11]*m[13]; 00896 T b5 = m[10]*m[15] - m[11]*m[14]; 00897 00898 T det = a0*b5 - a1*b4 + a2*b3 + a3*b2 - a4*b1 + a5*b0; 00899 00900 if( fabs( det ) > epsilon ) { 00901 inv.m[ 0] = + m[ 5]*b5 - m[ 6]*b4 + m[ 7]*b3; 00902 inv.m[ 4] = - m[ 4]*b5 + m[ 6]*b2 - m[ 7]*b1; 00903 inv.m[ 8] = + m[ 4]*b4 - m[ 5]*b2 + m[ 7]*b0; 00904 inv.m[12] = - m[ 4]*b3 + m[ 5]*b1 - m[ 6]*b0; 00905 inv.m[ 1] = - m[ 1]*b5 + m[ 2]*b4 - m[ 3]*b3; 00906 inv.m[ 5] = + m[ 0]*b5 - m[ 2]*b2 + m[ 3]*b1; 00907 inv.m[ 9] = - m[ 0]*b4 + m[ 1]*b2 - m[ 3]*b0; 00908 inv.m[13] = + m[ 0]*b3 - m[ 1]*b1 + m[ 2]*b0; 00909 inv.m[ 2] = + m[13]*a5 - m[14]*a4 + m[15]*a3; 00910 inv.m[ 6] = - m[12]*a5 + m[14]*a2 - m[15]*a1; 00911 inv.m[10] = + m[12]*a4 - m[13]*a2 + m[15]*a0; 00912 inv.m[14] = - m[12]*a3 + m[13]*a1 - m[14]*a0; 00913 inv.m[ 3] = - m[ 9]*a5 + m[10]*a4 - m[11]*a3; 00914 inv.m[ 7] = + m[ 8]*a5 - m[10]*a2 + m[11]*a1; 00915 inv.m[11] = - m[ 8]*a4 + m[ 9]*a2 - m[11]*a0; 00916 inv.m[15] = + m[ 8]*a3 - m[ 9]*a1 + m[10]*a0; 00917 00918 T invDet = ((T)1)/det; 00919 inv.m[ 0] *= invDet; 00920 inv.m[ 1] *= invDet; 00921 inv.m[ 2] *= invDet; 00922 inv.m[ 3] *= invDet; 00923 inv.m[ 4] *= invDet; 00924 inv.m[ 5] *= invDet; 00925 inv.m[ 6] *= invDet; 00926 inv.m[ 7] *= invDet; 00927 inv.m[ 8] *= invDet; 00928 inv.m[ 9] *= invDet; 00929 inv.m[10] *= invDet; 00930 inv.m[11] *= invDet; 00931 inv.m[12] *= invDet; 00932 inv.m[13] *= invDet; 00933 inv.m[14] *= invDet; 00934 inv.m[15] *= invDet; 00935 } 00936 00937 return inv; 00938 } 00939 00940 template< typename T > 00941 Vec3<T> Matrix44<T>::preMultiply( const Vec3<T> &v ) const 00942 { 00943 return Vec3<T>( 00944 v.x*m00 + v.y*m10 + v.z*m20, 00945 v.x*m01 + v.y*m11 + v.z*m21, 00946 v.x*m02 + v.y*m12 + v.z*m22 00947 ); 00948 } 00949 00950 template< typename T > 00951 Vec4<T> Matrix44<T>::preMultiply( const Vec4<T> &v ) const 00952 { 00953 return Vec4<T>( 00954 v.x*m00 + v.y*m10 + v.z*m20 + v.w*m30, 00955 v.x*m01 + v.y*m11 + v.z*m21 + v.w*m31, 00956 v.x*m02 + v.y*m12 + v.z*m22 + v.w*m32, 00957 v.x*m03 + v.y*m13 + v.z*m23 + v.w*m33 00958 ); 00959 } 00960 00961 template< typename T > 00962 Vec3<T> Matrix44<T>::postMultiply( const Vec3<T> &v ) const 00963 { 00964 return Vec3<T>( 00965 m00*v.x + m01*v.y + m02*v.z, 00966 m10*v.x + m11*v.y + m12*v.z, 00967 m20*v.x + m21*v.y + m22*v.z 00968 ); 00969 } 00970 00971 template< typename T > 00972 Vec4<T> Matrix44<T>::postMultiply( const Vec4<T> &v ) const 00973 { 00974 return Vec4<T>( 00975 m00*v.x + m01*v.y + m02*v.z + m03*v.w, 00976 m10*v.x + m11*v.y + m12*v.z + m13*v.w, 00977 m20*v.x + m21*v.y + m22*v.z + m23*v.w, 00978 m30*v.x + m31*v.y + m32*v.z + m33*v.w 00979 ); 00980 } 00981 00982 template< typename T > 00983 Matrix44<T> Matrix44<T>::affineInverted() const 00984 { 00985 Matrix44<T> ret; 00986 00987 // compute upper left 3x3 matrix determinant 00988 T cofactor0 = m[ 5]*m[10] - m[6]*m[ 9]; 00989 T cofactor4 = m[ 2]*m[ 9] - m[1]*m[10]; 00990 T cofactor8 = m[ 1]*m[ 6] - m[2]*m[ 5]; 00991 T det = m[0]*cofactor0 + m[4]*cofactor4 + m[8]*cofactor8; 00992 if( fabs( det ) <= EPSILON ) { 00993 return ret; 00994 } 00995 00996 // create adjunct matrix and multiply by 1/det to get upper 3x3 00997 T invDet = ((T)1.0) / det; 00998 ret.m[ 0] = invDet*cofactor0; 00999 ret.m[ 1] = invDet*cofactor4; 01000 ret.m[ 2] = invDet*cofactor8; 01001 01002 ret.m[ 4] = invDet*(m[ 6]*m[ 8] - m[ 4]*m[10]); 01003 ret.m[ 5] = invDet*(m[ 0]*m[10] - m[ 2]*m[ 8]); 01004 ret.m[ 6] = invDet*(m[ 2]*m[ 4] - m[ 0]*m[ 6]); 01005 01006 ret.m[ 8] = invDet*(m[ 4]*m[ 9] - m[ 5]*m[ 8]); 01007 ret.m[ 9] = invDet*(m[ 1]*m[ 8] - m[ 0]*m[ 9]); 01008 ret.m[10] = invDet*(m[ 0]*m[ 5] - m[ 1]*m[ 4]); 01009 01010 // multiply -translation by inverted 3x3 to get its inverse 01011 ret.m[12] = -ret.m[ 0]*m[12] - ret.m[ 4]*m[13] - ret.m[ 8]*m[14]; 01012 ret.m[13] = -ret.m[ 1]*m[12] - ret.m[ 5]*m[13] - ret.m[ 9]*m[14]; 01013 ret.m[14] = -ret.m[ 2]*m[12] - ret.m[ 6]*m[13] - ret.m[10]*m[14]; 01014 01015 return ret; 01016 } 01017 01018 template< typename T > 01019 Vec3<T> Matrix44<T>::transformPoint( const Vec3<T> &rhs ) const 01020 { 01021 T x = m00*rhs.x + m01*rhs.y + m02*rhs.z + m03; 01022 T y = m10*rhs.x + m11*rhs.y + m12*rhs.z + m13; 01023 T z = m20*rhs.x + m21*rhs.y + m22*rhs.z + m23; 01024 T w = m30*rhs.x + m31*rhs.y + m32*rhs.z + m33; 01025 01026 return Vec3<T>( x / w, y / w, z / w ); 01027 } 01028 01029 template< typename T > 01030 Vec3<T> Matrix44<T>::transformPointAffine( const Vec3<T> &rhs ) const 01031 { 01032 T x = m00*rhs.x + m01*rhs.y + m02*rhs.z + m03; 01033 T y = m10*rhs.x + m11*rhs.y + m12*rhs.z + m13; 01034 T z = m20*rhs.x + m21*rhs.y + m22*rhs.z + m23; 01035 01036 return Vec3<T>( x, y, z ); 01037 } 01038 01039 template< typename T > 01040 Vec3<T> Matrix44<T>::transformVec( const Vec3<T> &rhs ) const 01041 { 01042 T x = m00*rhs.x + m01*rhs.y + m02*rhs.z; 01043 T y = m10*rhs.x + m11*rhs.y + m12*rhs.z; 01044 T z = m20*rhs.x + m21*rhs.y + m22*rhs.z; 01045 01046 return Vec3<T>( x, y, z ); 01047 } 01048 01049 template< typename T > 01050 Matrix44<T> Matrix44<T>::invertTransform() const 01051 { 01052 Matrix44<T> ret; 01053 01054 // inverse translation 01055 ret.at( 0, 3 ) = -at( 0, 3 ); 01056 ret.at( 1, 3 ) = -at( 1, 3 ); 01057 ret.at( 2, 3 ) = -at( 2, 3 ); 01058 ret.at( 3, 3 ) = at( 3, 3 ); 01059 01060 // transpose rotation part 01061 for( int i = 0; i < 3; i++ ) { 01062 for( int j = 0; j < 3; j++ ) { 01063 ret.at( j, i ) = at( i, j ); 01064 } 01065 } 01066 01067 return ret; 01068 } 01069 01070 template<typename T> 01071 Matrix44<T> Matrix44<T>::createTranslation( const Vec3<T> &v, T w ) 01072 { 01073 Matrix44 ret; 01074 ret.m[12] = v.x; 01075 ret.m[13] = v.y; 01076 ret.m[14] = v.z; 01077 ret.m[15] = w; 01078 01079 return ret; 01080 } 01081 01083 // Matrix44 01084 template<typename T> 01085 Matrix44<T> Matrix44<T>::createRotation( const Vec3<T> &from, const Vec3<T> &to, const Vec3<T> &worldUp ) 01086 { 01087 // The goal is to obtain a rotation matrix that takes 01088 // "fromDir" to "toDir". We do this in two steps and 01089 // compose the resulting rotation matrices; 01090 // (a) rotate "fromDir" into the z-axis 01091 // (b) rotate the z-axis into "toDir" 01092 01093 // The from direction must be non-zero; but we allow zero to and up dirs. 01094 if( from.lengthSquared() == 0 ) { 01095 return Matrix44<T>(); 01096 } 01097 else { 01098 Matrix44<T> zAxis2FromDir = alignZAxisWithTarget( from, Vec3<T>::yAxis() ); 01099 Matrix44<T> fromDir2zAxis = zAxis2FromDir.transposed(); 01100 Matrix44<T> zAxis2ToDir = alignZAxisWithTarget( to, worldUp ); 01101 return fromDir2zAxis * zAxis2ToDir; 01102 } 01103 } 01104 01105 template<typename T> 01106 Matrix44<T> Matrix44<T>::createRotation( const Vec3<T> &axis, T angle ) 01107 { 01108 Vec3<T> unit( axis.normalized() ); 01109 T sine = math<T>::sin( angle ); 01110 T cosine = math<T>::cos( angle ); 01111 01112 Matrix44<T> ret; 01113 01114 ret.m[ 0] = unit.x * unit.x * (1 - cosine) + cosine; 01115 ret.m[ 1] = unit.x * unit.y * (1 - cosine) + unit.z * sine; 01116 ret.m[ 2] = unit.x * unit.z * (1 - cosine) - unit.y * sine; 01117 ret.m[ 3] = 0; 01118 01119 ret.m[ 4] = unit.x * unit.y * (1 - cosine) - unit.z * sine; 01120 ret.m[ 5] = unit.y * unit.y * (1 - cosine) + cosine; 01121 ret.m[ 6] = unit.y * unit.z * (1 - cosine) + unit.x * sine; 01122 ret.m[ 7] = 0; 01123 01124 ret.m[ 8] = unit.x * unit.z * (1 - cosine) + unit.y * sine; 01125 ret.m[ 9] = unit.y * unit.z * (1 - cosine) - unit.x * sine; 01126 ret.m[10] = unit.z * unit.z * (1 - cosine) + cosine; 01127 ret.m[11] = 0; 01128 01129 ret.m[12] = 0; 01130 ret.m[13] = 0; 01131 ret.m[14] = 0; 01132 ret.m[15] = 1; 01133 01134 return ret; 01135 } 01136 01137 template<typename T> 01138 Matrix44<T> Matrix44<T>::createRotation( const Vec3<T> &eulerRadians ) 01139 { 01140 // The ordering for this is XYZ. In OpenGL, the ordering 01141 // is the same, but the operations needs to happen in 01142 // reverse: 01143 // 01144 // glRotatef( eulerRadians.z, 0.0f, 0.0f 1.0f ); 01145 // glRotatef( eulerRadians.y, 0.0f, 1.0f 0.0f ); 01146 // glRotatef( eulerRadians.x, 1.0f, 0.0f 0.0f ); 01147 // 01148 01149 Matrix44<T> ret; 01150 T cos_rz, sin_rz, cos_ry, sin_ry, cos_rx, sin_rx; 01151 01152 cos_rx = math<T>::cos( eulerRadians.x ); 01153 cos_ry = math<T>::cos( eulerRadians.y ); 01154 cos_rz = math<T>::cos( eulerRadians.z ); 01155 01156 sin_rx = math<T>::sin( eulerRadians.x ); 01157 sin_ry = math<T>::sin( eulerRadians.y ); 01158 sin_rz = math<T>::sin( eulerRadians.z ); 01159 01160 ret.m[ 0] = cos_rz*cos_ry; 01161 ret.m[ 1] = sin_rz*cos_ry; 01162 ret.m[ 2] = -sin_ry; 01163 ret.m[ 3] = 0; 01164 01165 ret.m[ 4] = -sin_rz*cos_rx + cos_rz*sin_ry*sin_rx; 01166 ret.m[ 5] = cos_rz*cos_rx + sin_rz*sin_ry*sin_rx; 01167 ret.m[ 6] = cos_ry*sin_rx; 01168 ret.m[ 7] = 0; 01169 01170 ret.m[ 8] = sin_rz*sin_rx + cos_rz*sin_ry*cos_rx; 01171 ret.m[ 9] = -cos_rz*sin_rx + sin_rz*sin_ry*cos_rx; 01172 ret.m[10] = cos_ry*cos_rx; 01173 ret.m[11] = 0; 01174 01175 ret.m[12] = 0; 01176 ret.m[13] = 0; 01177 ret.m[14] = 0; 01178 ret.m[15] = 1; 01179 01180 return ret; 01181 } 01182 01183 template<typename T> 01184 Matrix44<T> Matrix44<T>::createScale( T s ) 01185 { 01186 Matrix44<T> ret; 01187 ret.setToIdentity(); 01188 ret.at(0,0) = s; 01189 ret.at(1,1) = s; 01190 ret.at(2,2) = s; 01191 ret.at(3,3) = s; 01192 return ret; 01193 } 01194 01195 template<typename T> 01196 Matrix44<T> Matrix44<T>::createScale( const Vec2<T> &v ) 01197 { 01198 Matrix44<T> ret; 01199 ret.setToIdentity(); 01200 ret.at(0,0) = v.x; 01201 ret.at(1,1) = v.y; 01202 ret.at(2,2) = 1; 01203 ret.at(3,3) = 1; 01204 return ret; 01205 } 01206 01207 template<typename T> 01208 Matrix44<T> Matrix44<T>::createScale( const Vec3<T> &v ) 01209 { 01210 Matrix44<T> ret; 01211 ret.setToIdentity(); 01212 ret.at(0,0) = v.x; 01213 ret.at(1,1) = v.y; 01214 ret.at(2,2) = v.z; 01215 ret.at(3,3) = 1; 01216 return ret; 01217 } 01218 01219 template<typename T> 01220 Matrix44<T> Matrix44<T>::createScale( const Vec4<T> &v ) 01221 { 01222 Matrix44<T> ret; 01223 ret.setToIdentity(); 01224 ret.at(0,0) = v.x; 01225 ret.at(1,1) = v.y; 01226 ret.at(2,2) = v.z; 01227 ret.at(3,3) = v.w; 01228 return ret; 01229 } 01230 01231 template<typename T> 01232 Matrix44<T> Matrix44<T>::alignZAxisWithTarget( Vec3<T> targetDir, Vec3<T> upDir ) 01233 { 01234 // Ensure that the target direction is non-zero. 01235 if( targetDir.lengthSquared() == 0 ) 01236 targetDir = Vec3<T>::zAxis(); 01237 01238 // Ensure that the up direction is non-zero. 01239 if( upDir.lengthSquared() == 0 ) 01240 upDir = Vec3<T>::yAxis(); 01241 01242 // Check for degeneracies. If the upDir and targetDir are parallel 01243 // or opposite, then compute a new, arbitrary up direction that is 01244 // not parallel or opposite to the targetDir. 01245 if( upDir.cross( targetDir ).lengthSquared() == 0 ) { 01246 upDir = targetDir.cross( Vec3<T>::xAxis() ); 01247 if( upDir.lengthSquared() == 0 ) 01248 upDir = targetDir.cross( Vec3<T>::zAxis() ); 01249 } 01250 01251 // Compute the x-, y-, and z-axis vectors of the new coordinate system. 01252 Vec3<T> targetPerpDir = upDir.cross( targetDir ); 01253 Vec3<T> targetUpDir = targetDir.cross( targetPerpDir ); 01254 01255 01256 // Rotate the x-axis into targetPerpDir (row 0), 01257 // rotate the y-axis into targetUpDir (row 1), 01258 // rotate the z-axis into targetDir (row 2). 01259 Vec3<T> row[3]; 01260 row[0] = targetPerpDir.normalized(); 01261 row[1] = targetUpDir.normalized(); 01262 row[2] = targetDir.normalized(); 01263 01264 Matrix44<T> mat( row[0].x, row[0].y, row[0].z, 0, 01265 row[1].x, row[1].y, row[1].z, 0, 01266 row[2].x, row[2].y, row[2].z, 0, 01267 0, 0, 0, 1 ); 01268 01269 return mat; 01270 } 01271 01273 // Typedefs 01274 typedef Matrix44<float> Matrix44f; 01275 typedef Matrix44<double> Matrix44d; 01276 01277 } // namespace cinder