00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 OSG_BEGIN_NAMESPACE
00040
00068 #if defined(__hpux)
00069 template<class ValueTypeT>
00070 const UInt32 TransformationMatrix<ValueTypeT>::JacobiRank;
00071 #endif
00072
00073 template<class ValueTypeT>
00074 TransformationMatrix<ValueTypeT>
00075 TransformationMatrix<ValueTypeT>::_identityMatrix;
00076
00077
00078
00079
00080 template<class ValueTypeT> inline
00081 const TransformationMatrix<ValueTypeT> &
00082 TransformationMatrix<ValueTypeT>::identity(void)
00083 {
00084 return _identityMatrix;
00085 }
00086
00087
00088
00089
00090 template<class ValueTypeT> inline
00091 TransformationMatrix<ValueTypeT>::TransformationMatrix(void)
00092 {
00093 for(UInt32 i = 0; i < 4; i++)
00094 {
00095 _matrix[i][i] = TypeTraits<ValueType>::getOneElement();
00096 }
00097 }
00098
00099 template<class ValueTypeT> inline
00100 TransformationMatrix<ValueTypeT>::TransformationMatrix(
00101 const TransformationMatrix &source)
00102 {
00103 for(UInt32 i = 0; i < 4; i++)
00104 {
00105 _matrix[i] = source._matrix[i];
00106 }
00107 }
00108
00109 template<class ValueTypeT> inline
00110 TransformationMatrix<ValueTypeT>::TransformationMatrix(
00111 const VectorType3f &vector1,
00112 const VectorType3f &vector2,
00113 const VectorType3f &vector3)
00114 {
00115 _matrix[0].setValue(vector1);
00116 _matrix[1].setValue(vector2);
00117 _matrix[2].setValue(vector3);
00118
00119 _matrix[3][3] = TypeTraits<ValueType>::getOneElement();
00120 }
00121
00122 template <class ValueTypeT> inline
00123 TransformationMatrix<ValueTypeT>::TransformationMatrix(
00124 const VectorType &vector1,
00125 const VectorType &vector2,
00126 const VectorType &vector3,
00127 const VectorType &vector4)
00128 {
00129 _matrix[0].setValue(vector1);
00130 _matrix[1].setValue(vector2);
00131 _matrix[2].setValue(vector3);
00132 _matrix[3].setValue(vector4);
00133 }
00134
00135 template<class ValueTypeT> inline
00136 TransformationMatrix<ValueTypeT>::TransformationMatrix(
00137 const VectorType3f &vector1,
00138 const VectorType3f &vector2,
00139 const VectorType3f &vector3,
00140 const VectorType3f &vector4)
00141 {
00142 _matrix[0].setValue(vector1);
00143 _matrix[1].setValue(vector2);
00144 _matrix[2].setValue(vector3);
00145 _matrix[3].setValue(vector4);
00146 }
00147
00148 template<class ValueTypeT> inline
00149 TransformationMatrix<ValueTypeT>::TransformationMatrix(
00150 const ValueTypeT rVal00,
00151 const ValueTypeT rVal10,
00152 const ValueTypeT rVal20,
00153 const ValueTypeT rVal30,
00154
00155 const ValueTypeT rVal01,
00156 const ValueTypeT rVal11,
00157 const ValueTypeT rVal21,
00158 const ValueTypeT rVal31,
00159
00160 const ValueTypeT rVal02,
00161 const ValueTypeT rVal12,
00162 const ValueTypeT rVal22,
00163 const ValueTypeT rVal32,
00164
00165 const ValueTypeT rVal03,
00166 const ValueTypeT rVal13,
00167 const ValueTypeT rVal23,
00168 const ValueTypeT rVal33)
00169 {
00170 _matrix[0].setValues(rVal00, rVal01, rVal02, rVal03);
00171 _matrix[1].setValues(rVal10, rVal11, rVal12, rVal13);
00172 _matrix[2].setValues(rVal20, rVal21, rVal22, rVal23);
00173 _matrix[3].setValues(rVal30, rVal31, rVal32, rVal33);
00174 }
00175
00176
00177
00178
00179 template<class ValueTypeT> inline
00180 TransformationMatrix<ValueTypeT>::~TransformationMatrix(void)
00181 {
00182 }
00183
00184
00185
00186
00187 template<class ValueTypeT> inline
00188 void TransformationMatrix<ValueTypeT>::setIdentity(void)
00189 {
00190 for(UInt32 i = 0; i < 4; i++)
00191 {
00192 _matrix[i].setNull();
00193 _matrix[i][i] = TypeTraits<ValueType>::getOneElement();
00194 }
00195 }
00196
00197 template<class ValueTypeT> inline
00198 void TransformationMatrix<ValueTypeT>::setValue(
00199 const TransformationMatrix &mat)
00200 {
00201 for(UInt32 i = 0; i < 4; i++)
00202 {
00203 _matrix[i] = mat._matrix[i];
00204 }
00205 }
00206
00207 template<class ValueTypeT>
00208 template<class ValueTypeR>
00209 inline
00210 void TransformationMatrix<ValueTypeT>::convertFrom(
00211 const TransformationMatrix<ValueTypeR>& mat)
00212 {
00213 for(UInt32 i = 0; i < 4; i++)
00214 {
00215 for(UInt32 j =0; j<4; j++)
00216 {
00217 (*this)[i][j] = mat[i][j];
00218 }
00219 }
00220 }
00221
00222
00223 template<class ValueTypeT> inline
00224 void TransformationMatrix<ValueTypeT>::setValue(const VectorType3f &vector1,
00225 const VectorType3f &vector2,
00226 const VectorType3f &vector3)
00227 {
00228 _matrix[0].setValue(vector1);
00229 _matrix[1].setValue(vector2);
00230 _matrix[2].setValue(vector3);
00231 }
00232
00233 template<class ValueTypeT> inline
00234 void TransformationMatrix<ValueTypeT>::setValue(const VectorType3f &vector1,
00235 const VectorType3f &vector2,
00236 const VectorType3f &vector3,
00237 const VectorType3f &vector4)
00238 {
00239 _matrix[0].setValue(vector1);
00240 _matrix[1].setValue(vector2);
00241 _matrix[2].setValue(vector3);
00242 _matrix[3].setValue(vector4);
00243 }
00244
00245 template<class ValueTypeT> inline
00246 void TransformationMatrix<ValueTypeT>::setValue(const ValueTypeT rVal00,
00247 const ValueTypeT rVal10,
00248 const ValueTypeT rVal20,
00249 const ValueTypeT rVal30,
00250
00251 const ValueTypeT rVal01,
00252 const ValueTypeT rVal11,
00253 const ValueTypeT rVal21,
00254 const ValueTypeT rVal31,
00255
00256 const ValueTypeT rVal02,
00257 const ValueTypeT rVal12,
00258 const ValueTypeT rVal22,
00259 const ValueTypeT rVal32,
00260
00261 const ValueTypeT rVal03,
00262 const ValueTypeT rVal13,
00263 const ValueTypeT rVal23,
00264 const ValueTypeT rVal33)
00265 {
00266 _matrix[0].setValues(rVal00, rVal01, rVal02, rVal03);
00267 _matrix[1].setValues(rVal10, rVal11, rVal12, rVal13);
00268 _matrix[2].setValues(rVal20, rVal21, rVal22, rVal23);
00269 _matrix[3].setValues(rVal30, rVal31, rVal32, rVal33);
00270 }
00271
00272 template<class ValueTypeT> inline
00273 void TransformationMatrix<ValueTypeT>::setValueTransposed(
00274 const ValueTypeT rVal00,
00275 const ValueTypeT rVal01,
00276 const ValueTypeT rVal02,
00277 const ValueTypeT rVal03,
00278
00279 const ValueTypeT rVal10,
00280 const ValueTypeT rVal11,
00281 const ValueTypeT rVal12,
00282 const ValueTypeT rVal13,
00283
00284 const ValueTypeT rVal20,
00285 const ValueTypeT rVal21,
00286 const ValueTypeT rVal22,
00287 const ValueTypeT rVal23,
00288
00289 const ValueTypeT rVal30,
00290 const ValueTypeT rVal31,
00291 const ValueTypeT rVal32,
00292 const ValueTypeT rVal33)
00293 {
00294 _matrix[0].setValues(rVal00, rVal01, rVal02, rVal03);
00295 _matrix[1].setValues(rVal10, rVal11, rVal12, rVal13);
00296 _matrix[2].setValues(rVal20, rVal21, rVal22, rVal23);
00297 _matrix[3].setValues(rVal30, rVal31, rVal32, rVal33);
00298 }
00299
00301
00302 template<class ValueTypeT> inline
00303 void TransformationMatrix<ValueTypeT>::setValue(const ValueTypeT *pMat,
00304 bool bTransposed)
00305 {
00306 const ValueTypeT *pTmpMat = pMat;
00307
00308 if(bTransposed == true)
00309 {
00310 for(UInt32 i = 0; i < 4; i++)
00311 {
00312 _matrix[i].setValue(pTmpMat);
00313
00314 pTmpMat += 4;
00315 }
00316 }
00317 else
00318 {
00319 for(UInt32 i = 0; i < 4; i++)
00320 {
00321 for(UInt32 j = 0; j < 4; j++)
00322 {
00323 _matrix[i][j] = pTmpMat[j * 4 + i];
00324 }
00325 }
00326 }
00327 }
00328
00330
00331 template<class ValueTypeT> inline
00332 void TransformationMatrix<ValueTypeT>::setValue(const VectorType *pMat)
00333 {
00334 for(UInt32 i = 0; i < 4; i++)
00335 {
00336 _matrix[i] = pMat[i];
00337 }
00338 }
00339
00341
00342 template<class ValueTypeT> inline
00343 void TransformationMatrix<ValueTypeT>::setValue(const VectorType3f *pMat)
00344 {
00345 for(UInt32 i = 0; i < 4; i++)
00346 {
00347 _matrix[i].setValue(pMat[i]);
00348 }
00349 }
00350
00355 template<class ValueTypeT> inline
00356 void TransformationMatrix<ValueTypeT>::setValue(const Char8 *str,
00357 bool bTransposed)
00358 {
00359 UInt32 i;
00360 UInt32 numOfToken = 16;
00361
00362 Char8 *c = const_cast<char*>(str);
00363
00364 Char8 *tokenC = 0;
00365 Char8 token[256];
00366
00367 ValueTypeT vec[16];
00368
00369 if( (str == NULL) ||
00370 (*str == '\0') )
00371 {
00372 setIdentity();
00373 return;
00374 }
00375
00376 for(i = 0; i < numOfToken; c++)
00377 {
00378 switch (*c)
00379 {
00380 case '\0':
00381 if (tokenC)
00382 {
00383 *tokenC = 0;
00384 vec[i++] = TypeTraits<ValueTypeT>::getFromCString(token);
00385
00386 }
00387
00388 while (i < numOfToken)
00389 {
00390 vec[i++] = TypeTraits<ValueTypeT>::getZeroElement();
00391 }
00392
00393 break;
00394 case ' ':
00395 case '\t':
00396 case '\n':
00397 case ',':
00398 if (tokenC)
00399 {
00400 *tokenC = 0;
00401 vec[i++] = TypeTraits<ValueTypeT>::getFromCString(token);
00402 tokenC = 0;
00403 }
00404 break;
00405 default:
00406 if (!tokenC)
00407 {
00408 tokenC = token;
00409 }
00410 *tokenC++ = *c;
00411 break;
00412 }
00413 }
00414
00415 if(bTransposed == true)
00416 {
00417 setValueTransposed(vec[0], vec[1], vec[2], vec[3],
00418 vec[4], vec[5], vec[6], vec[7],
00419 vec[8], vec[9], vec[10], vec[11],
00420 vec[12], vec[13], vec[14], vec[15]);
00421 }
00422 else
00423 {
00424 setValue(vec[0], vec[1], vec[2], vec[3],
00425 vec[4], vec[5], vec[6], vec[7],
00426 vec[8], vec[9], vec[10], vec[11],
00427 vec[12], vec[13], vec[14], vec[15]);
00428 }
00429 }
00430
00431
00432
00433
00435
00436 template<class ValueTypeT> inline
00437 ValueTypeT *TransformationMatrix<ValueTypeT>::getValues(void)
00438 {
00439 return _matrix[0].getValues();
00440 }
00441
00442 template<class ValueTypeT> inline
00443 const ValueTypeT *TransformationMatrix<ValueTypeT>::getValues(void) const
00444 {
00445 return _matrix[0].getValues();
00446 }
00447
00448
00449
00450
00451 template<class ValueTypeT>
00452 template<class ValueTypeR, class ValueTypeS> inline
00453 ValueTypeT TransformationMatrix<ValueTypeT>::rowMulCol4(
00454 const TransformationMatrix<ValueTypeR> &gRowMat, UInt32 iRow,
00455 const TransformationMatrix<ValueTypeS> &gColMat, UInt32 iColumn) const
00456 {
00457 return
00458 gRowMat[0][iRow] * gColMat[iColumn][0] +
00459 gRowMat[1][iRow] * gColMat[iColumn][1] +
00460 gRowMat[2][iRow] * gColMat[iColumn][2] +
00461 gRowMat[3][iRow] * gColMat[iColumn][3];
00462 }
00463
00464
00465 template<class ValueTypeT> inline
00466 ValueTypeT TransformationMatrix<ValueTypeT>::det2_calc(
00467 const ValueTypeT a1, const ValueTypeT a2,
00468 const ValueTypeT b1, const ValueTypeT b2) const
00469 {
00470 return (a1 * b2) - (a2 * b1);
00471 }
00472
00473 template<class ValueTypeT> inline
00474 ValueTypeT TransformationMatrix<ValueTypeT>::det3_calc(
00475 const ValueTypeT a1,
00476 const ValueTypeT a2,
00477 const ValueTypeT a3,
00478 const ValueTypeT b1,
00479 const ValueTypeT b2,
00480 const ValueTypeT b3,
00481 const ValueTypeT c1,
00482 const ValueTypeT c2,
00483 const ValueTypeT c3) const
00484 {
00485 return
00486 a1 * det2_calc(b2, b3, c2, c3) -
00487 a2 * det2_calc(b1, b3, c1, c3) +
00488 a3 * det2_calc(b1, b2, c1, c2);
00489 }
00490
00496 template <class ValueTypeT>
00497 inline typename TransformationMatrix<ValueTypeT>::ValueType
00498 TransformationMatrix<ValueTypeT>::norm1_3x3(void) const
00499 {
00500 ValueType max;
00501 ValueType t;
00502
00503 max = osgAbs(_matrix[0][0]) +
00504 osgAbs(_matrix[0][1]) +
00505 osgAbs(_matrix[0][2]);
00506
00507 if((t = osgAbs(_matrix[1][0]) +
00508 osgAbs(_matrix[1][1]) +
00509 osgAbs(_matrix[1][2]) ) > max)
00510 {
00511 max = t;
00512 }
00513
00514 if((t = osgAbs(_matrix[2][0]) +
00515 osgAbs(_matrix[2][1]) +
00516 osgAbs(_matrix[2][2]) ) > max)
00517 {
00518 max = t;
00519 }
00520
00521 return max;
00522 }
00523
00529 template <class ValueTypeT>
00530 inline typename TransformationMatrix<ValueTypeT>::ValueType
00531 TransformationMatrix<ValueTypeT>::normInf_3x3(void) const
00532 {
00533 ValueType max;
00534 ValueType t;
00535
00536 max = osgAbs(_matrix[0][0]) +
00537 osgAbs(_matrix[1][0]) +
00538 osgAbs(_matrix[2][0]);
00539
00540 if((t = osgAbs(_matrix[0][1]) +
00541 osgAbs(_matrix[1][1]) +
00542 osgAbs(_matrix[2][1]) ) > max)
00543 {
00544 max = t;
00545 }
00546
00547 if((t = osgAbs(_matrix[0][2]) +
00548 osgAbs(_matrix[1][2]) +
00549 osgAbs(_matrix[2][2]) ) > max)
00550 {
00551 max = t;
00552 }
00553
00554 return max;
00555 }
00556
00563 template <class ValueTypeT>
00564 inline void
00565 TransformationMatrix<ValueTypeT>::adjointT_3x3(
00566 TransformationMatrix<ValueTypeT> &result) const
00567 {
00568 result[0][0] = _matrix[1][1] * _matrix[2][2] - _matrix[2][1] * _matrix[1][2];
00569 result[1][0] = _matrix[2][1] * _matrix[0][2] - _matrix[0][1] * _matrix[2][2];
00570 result[2][0] = _matrix[0][1] * _matrix[1][2] - _matrix[1][1] * _matrix[0][2];
00571
00572 result[0][1] = _matrix[1][2] * _matrix[2][0] - _matrix[2][2] * _matrix[1][0];
00573 result[1][1] = _matrix[2][2] * _matrix[0][0] - _matrix[0][2] * _matrix[2][0];
00574 result[2][1] = _matrix[0][2] * _matrix[1][0] - _matrix[1][2] * _matrix[0][0];
00575
00576 result[0][2] = _matrix[1][0] * _matrix[2][1] - _matrix[2][0] * _matrix[1][1];
00577 result[1][2] = _matrix[2][0] * _matrix[0][1] - _matrix[0][0] * _matrix[2][1];
00578 result[2][2] = _matrix[0][0] * _matrix[1][1] - _matrix[1][0] * _matrix[0][1];
00579 }
00580
00594 template <class ValueTypeT>
00595 inline void
00596 TransformationMatrix<ValueTypeT>::polarDecompose(
00597 TransformationMatrix &Q,
00598 TransformationMatrix &S,
00599 ValueType &det) const
00600 {
00601 ValueType const TOL = ValueType(1.0e-6);
00602
00603 TransformationMatrix const &M = *this;
00604 TransformationMatrix Mk;
00605 TransformationMatrix Ek;
00606 TransformationMatrix MkAdjT;
00607
00608 Mk.transposeFrom(M);
00609
00610 ValueType Mk_one = Mk.norm1_3x3 ();
00611 ValueType Mk_inf = Mk.normInf_3x3();
00612
00613 ValueType MkAdjT_one;
00614 ValueType MkAdjT_inf;
00615
00616 ValueType Ek_one;
00617 ValueType Mk_det;
00618
00619 do
00620 {
00621
00622 Mk.adjointT_3x3(MkAdjT);
00623
00624
00625 Mk_det = Mk[0][0] * MkAdjT[0][0] +
00626 Mk[1][0] * MkAdjT[1][0] +
00627 Mk[2][0] * MkAdjT[2][0];
00628
00629
00630 if(Mk_det == TypeTraits<ValueType>::getZeroElement())
00631 {
00632 FWARNING(("polarDecompose: Mk_det == 0.0\n"));
00633 break;
00634 }
00635
00636 MkAdjT_one = MkAdjT.norm1_3x3 ();
00637 MkAdjT_inf = MkAdjT.normInf_3x3();
00638
00639
00640 ValueType gamma =
00641 osgSqrt(
00642 osgSqrt((MkAdjT_one * MkAdjT_inf) / (Mk_one * Mk_inf)) /
00643 osgAbs(Mk_det));
00644
00645 ValueType g1 = 0.5 * gamma;
00646 ValueType g2 = 0.5 / (gamma * Mk_det);
00647
00648 Ek = Mk;
00649 Mk.scale (g1 );
00650 Mk.addScaled(MkAdjT, g2 );
00651 Ek.addScaled(Mk, -1.0);
00652
00653 Ek_one = Ek.norm1_3x3 ();
00654 Mk_one = Mk.norm1_3x3 ();
00655 Mk_inf = Mk.normInf_3x3();
00656
00657 } while(Ek_one > (Mk_one * TOL));
00658
00659 Q = Mk;
00660 Q.transpose();
00661
00662 S = Mk;
00663 S.mult(M);
00664
00665 for(UInt32 i = 0; i < 3; ++i)
00666 {
00667 for(UInt32 j = i; j < 3; ++j)
00668 {
00669 S[j][i] = S[i][j] = 0.5 * (S[j][i] + S[i][j]);
00670 }
00671 }
00672
00673 det = Mk_det;
00674 }
00675
00687 template <class ValueTypeT>
00688 inline void
00689 TransformationMatrix<ValueTypeT>::spectralDecompose(
00690 TransformationMatrix &SO,
00691 VectorType3f &k ) const
00692 {
00693 UInt32 const next[3] = {1, 2, 0};
00694 UInt32 const maxIterations = 20;
00695
00696 TransformationMatrix const &S = *this;
00697
00698 ValueType diag[3];
00699 ValueType offDiag[3];
00700
00701 diag[0] = S[0][0];
00702 diag[1] = S[1][1];
00703 diag[2] = S[2][2];
00704
00705 offDiag[0] = S[2][1];
00706 offDiag[1] = S[0][2];
00707 offDiag[2] = S[1][0];
00708
00709 for(UInt32 iter = 0; iter < maxIterations; ++iter)
00710 {
00711 ValueType sm = osgAbs(offDiag[0]) + osgAbs(offDiag[1]) + osgAbs(offDiag[2]);
00712
00713 if(sm == TypeTraits<ValueType>::getZeroElement())
00714 {
00715 break;
00716 }
00717
00718 for(Int32 i = 2; i >= 0; --i)
00719 {
00720 UInt32 p = next[i];
00721 UInt32 q = next[p];
00722
00723 ValueType absOffDiag = osgAbs(offDiag[i]);
00724 ValueType g = 100.0 * absOffDiag;
00725
00726 if(absOffDiag > 0.0)
00727 {
00728 ValueType t;
00729 ValueType h = diag[q] - diag[p];
00730 ValueType absh = osgAbs(h);
00731
00732 if(absh + g == absh)
00733 {
00734 t = offDiag[i] / h;
00735 }
00736 else
00737 {
00738 ValueType theta = 0.5 * h / offDiag[i];
00739 t = 1.0 / (osgAbs(theta) + osgSqrt(theta * theta + 1.0));
00740
00741 t = theta < 0.0 ? -t : t;
00742 }
00743
00744 ValueType c = 1.0 / osgSqrt(t * t + 1.0);
00745 ValueType s = t * c;
00746
00747 ValueType tau = s / (c + 1.0);
00748 ValueType ta = t * offDiag[i];
00749
00750 offDiag[i] = 0.0;
00751
00752 diag[p] -= ta;
00753 diag[q] += ta;
00754
00755 ValueType offDiagq = offDiag[q];
00756
00757 offDiag[q] -= s * (offDiag[p] + tau * offDiag[q]);
00758 offDiag[p] += s * (offDiagq - tau * offDiag[p]);
00759
00760 for(Int32 j = 2; j >= 0; --j)
00761 {
00762 ValueType a = SO[p][j];
00763 ValueType b = SO[q][j];
00764
00765 SO[p][j] -= s * (b + tau * a);
00766 SO[q][j] += s * (a - tau * b);
00767 }
00768 }
00769 }
00770 }
00771
00772 k[0] = diag[0];
00773 k[1] = diag[1];
00774 k[2] = diag[2];
00775 }
00776
00788 template <class ValueTypeT>
00789 inline void
00790 TransformationMatrix<ValueTypeT>::decompose(
00791 VectorType3f &t,
00792 ValueType &f,
00793 QuaternionType &r,
00794 QuaternionType &so,
00795 VectorType3f &s ) const
00796 {
00797 TransformationMatrix A = *this;
00798 TransformationMatrix Q;
00799 TransformationMatrix S;
00800 TransformationMatrix SO;
00801 ValueType det;
00802
00803 t[0] = A[3][0];
00804 t[1] = A[3][1];
00805 t[2] = A[3][2];
00806
00807 A[3][0] = 0.0;
00808 A[3][1] = 0.0;
00809 A[3][2] = 0.0;
00810
00811 A[0][3] = 0.0;
00812 A[1][3] = 0.0;
00813 A[2][3] = 0.0;
00814
00815 A.polarDecompose(Q, S, det);
00816
00817 if(det < 0.0)
00818 {
00819 Q.negate();
00820 f = - 1.0;
00821 }
00822 else
00823 {
00824 f = 1.0;
00825 }
00826
00827 r.setValue(Q);
00828
00829 S.spectralDecompose(SO, s);
00830
00831 so.setValue(SO);
00832 }
00833
00834 #ifdef __sgi
00835 #pragma set woff 1424
00836 #endif
00837
00838 template<class ValueTypeT> inline
00839 bool TransformationMatrix<ValueTypeT>::jacobi(
00840 ValueTypeT evalues [JacobiRank],
00841 VectorType3f evectors[JacobiRank],
00842 Int32 &rots)
00843 {
00844 Real64 sm;
00845 Real64 theta;
00846 Real64 c, s, t;
00847 Real64 tau;
00848 Real64 h, g;
00849 Real64 thresh;
00850 Real64 b[JacobiRank];
00851 Real64 z[JacobiRank];
00852 UInt32 p, q, i, j;
00853 Real64 a[JacobiRank][JacobiRank];
00854
00855
00856 for (i = 0; i < JacobiRank; i++)
00857 {
00858 b[i] = evalues[i] = _matrix[i][i];
00859 z[i] = 0.0;
00860
00861 for (j = 0; j < JacobiRank; j++)
00862 {
00863 evectors[i][j] = (i == j) ? 1.0f : 0.0f;
00864 a[i][j] = _matrix[i][j];
00865 }
00866 }
00867
00868 rots = 0;
00869
00870 for(i = 0; i < 50; i++)
00871 {
00872 sm = 0.0;
00873
00874 for(p = 0; p < JacobiRank - 1; p++)
00875 {
00876 for(q = p+1; q < JacobiRank; q++)
00877 {
00878 sm += osgAbs(a[p][q]);
00879 }
00880 }
00881
00882 if (sm == 0.0)
00883 return false;
00884
00885 thresh = (i < 3 ?
00886 (.2 * sm / (JacobiRank * JacobiRank)) :
00887 0.0);
00888
00889 for (p = 0; p < JacobiRank - 1; p++)
00890 {
00891 for (q = p + 1; q < JacobiRank; q++)
00892 {
00893 g = 100.0 * osgAbs(a[p][q]);
00894
00895 if (i > 3 &&
00896 (osgAbs(evalues[p]) + g == osgAbs(evalues[p])) &&
00897 (osgAbs(evalues[q]) + g == osgAbs(evalues[q])))
00898 {
00899 a[p][q] = 0.0;
00900 }
00901 else if (osgAbs(a[p][q]) > thresh)
00902 {
00903 h = evalues[q] - evalues[p];
00904
00905 if (osgAbs(h) + g == osgAbs(h))
00906 {
00907 t = a[p][q] / h;
00908 }
00909 else
00910 {
00911 theta = .5 * h / a[p][q];
00912 t = 1.0 / (osgAbs(theta) + osgSqrt(1 + theta * theta));
00913 if (theta < 0.0) t = -t;
00914 }
00915
00916
00917 c = 1.0 / osgSqrt(1.0 + t * t);
00918 s = t * c;
00919
00920 tau = s / (1.0 + c);
00921 h = t * a[p][q];
00922
00923 z[p] -= h;
00924 z[q] += h;
00925
00926 evalues[p] -= ValueTypeT(h);
00927 evalues[q] += ValueTypeT(h);
00928
00929 a[p][q] = 0.0;
00930
00931 for (j = 0; j < p; j++)
00932 {
00933 g = a[j][p];
00934 h = a[j][q];
00935
00936 a[j][p] = g - s * (h + g * tau);
00937 a[j][q] = h + s * (g - h * tau);
00938 }
00939
00940 for (j = p+1; j < q; j++)
00941 {
00942 g = a[p][j];
00943 h = a[j][q];
00944
00945 a[p][j] = g - s * (h + g * tau);
00946 a[j][q] = h + s * (g - h * tau);
00947 }
00948
00949 for (j = q+1; j < JacobiRank; j++)
00950 {
00951 g = a[p][j];
00952 h = a[q][j];
00953
00954 a[p][j] = g - s * (h + g * tau);
00955 a[q][j] = h + s * (g - h * tau);
00956 }
00957
00958 for (j = 0; j < JacobiRank; j++)
00959 {
00960 g = evectors[j][p];
00961 h = evectors[j][q];
00962
00963 evectors[j][p] = ValueTypeT(g - s * (h + g * tau));
00964 evectors[j][q] = ValueTypeT(h + s * (g - h * tau));
00965 }
00966 }
00967 rots++;
00968 }
00969 }
00970 for (p = 0; p < JacobiRank; p++)
00971 {
00972 evalues[p] = ValueTypeT(b[p] += z[p]);
00973
00974 z[p] = 0;
00975 }
00976 }
00977
00978 return true;
00979 }
00980
00981 #ifdef __sgi
00982 #pragma reset woff 1424
00983 #endif
00984
00985
00986
00987
00989
00990 template<class ValueTypeT> inline
00991 void TransformationMatrix<ValueTypeT>::setScale(const ValueTypeT s)
00992 {
00993 _matrix[0][0] = s;
00994 _matrix[1][1] = s;
00995 _matrix[2][2] = s;
00996 }
00997
00999
01000 template<class ValueTypeT> inline
01001 void TransformationMatrix<ValueTypeT>::setScale(const ValueTypeT sx,
01002 const ValueTypeT sy,
01003 const ValueTypeT sz)
01004 {
01005 _matrix[0][0] = sx;
01006 _matrix[1][1] = sy;
01007 _matrix[2][2] = sz;
01008 }
01009
01011
01012 template<class ValueTypeT> inline
01013 void TransformationMatrix<ValueTypeT>::setScale(const VectorType3f &s)
01014 {
01015 _matrix[0][0] = s[0];
01016 _matrix[1][1] = s[1];
01017 _matrix[2][2] = s[2];
01018 }
01019
01021
01022 template<class ValueTypeT> inline
01023 void TransformationMatrix<ValueTypeT>::setTranslate(const ValueTypeT tx,
01024 const ValueTypeT ty,
01025 const ValueTypeT tz)
01026 {
01027 _matrix[3][0] = tx;
01028 _matrix[3][1] = ty;
01029 _matrix[3][2] = tz;
01030 }
01031
01033
01034 template<class ValueTypeT> inline
01035 void TransformationMatrix<ValueTypeT>::setTranslate(const VectorType3f &t)
01036 {
01037 _matrix[3].setValue(t);
01038 }
01039
01041
01042 template<class ValueTypeT> inline
01043 void TransformationMatrix<ValueTypeT>::setTranslate(const PointType3f &t)
01044 {
01045 _matrix[3].setValue(t);
01046 }
01047
01049
01050 template<class ValueTypeT> inline
01051 void TransformationMatrix<ValueTypeT>::setRotate(const QuaternionType &q)
01052 {
01053 q.getValuesOnly(*this);
01054 }
01055
01057
01058 template<class ValueTypeT> inline
01059 void TransformationMatrix<ValueTypeT>::setTransform(const VectorType3f &t)
01060 {
01061 setIdentity();
01062
01063 _matrix[3][0] = t[0];
01064 _matrix[3][1] = t[1];
01065 _matrix[3][2] = t[2];
01066
01067 }
01068
01070
01071 template<class ValueTypeT> inline
01072 void TransformationMatrix<ValueTypeT>::setTransform(const QuaternionType &r)
01073 {
01074
01075 r.getValue(*this);
01076 }
01077
01079
01080 template<class ValueTypeT> inline
01081 void TransformationMatrix<ValueTypeT>::setTransform(const VectorType3f &t,
01082 const QuaternionType &r)
01083 {
01084 r.getValuesOnly(*this);
01085
01086
01087 _matrix[0][3] = 0.0;
01088 _matrix[1][3] = 0.0;
01089 _matrix[2][3] = 0.0;
01090
01091 _matrix[3][0] = t[0];
01092 _matrix[3][1] = t[1];
01093 _matrix[3][2] = t[2];
01094 _matrix[3][3] = 1.0;
01095 }
01096
01098
01099 template<class ValueTypeT> inline
01100 void TransformationMatrix<ValueTypeT>::setTransform(const VectorType3f &t,
01101 const QuaternionType &r,
01102 const VectorType3f &s)
01103 {
01104 typedef TypeTraits<ValueTypeT> ValueTraits;
01105
01106
01107 r.getValuesOnly(*this);
01108
01109
01110 _matrix[0][0] *= s[0]; _matrix[0][1] *= s[0]; _matrix[0][2] *=s[0];
01111 _matrix[1][0] *= s[1]; _matrix[1][1] *= s[1]; _matrix[1][2] *=s[1];
01112 _matrix[2][0] *= s[2]; _matrix[2][1] *= s[2]; _matrix[2][2] *=s[2];
01113
01114 _matrix[0][3] = ValueTraits::getZeroElement();
01115 _matrix[1][3] = ValueTraits::getZeroElement();
01116 _matrix[2][3] = ValueTraits::getZeroElement();
01117
01118 _matrix[3][0] = t[0];
01119 _matrix[3][1] = t[1];
01120 _matrix[3][2] = t[2];
01121 _matrix[3][3] = ValueTraits::getOneElement();
01122 }
01123
01125
01126 template<class ValueTypeT> inline
01127 void TransformationMatrix<ValueTypeT>::setTransform(const VectorType3f &t,
01128 const QuaternionType &r,
01129 const VectorType3f &s,
01130 const QuaternionType &so)
01131 {
01132 TransformationMatrix<ValueTypeT> tmpMat1;
01133 TransformationMatrix<ValueTypeT> tmpMat2;
01134
01135
01136 QuaternionType rg(r);
01137 rg.mult(so);
01138
01139
01140 QuaternionType soi(so);
01141 soi.invert();
01142
01143
01144 rg. getValue(tmpMat1);
01145 soi.getValue(tmpMat2);
01146
01147
01148 tmpMat1[0][0] *= s[0]; tmpMat1[0][1] *= s[0]; tmpMat1[0][2] *=s[0];
01149 tmpMat1[1][0] *= s[1]; tmpMat1[1][1] *= s[1]; tmpMat1[1][2] *=s[1];
01150 tmpMat1[2][0] *= s[2]; tmpMat1[2][1] *= s[2]; tmpMat1[2][2] *=s[2];
01151
01152 _matrix[0][0] =
01153 tmpMat2[0][0] * tmpMat1[0][0] +
01154 tmpMat2[0][1] * tmpMat1[1][0] +
01155 tmpMat2[0][2] * tmpMat1[2][0];
01156
01157 _matrix[0][1] =
01158 tmpMat2[0][0] * tmpMat1[0][1] +
01159 tmpMat2[0][1] * tmpMat1[1][1] +
01160 tmpMat2[0][2] * tmpMat1[2][1];
01161
01162 _matrix[0][2] =
01163 tmpMat2[0][0] * tmpMat1[0][2] +
01164 tmpMat2[0][1] * tmpMat1[1][2] +
01165 tmpMat2[0][2] * tmpMat1[2][2];
01166
01167 _matrix[0][3] = 0.0;
01168
01169
01170 _matrix[1][0] =
01171 tmpMat2[1][0] * tmpMat1[0][0] +
01172 tmpMat2[1][1] * tmpMat1[1][0] +
01173 tmpMat2[1][2] * tmpMat1[2][0];
01174
01175 _matrix[1][1] =
01176 tmpMat2[1][0] * tmpMat1[0][1] +
01177 tmpMat2[1][1] * tmpMat1[1][1] +
01178 tmpMat2[1][2] * tmpMat1[2][1];
01179
01180 _matrix[1][2] =
01181 tmpMat2[1][0] * tmpMat1[0][2] +
01182 tmpMat2[1][1] * tmpMat1[1][2] +
01183 tmpMat2[1][2] * tmpMat1[2][2];
01184
01185 _matrix[1][3] = 0.0;
01186
01187
01188 _matrix[2][0] =
01189 tmpMat2[2][0] * tmpMat1[0][0] +
01190 tmpMat2[2][1] * tmpMat1[1][0] +
01191 tmpMat2[2][2] * tmpMat1[2][0];
01192
01193 _matrix[2][1] =
01194 tmpMat2[2][0] * tmpMat1[0][1] +
01195 tmpMat2[2][1] * tmpMat1[1][1] +
01196 tmpMat2[2][2] * tmpMat1[2][1];
01197
01198 _matrix[2][2] =
01199 tmpMat2[2][0] * tmpMat1[0][2] +
01200 tmpMat2[2][1] * tmpMat1[1][2] +
01201 tmpMat2[2][2] * tmpMat1[2][2];
01202
01203 _matrix[2][3] = 0.0;
01204
01205 _matrix[3][0] = t[0];
01206 _matrix[3][1] = t[1];
01207 _matrix[3][2] = t[2];
01208 _matrix[3][3] = 1.0;
01209 }
01210
01217 template<class ValueTypeT> inline
01218 void TransformationMatrix<ValueTypeT>::setTransform(
01219 const VectorType3f &translation,
01220 const QuaternionType &rotation,
01221 const VectorType3f &scaleFactor,
01222 const QuaternionType &scaleOrientation,
01223 const VectorType3f ¢er)
01224 {
01225 typedef TypeTraits<ValueTypeT> ValueTraits;
01226
01227 TransformationMatrix<ValueTypeT> tmpMat1;
01228 TransformationMatrix<ValueTypeT> tmpMat2;
01229
01230
01231 VectorType3f tg(translation);
01232 tg += center;
01233
01234
01235 QuaternionType rg(rotation);
01236 rg *= scaleOrientation;
01237
01238
01239 QuaternionType soi(scaleOrientation);
01240 soi.invert();
01241
01242
01243 rg. getValue(tmpMat1);
01244 soi.getValue(tmpMat2);
01245
01246
01247
01248 tmpMat1[0][0] *= scaleFactor[0];
01249 tmpMat1[0][1] *= scaleFactor[0];
01250 tmpMat1[0][2] *= scaleFactor[0];
01251
01252 tmpMat1[1][0] *= scaleFactor[1];
01253 tmpMat1[1][1] *= scaleFactor[1];
01254 tmpMat1[1][2] *= scaleFactor[1];
01255
01256 tmpMat1[2][0] *= scaleFactor[2];
01257 tmpMat1[2][1] *= scaleFactor[2];
01258 tmpMat1[2][2] *= scaleFactor[2];
01259
01260 _matrix[0][0] =
01261 tmpMat2[0][0] * tmpMat1[0][0] +
01262 tmpMat2[0][1] * tmpMat1[1][0] +
01263 tmpMat2[0][2] * tmpMat1[2][0];
01264
01265 _matrix[0][1] =
01266 tmpMat2[0][0] * tmpMat1[0][1] +
01267 tmpMat2[0][1] * tmpMat1[1][1] +
01268 tmpMat2[0][2] * tmpMat1[2][1];
01269
01270 _matrix[0][2] =
01271 tmpMat2[0][0] * tmpMat1[0][2] +
01272 tmpMat2[0][1] * tmpMat1[1][2] +
01273 tmpMat2[0][2] * tmpMat1[2][2];
01274
01275 _matrix[0][3] = ValueTraits::getZeroElement();
01276
01277
01278 _matrix[1][0] =
01279 tmpMat2[1][0] * tmpMat1[0][0] +
01280 tmpMat2[1][1] * tmpMat1[1][0] +
01281 tmpMat2[1][2] * tmpMat1[2][0];
01282
01283 _matrix[1][1] =
01284 tmpMat2[1][0] * tmpMat1[0][1] +
01285 tmpMat2[1][1] * tmpMat1[1][1] +
01286 tmpMat2[1][2] * tmpMat1[2][1];
01287
01288 _matrix[1][2] =
01289 tmpMat2[1][0] * tmpMat1[0][2] +
01290 tmpMat2[1][1] * tmpMat1[1][2] +
01291 tmpMat2[1][2] * tmpMat1[2][2];
01292
01293 _matrix[1][3] = ValueTraits::getZeroElement();
01294
01295
01296 _matrix[2][0] =
01297 tmpMat2[2][0] * tmpMat1[0][0] +
01298 tmpMat2[2][1] * tmpMat1[1][0] +
01299 tmpMat2[2][2] * tmpMat1[2][0];
01300
01301 _matrix[2][1] =
01302 tmpMat2[2][0] * tmpMat1[0][1] +
01303 tmpMat2[2][1] * tmpMat1[1][1] +
01304 tmpMat2[2][2] * tmpMat1[2][1];
01305
01306 _matrix[2][2] =
01307 tmpMat2[2][0] * tmpMat1[0][2] +
01308 tmpMat2[2][1] * tmpMat1[1][2] +
01309 tmpMat2[2][2] * tmpMat1[2][2];
01310
01311 _matrix[2][3] = ValueTraits::getZeroElement();
01312
01313
01314 _matrix[3][0] =
01315 _matrix[0][0] * -center[0] +
01316 _matrix[1][0] * -center[1] +
01317 _matrix[2][0] * -center[2] + tg[0];
01318
01319 _matrix[3][1] =
01320 _matrix[0][1] * -center[0] +
01321 _matrix[1][1] * -center[1] +
01322 _matrix[2][1] * -center[2] + tg[1];
01323
01324 _matrix[3][2] =
01325 _matrix[0][2] * -center[0] +
01326 _matrix[1][2] * -center[1] +
01327 _matrix[2][2] * -center[2] + tg[2];
01328
01329 _matrix[3][3] = ValueTraits::getOneElement();
01330
01331 }
01332
01333
01334
01335
01345 template<class ValueTypeT> inline
01346 void TransformationMatrix<ValueTypeT>::getTransform(
01347 VectorType3f &translation,
01348 QuaternionType &rotation,
01349 VectorType3f &scaleFactor,
01350 QuaternionType &scaleOrientation,
01351 const VectorType3f ¢er ) const
01352 {
01353 TransformationMatrix m;
01354 TransformationMatrix c;
01355
01356 m.setTranslate(-center);
01357
01358 m.mult(*this);
01359
01360 c.setTranslate(center);
01361
01362 m.mult(c);
01363
01364 m.getTransform(translation, rotation, scaleFactor, scaleOrientation);
01365 }
01366
01368
01369 template<class ValueTypeT> inline
01370 void TransformationMatrix<ValueTypeT>::getTransform(
01371 VectorType3f &translation,
01372 QuaternionType &rotation,
01373 VectorType3f &scaleFactor,
01374 QuaternionType &scaleOrientation) const
01375 {
01376 ValueType flip;
01377 TransformationMatrix so;
01378 TransformationMatrix rot;
01379 TransformationMatrix proj;
01380
01381 this->decompose(translation, flip, rotation, scaleOrientation, scaleFactor);
01382
01383 scaleFactor *= flip;
01384 }
01385
01394 template<class ValueTypeT> inline
01395 bool TransformationMatrix<ValueTypeT>::factor(TransformationMatrix &r,
01396 VectorType3f &s,
01397 TransformationMatrix &u,
01398 VectorType3f &t,
01399 TransformationMatrix &proj) const
01400 {
01401 Real64 det;
01402 Real64 det_sign;
01403 Real64 scratch;
01404 Int32 i;
01405 Int32 j;
01406 Int32 junk;
01407 TransformationMatrix a;
01408 TransformationMatrix aT;
01409 TransformationMatrix rT;
01410 TransformationMatrix b;
01411 TransformationMatrix si;
01412 ValueTypeT evalues [3];
01413 VectorType3f evectors[3];
01414
01415 a = *this;
01416
01417 proj.setIdentity();
01418
01419 scratch = 1.0;
01420
01421 for (i = 0; i < 3; i++)
01422 {
01423 for (j = 0; j < 3; j++)
01424 {
01425 a._matrix[i][j] *= ValueTypeT(scratch);
01426 }
01427
01428 t[i] = _matrix[3][i] * ValueTypeT(scratch);
01429
01430 a._matrix[3][i] = a._matrix[i][3] = 0.0;
01431 }
01432
01433 a._matrix[3][3] = 1.0;
01434
01435
01436
01437 det = a.det3();
01438
01439 det_sign = (det < 0.0 ? -1.0 : 1.0);
01440
01441 if(det_sign * det < 1e-12)
01442 return false;
01443
01444
01445
01446 aT.transposeFrom(a);
01447 b = a;
01448 b.mult(aT);
01449
01450 b.jacobi(evalues, evectors, junk);
01451
01452 r.setValue(evectors[0][0], evectors[0][1], evectors[0][2], 0.0,
01453 evectors[1][0], evectors[1][1], evectors[1][2], 0.0,
01454 evectors[2][0], evectors[2][1], evectors[2][2], 0.0,
01455 0.0, 0.0, 0.0, 1.0);
01456
01457
01458
01459 si.setIdentity();
01460
01461 for(i = 0; i < 3; i++)
01462 {
01463 s[i] = ValueTypeT(det_sign * osgSqrt(evalues[i]));
01464
01465 si._matrix[i][i] = 1.0f / s[i];
01466 }
01467
01468
01469
01470 rT.transposeFrom(r);
01471 u = r;
01472 u.mult(si);
01473 u.mult(rT);
01474 u.mult(a);
01475
01476 return true;
01477 }
01478
01479
01480
01487 template <class ValueTypeT>
01488 inline void
01489 TransformationMatrix<ValueTypeT>::mult(
01490 const PointType &pntIn, PointType &pntOut) const
01491 {
01492 pntOut.setValues(
01493 (_matrix[0][0] * pntIn[0] +
01494 _matrix[1][0] * pntIn[1] +
01495 _matrix[2][0] * pntIn[2] +
01496 _matrix[3][0] * pntIn[3] ),
01497 (_matrix[0][1] * pntIn[0] +
01498 _matrix[1][1] * pntIn[1] +
01499 _matrix[2][1] * pntIn[2] +
01500 _matrix[3][1] * pntIn[3] ),
01501 (_matrix[0][2] * pntIn[0] +
01502 _matrix[1][2] * pntIn[1] +
01503 _matrix[2][2] * pntIn[2] +
01504 _matrix[3][2] * pntIn[3] ),
01505 (_matrix[0][3] * pntIn[0] +
01506 _matrix[1][3] * pntIn[1] +
01507 _matrix[2][3] * pntIn[2] +
01508 _matrix[3][3] * pntIn[3] ) );
01509 }
01510
01518 template <class ValueTypeT>
01519 inline void
01520 TransformationMatrix<ValueTypeT>::multFull(
01521 const PointType3f &pntIn, PointType3f &pntOut) const
01522 {
01523 ValueType w = _matrix[0][3] * pntIn[0] +
01524 _matrix[1][3] * pntIn[1] +
01525 _matrix[2][3] * pntIn[2] +
01526 _matrix[3][3];
01527
01528 if(w == TypeTraits<ValueType>::getZeroElement())
01529 {
01530 FWARNING(("TransformationMatrix<>::multFull(Pnt3, Pnt3): w == 0.0\n"));
01531
01532 pntOut.setValues(
01533 (_matrix[0][0] * pntIn[0] +
01534 _matrix[1][0] * pntIn[1] +
01535 _matrix[2][0] * pntIn[2] +
01536 _matrix[3][0] ),
01537 (_matrix[0][1] * pntIn[0] +
01538 _matrix[1][1] * pntIn[1] +
01539 _matrix[2][1] * pntIn[2] +
01540 _matrix[3][1] ),
01541 (_matrix[0][2] * pntIn[0] +
01542 _matrix[1][2] * pntIn[1] +
01543 _matrix[2][2] * pntIn[2] +
01544 _matrix[3][2] ) );
01545 }
01546 else
01547 {
01548 w = TypeTraits<ValueType>::getOneElement() / w;
01549
01550 pntOut.setValues(
01551 (_matrix[0][0] * pntIn[0] +
01552 _matrix[1][0] * pntIn[1] +
01553 _matrix[2][0] * pntIn[2] +
01554 _matrix[3][0] ) * w,
01555 (_matrix[0][1] * pntIn[0] +
01556 _matrix[1][1] * pntIn[1] +
01557 _matrix[2][1] * pntIn[2] +
01558 _matrix[3][1] ) * w,
01559 (_matrix[0][2] * pntIn[0] +
01560 _matrix[1][2] * pntIn[1] +
01561 _matrix[2][2] * pntIn[2] +
01562 _matrix[3][2] ) * w );
01563 }
01564 }
01565
01572 template <class ValueTypeT>
01573 inline void
01574 TransformationMatrix<ValueTypeT>::mult(
01575 const PointType3f &pntIn, PointType3f &pntOut) const
01576 {
01577 pntOut.setValues(
01578 (_matrix[0][0] * pntIn[0] +
01579 _matrix[1][0] * pntIn[1] +
01580 _matrix[2][0] * pntIn[2] +
01581 _matrix[3][0] ),
01582 (_matrix[0][1] * pntIn[0] +
01583 _matrix[1][1] * pntIn[1] +
01584 _matrix[2][1] * pntIn[2] +
01585 _matrix[3][1] ),
01586 (_matrix[0][2] * pntIn[0] +
01587 _matrix[1][2] * pntIn[1] +
01588 _matrix[2][2] * pntIn[2] +
01589 _matrix[3][2] ) );
01590
01591 }
01592
01599 template <class ValueTypeT>
01600 inline void
01601 TransformationMatrix<ValueTypeT>::mult(
01602 const VectorType &vecIn, VectorType &vecOut) const
01603 {
01604 vecOut.setValues(
01605 (_matrix[0][0] * vecIn[0] +
01606 _matrix[1][0] * vecIn[1] +
01607 _matrix[2][0] * vecIn[2] +
01608 _matrix[3][0] * vecIn[3] ),
01609 (_matrix[0][1] * vecIn[0] +
01610 _matrix[1][1] * vecIn[1] +
01611 _matrix[2][1] * vecIn[2] +
01612 _matrix[3][1] * vecIn[3] ),
01613 (_matrix[0][2] * vecIn[0] +
01614 _matrix[1][2] * vecIn[1] +
01615 _matrix[2][2] * vecIn[2] +
01616 _matrix[3][2] * vecIn[3] ),
01617 (_matrix[0][3] * vecIn[0] +
01618 _matrix[1][3] * vecIn[1] +
01619 _matrix[2][3] * vecIn[2] +
01620 _matrix[3][3] * vecIn[3] ) );
01621 }
01622
01630 template <class ValueTypeT>
01631 inline void
01632 TransformationMatrix<ValueTypeT>::multFull(
01633 const VectorType3f &vecIn, VectorType3f &vecOut) const
01634 {
01635 ValueType w = _matrix[0][3] * vecIn[0] +
01636 _matrix[1][3] * vecIn[1] +
01637 _matrix[2][3] * vecIn[2];
01638
01639 if(w == TypeTraits<ValueType>::getZeroElement())
01640 {
01641 FWARNING(("TransformationMatrix<>::multFull(Vec3, Vec3): w == 0.0\n"));
01642
01643 vecOut.setValues(
01644 (_matrix[0][0] * vecIn[0] +
01645 _matrix[1][0] * vecIn[1] +
01646 _matrix[2][0] * vecIn[2] ),
01647 (_matrix[0][1] * vecIn[0] +
01648 _matrix[1][1] * vecIn[1] +
01649 _matrix[2][1] * vecIn[2] ),
01650 (_matrix[0][2] * vecIn[0] +
01651 _matrix[1][2] * vecIn[1] +
01652 _matrix[2][2] * vecIn[2] ) );
01653 }
01654 else
01655 {
01656 w = TypeTraits<ValueType>::getOneElement() / w;
01657
01658 vecOut.setValues(
01659 (_matrix[0][0] * vecIn[0] +
01660 _matrix[1][0] * vecIn[1] +
01661 _matrix[2][0] * vecIn[2] ) * w,
01662 (_matrix[0][1] * vecIn[0] +
01663 _matrix[1][1] * vecIn[1] +
01664 _matrix[2][1] * vecIn[2] ) * w,
01665 (_matrix[0][2] * vecIn[0] +
01666 _matrix[1][2] * vecIn[1] +
01667 _matrix[2][2] * vecIn[2] ) * w );
01668 }
01669 }
01670
01678 template <class ValueTypeT>
01679 inline void
01680 TransformationMatrix<ValueTypeT>::mult(
01681 const VectorType3f &vecIn, VectorType3f &vecOut) const
01682 {
01683 vecOut.setValues(
01684 (_matrix[0][0] * vecIn[0] +
01685 _matrix[1][0] * vecIn[1] +
01686 _matrix[2][0] * vecIn[2] ),
01687 (_matrix[0][1] * vecIn[0] +
01688 _matrix[1][1] * vecIn[1] +
01689 _matrix[2][1] * vecIn[2] ),
01690 (_matrix[0][2] * vecIn[0] +
01691 _matrix[1][2] * vecIn[1] +
01692 _matrix[2][2] * vecIn[2] ) );
01693 }
01694
01700 template <class ValueTypeT>
01701 inline void
01702 TransformationMatrix<ValueTypeT>::mult3x3(
01703 const PointType3f &pntIn, PointType3f &pntOut) const
01704 {
01705 pntOut.setValues(
01706 (_matrix[0][0] * pntIn[0] +
01707 _matrix[1][0] * pntIn[1] +
01708 _matrix[2][0] * pntIn[2] ),
01709 (_matrix[0][1] * pntIn[0] +
01710 _matrix[1][1] * pntIn[1] +
01711 _matrix[2][1] * pntIn[2] ),
01712 (_matrix[0][2] * pntIn[0] +
01713 _matrix[1][2] * pntIn[1] +
01714 _matrix[2][2] * pntIn[2] ) );
01715 }
01716
01722 template <class ValueTypeT>
01723 inline void
01724 TransformationMatrix<ValueTypeT>::mult3x3(
01725 const VectorType3f &vecIn, VectorType3f &vecOut) const
01726 {
01727 vecOut.setValues(
01728 (_matrix[0][0] * vecIn[0] +
01729 _matrix[1][0] * vecIn[1] +
01730 _matrix[2][0] * vecIn[2] ),
01731 (_matrix[0][1] * vecIn[0] +
01732 _matrix[1][1] * vecIn[1] +
01733 _matrix[2][1] * vecIn[2] ),
01734 (_matrix[0][2] * vecIn[0] +
01735 _matrix[1][2] * vecIn[1] +
01736 _matrix[2][2] * vecIn[2] ) );
01737 }
01738
01743 template<class ValueTypeT> inline
01744 void TransformationMatrix<ValueTypeT>::multLeft(
01745 const PointType3f &src,
01746 PointType3f &dst) const
01747 {
01748 dst.setValues((src[0] * _matrix[0][0] +
01749 src[1] * _matrix[0][1] +
01750 src[2] * _matrix[0][2] +
01751 _matrix[0][3]),
01752 (src[0] * _matrix[1][0] +
01753 src[1] * _matrix[1][1] +
01754 src[2] * _matrix[1][2] +
01755 _matrix[1][3]),
01756 (src[0] * _matrix[2][0] +
01757 src[1] * _matrix[2][1] +
01758 src[2] * _matrix[2][2] +
01759 _matrix[2][3]));
01760 }
01761
01766 template<class ValueTypeT> inline
01767 void TransformationMatrix<ValueTypeT>::multLeftFull(
01768 const PointType3f &src,
01769 PointType3f &dst) const
01770 {
01771 ValueTypeT w = src[0] * _matrix[3][0] +
01772 src[1] * _matrix[3][1] +
01773 src[2] * _matrix[3][2] +
01774 _matrix[3][3];
01775
01776 if(w == TypeTraits<ValueTypeT>::getZeroElement())
01777 {
01778 FWARNING(("TransformationMatrix<>::multLeftFull(Pnt3, Pnt3): "
01779 "w == 0.0!\n"));
01780
01781 dst.setValues(0, 0, 0);
01782
01783 return;
01784 }
01785
01786 w = 1./w;
01787
01788 dst.setValues((src[0] * _matrix[0][0] +
01789 src[1] * _matrix[0][1] +
01790 src[2] * _matrix[0][2] +
01791 _matrix[0][3]) * w,
01792 (src[0] * _matrix[1][0] +
01793 src[1] * _matrix[1][1] +
01794 src[2] * _matrix[1][2] +
01795 _matrix[1][3]) * w,
01796 (src[0] * _matrix[2][0] +
01797 src[1] * _matrix[2][1] +
01798 src[2] * _matrix[2][2] +
01799 _matrix[2][3]) * w);
01800 }
01801
01809 template <class ValueTypeT>
01810 inline void
01811 TransformationMatrix<ValueTypeT>::multLeftFull(
01812 const VectorType3f &vecIn, VectorType3f &vecOut) const
01813 {
01814 ValueType w = _matrix[3][0] * vecIn[0] +
01815 _matrix[3][1] * vecIn[1] +
01816 _matrix[3][2] * vecIn[2];
01817
01818 if(w == TypeTraits<ValueType>::getZeroElement())
01819 {
01820 FWARNING(("TransformationMatrix<>::multLeftFull(Vec3, Vec3): "
01821 "w == 0.0\n"));
01822
01823 vecOut.setValues(
01824 (_matrix[0][0] * vecIn[0] +
01825 _matrix[0][1] * vecIn[1] +
01826 _matrix[0][2] * vecIn[2] ),
01827 (_matrix[1][0] * vecIn[0] +
01828 _matrix[1][1] * vecIn[1] +
01829 _matrix[1][2] * vecIn[2] ),
01830 (_matrix[2][0] * vecIn[0] +
01831 _matrix[2][1] * vecIn[1] +
01832 _matrix[2][2] * vecIn[2] ) );
01833 }
01834 else
01835 {
01836 w = TypeTraits<ValueType>::getOneElement() / w;
01837
01838 vecOut.setValues(
01839 (_matrix[0][0] * vecIn[0] +
01840 _matrix[0][1] * vecIn[1] +
01841 _matrix[0][2] * vecIn[2] ) * w,
01842 (_matrix[1][0] * vecIn[0] +
01843 _matrix[1][1] * vecIn[1] +
01844 _matrix[1][2] * vecIn[2] ) * w,
01845 (_matrix[2][0] * vecIn[0] +
01846 _matrix[2][1] * vecIn[1] +
01847 _matrix[2][2] * vecIn[2] ) * w );
01848 }
01849 }
01850
01855 template<class ValueTypeT> inline
01856 void TransformationMatrix<ValueTypeT>::multLeft(
01857 const VectorType3f &src,
01858 VectorType3f &dst) const
01859 {
01860 dst.setValues((src[0] * _matrix[0][0] +
01861 src[1] * _matrix[0][1] +
01862 src[2] * _matrix[0][2]),
01863 (src[0] * _matrix[1][0] +
01864 src[1] * _matrix[1][1] +
01865 src[2] * _matrix[1][2]),
01866 (src[0] * _matrix[2][0] +
01867 src[1] * _matrix[2][1] +
01868 src[2] * _matrix[2][2]));
01869 }
01870
01871
01878 template <class ValueTypeT>
01879 inline typename TransformationMatrix<ValueTypeT>::PointType
01880 TransformationMatrix<ValueTypeT>::operator *(
01881 const PointType &pntIn) const
01882 {
01883 PointType pntOut;
01884
01885 this->mult(pntIn, pntOut);
01886
01887 return pntOut;
01888 }
01889
01895 template <class ValueTypeT>
01896 inline typename TransformationMatrix<ValueTypeT>::PointType3f
01897 TransformationMatrix<ValueTypeT>::operator *(
01898 const PointType3f &pntIn) const
01899 {
01900 PointType3f pntOut;
01901
01902 this->mult(pntIn, pntOut);
01903
01904 return pntOut;
01905 }
01906
01912 template <class ValueTypeT>
01913 inline typename TransformationMatrix<ValueTypeT>::VectorType
01914 TransformationMatrix<ValueTypeT>::operator *(
01915 const VectorType &vecIn) const
01916 {
01917 VectorType vecOut;
01918
01919 this->mult(vecIn, vecOut);
01920
01921 return vecOut;
01922 }
01923
01930 template <class ValueTypeT>
01931 inline typename TransformationMatrix<ValueTypeT>::VectorType3f
01932 TransformationMatrix<ValueTypeT>::operator *(
01933 const VectorType3f &vecIn) const
01934 {
01935 VectorType3f vecOut;
01936
01937 this->mult(vecIn, vecOut);
01938
01939 return vecOut;
01940 }
01941
01942
01943
01948 template<class ValueTypeT> inline
01949 bool TransformationMatrix<ValueTypeT>::equals(
01950 const TransformationMatrix &matrix,
01951 const ValueType tolerance) const
01952 {
01953 UInt32 i;
01954 bool returnValue = true;
01955
01956 for(i = 0; i < 4; i++)
01957 {
01958 returnValue &= _matrix[i].equals(matrix._matrix[i], tolerance);
01959
01960 if(returnValue == false)
01961 break;
01962 }
01963
01964 return returnValue;
01965 }
01966
01968
01969 template<class ValueTypeT> inline
01970 ValueTypeT TransformationMatrix<ValueTypeT>::det3(void) const
01971 {
01972 return (_matrix[0][0] * _matrix[1][1] * _matrix[2][2] +
01973 _matrix[0][1] * _matrix[1][2] * _matrix[2][0] +
01974 _matrix[0][2] * _matrix[1][0] * _matrix[2][1] -
01975 _matrix[0][2] * _matrix[1][1] * _matrix[2][0] -
01976 _matrix[0][1] * _matrix[1][0] * _matrix[2][2] -
01977 _matrix[0][0] * _matrix[1][2] * _matrix[2][1]);
01978 }
01979
01981
01982 template<class ValueTypeT> inline
01983 ValueTypeT TransformationMatrix<ValueTypeT>::det (void) const
01984 {
01985 ValueTypeT
01986 a1, a2, a3, a4,
01987 b1, b2, b3, b4,
01988 c1, c2, c3, c4,
01989 d1, d2, d3, d4;
01990
01991 a1 = _matrix[0][0];
01992 b1 = _matrix[1][0];
01993 c1 = _matrix[2][0];
01994 d1 = _matrix[3][0];
01995
01996 a2 = _matrix[0][1];
01997 b2 = _matrix[1][1];
01998 c2 = _matrix[2][1];
01999 d2 = _matrix[3][1];
02000
02001 a3 = _matrix[0][2];
02002 b3 = _matrix[1][2];
02003 c3 = _matrix[2][2];
02004 d3 = _matrix[3][2];
02005
02006 a4 = _matrix[0][3];
02007 b4 = _matrix[1][3];
02008 c4 = _matrix[2][3];
02009 d4 = _matrix[3][3];
02010
02011 return( a1 * det3_calc(b2, b3, b4, c2, c3, c4, d2, d3, d4)
02012 - b1 * det3_calc(a2, a3, a4, c2, c3, c4, d2, d3, d4)
02013 + c1 * det3_calc(a2, a3, a4, b2, b3, b4, d2, d3, d4)
02014 - d1 * det3_calc(a2, a3, a4, b2, b3, b4, c2, c3, c4));
02015
02016 }
02017
02022 template<class ValueTypeT> inline
02023 bool TransformationMatrix<ValueTypeT>::inverse(
02024 TransformationMatrix &result) const
02025 {
02026 return calcInverse(&result, this);
02027 }
02028
02033 template<class ValueTypeT> inline
02034 bool TransformationMatrix<ValueTypeT>::invert(void)
02035 {
02036 TransformationMatrix thisCopy = *this;
02037
02038 return calcInverse(this, &thisCopy);
02039 }
02040
02045 template<class ValueTypeT> inline
02046 bool TransformationMatrix<ValueTypeT>::invertFrom(
02047 const TransformationMatrix &matrix)
02048 {
02049 return calcInverse(this, &matrix);
02050 }
02051
02052 template<class ValueTypeT> inline
02053 bool TransformationMatrix<ValueTypeT>::inverse3(
02054 TransformationMatrix &result) const
02055 {
02056 return calcInverse3(&result, this);
02057 }
02058
02059 template<class ValueTypeT> inline
02060 bool TransformationMatrix<ValueTypeT>::invert3(void)
02061 {
02062 TransformationMatrix thisCopy = *this;
02063
02064 return calcInverse3(this, &thisCopy);
02065 }
02066
02067 template<class ValueTypeT> inline
02068 bool TransformationMatrix<ValueTypeT>::invertFrom3(
02069 const TransformationMatrix &matrix)
02070 {
02071 return calcInverse3(this, &matrix);
02072 }
02073
02074 template<class ValueTypeT> inline
02075 bool TransformationMatrix<ValueTypeT>::transposed(
02076 TransformationMatrix &result) const
02077 {
02078 result.setValueTransposed(
02079 (*this)[0][0], (*this)[1][0], (*this)[2][0], (*this)[3][0],
02080 (*this)[0][1], (*this)[1][1], (*this)[2][1], (*this)[3][1],
02081 (*this)[0][2], (*this)[1][2], (*this)[2][2], (*this)[3][2],
02082 (*this)[0][3], (*this)[1][3], (*this)[2][3], (*this)[3][3]);
02083
02084 return true;
02085 }
02086
02087 template<class ValueTypeT> inline
02088 bool TransformationMatrix<ValueTypeT>::transpose(void)
02089 {
02090 ValueTypeT tmp;
02091
02092 tmp = (*this)[1][0]; (*this)[1][0] = (*this)[0][1]; (*this)[0][1] = tmp;
02093 tmp = (*this)[2][0]; (*this)[2][0] = (*this)[0][2]; (*this)[0][2] = tmp;
02094 tmp = (*this)[3][0]; (*this)[3][0] = (*this)[0][3]; (*this)[0][3] = tmp;
02095 tmp = (*this)[2][1]; (*this)[2][1] = (*this)[1][2]; (*this)[1][2] = tmp;
02096 tmp = (*this)[3][1]; (*this)[3][1] = (*this)[1][3]; (*this)[1][3] = tmp;
02097 tmp = (*this)[3][2]; (*this)[3][2] = (*this)[2][3]; (*this)[2][3] = tmp;
02098
02099 return true;
02100 }
02101
02102 template<class ValueTypeT> inline
02103 bool TransformationMatrix<ValueTypeT>::transposeFrom(
02104 const TransformationMatrix &matrix)
02105 {
02106 this->setValueTransposed(
02107 matrix[0][0], matrix[1][0], matrix[2][0], matrix[3][0],
02108 matrix[0][1], matrix[1][1], matrix[2][1], matrix[3][1],
02109 matrix[0][2], matrix[1][2], matrix[2][2], matrix[3][2],
02110 matrix[0][3], matrix[1][3], matrix[2][3], matrix[3][3]);
02111
02112 return true;
02113 }
02114
02115 template<class ValueTypeT>
02116 template<class ValueTypeR> inline
02117 void TransformationMatrix<ValueTypeT>::mult(const TransformationMatrix<ValueTypeR> &matrix)
02118 {
02119 ValueTypeT rTmpMat[4][4];
02120
02121 (rTmpMat)[0][0] = rowMulCol4((*this), 0, (matrix), 0);
02122 (rTmpMat)[0][1] = rowMulCol4((*this), 1, (matrix), 0);
02123 (rTmpMat)[0][2] = rowMulCol4((*this), 2, (matrix), 0);
02124 (rTmpMat)[0][3] = rowMulCol4((*this), 3, (matrix), 0);
02125
02126 (rTmpMat)[1][0] = rowMulCol4((*this), 0, (matrix), 1);
02127 (rTmpMat)[1][1] = rowMulCol4((*this), 1, (matrix), 1);
02128 (rTmpMat)[1][2] = rowMulCol4((*this), 2, (matrix), 1);
02129 (rTmpMat)[1][3] = rowMulCol4((*this), 3, (matrix), 1);
02130
02131 (rTmpMat)[2][0] = rowMulCol4((*this), 0, (matrix), 2);
02132 (rTmpMat)[2][1] = rowMulCol4((*this), 1, (matrix), 2);
02133 (rTmpMat)[2][2] = rowMulCol4((*this), 2, (matrix), 2);
02134 (rTmpMat)[2][3] = rowMulCol4((*this), 3, (matrix), 2);
02135
02136 (rTmpMat)[3][0] = rowMulCol4((*this), 0, (matrix), 3);
02137 (rTmpMat)[3][1] = rowMulCol4((*this), 1, (matrix), 3);
02138 (rTmpMat)[3][2] = rowMulCol4((*this), 2, (matrix), 3);
02139 (rTmpMat)[3][3] = rowMulCol4((*this), 3, (matrix), 3);
02140
02141 _matrix[0][0] = rTmpMat[0][0];
02142 _matrix[0][1] = rTmpMat[0][1];
02143 _matrix[0][2] = rTmpMat[0][2];
02144 _matrix[0][3] = rTmpMat[0][3];
02145
02146 _matrix[1][0] = rTmpMat[1][0];
02147 _matrix[1][1] = rTmpMat[1][1];
02148 _matrix[1][2] = rTmpMat[1][2];
02149 _matrix[1][3] = rTmpMat[1][3];
02150
02151 _matrix[2][0] = rTmpMat[2][0];
02152 _matrix[2][1] = rTmpMat[2][1];
02153 _matrix[2][2] = rTmpMat[2][2];
02154 _matrix[2][3] = rTmpMat[2][3];
02155
02156 _matrix[3][0] = rTmpMat[3][0];
02157 _matrix[3][1] = rTmpMat[3][1];
02158 _matrix[3][2] = rTmpMat[3][2];
02159 _matrix[3][3] = rTmpMat[3][3];
02160 }
02161
02162 template<class ValueTypeT>
02163 template<class ValueTypeR> inline
02164 void TransformationMatrix<ValueTypeT>::multLeft(
02165 const TransformationMatrix<ValueTypeR> &matrix)
02166 {
02167 ValueTypeT rTmpMat[4][4];
02168
02169 (rTmpMat)[0][0] = rowMulCol4((matrix), 0, (*this), 0);
02170 (rTmpMat)[0][1] = rowMulCol4((matrix), 1, (*this), 0);
02171 (rTmpMat)[0][2] = rowMulCol4((matrix), 2, (*this), 0);
02172 (rTmpMat)[0][3] = rowMulCol4((matrix), 3, (*this), 0);
02173
02174 (rTmpMat)[1][0] = rowMulCol4((matrix), 0, (*this), 1);
02175 (rTmpMat)[1][1] = rowMulCol4((matrix), 1, (*this), 1);
02176 (rTmpMat)[1][2] = rowMulCol4((matrix), 2, (*this), 1);
02177 (rTmpMat)[1][3] = rowMulCol4((matrix), 3, (*this), 1);
02178
02179 (rTmpMat)[2][0] = rowMulCol4((matrix), 0, (*this), 2);
02180 (rTmpMat)[2][1] = rowMulCol4((matrix), 1, (*this), 2);
02181 (rTmpMat)[2][2] = rowMulCol4((matrix), 2, (*this), 2);
02182 (rTmpMat)[2][3] = rowMulCol4((matrix), 3, (*this), 2);
02183
02184 (rTmpMat)[3][0] = rowMulCol4((matrix), 0, (*this), 3);
02185 (rTmpMat)[3][1] = rowMulCol4((matrix), 1, (*this), 3);
02186 (rTmpMat)[3][2] = rowMulCol4((matrix), 2, (*this), 3);
02187 (rTmpMat)[3][3] = rowMulCol4((matrix), 3, (*this), 3);
02188
02189 _matrix[0][0] = rTmpMat[0][0];
02190 _matrix[0][1] = rTmpMat[0][1];
02191 _matrix[0][2] = rTmpMat[0][2];
02192 _matrix[0][3] = rTmpMat[0][3];
02193
02194 _matrix[1][0] = rTmpMat[1][0];
02195 _matrix[1][1] = rTmpMat[1][1];
02196 _matrix[1][2] = rTmpMat[1][2];
02197 _matrix[1][3] = rTmpMat[1][3];
02198
02199 _matrix[2][0] = rTmpMat[2][0];
02200 _matrix[2][1] = rTmpMat[2][1];
02201 _matrix[2][2] = rTmpMat[2][2];
02202 _matrix[2][3] = rTmpMat[2][3];
02203
02204 _matrix[3][0] = rTmpMat[3][0];
02205 _matrix[3][1] = rTmpMat[3][1];
02206 _matrix[3][2] = rTmpMat[3][2];
02207 _matrix[3][3] = rTmpMat[3][3];
02208 }
02209
02211
02212 template<class ValueTypeT> inline
02213 void TransformationMatrix<ValueTypeT>::add(const TransformationMatrix &matrix)
02214 {
02215 _matrix[0][0] += matrix._matrix[0][0];
02216 _matrix[0][1] += matrix._matrix[0][1];
02217 _matrix[0][2] += matrix._matrix[0][2];
02218 _matrix[0][3] += matrix._matrix[0][3];
02219
02220 _matrix[1][0] += matrix._matrix[1][0];
02221 _matrix[1][1] += matrix._matrix[1][1];
02222 _matrix[1][2] += matrix._matrix[1][2];
02223 _matrix[1][3] += matrix._matrix[1][3];
02224
02225 _matrix[2][0] += matrix._matrix[2][0];
02226 _matrix[2][1] += matrix._matrix[2][1];
02227 _matrix[2][2] += matrix._matrix[2][2];
02228 _matrix[2][3] += matrix._matrix[2][3];
02229
02230 _matrix[3][0] += matrix._matrix[3][0];
02231 _matrix[3][1] += matrix._matrix[3][1];
02232 _matrix[3][2] += matrix._matrix[3][2];
02233 _matrix[3][3] += matrix._matrix[3][3];
02234 }
02235
02237
02238 template<class ValueTypeT> inline
02239 void TransformationMatrix<ValueTypeT>::scale(ValueTypeT s)
02240 {
02241 _matrix[0][0] *= s;
02242 _matrix[0][1] *= s;
02243 _matrix[0][2] *= s;
02244 _matrix[0][3] *= s;
02245
02246 _matrix[1][0] *= s;
02247 _matrix[1][1] *= s;
02248 _matrix[1][2] *= s;
02249 _matrix[1][3] *= s;
02250
02251 _matrix[2][0] *= s;
02252 _matrix[2][1] *= s;
02253 _matrix[2][2] *= s;
02254 _matrix[2][3] *= s;
02255
02256 _matrix[3][0] *= s;
02257 _matrix[3][1] *= s;
02258 _matrix[3][2] *= s;
02259 _matrix[3][3] *= s;
02260 }
02261
02263
02264 template<class ValueTypeT> inline
02265 void TransformationMatrix<ValueTypeT>::addScaled(
02266 const TransformationMatrix &matrix,
02267 ValueTypeT s)
02268 {
02269 _matrix[0][0] += s*matrix._matrix[0][0];
02270 _matrix[0][1] += s*matrix._matrix[0][1];
02271 _matrix[0][2] += s*matrix._matrix[0][2];
02272 _matrix[0][3] += s*matrix._matrix[0][3];
02273
02274 _matrix[1][0] += s*matrix._matrix[1][0];
02275 _matrix[1][1] += s*matrix._matrix[1][1];
02276 _matrix[1][2] += s*matrix._matrix[1][2];
02277 _matrix[1][3] += s*matrix._matrix[1][3];
02278
02279 _matrix[2][0] += s*matrix._matrix[2][0];
02280 _matrix[2][1] += s*matrix._matrix[2][1];
02281 _matrix[2][2] += s*matrix._matrix[2][2];
02282 _matrix[2][3] += s*matrix._matrix[2][3];
02283
02284 _matrix[3][0] += s*matrix._matrix[3][0];
02285 _matrix[3][1] += s*matrix._matrix[3][1];
02286 _matrix[3][2] += s*matrix._matrix[3][2];
02287 _matrix[3][3] += s*matrix._matrix[3][3];
02288 }
02289
02291
02292 template<class ValueTypeT> inline
02293 void TransformationMatrix<ValueTypeT>::negate(void)
02294 {
02295 _matrix[0][0] *= -1.0;
02296 _matrix[0][1] *= -1.0;
02297 _matrix[0][2] *= -1.0;
02298 _matrix[0][3] *= -1.0;
02299
02300 _matrix[1][0] *= -1.0;
02301 _matrix[1][1] *= -1.0;
02302 _matrix[1][2] *= -1.0;
02303 _matrix[1][3] *= -1.0;
02304
02305 _matrix[2][0] *= -1.0;
02306 _matrix[2][1] *= -1.0;
02307 _matrix[2][2] *= -1.0;
02308 _matrix[2][3] *= -1.0;
02309
02310 _matrix[3][0] *= -1.0;
02311 _matrix[3][1] *= -1.0;
02312 _matrix[3][2] *= -1.0;
02313 _matrix[3][3] *= -1.0;
02314 }
02315
02321 template<class ValueTypeT> inline
02322 ValueTypeT TransformationMatrix<ValueTypeT>::norm1(void) const
02323 {
02324 ValueTypeT m = TypeTraits<ValueType>::getZeroElement();
02325
02326 m += osgAbs(_matrix[0][0]);
02327 m += osgAbs(_matrix[0][1]);
02328 m += osgAbs(_matrix[0][2]);
02329 m += osgAbs(_matrix[0][3]);
02330 m += osgAbs(_matrix[1][0]);
02331 m += osgAbs(_matrix[1][1]);
02332 m += osgAbs(_matrix[1][2]);
02333 m += osgAbs(_matrix[1][3]);
02334 m += osgAbs(_matrix[2][0]);
02335 m += osgAbs(_matrix[2][1]);
02336 m += osgAbs(_matrix[2][2]);
02337 m += osgAbs(_matrix[2][3]);
02338 m += osgAbs(_matrix[3][0]);
02339 m += osgAbs(_matrix[3][1]);
02340 m += osgAbs(_matrix[3][2]);
02341 m += osgAbs(_matrix[3][3]);
02342
02343 return m;
02344 }
02345
02351 template<class ValueTypeT> inline
02352 ValueTypeT TransformationMatrix<ValueTypeT>::norm2(void) const
02353 {
02354 ValueTypeT m = TypeTraits<ValueType>::getZeroElement();
02355 ValueTypeT t;
02356
02357 t = _matrix[0][0]; m += t*t;
02358 t = _matrix[0][1]; m += t*t;
02359 t = _matrix[0][2]; m += t*t;
02360 t = _matrix[0][3]; m += t*t;
02361 t = _matrix[1][0]; m += t*t;
02362 t = _matrix[1][1]; m += t*t;
02363 t = _matrix[1][2]; m += t*t;
02364 t = _matrix[1][3]; m += t*t;
02365 t = _matrix[2][0]; m += t*t;
02366 t = _matrix[2][1]; m += t*t;
02367 t = _matrix[2][2]; m += t*t;
02368 t = _matrix[2][3]; m += t*t;
02369 t = _matrix[3][0]; m += t*t;
02370 t = _matrix[3][1]; m += t*t;
02371 t = _matrix[3][2]; m += t*t;
02372 t = _matrix[3][3]; m += t*t;
02373
02374 return osgSqrt(m);
02375 }
02376
02382 template<class ValueTypeT> inline
02383 ValueTypeT TransformationMatrix<ValueTypeT>::normInfinity(void) const
02384 {
02385 ValueTypeT m = TypeTraits<ValueType>::getZeroElement();
02386 ValueTypeT t;
02387
02388 if((t = osgAbs(_matrix[0][0])) > m)
02389 m = t;
02390 if((t = osgAbs(_matrix[0][1])) > m)
02391 m = t;
02392 if((t = osgAbs(_matrix[0][2])) > m)
02393 m = t;
02394 if((t = osgAbs(_matrix[0][3])) > m)
02395 m = t;
02396 if((t = osgAbs(_matrix[1][0])) > m)
02397 m = t;
02398 if((t = osgAbs(_matrix[1][1])) > m)
02399 m = t;
02400 if((t = osgAbs(_matrix[1][2])) > m)
02401 m = t;
02402 if((t = osgAbs(_matrix[1][3])) > m)
02403 m = t;
02404 if((t = osgAbs(_matrix[2][0])) > m)
02405 m = t;
02406 if((t = osgAbs(_matrix[2][1])) > m)
02407 m = t;
02408 if((t = osgAbs(_matrix[2][2])) > m)
02409 m = t;
02410 if((t = osgAbs(_matrix[2][3])) > m)
02411 m = t;
02412 if((t = osgAbs(_matrix[3][0])) > m)
02413 m = t;
02414 if((t = osgAbs(_matrix[3][1])) > m)
02415 m = t;
02416 if((t = osgAbs(_matrix[3][2])) > m)
02417 m = t;
02418 if((t = osgAbs(_matrix[3][3])) > m)
02419 m = t;
02420
02421 return m;
02422 }
02423
02428 template<class ValueTypeT> inline
02429 bool TransformationMatrix<ValueTypeT>::sqrt(TransformationMatrix &result) const
02430 {
02431 TransformationMatrix iX;
02432 TransformationMatrix Y;
02433 TransformationMatrix iY;
02434
02435 ValueTypeT g;
02436 ValueTypeT ig;
02437
02438 result.setValue(*this);
02439
02440 Y.setIdentity();
02441
02442 for(UInt32 i = 0; i < 6; i++)
02443 {
02444 result.inverse(iX);
02445
02446 Y.inverse(iY);
02447
02448 g = osgAbs(osgPow(result.det() * Y.det(), ValueTypeT(-0.125)));
02449
02450 ig = ValueTypeT(1.0 / g);
02451
02452 result.scale (g );
02453 result.addScaled(iY, ig);
02454 result.scale (0.5 );
02455
02456 Y.scale (g );
02457 Y.addScaled(iX, ig);
02458 Y.scale (0.5 );
02459 }
02460
02461
02462 return true;
02463 }
02464
02469 template<class ValueTypeT> inline
02470 bool TransformationMatrix<ValueTypeT>::sqrtOf(
02471 const TransformationMatrix &matrix)
02472 {
02473 TransformationMatrix iX;
02474 TransformationMatrix Y;
02475 TransformationMatrix iY;
02476
02477 ValueTypeT g;
02478 ValueTypeT ig;
02479
02480 setValue(matrix);
02481
02482 Y.setIdentity();
02483
02484 for(Int32 i = 0; i < 6; i++)
02485 {
02486 inverse(iX);
02487
02488 Y.inverse(iY);
02489
02490 g = osgAbs(osgPow(det() * Y.det(), ValueTypeT(-0.125)));
02491
02492 ig = ValueTypeT(1.0 / g);
02493
02494 scale (g );
02495 addScaled(iY, ig);
02496 scale (0.5 );
02497
02498 Y.scale (g );
02499 Y.addScaled(iX, ig);
02500 Y.scale (0.5);
02501 }
02502
02503
02504 return true;
02505 }
02506
02508
02509 template<class ValueTypeT> inline
02510 bool TransformationMatrix<ValueTypeT>::sqrt(void)
02511 {
02512 TransformationMatrix iX;
02513 TransformationMatrix Y;
02514 TransformationMatrix iY;
02515
02516 ValueTypeT g;
02517 ValueTypeT ig;
02518
02519 Y.setIdentity();
02520
02521 for(Int32 i = 0; i < 6; i++)
02522 {
02523 inverse(iX);
02524
02525 Y.inverse(iY);
02526
02527 g = osgAbs(osgPow(det() * Y.det(), ValueTypeT(-0.125)));
02528
02529 ig = ValueTypeT(1.0 / g);
02530
02531 scale (g );
02532 addScaled(iY, ig);
02533 scale (0.5 );
02534
02535 Y.scale (g );
02536 Y.addScaled(iX, ig);
02537 Y.scale (0.5 );
02538 }
02539
02540
02541 return true;
02542 }
02543
02548 template<class ValueTypeT> inline
02549 bool TransformationMatrix<ValueTypeT>::log(TransformationMatrix &result) const
02550 {
02551 const Int32 maxiter = 12;
02552 Int32 k = 0;
02553 Int32 i = 0;
02554 const ValueTypeT eps = 1e-12;
02555
02556 TransformationMatrix A(*this);
02557 TransformationMatrix Z;
02558
02559
02560 Z.setValue(A);
02561
02562 Z[0][0] -= TypeTraits<ValueType>::getOneElement();
02563 Z[1][1] -= TypeTraits<ValueType>::getOneElement();
02564 Z[2][2] -= TypeTraits<ValueType>::getOneElement();
02565 Z[3][3] -= TypeTraits<ValueType>::getOneElement();
02566
02567 while(Z.normInfinity() > 0.5)
02568 {
02569 A.sqrt ( );
02570 Z.setValue(A);
02571
02572 Z[0][0] -= TypeTraits<ValueType>::getOneElement();
02573 Z[1][1] -= TypeTraits<ValueType>::getOneElement();
02574 Z[2][2] -= TypeTraits<ValueType>::getOneElement();
02575 Z[3][3] -= TypeTraits<ValueType>::getOneElement();
02576
02577 k++;
02578 }
02579
02580 A[0][0] -= TypeTraits<ValueType>::getOneElement();
02581 A[1][1] -= TypeTraits<ValueType>::getOneElement();
02582 A[2][2] -= TypeTraits<ValueType>::getOneElement();
02583 A[3][3] -= TypeTraits<ValueType>::getOneElement();
02584
02585 A.negate();
02586
02587 result.setValue(A);
02588
02589 Z.setValue(A);
02590
02591 i = 1;
02592
02593 while(Z.normInfinity() > eps && i < maxiter)
02594 {
02595 Z.mult(A);
02596
02597 i++;
02598
02599 result.addScaled(Z, ValueTypeT(1.0) / ValueTypeT(i));
02600 }
02601
02602 result.scale(ValueTypeT(-1.0) * ValueTypeT(1 << k));
02603
02604 return (i < maxiter);
02605 }
02606
02611 template<class ValueTypeT> inline
02612 bool TransformationMatrix<ValueTypeT>::logOf(
02613 const TransformationMatrix &matrix)
02614 {
02615 const Int32 maxiter = 12;
02616 Int32 k = 0;
02617 Int32 i = 0;
02618 const ValueTypeT eps = 1e-12;
02619
02620 TransformationMatrix<ValueTypeT> A(matrix),Z;
02621
02622
02623 Z.setValue(A);
02624
02625 Z[0][0] -= TypeTraits<ValueType>::getOneElement();
02626 Z[1][1] -= TypeTraits<ValueType>::getOneElement();
02627 Z[2][2] -= TypeTraits<ValueType>::getOneElement();
02628 Z[3][3] -= TypeTraits<ValueType>::getOneElement();
02629
02630 while(Z.normInfinity() > 0.5)
02631 {
02632 A.sqrt();
02633
02634 Z.setValue(A);
02635
02636 Z[0][0] -= TypeTraits<ValueType>::getOneElement();
02637 Z[1][1] -= TypeTraits<ValueType>::getOneElement();
02638 Z[2][2] -= TypeTraits<ValueType>::getOneElement();
02639 Z[3][3] -= TypeTraits<ValueType>::getOneElement();
02640
02641 k++;
02642 }
02643
02644 A[0][0] -= TypeTraits<ValueType>::getOneElement();
02645 A[1][1] -= TypeTraits<ValueType>::getOneElement();
02646 A[2][2] -= TypeTraits<ValueType>::getOneElement();
02647 A[3][3] -= TypeTraits<ValueType>::getOneElement();
02648
02649 A.negate();
02650
02651 setValue(A);
02652
02653 Z.setValue(A);
02654
02655 i = 1;
02656 while(Z.normInfinity() > eps && i < maxiter)
02657 {
02658 Z.mult(A);
02659
02660 i++;
02661
02662 addScaled(Z, ValueTypeT(1.0) / ValueTypeT(i));
02663 }
02664
02665 scale(ValueTypeT(-1.0) * ValueTypeT(1 << k));
02666
02667 return (i<maxiter);
02668 }
02669
02674 template<class ValueTypeT> inline
02675 bool TransformationMatrix<ValueTypeT>::exp(TransformationMatrix &result) const
02676 {
02677 const Int32 q = 6;
02678
02679 TransformationMatrix A(*this);
02680 TransformationMatrix D;
02681 TransformationMatrix N;
02682
02683 Int32 j = 1;
02684 Int32 k;
02685
02686 ValueTypeT c(1.0);
02687
02688 j += Int32(osgLog(A.normInfinity() / 0.693));
02689
02690 if(j < 0)
02691 j = 0;
02692
02693 A.scale(ValueTypeT(1.0f / (1 << j)));
02694
02695 result.setIdentity();
02696
02697 for(k = 1; k <= q; k++)
02698 {
02699 c *= ValueTypeT(q - k + 1) / ValueTypeT(k * (2 * q - k + 1));
02700
02701 result.multLeft(A);
02702
02703 N.addScaled(result, c);
02704
02705 if(k % 2)
02706 {
02707 D.addScaled(result, -c);
02708 }
02709 else
02710 {
02711 D.addScaled(result, c);
02712 }
02713 }
02714
02715 result.invertFrom(D);
02716 result.mult (N);
02717
02718 for(k = 0; k < j; k++)
02719 result.mult(result);
02720
02721
02722 return true;
02723 }
02724
02728 template<class ValueTypeT> inline
02729 bool TransformationMatrix<ValueTypeT>::expOf(
02730 const TransformationMatrix &matrix)
02731 {
02732 const Int32 q = 6;
02733
02734 TransformationMatrix A(matrix);
02735 TransformationMatrix D;
02736 TransformationMatrix N;
02737
02738 Int32 j = 1;
02739 Int32 k;
02740
02741 ValueTypeT c(1.0);
02742
02743 j += int(osgLog(A.normInfinity() / 0.693));
02744
02745 if(j < 0)
02746 j = 0;
02747
02748 A.scale(1.0 / (ValueTypeT(1 << j)));
02749
02750 setIdentity();
02751
02752 for(k = 1; k <= q; k++)
02753 {
02754 c *= ValueTypeT(q - k + 1) / ValueTypeT(k * (2 *q - k + 1));
02755
02756 multLeft(A);
02757
02758 N.addScaled(*this,c);
02759
02760 if(k % 2)
02761 {
02762 D.addScaled(*this, -c);
02763 }
02764 else
02765 {
02766 D.addScaled(*this, c);
02767 }
02768 }
02769
02770 invertFrom(D);
02771 mult (N);
02772
02773 for(k = 0; k < j; k++)
02774 mult(*this);
02775
02776
02777 return true;
02778 }
02779
02780
02781
02782
02783 template<class ValueTypeT> inline
02784 typename TransformationMatrix<ValueTypeT>::VectorType &
02785 TransformationMatrix<ValueTypeT>::operator [](UInt32 uiIndex)
02786 {
02787 return _matrix[uiIndex];
02788 }
02789
02790 template<class ValueTypeT> inline
02791 const typename TransformationMatrix<ValueTypeT>::VectorType &
02792 TransformationMatrix<ValueTypeT>::operator [](UInt32 uiIndex) const
02793 {
02794 return _matrix[uiIndex];
02795 }
02796
02797
02798
02799
02800 template<class ValueTypeT> inline
02801 TransformationMatrix<ValueTypeT> &
02802 TransformationMatrix<ValueTypeT>::operator = (
02803 const TransformationMatrix &source)
02804 {
02805 UInt32 i;
02806
02807 if (this == &source)
02808 return *this;
02809
02810 for(i = 0; i < 4; i++)
02811 _matrix[i] = source._matrix[i];
02812
02813 return *this;
02814 }
02815
02816
02817
02818
02823 template<class ValueTypeT> inline
02824 bool TransformationMatrix<ValueTypeT>::operator == (
02825 const TransformationMatrix &other) const
02826 {
02827 return equals(other, Eps);
02828 }
02829
02834 template<class ValueTypeT> inline
02835 bool TransformationMatrix<ValueTypeT>::operator != (
02836 const TransformationMatrix &other) const
02837 {
02838 return ! (*this == other);
02839 }
02840
02845 template <class ValueTypeT> inline
02846 bool TransformationMatrix<ValueTypeT>::calcInverse(
02847 TransformationMatrix *destM, const TransformationMatrix *srcM) const
02848 {
02849 VectorType *dM = destM->_matrix;
02850 const VectorType *sM = srcM ->_matrix;
02851
02852 ValueTypeT det3A0 = det3_calc(sM[1][1], sM[1][2], sM[1][3],
02853 sM[2][1], sM[2][2], sM[2][3],
02854 sM[3][1], sM[3][2], sM[3][3] );
02855 ValueTypeT det3A1 = det3_calc(sM[1][0], sM[1][2], sM[1][3],
02856 sM[2][0], sM[2][2], sM[2][3],
02857 sM[3][0], sM[3][2], sM[3][3] );
02858 ValueTypeT det3A2 = det3_calc(sM[1][0], sM[1][1], sM[1][3],
02859 sM[2][0], sM[2][1], sM[2][3],
02860 sM[3][0], sM[3][1], sM[3][3] );
02861 ValueTypeT det3A3 = det3_calc(sM[1][0], sM[1][1], sM[1][2],
02862 sM[2][0], sM[2][1], sM[2][2],
02863 sM[3][0], sM[3][1], sM[3][2] );
02864
02865 ValueTypeT det4 = sM[0][0] * det3A0 - sM[0][1] * det3A1 +
02866 sM[0][2] * det3A2 - sM[0][3] * det3A3;
02867
02868 if(osgAbs(det4) < TypeTraits<ValueTypeT>::ZeroEps())
02869 {
02870 FWARNING(("TransformationMatrix<>::calcInverse: "
02871 "Singular matrix, no inverse!\n"));
02872
02873 destM->setIdentity();
02874
02875 return false;
02876 }
02877
02878 ValueTypeT det4Inv = TypeTraits<ValueTypeT>::getOneElement() / det4;
02879
02880 dM[0][0] = det3A0 * det4Inv;
02881 dM[0][1] = - det3_calc(sM[0][1], sM[0][2], sM[0][3],
02882 sM[2][1], sM[2][2], sM[2][3],
02883 sM[3][1], sM[3][2], sM[3][3] ) * det4Inv;
02884 dM[0][2] = det3_calc(sM[0][1], sM[0][2], sM[0][3],
02885 sM[1][1], sM[1][2], sM[1][3],
02886 sM[3][1], sM[3][2], sM[3][3] ) * det4Inv;
02887 dM[0][3] = - det3_calc(sM[0][1], sM[0][2], sM[0][3],
02888 sM[1][1], sM[1][2], sM[1][3],
02889 sM[2][1], sM[2][2], sM[2][3] ) * det4Inv;
02890
02891 dM[1][0] = - det3A1 * det4Inv;
02892 dM[1][1] = det3_calc(sM[0][0], sM[0][2], sM[0][3],
02893 sM[2][0], sM[2][2], sM[2][3],
02894 sM[3][0], sM[3][2], sM[3][3] ) * det4Inv;
02895 dM[1][2] = - det3_calc(sM[0][0], sM[0][2], sM[0][3],
02896 sM[1][0], sM[1][2], sM[1][3],
02897 sM[3][0], sM[3][2], sM[3][3] ) * det4Inv;
02898 dM[1][3] = det3_calc(sM[0][0], sM[0][2], sM[0][3],
02899 sM[1][0], sM[1][2], sM[1][3],
02900 sM[2][0], sM[2][2], sM[2][3] ) * det4Inv;
02901
02902 dM[2][0] = det3A2 * det4Inv;
02903 dM[2][1] = - det3_calc(sM[0][0], sM[0][1], sM[0][3],
02904 sM[2][0], sM[2][1], sM[2][3],
02905 sM[3][0], sM[3][1], sM[3][3] ) * det4Inv;
02906 dM[2][2] = det3_calc(sM[0][0], sM[0][1], sM[0][3],
02907 sM[1][0], sM[1][1], sM[1][3],
02908 sM[3][0], sM[3][1], sM[3][3] ) * det4Inv;
02909 dM[2][3] = - det3_calc(sM[0][0], sM[0][1], sM[0][3],
02910 sM[1][0], sM[1][1], sM[1][3],
02911 sM[2][0], sM[2][1], sM[2][3] ) * det4Inv;
02912
02913
02914 dM[3][0] = - det3A3 * det4Inv;
02915 dM[3][1] = det3_calc(sM[0][0], sM[0][1], sM[0][2],
02916 sM[2][0], sM[2][1], sM[2][2],
02917 sM[3][0], sM[3][1], sM[3][2] ) * det4Inv;
02918 dM[3][2] = - det3_calc(sM[0][0], sM[0][1], sM[0][2],
02919 sM[1][0], sM[1][1], sM[1][2],
02920 sM[3][0], sM[3][1], sM[3][2] ) * det4Inv;
02921 dM[3][3] = det3_calc(sM[0][0], sM[0][1], sM[0][2],
02922 sM[1][0], sM[1][1], sM[1][2],
02923 sM[2][0], sM[2][1], sM[2][2] ) * det4Inv;
02924
02925 return true;
02926 }
02927
02933 template <class ValueTypeT> inline
02934 bool TransformationMatrix<ValueTypeT>::calcInverse3(
02935 TransformationMatrix *destM, const TransformationMatrix *srcM) const
02936 {
02937 VectorType *dM = destM->_matrix;
02938 const VectorType *sM = srcM ->_matrix;
02939
02940 ValueTypeT det2A0 = det2_calc(sM[1][1], sM[1][2], sM[2][1], sM[2][2]);
02941 ValueTypeT det2A1 = det2_calc(sM[1][0], sM[1][2], sM[2][0], sM[2][2]);
02942 ValueTypeT det2A2 = det2_calc(sM[1][0], sM[1][1], sM[2][0], sM[2][1]);
02943
02944 ValueTypeT det3 = sM[0][0] * det2A0 -
02945 sM[0][1] * det2A1 +
02946 sM[0][2] * det2A2;
02947
02948
02949 if(osgAbs(det3) < TypeTraits<ValueTypeT>::ZeroEps())
02950 {
02951 FWARNING(("TransformationMatrix<>::calcInverse3: "
02952 "Singular matrix, no inverse!\n"));
02953
02954 destM->setIdentity();
02955
02956 return false;
02957 }
02958
02959 ValueTypeT det3Inv = TypeTraits<ValueTypeT>::getOneElement() / det3;
02960
02961 dM[0][0] = det2A0 * det3Inv;
02962 dM[0][1] = - det2_calc(sM[0][1], sM[0][2], sM[2][1], sM[2][2]) * det3Inv;
02963 dM[0][2] = det2_calc(sM[0][1], sM[0][2], sM[1][1], sM[1][2]) * det3Inv;
02964 dM[0][3] = TypeTraits<ValueTypeT>::getZeroElement();
02965
02966 dM[1][0] = - det2A1 * det3Inv;
02967 dM[1][1] = det2_calc(sM[0][0], sM[0][2], sM[2][0], sM[2][2]) * det3Inv;
02968 dM[1][2] = - det2_calc(sM[0][0], sM[0][2], sM[1][0], sM[1][2]) * det3Inv;
02969 dM[1][3] = TypeTraits<ValueTypeT>::getZeroElement();
02970
02971 dM[2][0] = det2A2 * det3Inv;
02972 dM[2][1] = - det2_calc(sM[0][0], sM[0][1], sM[2][0], sM[2][1]) * det3Inv;
02973 dM[2][2] = det2_calc(sM[0][0], sM[0][1], sM[1][0], sM[1][1]) * det3Inv;
02974 dM[2][3] = TypeTraits<ValueTypeT>::getZeroElement();
02975
02976 dM[3][0] = TypeTraits<ValueTypeT>::getZeroElement();
02977 dM[3][1] = TypeTraits<ValueTypeT>::getZeroElement();
02978 dM[3][2] = TypeTraits<ValueTypeT>::getZeroElement();
02979 dM[3][3] = TypeTraits<ValueTypeT>::getOneElement ();
02980
02981 return true;
02982 }
02983
02984
02985
02986
02987 #ifdef OSG_1_COMPAT
02988 template <class ValueTypeT> inline
02989 void TransformationMatrix<ValueTypeT>::mult(PointType3f &pnt) const
02990 {
02991 Self::mult(pnt, pnt);
02992 }
02993
02994 template <class ValueTypeT> inline
02995 void TransformationMatrix<ValueTypeT>::multMatrixPnt(PointType3f &pnt) const
02996 {
02997 Self::mult(pnt, pnt);
02998 }
02999
03000 template <class ValueTypeT> inline
03001 void TransformationMatrix<ValueTypeT>::multMatrixPnt(
03002 const PointType3f &src,
03003 PointType3f &dst) const
03004 {
03005 Self::mult(src, dst);
03006 }
03007
03008 template <class ValueTypeT> inline
03009 void TransformationMatrix<ValueTypeT>::multMatrixVec(VectorType3f &vec) const
03010 {
03011 Self::mult(vec, vec);
03012 }
03013
03014 template <class ValueTypeT> inline
03015 void TransformationMatrix<ValueTypeT>::multMatrixVec(
03016 const VectorType3f &src,
03017 VectorType3f &dst) const
03018 {
03019 Self::mult(src, dst);
03020 }
03021
03022 template <class ValueTypeT> inline
03023 void TransformationMatrix<ValueTypeT>::multFullMatrixPnt(
03024 PointType3f &pnt) const
03025 {
03026 Self::multFull(pnt, pnt);
03027 }
03028
03029 template <class ValueTypeT> inline
03030 void TransformationMatrix<ValueTypeT>::multFullMatrixPnt(
03031 const PointType3f &src,
03032 PointType3f &dst) const
03033 {
03034 Self::multFull(src, dst);
03035 }
03036
03037 template <class ValueTypeT> inline
03038 void TransformationMatrix<ValueTypeT>::multPntFullMatrix(
03039 const PointType3f &src,
03040 PointType3f &dst) const
03041 {
03042 Self::multLeftFull(src, dst);
03043 }
03044 #endif
03045
03046
03047
03048
03050
03051 template<class ValueTypeT> inline
03052 std::ostream &operator <<( std::ostream &os,
03053 const TransformationMatrix<ValueTypeT> &obj)
03054 {
03055 UInt32 i;
03056 UInt32 j;
03057
03058 std::ios::fmtflags oldflags = os.flags(std::ios::showpoint |
03059 std::ios::fixed);
03060
03061 Int32 pr = os.precision(3 );
03062 Char8 fill = os.fill (' ');
03063 Int32 width = os.width (8 );
03064
03065 for(j = 0; j < 4; j++)
03066 {
03067 for(i = 0; i < 4; i++)
03068 {
03069 os << std::setw(8) << obj[i][j] << " ";
03070 }
03071
03072 os << "\n";
03073 }
03074
03075 os.flags (oldflags);
03076 os.precision(pr );
03077 os.fill (fill );
03078 os.width (width );
03079
03080 return os;
03081 }
03082
03083 OSG_END_NAMESPACE
