00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
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
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061 union {
00062 T m[16];
00063 struct {
00064
00065
00066
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
00073 T mcols[4][4];
00074 };
00075
00076 Matrix44();
00077
00078 Matrix44( T s );
00079
00080
00081 Matrix44( const T *dt, bool srcIsRowMajor = false );
00082
00083
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
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
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
00131 const Vec3<T> operator*( const Vec3<T> &rhs ) const;
00132
00133
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
00142 T& at( int row, int col );
00143 const T& at( int row, int col ) const;
00144
00145
00146 void set( const T *dt, bool srcIsRowMajor = false );
00147
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
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
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
00185 Vec3<T> preMultiply( const Vec3<T> &v ) const;
00186 Vec4<T> preMultiply( const Vec4<T> &v ) const;
00187
00188
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
00201 Vec3<T> transformPoint( const Vec3<T> &rhs ) const;
00202
00203
00204 Vec3<T> transformPointAffine( const Vec3<T> &rhs ) const;
00205
00206
00207 Vec3<T> transformVec( const Vec3<T> &rhs ) const;
00208 Vec4<T> transformVec( const Vec4<T> &rhs ) const { return transformVec( rhs.xyz() ); }
00209
00210
00211 Vec4<T> getTranslate() const { return Vec4<T>( m03, m13, m23, m33 ); }
00212
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
00217 void translate( const Vec3<T> &tr ) { *this *= createTranslation( tr ); }
00218 void translate( const Vec4<T> &tr ) { *this *= createTranslation( tr ); }
00219
00220
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
00224 void rotate( const Vec3<T> &eulerRadians ) { *this *= createRotation( eulerRadians ); }
00225 void rotate( const Vec4<T> &eulerRadians ) { *this *= createRotation( eulerRadians ); }
00226
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
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
00237 Matrix44<T> invertTransform() const;
00238
00239
00240 static Matrix44<T> identity() { return Matrix44( 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 ); }
00241
00242 static Matrix44<T> one() { return Matrix44( (T)1 ); }
00243
00244 static Matrix44<T> zero() { return Matrix44( (T)0 ); }
00245
00246
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
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
00256 static Matrix44<T> createRotation( const Vec3<T> &eulerRadians );
00257 static Matrix44<T> createRotation( const Vec4<T> &eulerRadians ) { return createRotation( eulerRadians.xyz() ); }
00258
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
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
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
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
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
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 >
01081 void Matrix44<T>::orthonormalInvert()
01082 {
01083
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
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
01109 template<typename T>
01110 Matrix44<T> Matrix44<T>::createRotation( const Vec3<T> &from, const Vec3<T> &to, const Vec3<T> &worldUp )
01111 {
01112
01113
01114
01115
01116
01117
01118
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
01166
01167
01168
01169
01170
01171
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
01271 if( targetDir.lengthSquared() == 0 )
01272 targetDir = Vec3<T>::zAxis();
01273
01274
01275 if( upDir.lengthSquared() == 0 )
01276 upDir = Vec3<T>::yAxis();
01277
01278
01279
01280
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
01288 Vec3<T> targetPerpDir = upDir.cross( targetDir );
01289 Vec3<T> targetUpDir = targetDir.cross( targetPerpDir );
01290
01291
01292
01293
01294
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
01310 typedef Matrix44<float> Matrix44f;
01311 typedef Matrix44<double> Matrix44d;
01312
01313 }