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 Matrix33
00038 {
00039 public:
00040 typedef T TYPE;
00041 typedef T value_type;
00042
00043 static const size_t DIM = 3;
00044 static const size_t DIM_SQ = DIM*DIM;
00045 static const size_t MEM_LEN = sizeof(T)*DIM_SQ;
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059 union {
00060 T m[9];
00061 struct {
00062
00063
00064
00065 T m00, m10, m20;
00066 T m01, m11, m21;
00067 T m02, m12, m22;
00068 };
00069
00070 T mcols[3][3];
00071 };
00072
00073 Matrix33();
00074
00075 Matrix33( T s );
00076
00077
00078 Matrix33( const T *dt, bool srcIsRowMajor = false );
00079
00080
00081 Matrix33( T d0, T d1, T d2, T d3, T d4, T d5, T d6, T d7, T d8, bool srcIsRowMajor = false );
00082
00083
00084 Matrix33( const Vec3<T> &vx, const Vec3<T> &vy, const Vec3<T> &vz );
00085
00086 template< typename FromT >
00087 Matrix33( const Matrix33<FromT>& src );
00088
00089 Matrix33( const Matrix22<T>& src );
00090
00091 Matrix33( const Matrix33<T>& src );
00092
00093 operator T*() { return (T*)m; }
00094 operator const T*() const { return (const T*)m; }
00095
00096 Matrix33<T>& operator=( const Matrix33<T>& rhs );
00097 Matrix33<T>& operator=( T rhs );
00098
00099 template< typename FromT >
00100 Matrix33<T>& operator=( const Matrix33<FromT>& rhs );
00101
00102
00103 Matrix33<T>& operator=( const Matrix22<T>& rhs );
00104
00105 bool equalCompare( const Matrix33<T>& rhs, T epsilon ) const;
00106 bool operator==( const Matrix33<T> &rhs ) const { return equalCompare( rhs, (T)EPSILON ); }
00107 bool operator!=( const Matrix33<T> &rhs ) const { return ! ( *this == rhs ); }
00108
00109 Matrix33<T>& operator*=( const Matrix33<T> &rhs );
00110 Matrix33<T>& operator+=( const Matrix33<T> &rhs );
00111 Matrix33<T>& operator-=( const Matrix33<T> &rhs );
00112
00113 Matrix33<T>& operator*=( T s );
00114 Matrix33<T>& operator/=( T s );
00115 Matrix33<T>& operator+=( T s );
00116 Matrix33<T>& operator-=( T s );
00117
00118 const Matrix33<T> operator*( const Matrix33<T> &rhs ) const;
00119 const Matrix33<T> operator+( const Matrix33<T> &rhs ) const;
00120 const Matrix33<T> operator-( const Matrix33<T> &rhs ) const;
00121
00122
00123 const Vec3<T> operator*( const Vec3<T> &rhs ) const;
00124
00125 const Matrix33<T> operator*( T rhs ) const;
00126 const Matrix33<T> operator/( T rhs ) const;
00127 const Matrix33<T> operator+( T rhs ) const;
00128 const Matrix33<T> operator-( T rhs ) const;
00129
00130
00131 T& at( int row, int col );
00132 const T& at( int row, int col ) const;
00133
00134
00135 void set( const T *dt, bool srcIsRowMajor = false );
00136
00137 void set( T d0, T d1, T d2, T d3, T d4, T d5, T d6, T d7, T d8, bool srcIsRowMajor = false );
00138
00139 Vec3<T> getColumn( int col ) const;
00140 void setColumn( int col, const Vec3<T> &v );
00141
00142 Vec3<T> getRow( int row ) const;
00143 void setRow( int row, const Vec3<T> &v );
00144
00145 void getColumns( Vec3<T> *c0, Vec3<T> *c1, Vec3<T> *c2 ) const;
00146 void setColumns( const Vec3<T> &c0, const Vec3<T> &c1, const Vec3<T> &c2 );
00147
00148 void getRows( Vec3<T> *r0, Vec3<T> *r1, Vec3<T> *r2 ) const;
00149 void setRows( const Vec3<T> &r0, const Vec3<T> &r1, const Vec3<T> &r2 );
00150
00151
00152 Matrix22<T> subMatrix22( int row, int col ) const;
00153
00154 void setToNull();
00155 void setToIdentity();
00156
00157 T determinant() const;
00158 T trace() const;
00159
00160 Matrix33<T> diagonal() const;
00161
00162 Matrix33<T> lowerTriangular() const;
00163 Matrix33<T> upperTriangular() const;
00164
00165 void transpose();
00166 Matrix33<T> transposed() const;
00167
00168 void invert (T epsilon = EPSILON ) { *this = inverted( epsilon ); }
00169 Matrix33<T> inverted( T epsilon = EPSILON ) const;
00170
00171
00172 Vec3<T> preMultiply( const Vec3<T> &v ) const;
00173
00174
00175 Vec3<T> postMultiply( const Vec3<T> &v ) const;
00176
00177
00178 Vec3<T> transformVec( const Vec3<T> &v ) const { return postMultiply( v ); }
00179
00180
00181 template <template <typename> class VecT>
00182 void rotate( const VecT<T> &axis, T radians ) { *this *= Matrix33<T>::createRotation( axis, radians ); }
00183
00184 template <template <typename> class VecT>
00185 void rotate( const VecT<T> &eulerRadians ) { *this *= Matrix33<T>::createRotation( eulerRadians ); }
00186
00187 template <template <typename> class VecT>
00188 void rotate( const VecT<T> &from, const VecT<T> &to, const VecT<T> &worldUp ) { *this *= Matrix33<T>::createRotation( from, to, worldUp ); }
00189
00190
00191 Matrix33<T> invertTransform() const;
00192
00193
00194 static Matrix33<T> identity() { return Matrix33( 1, 0, 0, 0, 1, 0, 0, 0, 1 ); }
00195
00196 static Matrix33<T> one() { return Matrix33( (T)1 ); }
00197
00198 static Matrix33<T> zero() { return Matrix33( (T)0 ); }
00199
00200
00201 static Matrix33<T> createRotation( const Vec3<T> &axis, T radians );
00202 static Matrix33<T> createRotation( const Vec3<T> &from, const Vec3<T> &to, const Vec3<T> &worldUp );
00203
00204 static Matrix33<T> createRotation( const Vec3<T> &eulerRadians );
00205
00206
00207 static Matrix33<T> createScale( T s );
00208 static Matrix33<T> createScale( const Vec2<T> &v );
00209 static Matrix33<T> createScale( const Vec3<T> &v );
00210
00211
00212 static Matrix33<T> alignZAxisWithTarget( Vec3<T> targetDir, Vec3<T> upDir );
00213
00214 friend std::ostream& operator<<( std::ostream &lhs, const Matrix33<T> &rhs )
00215 {
00216 for( int i = 0; i < 3; i++) {
00217 lhs << " |";
00218 for( int j = 0; j < 3; j++) {
00219 lhs << std::setw( 12 ) << std::setprecision( 5 ) << rhs.m[j*3+i];
00220 }
00221 lhs << "|" << std::endl;
00222 }
00223
00224 return lhs;
00225 }
00226 };
00227
00228 template< typename T >
00229 Matrix33<T>::Matrix33()
00230 {
00231 setToIdentity();
00232 }
00233
00234 template< typename T >
00235 Matrix33<T>::Matrix33( T s )
00236 {
00237 for( int i = 0; i < DIM_SQ; ++i ) {
00238 m[i] = s;
00239 }
00240 }
00241
00242 template< typename T >
00243 Matrix33<T>::Matrix33( const T *dt, bool srcIsRowMajor )
00244 {
00245 set( dt, srcIsRowMajor );
00246 }
00247
00248 template< typename T >
00249 Matrix33<T>::Matrix33( T d0, T d1, T d2, T d3, T d4, T d5, T d6, T d7, T d8, bool srcIsRowMajor )
00250 {
00251 set( d0, d1, d2,
00252 d3, d4, d5,
00253 d6, d7, d8, srcIsRowMajor );
00254 }
00255
00256 template< typename T >
00257 Matrix33<T>::Matrix33( const Vec3<T> &vx, const Vec3<T> &vy, const Vec3<T> &vz )
00258 {
00259 m00 = vx.x; m01 = vy.x; m02 = vz.x;
00260 m10 = vx.y; m11 = vy.y; m12 = vz.y;
00261 m20 = vx.z; m21 = vy.z; m22 = vz.z;
00262 }
00263
00264 template< typename T >
00265 template< typename FromT >
00266 Matrix33<T>::Matrix33( const Matrix33<FromT>& src )
00267 {
00268 for( int i = 0; i < DIM_SQ; ++i ) {
00269 m[i] = static_cast<T>( src.m[i] );
00270 }
00271 }
00272
00273 template< typename T >
00274 Matrix33<T>::Matrix33( const Matrix22<T>& src )
00275 {
00276 setToIdentity();
00277 m00 = src.m00; m01 = src.m01;
00278 m10 = src.m10; m11 = src.m11;
00279 }
00280
00281 template< typename T >
00282 Matrix33<T>::Matrix33( const Matrix33<T>& src )
00283 {
00284 std::memcpy( m, src.m, MEM_LEN );
00285 }
00286
00287 template< typename T >
00288 Matrix33<T>& Matrix33<T>::operator=( const Matrix33<T>& rhs )
00289 {
00290 memcpy( m, rhs.m, MEM_LEN );
00291 return *this;
00292 }
00293
00294 template< typename T >
00295 Matrix33<T>& Matrix33<T>::operator=( T rhs )
00296 {
00297 for( int i = 0; i < DIM_SQ; ++i ) {
00298 m[i] = rhs;
00299 }
00300 return *this;
00301 }
00302
00303 template< typename T >
00304 template< typename FromT >
00305 Matrix33<T>& Matrix33<T>::operator=( const Matrix33<FromT>& rhs )
00306 {
00307 for( int i = 0; i < DIM_SQ; i++ ) {
00308 m[i] = static_cast<T>(rhs.m[i]);
00309 }
00310 return *this;
00311 }
00312
00313 template< typename T >
00314 Matrix33<T>& Matrix33<T>::operator=( const Matrix22<T>& rhs )
00315 {
00316 setToIdentity();
00317 m00 = rhs.m00; m01 = rhs.m01;
00318 m10 = rhs.m10; m11 = rhs.m11;
00319 return *this;
00320 }
00321
00322 template< typename T >
00323 bool Matrix33<T>::equalCompare( const Matrix33<T>& rhs, T epsilon ) const
00324 {
00325 for( int i = 0; i < DIM_SQ; ++i ) {
00326 if( math<T>::abs(m[i] - rhs.m[i]) >= epsilon )
00327 return false;
00328 }
00329 return true;
00330 }
00331
00332 template< typename T >
00333 Matrix33<T>& Matrix33<T>::operator*=( const Matrix33<T> &rhs )
00334 {
00335 Matrix33<T> mat;
00336
00337 mat.m[0] = m[0]*rhs.m[0] + m[3]*rhs.m[1] + m[6]*rhs.m[2];
00338 mat.m[1] = m[1]*rhs.m[0] + m[4]*rhs.m[1] + m[7]*rhs.m[2];
00339 mat.m[2] = m[2]*rhs.m[0] + m[5]*rhs.m[1] + m[8]*rhs.m[2];
00340
00341 mat.m[3] = m[0]*rhs.m[3] + m[3]*rhs.m[4] + m[6]*rhs.m[5];
00342 mat.m[4] = m[1]*rhs.m[3] + m[4]*rhs.m[4] + m[7]*rhs.m[5];
00343 mat.m[5] = m[2]*rhs.m[3] + m[5]*rhs.m[4] + m[8]*rhs.m[5];
00344
00345 mat.m[6] = m[0]*rhs.m[6] + m[3]*rhs.m[7] + m[6]*rhs.m[8];
00346 mat.m[7] = m[1]*rhs.m[6] + m[4]*rhs.m[7] + m[7]*rhs.m[8];
00347 mat.m[8] = m[2]*rhs.m[6] + m[5]*rhs.m[7] + m[8]*rhs.m[8];
00348
00349 *this = mat;
00350
00351 return *this;
00352 }
00353
00354 template< typename T >
00355 Matrix33<T>& Matrix33<T>::operator+=( const Matrix33<T> &rhs )
00356 {
00357 for( int i = 0; i < DIM_SQ; ++i ) {
00358 m[i] += rhs.m[i];
00359 }
00360 return *this;
00361 }
00362
00363 template< typename T >
00364 Matrix33<T>& Matrix33<T>::operator-=( const Matrix33<T> &rhs )
00365 {
00366 for( int i = 0; i < DIM_SQ; ++i ) {
00367 m[i] -= rhs.m[i];
00368 }
00369 return *this;
00370 }
00371
00372 template< typename T >
00373 Matrix33<T>& Matrix33<T>::operator*=( T s )
00374 {
00375 for( int i = 0; i < DIM_SQ; ++i ) {
00376 m[i] *= s;
00377 }
00378 return *this;
00379 }
00380
00381 template< typename T >
00382 Matrix33<T>& Matrix33<T>::operator/=( T s )
00383 {
00384 T invS = (T)1/s;
00385 for( int i = 0; i < DIM_SQ; ++i ) {
00386 m[i] *= invS;
00387 }
00388 return *this;
00389 }
00390
00391 template< typename T >
00392 Matrix33<T>& Matrix33<T>::operator+=( T s )
00393 {
00394 for( int i = 0; i < DIM_SQ; ++i ) {
00395 m[i] += s;
00396 }
00397 return *this;
00398 }
00399
00400 template< typename T >
00401 Matrix33<T>& Matrix33<T>::operator-=( T s )
00402 {
00403 for( int i = 0; i < DIM_SQ; ++i ) {
00404 m[i] -= s;
00405 }
00406 return *this;
00407 }
00408
00409 template< typename T >
00410 const Matrix33<T> Matrix33<T>::operator*( const Matrix33<T> &rhs ) const
00411 {
00412 Matrix33<T> ret;
00413
00414 ret.m[0] = m[0]*rhs.m[0] + m[3]*rhs.m[1] + m[6]*rhs.m[2];
00415 ret.m[1] = m[1]*rhs.m[0] + m[4]*rhs.m[1] + m[7]*rhs.m[2];
00416 ret.m[2] = m[2]*rhs.m[0] + m[5]*rhs.m[1] + m[8]*rhs.m[2];
00417
00418 ret.m[3] = m[0]*rhs.m[3] + m[3]*rhs.m[4] + m[6]*rhs.m[5];
00419 ret.m[4] = m[1]*rhs.m[3] + m[4]*rhs.m[4] + m[7]*rhs.m[5];
00420 ret.m[5] = m[2]*rhs.m[3] + m[5]*rhs.m[4] + m[8]*rhs.m[5];
00421
00422 ret.m[6] = m[0]*rhs.m[6] + m[3]*rhs.m[7] + m[6]*rhs.m[8];
00423 ret.m[7] = m[1]*rhs.m[6] + m[4]*rhs.m[7] + m[7]*rhs.m[8];
00424 ret.m[8] = m[2]*rhs.m[6] + m[5]*rhs.m[7] + m[8]*rhs.m[8];
00425
00426 return ret;
00427 }
00428
00429 template< typename T >
00430 const Matrix33<T> Matrix33<T>::operator+( const Matrix33<T> &rhs ) const
00431 {
00432 Matrix33<T> ret;
00433 for( int i = 0; i < DIM_SQ; ++i ) {
00434 ret.m[i] = m[i] + rhs.m[i];
00435 }
00436 return ret;
00437 }
00438
00439 template< typename T >
00440 const Matrix33<T> Matrix33<T>::operator-( const Matrix33<T> &rhs ) const
00441 {
00442 Matrix33<T> ret;
00443 for( int i = 0; i < DIM_SQ; ++i ) {
00444 ret.m[i] = m[i] - rhs.m[i];
00445 }
00446 return ret;
00447 }
00448
00449 template< typename T >
00450 const Vec3<T> Matrix33<T>::operator*( const Vec3<T> &rhs ) const
00451 {
00452 return Vec3<T>(
00453 m[0]*rhs.x + m[3]*rhs.y + m[6]*rhs.z,
00454 m[1]*rhs.x + m[4]*rhs.y + m[7]*rhs.z,
00455 m[2]*rhs.x + m[5]*rhs.y + m[8]*rhs.z
00456 );
00457 }
00458
00459 template< typename T >
00460 const Matrix33<T> Matrix33<T>::operator*( T rhs ) const
00461 {
00462 Matrix33<T> ret;
00463 for( int i = 0; i < DIM_SQ; ++i ) {
00464 ret.m[i] = m[i]*rhs;
00465 }
00466 return ret;
00467 }
00468
00469 template< typename T >
00470 const Matrix33<T> Matrix33<T>::operator/( T rhs ) const
00471 {
00472 Matrix33<T> ret;
00473 T s = (T)1/rhs;
00474 for( int i = 0; i < DIM_SQ; ++i ) {
00475 ret.m[i] = m[i]*s;
00476 }
00477 return ret;
00478 }
00479
00480 template< typename T >
00481 const Matrix33<T> Matrix33<T>::operator+( T rhs ) const
00482 {
00483 Matrix33<T> ret;
00484 for( int i = 0; i < DIM_SQ; ++i ) {
00485 ret.m[i] = m[i] + rhs;
00486 }
00487 return ret;
00488 }
00489
00490 template< typename T >
00491 const Matrix33<T> Matrix33<T>::operator-( T rhs ) const
00492 {
00493 Matrix33<T> ret;
00494 for( int i = 0; i < DIM_SQ; ++i ) {
00495 ret.m[i] = m[i] - rhs;
00496 }
00497 return ret;
00498 }
00499
00500 template< typename T >
00501 T& Matrix33<T>::at( int row, int col )
00502 {
00503 assert(row >= 0 && row < DIM);
00504 assert(col >= 0 && col < DIM);
00505 return m[col*DIM + row];
00506 }
00507
00508 template< typename T >
00509 const T& Matrix33<T>::at( int row, int col ) const
00510 {
00511 assert(row >= 0 && row < DIM);
00512 assert(col >= 0 && col < DIM);
00513 return m[col*DIM + row];
00514 }
00515
00516 template< typename T >
00517 void Matrix33<T>::set( const T *dt, bool srcIsRowMajor )
00518 {
00519 if( srcIsRowMajor ) {
00520 m[0] = dt[0]; m[3] = dt[1]; m[6] = dt[2];
00521 m[1] = dt[3]; m[4] = dt[4]; m[7] = dt[5];
00522 m[2] = dt[6]; m[5] = dt[7]; m[8] = dt[8];
00523 }
00524 else {
00525 std::memcpy( m, dt, MEM_LEN );
00526 }
00527 }
00528
00529 template< typename T >
00530 void Matrix33<T>::set( T d0, T d1, T d2, T d3, T d4, T d5, T d6, T d7, T d8, bool srcIsRowMajor )
00531 {
00532 if( srcIsRowMajor ) {
00533 m[0] = d0; m[3] = d1; m[6] = d2;
00534 m[1] = d3; m[4] = d4; m[7] = d5;
00535 m[2] = d6; m[5] = d7; m[8] = d8;
00536 }
00537 else {
00538 m[0] = d0; m[3] = d3; m[6] = d6;
00539 m[1] = d1; m[4] = d4; m[7] = d7;
00540 m[2] = d2; m[5] = d5; m[8] = d8;
00541 }
00542 }
00543
00544 template< typename T >
00545 Vec3<T> Matrix33<T>::getColumn( int col ) const
00546 {
00547 size_t i = col*DIM;
00548 return Vec3<T>(
00549 m[i + 0],
00550 m[i + 1],
00551 m[i + 2]
00552 );
00553 }
00554
00555 template< typename T >
00556 void Matrix33<T>::setColumn( int col, const Vec3<T> &v )
00557 {
00558 size_t i = col*DIM;
00559 m[i + 0] = v.x;
00560 m[i + 1] = v.y;
00561 m[i + 2] = v.z;
00562 }
00563
00564 template< typename T >
00565 Vec3<T> Matrix33<T>::getRow( int row ) const
00566 {
00567 return Vec3<T>(
00568 m[row + 0],
00569 m[row + 3],
00570 m[row + 6]
00571 );
00572 }
00573
00574 template< typename T >
00575 void Matrix33<T>::setRow( int row, const Vec3<T> &v )
00576 {
00577 m[row + 0] = v.x;
00578 m[row + 3] = v.y;
00579 m[row + 6] = v.z;
00580 }
00581
00582 template< typename T >
00583 void Matrix33<T>::getColumns( Vec3<T> *c0, Vec3<T> *c1, Vec3<T> *c2 ) const
00584 {
00585 *c0 = getColumn( 0 );
00586 *c1 = getColumn( 1 );
00587 *c2 = getColumn( 2 );
00588 }
00589
00590 template< typename T >
00591 void Matrix33<T>::setColumns( const Vec3<T> &c0, const Vec3<T> &c1, const Vec3<T> &c2 )
00592 {
00593 setColumn( 0, c0 );
00594 setColumn( 1, c1 );
00595 setColumn( 2, c2 );
00596 }
00597
00598 template< typename T >
00599 void Matrix33<T>::getRows( Vec3<T> *r0, Vec3<T> *r1, Vec3<T> *r2 ) const
00600 {
00601 *r0 = getRow( 0 );
00602 *r1 = getRow( 1 );
00603 *r2 = getRow( 2 );
00604 }
00605
00606 template< typename T >
00607 void Matrix33<T>::setRows( const Vec3<T> &r0, const Vec3<T> &r1, const Vec3<T> &r2 )
00608 {
00609 setRow( 0, r0 );
00610 setRow( 1, r1 );
00611 setRow( 2, r2 );
00612 }
00613
00614 template< typename T >
00615 Matrix22<T> Matrix33<T>::subMatrix22( int row, int col ) const
00616 {
00617 Matrix22<T> ret;
00618 ret.setToNull();
00619
00620 for( int i = 0; i < 2; ++i ) {
00621 int r = row + i;
00622 if( r >= 3 ) {
00623 continue;
00624 }
00625 for( int j = 0; j < 2; ++j ) {
00626 int c = col + j;
00627 if( c >= 3 ) {
00628 continue;
00629 }
00630 ret.at( i, j ) = at( r, c );
00631 }
00632 }
00633
00634 return ret;
00635 }
00636
00637 template< typename T >
00638 void Matrix33<T>::setToNull()
00639 {
00640 std::memset( m, 0, MEM_LEN );
00641 }
00642
00643 template< typename T >
00644 void Matrix33<T>::setToIdentity()
00645 {
00646 m00 = 1; m01 = 0; m02 = 0;
00647 m10 = 0; m11 = 1; m12 = 0;
00648 m20 = 0; m21 = 0; m22 = 1;
00649 }
00650
00651 template< typename T >
00652 T Matrix33<T>::determinant() const
00653 {
00654 T co00 = m[4]*m[8] - m[5]*m[7];
00655 T co10 = m[5]*m[6] - m[3]*m[8];
00656 T co20 = m[3]*m[7] - m[4]*m[6];
00657
00658 T det = m[0]*co00 + m[1]*co10 + m[2]*co20;
00659
00660 return det;
00661 }
00662
00663 template< typename T >
00664 T Matrix33<T>::trace() const
00665 {
00666 return m00 + m11 + m22;
00667 }
00668
00669 template< typename T >
00670 Matrix33<T> Matrix33<T>::diagonal() const
00671 {
00672 Matrix33 ret;
00673 ret.m00 = m00; ret.m01 = 0; ret.m02 = 0;
00674 ret.m10 = 0; ret.m11 = m11; ret.m12 = 0;
00675 ret.m20 = 0; ret.m21 = 0; ret.m22 = m22;
00676 return ret;
00677 }
00678
00679 template< typename T >
00680 Matrix33<T> Matrix33<T>::lowerTriangular() const
00681 {
00682 Matrix33 ret;
00683 ret.m00 = m00; ret.m01 = 0; ret.m02 = 0;
00684 ret.m10 = m10; ret.m11 = m11; ret.m12 = 0;
00685 ret.m20 = m20; ret.m21 = m21; ret.m22 = m22;
00686 return ret;
00687 }
00688
00689 template< typename T >
00690 Matrix33<T> Matrix33<T>::upperTriangular() const
00691 {
00692 Matrix33 ret;
00693 ret.m00 = m00; ret.m01 = m01; ret.m02 = m02;
00694 ret.m10 = 0; ret.m11 = m11; ret.m12 = m12;
00695 ret.m20 = 0; ret.m21 = 0; ret.m22 = m22;
00696 return ret;
00697 }
00698
00699 template< typename T >
00700 void Matrix33<T>::transpose()
00701 {
00702 T t;
00703 t = m01; m01 = m10; m10 = t;
00704 t = m02; m02 = m20; m20 = t;
00705 t = m12; m12 = m21; m21 = t;
00706 }
00707
00708 template< typename T >
00709 Matrix33<T> Matrix33<T>::transposed() const
00710 {
00711 return Matrix33<T>(
00712 m[ 0], m[ 3], m[6],
00713 m[ 1], m[ 4], m[7],
00714 m[ 2], m[ 5], m[8]
00715 );
00716 }
00717
00718 template< typename T >
00719 Matrix33<T> Matrix33<T>::inverted( T epsilon ) const
00720 {
00721 Matrix33<T> inv( (T)0 );
00722
00723
00724 inv.m[0] = m[4]*m[8] - m[5]*m[7];
00725 inv.m[1] = m[2]*m[7] - m[1]*m[8];
00726 inv.m[2] = m[1]*m[5] - m[2]*m[4];
00727 inv.m[3] = m[5]*m[6] - m[3]*m[8];
00728 inv.m[4] = m[0]*m[8] - m[2]*m[6];
00729 inv.m[5] = m[2]*m[3] - m[0]*m[5];
00730 inv.m[6] = m[3]*m[7] - m[4]*m[6];
00731 inv.m[7] = m[1]*m[6] - m[0]*m[7];
00732 inv.m[8] = m[0]*m[4] - m[1]*m[3];
00733
00734 T det = m[0]*inv.m[0] + m[1]*inv.m[3] + m[2]*inv.m[6];
00735
00736 if( fabs( det ) > epsilon ) {
00737 T invDet = (T)1/det;
00738 inv.m[0] *= invDet;
00739 inv.m[1] *= invDet;
00740 inv.m[2] *= invDet;
00741 inv.m[3] *= invDet;
00742 inv.m[4] *= invDet;
00743 inv.m[5] *= invDet;
00744 inv.m[6] *= invDet;
00745 inv.m[7] *= invDet;
00746 inv.m[8] *= invDet;
00747 }
00748
00749 return inv;
00750 }
00751
00752 template< typename T >
00753 Vec3<T> Matrix33<T>::preMultiply( const Vec3<T> &v ) const
00754 {
00755 return Vec3<T>(
00756 v.x*m00 + v.y*m10 + v.z*m20,
00757 v.x*m01 + v.y*m11 + v.z*m21,
00758 v.x*m02 + v.y*m12 + v.z*m22
00759 );
00760 }
00761
00762 template< typename T >
00763 Vec3<T> Matrix33<T>::postMultiply( const Vec3<T> &v ) const
00764 {
00765 return Vec3<T>(
00766 m00*v.x + m01*v.y + m02*v.z,
00767 m10*v.x + m11*v.y + m12*v.z,
00768 m20*v.x + m21*v.y + m22*v.z
00769 );
00770 }
00771
00772 template< typename T >
00773 Matrix33<T> Matrix33<T>::invertTransform() const
00774 {
00775 Matrix33<T> ret;
00776
00777
00778 for( int i = 0; i < DIM; i++ ) {
00779 for( int j = 0; j < DIM; j++ ) {
00780 ret.at( j, i ) = at( i, j );
00781 }
00782 }
00783
00784 return ret;
00785 }
00786
00787 template<typename T>
00788 Matrix33<T> Matrix33<T>::createRotation( const Vec3<T> &from, const Vec3<T> &to, const Vec3<T> &worldUp )
00789 {
00790
00791
00792
00793
00794
00795
00796
00797 if( from.lengthSquared() == 0 ) {
00798 return Matrix33<T>();
00799 }
00800 else {
00801 Matrix33<T> zAxis2FromDir = alignZAxisWithTarget( from, Vec3<T>::yAxis() );
00802 Matrix33<T> fromDir2zAxis = zAxis2FromDir.transposed();
00803 Matrix33<T> zAxis2ToDir = alignZAxisWithTarget( to, worldUp );
00804 return fromDir2zAxis * zAxis2ToDir;
00805 }
00806 }
00807
00808 template<typename T>
00809 Matrix33<T> Matrix33<T>::createRotation( const Vec3<T> &axis, T angle )
00810 {
00811 Vec3<T> unit( axis.normalized() );
00812 T sine = math<T>::sin( angle );
00813 T cosine = math<T>::cos( angle );
00814
00815 Matrix33<T> ret;
00816
00817 ret.m[0] = unit.x * unit.x * (1 - cosine) + cosine;
00818 ret.m[1] = unit.x * unit.y * (1 - cosine) + unit.z * sine;
00819 ret.m[2] = unit.x * unit.z * (1 - cosine) - unit.y * sine;
00820
00821 ret.m[3] = unit.x * unit.y * (1 - cosine) - unit.z * sine;
00822 ret.m[4] = unit.y * unit.y * (1 - cosine) + cosine;
00823 ret.m[5] = unit.y * unit.z * (1 - cosine) + unit.x * sine;
00824
00825 ret.m[6] = unit.x * unit.z * (1 - cosine) + unit.y * sine;
00826 ret.m[7] = unit.y * unit.z * (1 - cosine) - unit.x * sine;
00827 ret.m[8] = unit.z * unit.z * (1 - cosine) + cosine;
00828
00829 return ret;
00830 }
00831
00832 template<typename T>
00833 Matrix33<T> Matrix33<T>::createRotation( const Vec3<T> &eulerRadians )
00834 {
00835
00836
00837
00838
00839
00840
00841
00842
00843
00844 Matrix33<T> ret;
00845 T cos_rz, sin_rz, cos_ry, sin_ry, cos_rx, sin_rx;
00846
00847 cos_rx = math<T>::cos( eulerRadians.x );
00848 cos_ry = math<T>::cos( eulerRadians.y );
00849 cos_rz = math<T>::cos( eulerRadians.z );
00850
00851 sin_rx = math<T>::sin( eulerRadians.x );
00852 sin_ry = math<T>::sin( eulerRadians.y );
00853 sin_rz = math<T>::sin( eulerRadians.z );
00854
00855 ret.m[0] = cos_rz*cos_ry;
00856 ret.m[1] = sin_rz*cos_ry;
00857 ret.m[2] = -sin_ry;
00858
00859 ret.m[3] = -sin_rz*cos_rx + cos_rz*sin_ry*sin_rx;
00860 ret.m[4] = cos_rz*cos_rx + sin_rz*sin_ry*sin_rx;
00861 ret.m[5] = cos_ry*sin_rx;
00862
00863 ret.m[6] = sin_rz*sin_rx + cos_rz*sin_ry*cos_rx;
00864 ret.m[7] = -cos_rz*sin_rx + sin_rz*sin_ry*cos_rx;
00865 ret.m[8] = cos_ry*cos_rx;
00866
00867 return ret;
00868 }
00869
00870 template<typename T>
00871 Matrix33<T> Matrix33<T>::createScale( T s )
00872 {
00873 Matrix33<T> ret;
00874 ret.setToIdentity();
00875 ret.at(0,0) = s;
00876 ret.at(1,1) = s;
00877 ret.at(2,2) = s;
00878 return ret;
00879 }
00880
00881 template<typename T>
00882 Matrix33<T> Matrix33<T>::createScale( const Vec2<T> &v )
00883 {
00884 Matrix33<T> ret;
00885 ret.setToIdentity();
00886 ret.at(0,0) = v.x;
00887 ret.at(1,1) = v.y;
00888 ret.at(2,2) = 1;
00889 return ret;
00890 }
00891
00892 template<typename T>
00893 Matrix33<T> Matrix33<T>::createScale( const Vec3<T> &v )
00894 {
00895 Matrix33<T> ret;
00896 ret.setToIdentity();
00897 ret.at(0,0) = v.x;
00898 ret.at(1,1) = v.y;
00899 ret.at(2,2) = v.z;
00900 return ret;
00901 }
00902
00903 template<typename T>
00904 Matrix33<T> Matrix33<T>::alignZAxisWithTarget( Vec3<T> targetDir, Vec3<T> upDir )
00905 {
00906
00907
00908 if( targetDir.lengthSquared() < EPSILON ) {
00909
00910 targetDir = -Vec3<T>::zAxis();
00911 }
00912
00913
00914 if( upDir.lengthSquared() < EPSILON ) {
00915 upDir = Vec3<T>::yAxis();
00916 }
00917
00918
00919
00920
00921 if( upDir.cross( targetDir ).lengthSquared() == 0 ) {
00922 upDir = targetDir.cross( Vec3<T>::xAxis() );
00923 if( upDir.lengthSquared() == 0 ) {
00924 upDir = targetDir.cross( Vec3<T>::zAxis() );
00925 }
00926 }
00927
00928
00929 Vec3<T> targetPerpDir = targetDir.cross( upDir );
00930 Vec3<T> targetUpDir = targetPerpDir.cross( targetDir );
00931
00932
00933
00934
00935
00936 Vec3<T> row[3];
00937 row[0] = targetPerpDir.normalized();
00938 row[1] = targetUpDir.normalized();
00939 row[2] = targetDir.normalized();
00940
00941 Matrix33<T> mat(
00942 row[0].x, row[0].y, row[0].z,
00943 row[1].x, row[1].y, row[1].z,
00944 row[2].x, row[2].y, row[2].z
00945 );
00946
00947 return mat;
00948 }
00949
00951
00952 typedef Matrix33<float> Matrix33f;
00953 typedef Matrix33<double> Matrix33d;
00954
00955 }