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
00120 template<class ValueTypeT> inline
00121 TransformationMatrix<ValueTypeT>::TransformationMatrix(
00122 const VectorType3f &vector1,
00123 const VectorType3f &vector2,
00124 const VectorType3f &vector3,
00125 const VectorType3f &vector4)
00126 {
00127 _matrix[0].setValue(vector1);
00128 _matrix[1].setValue(vector2);
00129 _matrix[2].setValue(vector3);
00130 _matrix[3].setValue(vector4);
00131 }
00132
00133 template<class ValueTypeT> inline
00134 TransformationMatrix<ValueTypeT>::TransformationMatrix(
00135 const ValueTypeT rVal00,
00136 const ValueTypeT rVal10,
00137 const ValueTypeT rVal20,
00138 const ValueTypeT rVal30,
00139
00140 const ValueTypeT rVal01,
00141 const ValueTypeT rVal11,
00142 const ValueTypeT rVal21,
00143 const ValueTypeT rVal31,
00144
00145 const ValueTypeT rVal02,
00146 const ValueTypeT rVal12,
00147 const ValueTypeT rVal22,
00148 const ValueTypeT rVal32,
00149
00150 const ValueTypeT rVal03,
00151 const ValueTypeT rVal13,
00152 const ValueTypeT rVal23,
00153 const ValueTypeT rVal33)
00154 {
00155 _matrix[0].setValues(rVal00, rVal01, rVal02, rVal03);
00156 _matrix[1].setValues(rVal10, rVal11, rVal12, rVal13);
00157 _matrix[2].setValues(rVal20, rVal21, rVal22, rVal23);
00158 _matrix[3].setValues(rVal30, rVal31, rVal32, rVal33);
00159 }
00160
00161
00162
00163
00164 template<class ValueTypeT> inline
00165 TransformationMatrix<ValueTypeT>::~TransformationMatrix(void)
00166 {
00167 }
00168
00169
00170
00171
00172 template<class ValueTypeT> inline
00173 void TransformationMatrix<ValueTypeT>::setIdentity(void)
00174 {
00175 for(UInt32 i = 0; i < 4; i++)
00176 {
00177 _matrix[i].setNull();
00178 _matrix[i][i] = TypeTraits<ValueType>::getOneElement();
00179 }
00180 }
00181
00182 template<class ValueTypeT> inline
00183 void TransformationMatrix<ValueTypeT>::setValue(
00184 const TransformationMatrix &mat)
00185 {
00186 for(UInt32 i = 0; i < 4; i++)
00187 {
00188 _matrix[i] = mat._matrix[i];
00189 }
00190 }
00191
00192 template<class ValueTypeT>
00193 template<class ValueTypeR>
00194 inline
00195 void TransformationMatrix<ValueTypeT>::convertFrom(
00196 const TransformationMatrix<ValueTypeR>& mat)
00197 {
00198 for(UInt32 i = 0; i < 4; i++)
00199 {
00200 for(UInt32 j =0; j<4; j++)
00201 {
00202 (*this)[i][j] = mat[i][j];
00203 }
00204 }
00205 }
00206
00207
00208 template<class ValueTypeT> inline
00209 void TransformationMatrix<ValueTypeT>::setValue(const VectorType3f &vector1,
00210 const VectorType3f &vector2,
00211 const VectorType3f &vector3)
00212 {
00213 _matrix[0].setValue(vector1);
00214 _matrix[1].setValue(vector2);
00215 _matrix[2].setValue(vector3);
00216 }
00217
00218 template<class ValueTypeT> inline
00219 void TransformationMatrix<ValueTypeT>::setValue(const VectorType3f &vector1,
00220 const VectorType3f &vector2,
00221 const VectorType3f &vector3,
00222 const VectorType3f &vector4)
00223 {
00224 _matrix[0].setValue(vector1);
00225 _matrix[1].setValue(vector2);
00226 _matrix[2].setValue(vector3);
00227 _matrix[3].setValue(vector4);
00228 }
00229
00230 template<class ValueTypeT> inline
00231 void TransformationMatrix<ValueTypeT>::setValue(const ValueTypeT rVal00,
00232 const ValueTypeT rVal10,
00233 const ValueTypeT rVal20,
00234 const ValueTypeT rVal30,
00235
00236 const ValueTypeT rVal01,
00237 const ValueTypeT rVal11,
00238 const ValueTypeT rVal21,
00239 const ValueTypeT rVal31,
00240
00241 const ValueTypeT rVal02,
00242 const ValueTypeT rVal12,
00243 const ValueTypeT rVal22,
00244 const ValueTypeT rVal32,
00245
00246 const ValueTypeT rVal03,
00247 const ValueTypeT rVal13,
00248 const ValueTypeT rVal23,
00249 const ValueTypeT rVal33)
00250 {
00251 _matrix[0].setValues(rVal00, rVal01, rVal02, rVal03);
00252 _matrix[1].setValues(rVal10, rVal11, rVal12, rVal13);
00253 _matrix[2].setValues(rVal20, rVal21, rVal22, rVal23);
00254 _matrix[3].setValues(rVal30, rVal31, rVal32, rVal33);
00255 }
00256
00257 template<class ValueTypeT> inline
00258 void TransformationMatrix<ValueTypeT>::setValueTransposed(
00259 const ValueTypeT rVal00,
00260 const ValueTypeT rVal01,
00261 const ValueTypeT rVal02,
00262 const ValueTypeT rVal03,
00263
00264 const ValueTypeT rVal10,
00265 const ValueTypeT rVal11,
00266 const ValueTypeT rVal12,
00267 const ValueTypeT rVal13,
00268
00269 const ValueTypeT rVal20,
00270 const ValueTypeT rVal21,
00271 const ValueTypeT rVal22,
00272 const ValueTypeT rVal23,
00273
00274 const ValueTypeT rVal30,
00275 const ValueTypeT rVal31,
00276 const ValueTypeT rVal32,
00277 const ValueTypeT rVal33)
00278 {
00279 _matrix[0].setValues(rVal00, rVal01, rVal02, rVal03);
00280 _matrix[1].setValues(rVal10, rVal11, rVal12, rVal13);
00281 _matrix[2].setValues(rVal20, rVal21, rVal22, rVal23);
00282 _matrix[3].setValues(rVal30, rVal31, rVal32, rVal33);
00283 }
00284
00286
00287 template<class ValueTypeT> inline
00288 void TransformationMatrix<ValueTypeT>::setValue(const ValueTypeT *pMat,
00289 bool bTransposed)
00290 {
00291 const ValueTypeT *pTmpMat = pMat;
00292
00293 if(bTransposed == true)
00294 {
00295 for(UInt32 i = 0; i < 4; i++)
00296 {
00297 _matrix[i].setValue(pTmpMat);
00298
00299 pTmpMat += 4;
00300 }
00301 }
00302 else
00303 {
00304 for(UInt32 i = 0; i < 4; i++)
00305 {
00306 for(UInt32 j = 0; j < 4; j++)
00307 {
00308 _matrix[i][j] = pTmpMat[j * 4 + i];
00309 }
00310 }
00311 }
00312 }
00313
00315
00316 template<class ValueTypeT> inline
00317 void TransformationMatrix<ValueTypeT>::setValue(const VectorType *pMat)
00318 {
00319 for(UInt32 i = 0; i < 4; i++)
00320 {
00321 _matrix[i] = pMat[i];
00322 }
00323 }
00324
00325 #ifndef WIN32
00326
00328
00329 template<class ValueTypeT> inline
00330 void TransformationMatrix<ValueTypeT>::setValue(const VectorType3f *pMat)
00331 {
00332 for(UInt32 i = 0; i < 4; i++)
00333 {
00334 _matrix[i].setValue(pMat[i]);
00335 }
00336 }
00337
00338 #endif
00339
00344 template<class ValueTypeT> inline
00345 void TransformationMatrix<ValueTypeT>::setValue(const Char8 *str,
00346 bool bTransposed)
00347 {
00348 UInt32 i;
00349 UInt32 numOfToken = 16;
00350
00351 Char8 *c = const_cast<char*>(str);
00352
00353 Char8 *tokenC = 0;
00354 Char8 token[256];
00355
00356 ValueTypeT vec[16];
00357
00358 if( (str == NULL) ||
00359 (*str == '\0') )
00360 {
00361 setIdentity();
00362 return;
00363 }
00364
00365 for(i = 0; i < numOfToken; c++)
00366 {
00367 switch (*c)
00368 {
00369 case '\0':
00370 if (tokenC)
00371 {
00372 *tokenC = 0;
00373 vec[i++] = TypeTraits<ValueTypeT>::getFromCString(token);
00374
00375 }
00376
00377 while (i < numOfToken)
00378 {
00379 vec[i++] = TypeTraits<ValueTypeT>::getZeroElement();
00380 }
00381
00382 break;
00383 case ' ':
00384 case '\t':
00385 case '\n':
00386 case ',':
00387 if (tokenC)
00388 {
00389 *tokenC = 0;
00390 vec[i++] = TypeTraits<ValueTypeT>::getFromCString(token);
00391 tokenC = 0;
00392 }
00393 break;
00394 default:
00395 if (!tokenC)
00396 {
00397 tokenC = token;
00398 }
00399 *tokenC++ = *c;
00400 break;
00401 }
00402 }
00403
00404 if(bTransposed == true)
00405 {
00406 setValueTransposed(vec[0], vec[1], vec[2], vec[3],
00407 vec[4], vec[5], vec[6], vec[7],
00408 vec[8], vec[9], vec[10], vec[11],
00409 vec[12], vec[13], vec[14], vec[15]);
00410 }
00411 else
00412 {
00413 setValue(vec[0], vec[1], vec[2], vec[3],
00414 vec[4], vec[5], vec[6], vec[7],
00415 vec[8], vec[9], vec[10], vec[11],
00416 vec[12], vec[13], vec[14], vec[15]);
00417 }
00418 }
00419
00420
00421
00422
00424
00425 template<class ValueTypeT> inline
00426 ValueTypeT *TransformationMatrix<ValueTypeT>::getValues(void)
00427 {
00428 return _matrix[0].getValues();
00429 }
00430
00431 template<class ValueTypeT> inline
00432 const ValueTypeT *TransformationMatrix<ValueTypeT>::getValues(void) const
00433 {
00434 return _matrix[0].getValues();
00435 }
00436
00437
00438
00439
00440 template<class ValueTypeT> inline
00441 ValueTypeT TransformationMatrix<ValueTypeT>::rowMulCol4(
00442 const TransformationMatrix &gRowMat, UInt32 iRow,
00443 const TransformationMatrix &gColMat, UInt32 iColumn) const
00444 {
00445 return
00446 gRowMat[0][iRow] * gColMat[iColumn][0] +
00447 gRowMat[1][iRow] * gColMat[iColumn][1] +
00448 gRowMat[2][iRow] * gColMat[iColumn][2] +
00449 gRowMat[3][iRow] * gColMat[iColumn][3];
00450 }
00451
00452
00453 template<class ValueTypeT> inline
00454 ValueTypeT TransformationMatrix<ValueTypeT>::det2_calc(
00455 const ValueTypeT a1, const ValueTypeT a2,
00456 const ValueTypeT b1, const ValueTypeT b2) const
00457 {
00458 return (a1 * b2) - (a2 * b1);
00459 }
00460
00461 template<class ValueTypeT> inline
00462 ValueTypeT TransformationMatrix<ValueTypeT>::det3_calc(
00463 const ValueTypeT a1,
00464 const ValueTypeT a2,
00465 const ValueTypeT a3,
00466 const ValueTypeT b1,
00467 const ValueTypeT b2,
00468 const ValueTypeT b3,
00469 const ValueTypeT c1,
00470 const ValueTypeT c2,
00471 const ValueTypeT c3) const
00472 {
00473 return
00474 (a1 * b2 * c3) + (a2 * b3 * c1) + (a3 * b1 * c2) -
00475 (a1 * b3 * c2) - (a2 * b1 * c3) - (a3 * b2 * c1);
00476 }
00477
00478 #ifdef __sgi
00479 #pragma set woff 1424
00480 #endif
00481
00482 template<class ValueTypeT> inline
00483 bool TransformationMatrix<ValueTypeT>::jacobi(
00484 ValueTypeT evalues [JacobiRank],
00485 VectorType3f evectors[JacobiRank],
00486 Int32 &rots)
00487 {
00488 Real64 sm;
00489 Real64 theta;
00490 Real64 c, s, t;
00491 Real64 tau;
00492 Real64 h, g;
00493 Real64 thresh;
00494 Real64 b[JacobiRank];
00495 Real64 z[JacobiRank];
00496 UInt32 p, q, i, j;
00497 Real64 a[JacobiRank][JacobiRank];
00498
00499
00500 for (i = 0; i < JacobiRank; i++)
00501 {
00502 b[i] = evalues[i] = _matrix[i][i];
00503 z[i] = 0.0;
00504
00505 for (j = 0; j < JacobiRank; j++)
00506 {
00507 evectors[i][j] = (i == j) ? 1.0f : 0.0f;
00508 a[i][j] = _matrix[i][j];
00509 }
00510 }
00511
00512 rots = 0;
00513
00514 for(i = 0; i < 50; i++)
00515 {
00516 sm = 0.0;
00517
00518 for(p = 0; p < JacobiRank - 1; p++)
00519 {
00520 for(q = p+1; q < JacobiRank; q++)
00521 {
00522 sm += osgAbs(a[p][q]);
00523 }
00524 }
00525
00526 if (sm == 0.0)
00527 return false;
00528
00529 thresh = (i < 3 ?
00530 (.2 * sm / (JacobiRank * JacobiRank)) :
00531 0.0);
00532
00533 for (p = 0; p < JacobiRank - 1; p++)
00534 {
00535 for (q = p + 1; q < JacobiRank; q++)
00536 {
00537 g = 100.0 * osgAbs(a[p][q]);
00538
00539 if (i > 3 &&
00540 (osgAbs(evalues[p]) + g == osgAbs(evalues[p])) &&
00541 (osgAbs(evalues[q]) + g == osgAbs(evalues[q])))
00542 {
00543 a[p][q] = 0.0;
00544 }
00545 else if (osgAbs(a[p][q]) > thresh)
00546 {
00547 h = evalues[q] - evalues[p];
00548
00549 if (osgAbs(h) + g == osgAbs(h))
00550 {
00551 t = a[p][q] / h;
00552 }
00553 else
00554 {
00555 theta = .5 * h / a[p][q];
00556 t = 1.0 / (osgAbs(theta) + osgSqrt(1 + theta * theta));
00557 if (theta < 0.0) t = -t;
00558 }
00559
00560
00561 c = 1.0 / osgSqrt(1.0 + t * t);
00562 s = t * c;
00563
00564 tau = s / (1.0 + c);
00565 h = t * a[p][q];
00566
00567 z[p] -= h;
00568 z[q] += h;
00569
00570 evalues[p] -= ValueTypeT(h);
00571 evalues[q] += ValueTypeT(h);
00572
00573 a[p][q] = 0.0;
00574
00575 for (j = 0; j < p; j++)
00576 {
00577 g = a[j][p];
00578 h = a[j][q];
00579
00580 a[j][p] = g - s * (h + g * tau);
00581 a[j][q] = h + s * (g - h * tau);
00582 }
00583
00584 for (j = p+1; j < q; j++)
00585 {
00586 g = a[p][j];
00587 h = a[j][q];
00588
00589 a[p][j] = g - s * (h + g * tau);
00590 a[j][q] = h + s * (g - h * tau);
00591 }
00592
00593 for (j = q+1; j < JacobiRank; j++)
00594 {
00595 g = a[p][j];
00596 h = a[q][j];
00597
00598 a[p][j] = g - s * (h + g * tau);
00599 a[q][j] = h + s * (g - h * tau);
00600 }
00601
00602 for (j = 0; j < JacobiRank; j++)
00603 {
00604 g = evectors[j][p];
00605 h = evectors[j][q];
00606
00607 evectors[j][p] = ValueTypeT(g - s * (h + g * tau));
00608 evectors[j][q] = ValueTypeT(h + s * (g - h * tau));
00609 }
00610 }
00611 rots++;
00612 }
00613 }
00614 for (p = 0; p < JacobiRank; p++)
00615 {
00616 evalues[p] = ValueTypeT(b[p] += z[p]);
00617
00618 z[p] = 0;
00619 }
00620 }
00621
00622 return true;
00623 }
00624
00625 #ifdef __sgi
00626 #pragma reset woff 1424
00627 #endif
00628
00629
00630
00631
00633
00634 template<class ValueTypeT> inline
00635 void TransformationMatrix<ValueTypeT>::setScale(const ValueTypeT s)
00636 {
00637 _matrix[0][0] = s;
00638 _matrix[1][1] = s;
00639 _matrix[2][2] = s;
00640 }
00641
00643
00644 template<class ValueTypeT> inline
00645 void TransformationMatrix<ValueTypeT>::setScale(const ValueTypeT sx,
00646 const ValueTypeT sy,
00647 const ValueTypeT sz)
00648 {
00649 _matrix[0][0] = sx;
00650 _matrix[1][1] = sy;
00651 _matrix[2][2] = sz;
00652 }
00653
00655
00656 template<class ValueTypeT> inline
00657 void TransformationMatrix<ValueTypeT>::setScale(const VectorType3f &s)
00658 {
00659 _matrix[0][0] = s[0];
00660 _matrix[1][1] = s[1];
00661 _matrix[2][2] = s[2];
00662 }
00663
00665
00666 template<class ValueTypeT> inline
00667 void TransformationMatrix<ValueTypeT>::setTranslate(const ValueTypeT tx,
00668 const ValueTypeT ty,
00669 const ValueTypeT tz)
00670 {
00671 _matrix[3][0] = tx;
00672 _matrix[3][1] = ty;
00673 _matrix[3][2] = tz;
00674 }
00675
00677
00678 template<class ValueTypeT> inline
00679 void TransformationMatrix<ValueTypeT>::setTranslate(const VectorType3f &t)
00680 {
00681 _matrix[3].setValue(t);
00682 }
00683
00685
00686 template<class ValueTypeT> inline
00687 void TransformationMatrix<ValueTypeT>::setTranslate(const PointType3f &t)
00688 {
00689 _matrix[3].setValue(t);
00690 }
00691
00693
00694 template<class ValueTypeT> inline
00695 void TransformationMatrix<ValueTypeT>::setRotate(const QuaternionType &q)
00696 {
00697 q.getValuesOnly(*this);
00698 }
00699
00701
00702 template<class ValueTypeT> inline
00703 void TransformationMatrix<ValueTypeT>::setTransform(const VectorType3f &t)
00704 {
00705 setIdentity();
00706
00707 _matrix[3][0] = t[0];
00708 _matrix[3][1] = t[1];
00709 _matrix[3][2] = t[2];
00710
00711 }
00712
00714
00715 template<class ValueTypeT> inline
00716 void TransformationMatrix<ValueTypeT>::setTransform(const QuaternionType &r)
00717 {
00718
00719 r.getValue(*this);
00720 }
00721
00723
00724 template<class ValueTypeT> inline
00725 void TransformationMatrix<ValueTypeT>::setTransform(const VectorType3f &t,
00726 const QuaternionType &r)
00727 {
00728 r.getValuesOnly(*this);
00729
00730
00731 _matrix[0][3] = 0.0;
00732 _matrix[1][3] = 0.0;
00733 _matrix[2][3] = 0.0;
00734
00735 _matrix[3][0] = t[0];
00736 _matrix[3][1] = t[1];
00737 _matrix[3][2] = t[2];
00738 _matrix[3][3] = 1.0;
00739 }
00740
00742
00743 template<class ValueTypeT> inline
00744 void TransformationMatrix<ValueTypeT>::setTransform(const VectorType3f &t,
00745 const QuaternionType &r,
00746 const VectorType3f &s)
00747 {
00748 typedef TypeTraits<ValueTypeT> ValueTraits;
00749
00750
00751 r.getValuesOnly(*this);
00752
00753
00754 _matrix[0][0] *= s[0]; _matrix[0][1] *= s[0]; _matrix[0][2] *=s[0];
00755 _matrix[1][0] *= s[1]; _matrix[1][1] *= s[1]; _matrix[1][2] *=s[1];
00756 _matrix[2][0] *= s[2]; _matrix[2][1] *= s[2]; _matrix[2][2] *=s[2];
00757
00758 _matrix[0][3] = ValueTraits::getZeroElement();
00759 _matrix[1][3] = ValueTraits::getZeroElement();
00760 _matrix[2][3] = ValueTraits::getZeroElement();
00761
00762 _matrix[3][0] = t[0];
00763 _matrix[3][1] = t[1];
00764 _matrix[3][2] = t[2];
00765 _matrix[3][3] = ValueTraits::getOneElement();
00766 }
00767
00769
00770 template<class ValueTypeT> inline
00771 void TransformationMatrix<ValueTypeT>::setTransform(const VectorType3f &t,
00772 const QuaternionType &r,
00773 const VectorType3f &s,
00774 const QuaternionType &so)
00775 {
00776 Matrix tmpMat1;
00777 Matrix tmpMat2;
00778
00779
00780 QuaternionType rg(r);
00781 rg.mult(so);
00782
00783
00784 QuaternionType soi(so);
00785 soi.invert();
00786
00787
00788 rg. getValue(tmpMat1);
00789 soi.getValue(tmpMat2);
00790
00791
00792 tmpMat1[0][0] *= s[0]; tmpMat1[0][1] *= s[0]; tmpMat1[0][2] *=s[0];
00793 tmpMat1[1][0] *= s[1]; tmpMat1[1][1] *= s[1]; tmpMat1[1][2] *=s[1];
00794 tmpMat1[2][0] *= s[2]; tmpMat1[2][1] *= s[2]; tmpMat1[2][2] *=s[2];
00795
00796 _matrix[0][0] =
00797 tmpMat2[0][0] * tmpMat1[0][0] +
00798 tmpMat2[0][1] * tmpMat1[1][0] +
00799 tmpMat2[0][2] * tmpMat1[2][0];
00800
00801 _matrix[0][1] =
00802 tmpMat2[0][0] * tmpMat1[0][1] +
00803 tmpMat2[0][1] * tmpMat1[1][1] +
00804 tmpMat2[0][2] * tmpMat1[2][1];
00805
00806 _matrix[0][2] =
00807 tmpMat2[0][0] * tmpMat1[0][2] +
00808 tmpMat2[0][1] * tmpMat1[1][2] +
00809 tmpMat2[0][2] * tmpMat1[2][2];
00810
00811 _matrix[0][3] = 0.0;
00812
00813
00814 _matrix[1][0] =
00815 tmpMat2[1][0] * tmpMat1[0][0] +
00816 tmpMat2[1][1] * tmpMat1[1][0] +
00817 tmpMat2[1][2] * tmpMat1[2][0];
00818
00819 _matrix[1][1] =
00820 tmpMat2[1][0] * tmpMat1[0][1] +
00821 tmpMat2[1][1] * tmpMat1[1][1] +
00822 tmpMat2[1][2] * tmpMat1[2][1];
00823
00824 _matrix[1][2] =
00825 tmpMat2[1][0] * tmpMat1[0][2] +
00826 tmpMat2[1][1] * tmpMat1[1][2] +
00827 tmpMat2[1][2] * tmpMat1[2][2];
00828
00829 _matrix[1][3] = 0.0;
00830
00831
00832 _matrix[2][0] =
00833 tmpMat2[2][0] * tmpMat1[0][0] +
00834 tmpMat2[2][1] * tmpMat1[1][0] +
00835 tmpMat2[2][2] * tmpMat1[2][0];
00836
00837 _matrix[2][1] =
00838 tmpMat2[2][0] * tmpMat1[0][1] +
00839 tmpMat2[2][1] * tmpMat1[1][1] +
00840 tmpMat2[2][2] * tmpMat1[2][1];
00841
00842 _matrix[2][2] =
00843 tmpMat2[2][0] * tmpMat1[0][2] +
00844 tmpMat2[2][1] * tmpMat1[1][2] +
00845 tmpMat2[2][2] * tmpMat1[2][2];
00846
00847 _matrix[2][3] = 0.0;
00848
00849 _matrix[3][0] = t[0];
00850 _matrix[3][1] = t[1];
00851 _matrix[3][2] = t[2];
00852 _matrix[3][3] = 1.0;
00853 }
00854
00861 template<class ValueTypeT> inline
00862 void TransformationMatrix<ValueTypeT>::setTransform(
00863 const VectorType3f &translation,
00864 const QuaternionType &rotation,
00865 const VectorType3f &scaleFactor,
00866 const QuaternionType &scaleOrientation,
00867 const VectorType3f ¢er)
00868 {
00869 typedef TypeTraits<ValueTypeT> ValueTraits;
00870
00871 Matrixr tmpMat1;
00872 Matrixr tmpMat2;
00873
00874
00875 VectorType3f tg(translation);
00876 tg += center;
00877
00878
00879 QuaternionType rg(rotation);
00880 rg *= scaleOrientation;
00881
00882
00883 QuaternionType soi(scaleOrientation);
00884 soi.invert();
00885
00886
00887 rg. getValue(tmpMat1);
00888 soi.getValue(tmpMat2);
00889
00890
00891
00892 tmpMat1[0][0] *= scaleFactor[0];
00893 tmpMat1[0][1] *= scaleFactor[0];
00894 tmpMat1[0][2] *= scaleFactor[0];
00895
00896 tmpMat1[1][0] *= scaleFactor[1];
00897 tmpMat1[1][1] *= scaleFactor[1];
00898 tmpMat1[1][2] *= scaleFactor[1];
00899
00900 tmpMat1[2][0] *= scaleFactor[2];
00901 tmpMat1[2][1] *= scaleFactor[2];
00902 tmpMat1[2][2] *= scaleFactor[2];
00903
00904 _matrix[0][0] =
00905 tmpMat2[0][0] * tmpMat1[0][0] +
00906 tmpMat2[0][1] * tmpMat1[1][0] +
00907 tmpMat2[0][2] * tmpMat1[2][0];
00908
00909 _matrix[0][1] =
00910 tmpMat2[0][0] * tmpMat1[0][1] +
00911 tmpMat2[0][1] * tmpMat1[1][1] +
00912 tmpMat2[0][2] * tmpMat1[2][1];
00913
00914 _matrix[0][2] =
00915 tmpMat2[0][0] * tmpMat1[0][2] +
00916 tmpMat2[0][1] * tmpMat1[1][2] +
00917 tmpMat2[0][2] * tmpMat1[2][2];
00918
00919 _matrix[0][3] = ValueTraits::getZeroElement();
00920
00921
00922 _matrix[1][0] =
00923 tmpMat2[1][0] * tmpMat1[0][0] +
00924 tmpMat2[1][1] * tmpMat1[1][0] +
00925 tmpMat2[1][2] * tmpMat1[2][0];
00926
00927 _matrix[1][1] =
00928 tmpMat2[1][0] * tmpMat1[0][1] +
00929 tmpMat2[1][1] * tmpMat1[1][1] +
00930 tmpMat2[1][2] * tmpMat1[2][1];
00931
00932 _matrix[1][2] =
00933 tmpMat2[1][0] * tmpMat1[0][2] +
00934 tmpMat2[1][1] * tmpMat1[1][2] +
00935 tmpMat2[1][2] * tmpMat1[2][2];
00936
00937 _matrix[1][3] = ValueTraits::getZeroElement();
00938
00939
00940 _matrix[2][0] =
00941 tmpMat2[2][0] * tmpMat1[0][0] +
00942 tmpMat2[2][1] * tmpMat1[1][0] +
00943 tmpMat2[2][2] * tmpMat1[2][0];
00944
00945 _matrix[2][1] =
00946 tmpMat2[2][0] * tmpMat1[0][1] +
00947 tmpMat2[2][1] * tmpMat1[1][1] +
00948 tmpMat2[2][2] * tmpMat1[2][1];
00949
00950 _matrix[2][2] =
00951 tmpMat2[2][0] * tmpMat1[0][2] +
00952 tmpMat2[2][1] * tmpMat1[1][2] +
00953 tmpMat2[2][2] * tmpMat1[2][2];
00954
00955 _matrix[2][3] = ValueTraits::getZeroElement();
00956
00957
00958 _matrix[3][0] =
00959 _matrix[0][0] * -center[0] +
00960 _matrix[1][0] * -center[1] +
00961 _matrix[2][0] * -center[2] + tg[0];
00962
00963 _matrix[3][1] =
00964 _matrix[0][1] * -center[0] +
00965 _matrix[1][1] * -center[1] +
00966 _matrix[2][1] * -center[2] + tg[1];
00967
00968 _matrix[3][2] =
00969 _matrix[0][2] * -center[0] +
00970 _matrix[1][2] * -center[1] +
00971 _matrix[2][2] * -center[2] + tg[2];
00972
00973 _matrix[3][3] = ValueTraits::getOneElement();
00974
00975 }
00976
00977
00978
00979
00989 template<class ValueTypeT> inline
00990 void TransformationMatrix<ValueTypeT>::getTransform(
00991 VectorType3f &translation,
00992 QuaternionType &rotation,
00993 VectorType3f &scaleFactor,
00994 QuaternionType &scaleOrientation,
00995 const VectorType3f ¢er ) const
00996 {
00997 TransformationMatrix m;
00998 TransformationMatrix c;
00999
01000 m.setTranslate(-center);
01001
01002 m.mult(*this);
01003
01004 c.setTranslate(center);
01005
01006 m.mult(c);
01007
01008 m.getTransform(translation, rotation, scaleFactor, scaleOrientation);
01009 }
01010
01012
01013 template<class ValueTypeT> inline
01014 void TransformationMatrix<ValueTypeT>::getTransform(
01015 VectorType3f &translation,
01016 QuaternionType &rotation,
01017 VectorType3f &scaleFactor,
01018 QuaternionType &scaleOrientation) const
01019 {
01020 TransformationMatrix so;
01021 TransformationMatrix rot;
01022 TransformationMatrix proj;
01023
01024 this->factor(so, scaleFactor, rot, translation, proj);
01025
01026 so.transpose();
01027 scaleOrientation.setValue(so);
01028
01029
01030 rotation.setValue(rot);
01031
01032 }
01033
01040 template<class ValueTypeT> inline
01041 bool TransformationMatrix<ValueTypeT>::factor(TransformationMatrix &r,
01042 VectorType3f &s,
01043 TransformationMatrix &u,
01044 VectorType3f &t,
01045 TransformationMatrix &proj) const
01046 {
01047 Real64 det;
01048 Real64 det_sign;
01049 Real64 scratch;
01050 Int32 i;
01051 Int32 j;
01052 Int32 junk;
01053 TransformationMatrix a;
01054 TransformationMatrix aT;
01055 TransformationMatrix rT;
01056 TransformationMatrix b;
01057 TransformationMatrix si;
01058 ValueTypeT evalues [3];
01059 VectorType3f evectors[3];
01060
01061 a = *this;
01062
01063 proj.setIdentity();
01064
01065 scratch = 1.0;
01066
01067 for (i = 0; i < 3; i++)
01068 {
01069 for (j = 0; j < 3; j++)
01070 {
01071 a._matrix[i][j] *= ValueTypeT(scratch);
01072 }
01073
01074 t[i] = _matrix[3][i] * ValueTypeT(scratch);
01075
01076 a._matrix[3][i] = a._matrix[i][3] = 0.0;
01077 }
01078
01079 a._matrix[3][3] = 1.0;
01080
01081
01082
01083 det = a.det3();
01084
01085 det_sign = (det < 0.0 ? -1.0 : 1.0);
01086
01087 if(det_sign * det < 1e-12)
01088 return false;
01089
01090
01091
01092 aT.transposeFrom(a);
01093 b = a;
01094 b.mult(aT);
01095
01096 b.jacobi(evalues, evectors, junk);
01097
01098 r.setValue(evectors[0][0], evectors[0][1], evectors[0][2], 0.0,
01099 evectors[1][0], evectors[1][1], evectors[1][2], 0.0,
01100 evectors[2][0], evectors[2][1], evectors[2][2], 0.0,
01101 0.0, 0.0, 0.0, 1.0);
01102
01103
01104
01105 si.setIdentity();
01106
01107 for(i = 0; i < 3; i++)
01108 {
01109 s[i] = ValueTypeT(det_sign * osgSqrt(evalues[i]));
01110
01111 si._matrix[i][i] = 1.0f / s[i];
01112 }
01113
01114
01115
01116 rT.transposeFrom(r);
01117 u = r;
01118 u.mult(si);
01119 u.mult(rT);
01120 u.mult(a);
01121
01122 return true;
01123 }
01124
01125
01126
01131 template<class ValueTypeT> inline
01132 void TransformationMatrix<ValueTypeT>::multMatrixPnt(
01133 const PointType3f &src,
01134 PointType3f &dst) const
01135 {
01136 dst.setValues((src[0] * _matrix[0][0] +
01137 src[1] * _matrix[1][0] +
01138 src[2] * _matrix[2][0] +
01139 _matrix[3][0]),
01140 (src[0] * _matrix[0][1] +
01141 src[1] * _matrix[1][1] +
01142 src[2] * _matrix[2][1] +
01143 _matrix[3][1]),
01144 (src[0] * _matrix[0][2] +
01145 src[1] * _matrix[1][2] +
01146 src[2] * _matrix[2][2] +
01147 _matrix[3][2]));
01148 }
01149
01151
01152 template<class ValueTypeT> inline
01153 void TransformationMatrix<ValueTypeT>::multMatrixPnt(PointType3f &pnt) const
01154 {
01155 multMatrixPnt(pnt, pnt);
01156 }
01157
01162 template<class ValueTypeT> inline
01163 void TransformationMatrix<ValueTypeT>::multFullMatrixPnt(
01164 const PointType3f &src,
01165 PointType3f &dst) const
01166 {
01167 ValueTypeT w = src[0] * _matrix[0][3] +
01168 src[1] * _matrix[1][3] +
01169 src[2] * _matrix[2][3] +
01170 _matrix[3][3];
01171
01172 if(w == TypeTraits<ValueType>::getZeroElement())
01173 {
01174 SINFO << "multFullMatrixPnt: w == 0.0f!" << std::endl;
01175
01176 dst.setValues(TypeTraits<ValueType>::getZeroElement(),
01177 TypeTraits<ValueType>::getZeroElement(),
01178 TypeTraits<ValueType>::getZeroElement());
01179
01180 return;
01181 }
01182
01183
01184 w = ValueTypeT(1.f)/w;
01185 dst.setValues((src[0] * _matrix[0][0] +
01186 src[1] * _matrix[1][0] +
01187 src[2] * _matrix[2][0] +
01188 _matrix[3][0]) * w,
01189 (src[0] * _matrix[0][1] +
01190 src[1] * _matrix[1][1] +
01191 src[2] * _matrix[2][1] +
01192 _matrix[3][1]) * w,
01193 (src[0] * _matrix[0][2] +
01194 src[1] * _matrix[1][2] +
01195 src[2] * _matrix[2][2] +
01196 _matrix[3][2]) * w);
01197 }
01198
01200
01201 template<class ValueTypeT> inline
01202 void TransformationMatrix<ValueTypeT>::multFullMatrixPnt(PointType3f &pnt)const
01203 {
01204 multFullMatrixPnt(pnt, pnt);
01205 }
01206
01211 template<class ValueTypeT> inline
01212 void TransformationMatrix<ValueTypeT>::multMatrixVec(
01213 const VectorType3f &src,
01214 VectorType3f &dst) const
01215 {
01216 dst.setValues((src[0] * _matrix[0][0] +
01217 src[1] * _matrix[1][0] +
01218 src[2] * _matrix[2][0]),
01219 (src[0] * _matrix[0][1] +
01220 src[1] * _matrix[1][1] +
01221 src[2] * _matrix[2][1]),
01222 (src[0] * _matrix[0][2] +
01223 src[1] * _matrix[1][2] +
01224 src[2] * _matrix[2][2]));
01225 }
01226
01227
01229
01230 template<class ValueTypeT> inline
01231 void TransformationMatrix<ValueTypeT>::multMatrixVec(VectorType3f &vec) const
01232 {
01233 multMatrixVec(vec, vec);
01234 }
01235
01240 template<class ValueTypeT> inline
01241 void TransformationMatrix<ValueTypeT>::multMatrixVec(
01242 const VectorType &src,
01243 VectorType &dst) const
01244 {
01245 dst.setValues((src[0] * _matrix[0][0] +
01246 src[1] * _matrix[1][0] +
01247 src[2] * _matrix[2][0] +
01248 src[3] * _matrix[3][0]),
01249 (src[0] * _matrix[0][1] +
01250 src[1] * _matrix[1][1] +
01251 src[2] * _matrix[2][1] +
01252 src[3] * _matrix[3][1]),
01253 (src[0] * 