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