|
World of Rigid Bodies (WoRB)
|
|
World of Rigid Bodies (WoRB)
|
00001 #ifndef _QTENSOR_H_INCLUDED 00002 #define _QTENSOR_H_INCLUDED 00003 00004 /** 00005 * @file QTensor.h 00006 * @brief Definitions for the QTensor class that represents a quaternionic tensor. 00007 * @author Mikica Kocic 00008 * @version 0.5 00009 * @date 2012-05-05 00010 * @copyright GNU Public License. 00011 */ 00012 00013 #include "Quaternion.h" 00014 00015 namespace WoRB 00016 { 00017 ///////////////////////////////////////////////////////////////////////////////////// 00018 /** @class QTensor 00019 * 00020 * Encapsulates a quaternionic tensor (q-tensor) as a 4x4 row-major matrix. 00021 * 00022 * @note The QTensor class is implemented with inline methods only. 00023 */ 00024 class QTensor 00025 { 00026 const static int Length = 16; //!< Number of components 00027 00028 public: 00029 ///////////////////////////////////////////////////////////////////////////////// 00030 /** @name Q-Tensor components */ 00031 ///////////////////////////////////////////////////////////////////////////////// 00032 /*@{*/ 00033 /** Represents tensor components in a *column-major* order. 00034 * 00035 * Components are stored contiguously in linear memory as: 00036 * 00037 * `m.xx`, `m.yx`, `m.zx`... `m.zw`, `m.ww`. 00038 * 00039 * Offset of the component is calculated as `row + column * NumRows` 00040 * 00041 * @note C/C++ stores matrices in _row-major_ order. MATLAB and OpenGL 00042 * stores matrices in column-major order. 00043 * 00044 * See <a href="http://en.wikipedia.org/wiki/Row-major_order"> 00045 * Row-Major Order on Wikipedia </a> 00046 */ 00047 struct Components 00048 { 00049 double xx, yx, zx, /**/ wx; 00050 double xy, yy, zy, /**/ wy; 00051 double xz, yz, zz, /**/ wz; 00052 ///////////////////////////**/////// 00053 double xw, yw, zw, /**/ ww; 00054 }; 00055 00056 union 00057 { 00058 /** Holds the tensor components as a column-major matrix. 00059 */ 00060 Components m; 00061 00062 /** Holds the contigous memory array of the components 00063 */ 00064 double data[ Length ]; 00065 }; 00066 /*@}*/ 00067 ///////////////////////////////////////////////////////////////////////////////// 00068 /** @name Constructors and assignment operators */ 00069 ///////////////////////////////////////////////////////////////////////////////// 00070 /*@{*/ 00071 /** Represents type of initial matrix data for QTensor constructor. 00072 */ 00073 enum Initializer 00074 { 00075 Uninitialized, //!< Matrix with uninitialized components 00076 Normalized, //!< Matrix with m.ww = 1 and other components set to zero 00077 Zero, //!< Matrix with all components set to zero 00078 Identity //!< Identity matrix, i.e. 1 on the main diagonal 00079 }; 00080 00081 /** Creates a tensor with initial contents according to the given type. 00082 */ 00083 QTensor( Initializer type = Normalized ) 00084 { 00085 switch( type ) 00086 { 00087 case Uninitialized: 00088 break; 00089 00090 case Normalized: 00091 SetComponents( 0 ); 00092 m.ww = 1.0; 00093 break; 00094 00095 case Zero: 00096 SetComponents( 0 ); 00097 break; 00098 00099 case Identity: 00100 SetComponents( 0 ); 00101 m.xx = m.yy = m.zz = m.ww = 1.0; 00102 break; 00103 } 00104 } 00105 00106 /** Sets the main diagonal with quaternion components. 00107 */ 00108 QTensor( const Quaternion& q ) 00109 { 00110 SetComponents( 0 ); 00111 m.xx = q.x; 00112 m.yy = q.y; 00113 m.zz = q.z; 00114 m.ww = q.w; 00115 } 00116 00117 /** Sets the matrix with values along the main diagonal. 00118 */ 00119 QTensor( double xx, double yy, double zz, double ww = 1.0 ) 00120 { 00121 SetComponents( 0 ); 00122 m.xx = xx; 00123 m.yy = yy; 00124 m.zz = zz; 00125 m.ww = ww; 00126 } 00127 00128 /** Sets the main diagonal with the given value. 00129 */ 00130 QTensor& operator = ( double valueOnDiagonal ) 00131 { 00132 SetComponents( 0 ); 00133 m.xx = m.yy = m.zz = m.ww = valueOnDiagonal; 00134 return *this; 00135 } 00136 /*@}*/ 00137 ///////////////////////////////////////////////////////////////////////////////// 00138 /** @name Indexing operators */ 00139 ///////////////////////////////////////////////////////////////////////////////// 00140 /*@{*/ 00141 double& operator [] ( unsigned index ) 00142 { 00143 return *( &m.xx + index ); 00144 } 00145 /*@}*/ 00146 ///////////////////////////////////////////////////////////////////////////////// 00147 /** @name Components setters */ 00148 ///////////////////////////////////////////////////////////////////////////////// 00149 /*@{*/ 00150 /** Sets all components to the same value. 00151 */ 00152 QTensor& SetComponents( double valueForAllComponents ) 00153 { 00154 for ( int i = 0; i < Length; ++i ) { 00155 data[i] = valueForAllComponents; 00156 } 00157 return *this; 00158 } 00159 00160 /** Sets the q-tensor values from the given three vector components. 00161 * 00162 * These are arranged as the three columns of the vector. 00163 */ 00164 QTensor& SetColumnVectors( 00165 const Quaternion& v1, const Quaternion& v2, const Quaternion& v3 ) 00166 { 00167 m.xx = v1.x ; m.xy = v2.x ; m.xz = v3.x ; m.xw = 0 ; 00168 m.yx = v1.y ; m.yy = v2.y ; m.yz = v3.y ; m.yw = 0 ; 00169 m.zx = v1.z ; m.zy = v2.z ; m.zz = v3.z ; m.zw = 0 ; 00170 m.wx = 0 ; m.wy = 0 ; m.wz = 0 ; m.ww = 1 ; 00171 00172 return *this; 00173 } 00174 00175 /** Sets the matrix to be a skew symmetric matrix based on 00176 * the given quaternion. 00177 */ 00178 QTensor& SetSkewSymmetric( const Quaternion& q ) 00179 { 00180 m.xx = 0 ; m.xy = -q.z ; m.xz = q.y ; m.xw = 0 ; 00181 m.yx = q.z ; m.yy = 0 ; m.yz = -q.x ; m.yw = 0 ; 00182 m.zx = -q.y ; m.zy = q.x ; m.zz = 0 ; m.zw = 0 ; 00183 m.wx = 0 ; m.wy = 0 ; m.wz = 0 ; m.ww = 0 ; 00184 00185 return *this; 00186 } 00187 00188 /** Sets the q-tensor to represent a "left-multiplier quaternion" matrix L(q). 00189 * 00190 * See "Rotation of Moment of Inertia Tensor" Mathematica notebook by MBK. 00191 */ 00192 QTensor& SetLeftMultiplier( const Quaternion& q ) 00193 { 00194 m.xx = q.w ; m.xy = -q.z ; m.xz = q.y ; m.xw = q.x ; 00195 m.yx = q.z ; m.yy = q.w ; m.yz = -q.x ; m.yw = q.y ; 00196 m.zx = -q.y ; m.zy = q.x ; m.zz = q.w ; m.zw = q.z ; 00197 m.wx = -q.x ; m.wy = -q.y ; m.wz = -q.z ; m.ww = q.w ; 00198 00199 return *this; 00200 } 00201 00202 /** Sets the q-tensor to represent a "right-multiplier quaternion" matrix R(q). 00203 * 00204 * See "Rotation of Moment of Inertia Tensor" Mathematica notebook by MBK. 00205 */ 00206 QTensor& SetRightMultiplier( const Quaternion& q ) 00207 { 00208 m.xx = q.w ; m.xy = q.z ; m.xz = -q.y ; m.xw = q.x ; 00209 m.yx = -q.z ; m.yy = q.w ; m.yz = q.x ; m.yw = q.y ; 00210 m.zx = q.y ; m.zy = -q.x ; m.zz = q.w ; m.zw = q.z ; 00211 m.wx = -q.x ; m.wy = -q.y ; m.wz = -q.z ; m.ww = q.w ; 00212 00213 return *this; 00214 } 00215 00216 /** Creates a transform matrix from a position and orientation. 00217 * 00218 * This is so called Shoemake's matrix, which is obtained as L(q) * R(q*). 00219 * See "Rotation of Moment of Inertia Tensor" Mathematica notebook by MBK. 00220 */ 00221 QTensor& SetFromOrientationAndPosition( 00222 const Quaternion& q, const Quaternion& translate ) 00223 { 00224 m.xx = 1 - 2 * ( q.y * q.y + q.z * q.z ) ; 00225 m.xy = 2 * ( q.x * q.y - q.w * q.z ) ; 00226 m.xz = 2 * ( q.x * q.z + q.w * q.y ) ; 00227 m.xw = translate.x; 00228 00229 m.yx = 2 * ( q.x * q.y + q.w * q.z ) ; 00230 m.yy = 1 - 2 * ( q.x * q.x + q.z * q.z ) ; 00231 m.yz = 2 * ( q.y * q.z - q.w * q.x ) ; 00232 m.yw = translate.y; 00233 00234 m.zx = 2 * ( q.x * q.z - q.w * q.y ) ; 00235 m.zy = 2 * ( q.y * q.z + q.w * q.x ) ; 00236 m.zz = 1 - 2 * ( q.x * q.x + q.y * q.y ) ; 00237 m.zw = translate.z; 00238 00239 m.wx = m.wy = m.wz = 0; m.ww = 1; 00240 00241 return *this; 00242 } 00243 /*@}*/ 00244 ///////////////////////////////////////////////////////////////////////////////// 00245 /** @name Components getters */ 00246 ///////////////////////////////////////////////////////////////////////////////// 00247 /*@{*/ 00248 /** Gets a quaternion representing one row in the matrix. 00249 */ 00250 Quaternion Row( unsigned i /*!< Row index */ ) const 00251 { 00252 const double* p = data; 00253 return Quaternion( p[i+12], p[i], p[i+4], p[i+8] ); 00254 } 00255 00256 /** Gets a quaternion representing one unit base vector (axis). 00257 * Axis vectors are columns in the matrix. 00258 * Axis with index 3 corresponds to the position of the transform matrix. 00259 */ 00260 Quaternion Column( unsigned j /*!< Column index */ ) const 00261 { 00262 const double* p = data + j * 4; 00263 return Quaternion( p[3], p[0], p[1], p[2] ); 00264 } 00265 00266 /** Gets an OpenGL transform representing a combined translation and rotation. 00267 */ 00268 void GetGLTransform( double d[Length] ) const 00269 { 00270 // We can do simple memcpy since GL also use column-major matrix. 00271 // 00272 for ( int i = 0 ; i < Length; ++i ) { 00273 d[i] = data[i]; 00274 } 00275 00276 // Otherwise, we should setup GL matrix explicitly: 00277 // d[0] = m.xx ; d[4] = m.xy ; d[ 8] = m.xz ; d[12] = m.xw ; 00278 // d[1] = m.yx ; d[5] = m.yy ; d[ 9] = m.yz ; d[13] = m.yw ; 00279 // d[2] = m.zx ; d[6] = m.zy ; d[10] = m.zz ; d[14] = m.zw ; 00280 // d[3] = m.wx ; d[7] = m.wy ; d[11] = m.wz ; d[15] = m.ww ; 00281 } 00282 /*@}*/ 00283 ///////////////////////////////////////////////////////////////////////////////// 00284 /** @name Unary operations */ 00285 ///////////////////////////////////////////////////////////////////////////////// 00286 /*@{*/ 00287 /** Flips signs of all the components of the matrix 00288 */ 00289 QTensor operator - () const 00290 { 00291 QTensor result( Uninitialized ); 00292 00293 for ( int i = 0; i < Length; ++i ) { 00294 result.data[i] = - data[i]; 00295 } 00296 00297 return result; 00298 } 00299 /*@}*/ 00300 ///////////////////////////////////////////////////////////////////////////////// 00301 /** @name Binary operations */ 00302 ///////////////////////////////////////////////////////////////////////////////// 00303 /*@{*/ 00304 /** Does a component-wise addition of this matrix and other matrix. 00305 */ 00306 QTensor operator + ( const QTensor& T ) 00307 { 00308 QTensor result( Uninitialized ); 00309 00310 for ( int i = 0; i < Length; ++i ) { 00311 result.data[i] = data[i] + T.data[i]; 00312 } 00313 return result; 00314 } 00315 00316 /** Does a component-wise addition of this matrix and other matrix. 00317 */ 00318 QTensor& operator += ( const QTensor& T ) 00319 { 00320 for ( int i = 0; i < Length; ++i ) { 00321 data[i] += T.data[i]; 00322 } 00323 return *this; 00324 } 00325 00326 /** Does a component-wise subtraction of this matrix and other matrix. 00327 */ 00328 QTensor operator - ( const QTensor& T ) 00329 { 00330 QTensor result( Uninitialized ); 00331 00332 for ( int i = 0; i < Length; ++i ) { 00333 result.data[i] = data[i] - T.data[i]; 00334 } 00335 return result; 00336 } 00337 00338 /** Does a component-wise subtraction of this matrix and other matrix. 00339 */ 00340 QTensor& operator -= ( const QTensor& T ) 00341 { 00342 for ( int i = 0; i < Length; ++i ) { 00343 data[i] -= T.data[i]; 00344 } 00345 return *this; 00346 } 00347 00348 /** Multiplies this matrix by the given scalar. 00349 */ 00350 QTensor operator * ( double scalar ) const 00351 { 00352 QTensor result( Uninitialized ); 00353 00354 for ( int i = 0; i < Length; ++i ) { 00355 result.data[i] = data[i] * scalar; 00356 } 00357 return result; 00358 } 00359 00360 /** Multiplies this matrix by the given scalar. 00361 */ 00362 QTensor& operator *= ( double scalar ) 00363 { 00364 for ( int i = 0; i < Length; ++i ) { 00365 data[i] *= scalar; 00366 } 00367 return *this; 00368 } 00369 00370 /** Transform the vector part of a quaternion by this matrix. 00371 */ 00372 Quaternion operator * ( const Quaternion& q ) const 00373 { 00374 return Quaternion( 0, 00375 q.x * m.xx + q.y * m.xy + q.z * m.xz + m.xw, 00376 q.x * m.yx + q.y * m.yy + q.z * m.yz + m.yw, 00377 q.x * m.zx + q.y * m.zy + q.z * m.zz + m.zw 00378 ); 00379 } 00380 00381 /** Returns a q-tensor which is this matrix multiplied by 00382 * the given other q-tensor. 00383 */ 00384 QTensor operator * ( const QTensor& T ) const 00385 { 00386 QTensor result( Uninitialized ); 00387 00388 result.m.xx = m.xx * T.m.xx + m.xy * T.m.yx + m.xz * T.m.zx ; 00389 result.m.xy = m.xx * T.m.xy + m.xy * T.m.yy + m.xz * T.m.zy ; 00390 result.m.xz = m.xx * T.m.xz + m.xy * T.m.yz + m.xz * T.m.zz ; 00391 result.m.xw = m.xx * T.m.xw + m.xy * T.m.yw + m.xz * T.m.zw + m.xw ; 00392 00393 result.m.yx = m.yx * T.m.xx + m.yy * T.m.yx + m.yz * T.m.zx ; 00394 result.m.yy = m.yx * T.m.xy + m.yy * T.m.yy + m.yz * T.m.zy ; 00395 result.m.yz = m.yx * T.m.xz + m.yy * T.m.yz + m.yz * T.m.zz ; 00396 result.m.yw = m.yx * T.m.xw + m.yy * T.m.yw + m.yz * T.m.zw + m.yw ; 00397 00398 result.m.zx = m.zx * T.m.xx + m.zy * T.m.yx + m.zz * T.m.zx ; 00399 result.m.zy = m.zx * T.m.xy + m.zy * T.m.yy + m.zz * T.m.zy ; 00400 result.m.zz = m.zx * T.m.xz + m.zy * T.m.yz + m.zz * T.m.zz ; 00401 result.m.zw = m.zx * T.m.xw + m.zy * T.m.yw + m.zz * T.m.zw + m.zw ; 00402 00403 result.m.wx = result.m.wy = result.m.wz = 0; 00404 result.m.ww = 1; 00405 00406 return result; 00407 } 00408 00409 /** Multiplies this matrix in place by the given other matrix. 00410 */ 00411 QTensor& operator *= ( const QTensor& T ) 00412 { 00413 return *this = *this * T; 00414 } 00415 /*@}*/ 00416 ///////////////////////////////////////////////////////////////////////////////// 00417 /** @name Transpose and related methods */ 00418 ///////////////////////////////////////////////////////////////////////////////// 00419 /*@{*/ 00420 /** Sets the matrix to be the transpose of the given matrix. 00421 */ 00422 QTensor& SetTransposeOf( const QTensor& T ) 00423 { 00424 m.xx = T.m.xx ; m.xy = T.m.yx ; m.xz = T.m.zx ; m.xw = T.m.wx ; 00425 m.yx = T.m.xy ; m.yy = T.m.yy ; m.yz = T.m.zy ; m.yw = T.m.wy ; 00426 m.zx = T.m.xz ; m.zy = T.m.yz ; m.zz = T.m.zz ; m.zw = T.m.wz ; 00427 m.wx = T.m.xw ; m.wy = T.m.yw ; m.wz = T.m.zw ; m.ww = T.m.ww ; 00428 00429 return *this; 00430 } 00431 00432 /** Returns a new matrix containing the transpose of this matrix. 00433 */ 00434 QTensor Transpose () const 00435 { 00436 return QTensor( Uninitialized ).SetTransposeOf( *this ); 00437 } 00438 /*@}*/ 00439 ///////////////////////////////////////////////////////////////////////////////// 00440 /** @name Determinant, matrix Inverse and related methods */ 00441 ///////////////////////////////////////////////////////////////////////////////// 00442 /*@{*/ 00443 /** Returns the determinant of the matrix. 00444 */ 00445 double Determinant () const 00446 { 00447 return - m.zx * m.yy * m.xz 00448 + m.yx * m.zy * m.xz 00449 + m.zx * m.xy * m.yz 00450 - m.xx * m.zy * m.yz 00451 - m.yx * m.xy * m.zz 00452 + m.xx * m.yy * m.zz; 00453 } 00454 00455 /** Sets the matrix to be the inverse of the given matrix. 00456 */ 00457 QTensor& SetInverseOf( const QTensor& T ) 00458 { 00459 double det_T = T.Determinant(); 00460 00461 // Make sure the determinant is non-zero. 00462 // 00463 if ( det_T == 0 ) { 00464 SetComponents( 0 ); 00465 return *this; 00466 } 00467 00468 m.xx = - T.m.zy * T.m.yz + T.m.yy * T.m.zz; 00469 m.yx = T.m.zx * T.m.yz - T.m.yx * T.m.zz; 00470 m.zx = - T.m.zx * T.m.yy + T.m.yx * T.m.zy; 00471 00472 m.xy = T.m.zy * T.m.xz - T.m.xy * T.m.zz; 00473 m.yy = - T.m.zx * T.m.xz + T.m.xx * T.m.zz; 00474 m.zy = T.m.zx * T.m.xy - T.m.xx * T.m.zy; 00475 00476 m.xz = - T.m.yy * T.m.xz + T.m.xy * T.m.yz; 00477 m.yz = T.m.yx * T.m.xz - T.m.xx * T.m.yz; 00478 m.zz = - T.m.yx * T.m.xy + T.m.xx * T.m.yy; 00479 00480 m.xw = T.m.zy * T.m.yz * T.m.xw 00481 - T.m.yy * T.m.zz * T.m.xw 00482 - T.m.zy * T.m.xz * T.m.yw 00483 + T.m.xy * T.m.zz * T.m.yw 00484 + T.m.yy * T.m.xz * T.m.zw 00485 - T.m.xy * T.m.yz * T.m.zw; 00486 00487 m.yw = - T.m.zx * T.m.yz * T.m.xw 00488 + T.m.yx * T.m.zz * T.m.xw 00489 + T.m.zx * T.m.xz * T.m.yw 00490 - T.m.xx * T.m.zz * T.m.yw 00491 - T.m.yx * T.m.xz * T.m.zw 00492 + T.m.xx * T.m.yz * T.m.zw; 00493 00494 m.zw = T.m.zx * T.m.yy * T.m.xw 00495 - T.m.yx * T.m.zy * T.m.xw 00496 - T.m.zx * T.m.xy * T.m.yw 00497 + T.m.xx * T.m.zy * T.m.yw 00498 + T.m.yx * T.m.xy * T.m.zw 00499 - T.m.xx * T.m.yy * T.m.zw; 00500 00501 for ( int i = 0; i < Length; ++ i ) { 00502 data[i] /= det_T; 00503 } 00504 return *this; 00505 } 00506 00507 /** Returns a new matrix containing the inverse of this matrix. 00508 */ 00509 QTensor Inverse () const 00510 { 00511 return QTensor( Uninitialized ).SetInverseOf( *this ); 00512 } 00513 /*@}*/ 00514 ///////////////////////////////////////////////////////////////////////////////// 00515 /** @name Spatial transformations related methods */ 00516 ///////////////////////////////////////////////////////////////////////////////// 00517 /*@{*/ 00518 /** Transform the vector part of a quaternion by this matrix. 00519 */ 00520 Quaternion operator () ( const Quaternion& q ) const 00521 { 00522 return Quaternion( 0, 00523 q.x * m.xx + q.y * m.xy + q.z * m.xz + m.xw, 00524 q.x * m.yx + q.y * m.yy + q.z * m.yz + m.yw, 00525 q.x * m.zx + q.y * m.zy + q.z * m.zz + m.zw 00526 ); 00527 } 00528 00529 /** Transform the given quaternion by the inverse of this matrix. 00530 */ 00531 Quaternion TransformInverse( const Quaternion &q ) const 00532 { 00533 Quaternion del = q - Quaternion( 0, m.xw, m.yw, m.zw ); 00534 00535 return Quaternion( 0, 00536 del.x * m.xx + del.y * m.yx + del.z * m.zx, 00537 del.x * m.xy + del.y * m.yy + del.z * m.zy, 00538 del.x * m.xz + del.y * m.yz + del.z * m.zz 00539 ); 00540 } 00541 00542 /** Transforms a tensor from one to another frame of reference. 00543 * 00544 * Equivalent to: `result = (*this) * T * Transpose()` 00545 */ 00546 QTensor operator () ( const QTensor& T ) const 00547 { 00548 QTensor result( Uninitialized ); 00549 00550 double t_xx = m.xx * T.m.xx + m.xy * T.m.yx + m.xz * T.m.zx ; 00551 double t_xy = m.xx * T.m.xy + m.xy * T.m.yy + m.xz * T.m.zy ; 00552 double t_xz = m.xx * T.m.xz + m.xy * T.m.yz + m.xz * T.m.zz ; 00553 00554 double t_yx = m.yx * T.m.xx + m.yy * T.m.yx + m.yz * T.m.zx ; 00555 double t_yy = m.yx * T.m.xy + m.yy * T.m.yy + m.yz * T.m.zy ; 00556 double t_yz = m.yx * T.m.xz + m.yy * T.m.yz + m.yz * T.m.zz ; 00557 00558 double t_zx = m.zx * T.m.xx + m.zy * T.m.yx + m.zz * T.m.zx ; 00559 double t_zy = m.zx * T.m.xy + m.zy * T.m.yy + m.zz * T.m.zy ; 00560 double t_zz = m.zx * T.m.xz + m.zy * T.m.yz + m.zz * T.m.zz ; 00561 00562 result.m.xx = t_xx * m.xx + t_xy * m.xy + t_xz * m.xz ; 00563 result.m.xy = t_xx * m.yx + t_xy * m.yy + t_xz * m.yz ; 00564 result.m.xz = t_xx * m.zx + t_xy * m.zy + t_xz * m.zz ; 00565 result.m.xw = 0 ; 00566 00567 result.m.yx = t_yx * m.xx + t_yy * m.xy + t_yz * m.xz ; 00568 result.m.yy = t_yx * m.yx + t_yy * m.yy + t_yz * m.yz ; 00569 result.m.yz = t_yx * m.zx + t_yy * m.zy + t_yz * m.zz ; 00570 result.m.yw = 0 ; 00571 00572 result.m.zx = t_zx * m.xx + t_zy * m.xy + t_zz * m.xz ; 00573 result.m.zy = t_zx * m.yx + t_zy * m.yy + t_zz * m.yz ; 00574 result.m.zz = t_zx * m.zx + t_zy * m.zy + t_zz * m.zz ; 00575 result.m.zw = 0 ; 00576 00577 result.m.wx = result.m.wy = result.m.ww = 0; 00578 result.m.ww = 1; 00579 00580 return result; 00581 } 00582 00583 /** Transform the given matrix by the inverse of this matrix. 00584 * 00585 * Equivalent to: `result = Transpose() * T * (*this)` 00586 */ 00587 QTensor TransformInverse( const QTensor &T ) const 00588 { 00589 QTensor result( Uninitialized ); 00590 00591 double t_xx = m.xx * T.m.xx + m.yx * T.m.yx + m.zx * T.m.zx ; 00592 double t_xy = m.xx * T.m.xy + m.yx * T.m.yy + m.zx * T.m.zy ; 00593 double t_xz = m.xx * T.m.xz + m.yx * T.m.yz + m.zx * T.m.zz ; 00594 00595 double t_yx = m.xy * T.m.xx + m.yy * T.m.yx + m.zy * T.m.zx ; 00596 double t_yy = m.xy * T.m.xy + m.yy * T.m.yy + m.zy * T.m.zy ; 00597 double t_yz = m.xy * T.m.xz + m.yy * T.m.yz + m.zy * T.m.zz ; 00598 00599 double t_zx = m.xz * T.m.xx + m.yz * T.m.yx + m.zz * T.m.zx ; 00600 double t_zy = m.xz * T.m.xy + m.yz * T.m.yy + m.zz * T.m.zy ; 00601 double t_zz = m.xz * T.m.xz + m.yz * T.m.yz + m.zz * T.m.zz ; 00602 00603 result.m.xx = t_xx * m.xx + t_xy * m.yx + t_xz * m.zx ; 00604 result.m.xy = t_xx * m.xy + t_xy * m.yy + t_xz * m.zy ; 00605 result.m.xz = t_xx * m.xz + t_xy * m.yz + t_xz * m.zz ; 00606 result.m.xw = 0 ; 00607 00608 result.m.yx = t_yx * m.xx + t_yy * m.yx + t_yz * m.zx ; 00609 result.m.yy = t_yx * m.xy + t_yy * m.yy + t_yz * m.zy ; 00610 result.m.yz = t_yx * m.xz + t_yy * m.yz + t_yz * m.zz ; 00611 result.m.yw = 0 ; 00612 00613 result.m.zx = t_zx * m.xx + t_zy * m.yx + t_zz * m.zx ; 00614 result.m.zy = t_zx * m.xy + t_zy * m.yy + t_zz * m.zy ; 00615 result.m.zz = t_zx * m.xz + t_zy * m.yz + t_zz * m.zz ; 00616 result.m.zw = 0 ; 00617 00618 result.m.wx = result.m.wy = result.m.ww = 0; 00619 result.m.ww = 1; 00620 00621 return result; 00622 } 00623 /*@}*/ 00624 ///////////////////////////////////////////////////////////////////////////////// 00625 }; 00626 00627 } // namespace WoRB 00628 00629 #endif // _QTENSOR_H_INCLUDED
1.8.0 
