00001 /* 00002 ----------------------------------------------------------------------------- 00003 This source file is part of OGRE 00004 (Object-oriented Graphics Rendering Engine) 00005 For the latest info, see http://www.ogre3d.org/ 00006 00007 Copyright © 2000-2002 The OGRE Team 00008 Also see acknowledgements in Readme.html 00009 00010 This program is free software; you can redistribute it and/or modify it under 00011 the terms of the GNU Lesser General Public License as published by the Free Software 00012 Foundation; either version 2 of the License, or (at your option) any later 00013 version. 00014 00015 This program is distributed in the hope that it will be useful, but WITHOUT 00016 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 00017 FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. 00018 00019 You should have received a copy of the GNU Lesser General Public License along with 00020 this program; if not, write to the Free Software Foundation, Inc., 59 Temple 00021 Place - Suite 330, Boston, MA 02111-1307, USA, or go to 00022 http://www.gnu.org/copyleft/lesser.txt. 00023 ----------------------------------------------------------------------------- 00024 */ 00025 #include "OgreStableHeaders.h" 00026 #include "OgreMatrix3.h" 00027 00028 #include "OgreMath.h" 00029 00030 // Adapted from Matrix math by Wild Magic http://www.magic-software.com 00031 00032 namespace Ogre 00033 { 00034 const Real Matrix3::EPSILON = 1e-06; 00035 const Matrix3 Matrix3::ZERO(0,0,0,0,0,0,0,0,0); 00036 const Matrix3 Matrix3::IDENTITY(1,0,0,0,1,0,0,0,1); 00037 const Real Matrix3::ms_fSvdEpsilon = 1e-04; 00038 const unsigned int Matrix3::ms_iSvdMaxIterations = 32; 00039 00040 //----------------------------------------------------------------------- 00041 Vector3 Matrix3::GetColumn (size_t iCol) const 00042 { 00043 assert( 0 <= iCol && iCol < 3 ); 00044 return Vector3(m[0][iCol],m[1][iCol], 00045 m[2][iCol]); 00046 } 00047 //----------------------------------------------------------------------- 00048 void Matrix3::SetColumn(size_t iCol, const Vector3& vec) 00049 { 00050 assert( 0 <= iCol && iCol < 3 ); 00051 m[0][iCol] = vec.x; 00052 m[1][iCol] = vec.y; 00053 m[2][iCol] = vec.z; 00054 00055 } 00056 //----------------------------------------------------------------------- 00057 void Matrix3::FromAxes(const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis) 00058 { 00059 SetColumn(0,xAxis); 00060 SetColumn(1,yAxis); 00061 SetColumn(2,zAxis); 00062 00063 } 00064 00065 //----------------------------------------------------------------------- 00066 bool Matrix3::operator== (const Matrix3& rkMatrix) const 00067 { 00068 for (size_t iRow = 0; iRow < 3; iRow++) 00069 { 00070 for (size_t iCol = 0; iCol < 3; iCol++) 00071 { 00072 if ( m[iRow][iCol] != rkMatrix.m[iRow][iCol] ) 00073 return false; 00074 } 00075 } 00076 00077 return true; 00078 } 00079 //----------------------------------------------------------------------- 00080 Matrix3 Matrix3::operator+ (const Matrix3& rkMatrix) const 00081 { 00082 Matrix3 kSum; 00083 for (size_t iRow = 0; iRow < 3; iRow++) 00084 { 00085 for (size_t iCol = 0; iCol < 3; iCol++) 00086 { 00087 kSum.m[iRow][iCol] = m[iRow][iCol] + 00088 rkMatrix.m[iRow][iCol]; 00089 } 00090 } 00091 return kSum; 00092 } 00093 //----------------------------------------------------------------------- 00094 Matrix3 Matrix3::operator- (const Matrix3& rkMatrix) const 00095 { 00096 Matrix3 kDiff; 00097 for (size_t iRow = 0; iRow < 3; iRow++) 00098 { 00099 for (size_t iCol = 0; iCol < 3; iCol++) 00100 { 00101 kDiff.m[iRow][iCol] = m[iRow][iCol] - 00102 rkMatrix.m[iRow][iCol]; 00103 } 00104 } 00105 return kDiff; 00106 } 00107 //----------------------------------------------------------------------- 00108 Matrix3 Matrix3::operator* (const Matrix3& rkMatrix) const 00109 { 00110 Matrix3 kProd; 00111 for (size_t iRow = 0; iRow < 3; iRow++) 00112 { 00113 for (size_t iCol = 0; iCol < 3; iCol++) 00114 { 00115 kProd.m[iRow][iCol] = 00116 m[iRow][0]*rkMatrix.m[0][iCol] + 00117 m[iRow][1]*rkMatrix.m[1][iCol] + 00118 m[iRow][2]*rkMatrix.m[2][iCol]; 00119 } 00120 } 00121 return kProd; 00122 } 00123 //----------------------------------------------------------------------- 00124 Vector3 Matrix3::operator* (const Vector3& rkPoint) const 00125 { 00126 Vector3 kProd; 00127 for (size_t iRow = 0; iRow < 3; iRow++) 00128 { 00129 kProd[iRow] = 00130 m[iRow][0]*rkPoint[0] + 00131 m[iRow][1]*rkPoint[1] + 00132 m[iRow][2]*rkPoint[2]; 00133 } 00134 return kProd; 00135 } 00136 //----------------------------------------------------------------------- 00137 Vector3 operator* (const Vector3& rkPoint, const Matrix3& rkMatrix) 00138 { 00139 Vector3 kProd; 00140 for (size_t iRow = 0; iRow < 3; iRow++) 00141 { 00142 kProd[iRow] = 00143 rkPoint[0]*rkMatrix.m[0][iRow] + 00144 rkPoint[1]*rkMatrix.m[1][iRow] + 00145 rkPoint[2]*rkMatrix.m[2][iRow]; 00146 } 00147 return kProd; 00148 } 00149 //----------------------------------------------------------------------- 00150 Matrix3 Matrix3::operator- () const 00151 { 00152 Matrix3 kNeg; 00153 for (size_t iRow = 0; iRow < 3; iRow++) 00154 { 00155 for (size_t iCol = 0; iCol < 3; iCol++) 00156 kNeg[iRow][iCol] = -m[iRow][iCol]; 00157 } 00158 return kNeg; 00159 } 00160 //----------------------------------------------------------------------- 00161 Matrix3 Matrix3::operator* (Real fScalar) const 00162 { 00163 Matrix3 kProd; 00164 for (size_t iRow = 0; iRow < 3; iRow++) 00165 { 00166 for (size_t iCol = 0; iCol < 3; iCol++) 00167 kProd[iRow][iCol] = fScalar*m[iRow][iCol]; 00168 } 00169 return kProd; 00170 } 00171 //----------------------------------------------------------------------- 00172 Matrix3 operator* (Real fScalar, const Matrix3& rkMatrix) 00173 { 00174 Matrix3 kProd; 00175 for (size_t iRow = 0; iRow < 3; iRow++) 00176 { 00177 for (size_t iCol = 0; iCol < 3; iCol++) 00178 kProd[iRow][iCol] = fScalar*rkMatrix.m[iRow][iCol]; 00179 } 00180 return kProd; 00181 } 00182 //----------------------------------------------------------------------- 00183 Matrix3 Matrix3::Transpose () const 00184 { 00185 Matrix3 kTranspose; 00186 for (size_t iRow = 0; iRow < 3; iRow++) 00187 { 00188 for (size_t iCol = 0; iCol < 3; iCol++) 00189 kTranspose[iRow][iCol] = m[iCol][iRow]; 00190 } 00191 return kTranspose; 00192 } 00193 //----------------------------------------------------------------------- 00194 bool Matrix3::Inverse (Matrix3& rkInverse, Real fTolerance) const 00195 { 00196 // Invert a 3x3 using cofactors. This is about 8 times faster than 00197 // the Numerical Recipes code which uses Gaussian elimination. 00198 00199 rkInverse[0][0] = m[1][1]*m[2][2] - 00200 m[1][2]*m[2][1]; 00201 rkInverse[0][1] = m[0][2]*m[2][1] - 00202 m[0][1]*m[2][2]; 00203 rkInverse[0][2] = m[0][1]*m[1][2] - 00204 m[0][2]*m[1][1]; 00205 rkInverse[1][0] = m[1][2]*m[2][0] - 00206 m[1][0]*m[2][2]; 00207 rkInverse[1][1] = m[0][0]*m[2][2] - 00208 m[0][2]*m[2][0]; 00209 rkInverse[1][2] = m[0][2]*m[1][0] - 00210 m[0][0]*m[1][2]; 00211 rkInverse[2][0] = m[1][0]*m[2][1] - 00212 m[1][1]*m[2][0]; 00213 rkInverse[2][1] = m[0][1]*m[2][0] - 00214 m[0][0]*m[2][1]; 00215 rkInverse[2][2] = m[0][0]*m[1][1] - 00216 m[0][1]*m[1][0]; 00217 00218 Real fDet = 00219 m[0][0]*rkInverse[0][0] + 00220 m[0][1]*rkInverse[1][0]+ 00221 m[0][2]*rkInverse[2][0]; 00222 00223 if ( Math::Abs(fDet) <= fTolerance ) 00224 return false; 00225 00226 Real fInvDet = 1.0/fDet; 00227 for (size_t iRow = 0; iRow < 3; iRow++) 00228 { 00229 for (size_t iCol = 0; iCol < 3; iCol++) 00230 rkInverse[iRow][iCol] *= fInvDet; 00231 } 00232 00233 return true; 00234 } 00235 //----------------------------------------------------------------------- 00236 Matrix3 Matrix3::Inverse (Real fTolerance) const 00237 { 00238 Matrix3 kInverse = Matrix3::ZERO; 00239 Inverse(kInverse,fTolerance); 00240 return kInverse; 00241 } 00242 //----------------------------------------------------------------------- 00243 Real Matrix3::Determinant () const 00244 { 00245 Real fCofactor00 = m[1][1]*m[2][2] - 00246 m[1][2]*m[2][1]; 00247 Real fCofactor10 = m[1][2]*m[2][0] - 00248 m[1][0]*m[2][2]; 00249 Real fCofactor20 = m[1][0]*m[2][1] - 00250 m[1][1]*m[2][0]; 00251 00252 Real fDet = 00253 m[0][0]*fCofactor00 + 00254 m[0][1]*fCofactor10 + 00255 m[0][2]*fCofactor20; 00256 00257 return fDet; 00258 } 00259 //----------------------------------------------------------------------- 00260 void Matrix3::Bidiagonalize (Matrix3& kA, Matrix3& kL, 00261 Matrix3& kR) 00262 { 00263 Real afV[3], afW[3]; 00264 Real fLength, fSign, fT1, fInvT1, fT2; 00265 bool bIdentity; 00266 00267 // map first column to (*,0,0) 00268 fLength = Math::Sqrt(kA[0][0]*kA[0][0] + kA[1][0]*kA[1][0] + 00269 kA[2][0]*kA[2][0]); 00270 if ( fLength > 0.0 ) 00271 { 00272 fSign = (kA[0][0] > 0.0 ? 1.0 : -1.0); 00273 fT1 = kA[0][0] + fSign*fLength; 00274 fInvT1 = 1.0/fT1; 00275 afV[1] = kA[1][0]*fInvT1; 00276 afV[2] = kA[2][0]*fInvT1; 00277 00278 fT2 = -2.0/(1.0+afV[1]*afV[1]+afV[2]*afV[2]); 00279 afW[0] = fT2*(kA[0][0]+kA[1][0]*afV[1]+kA[2][0]*afV[2]); 00280 afW[1] = fT2*(kA[0][1]+kA[1][1]*afV[1]+kA[2][1]*afV[2]); 00281 afW[2] = fT2*(kA[0][2]+kA[1][2]*afV[1]+kA[2][2]*afV[2]); 00282 kA[0][0] += afW[0]; 00283 kA[0][1] += afW[1]; 00284 kA[0][2] += afW[2]; 00285 kA[1][1] += afV[1]*afW[1]; 00286 kA[1][2] += afV[1]*afW[2]; 00287 kA[2][1] += afV[2]*afW[1]; 00288 kA[2][2] += afV[2]*afW[2]; 00289 00290 kL[0][0] = 1.0+fT2; 00291 kL[0][1] = kL[1][0] = fT2*afV[1]; 00292 kL[0][2] = kL[2][0] = fT2*afV[2]; 00293 kL[1][1] = 1.0+fT2*afV[1]*afV[1]; 00294 kL[1][2] = kL[2][1] = fT2*afV[1]*afV[2]; 00295 kL[2][2] = 1.0+fT2*afV[2]*afV[2]; 00296 bIdentity = false; 00297 } 00298 else 00299 { 00300 kL = Matrix3::IDENTITY; 00301 bIdentity = true; 00302 } 00303 00304 // map first row to (*,*,0) 00305 fLength = Math::Sqrt(kA[0][1]*kA[0][1]+kA[0][2]*kA[0][2]); 00306 if ( fLength > 0.0 ) 00307 { 00308 fSign = (kA[0][1] > 0.0 ? 1.0 : -1.0); 00309 fT1 = kA[0][1] + fSign*fLength; 00310 afV[2] = kA[0][2]/fT1; 00311 00312 fT2 = -2.0/(1.0+afV[2]*afV[2]); 00313 afW[0] = fT2*(kA[0][1]+kA[0][2]*afV[2]); 00314 afW[1] = fT2*(kA[1][1]+kA[1][2]*afV[2]); 00315 afW[2] = fT2*(kA[2][1]+kA[2][2]*afV[2]); 00316 kA[0][1] += afW[0]; 00317 kA[1][1] += afW[1]; 00318 kA[1][2] += afW[1]*afV[2]; 00319 kA[2][1] += afW[2]; 00320 kA[2][2] += afW[2]*afV[2]; 00321 00322 kR[0][0] = 1.0; 00323 kR[0][1] = kR[1][0] = 0.0; 00324 kR[0][2] = kR[2][0] = 0.0; 00325 kR[1][1] = 1.0+fT2; 00326 kR[1][2] = kR[2][1] = fT2*afV[2]; 00327 kR[2][2] = 1.0+fT2*afV[2]*afV[2]; 00328 } 00329 else 00330 { 00331 kR = Matrix3::IDENTITY; 00332 } 00333 00334 // map second column to (*,*,0) 00335 fLength = Math::Sqrt(kA[1][1]*kA[1][1]+kA[2][1]*kA[2][1]); 00336 if ( fLength > 0.0 ) 00337 { 00338 fSign = (kA[1][1] > 0.0 ? 1.0 : -1.0); 00339 fT1 = kA[1][1] + fSign*fLength; 00340 afV[2] = kA[2][1]/fT1; 00341 00342 fT2 = -2.0/(1.0+afV[2]*afV[2]); 00343 afW[1] = fT2*(kA[1][1]+kA[2][1]*afV[2]); 00344 afW[2] = fT2*(kA[1][2]+kA[2][2]*afV[2]); 00345 kA[1][1] += afW[1]; 00346 kA[1][2] += afW[2]; 00347 kA[2][2] += afV[2]*afW[2]; 00348 00349 Real fA = 1.0+fT2; 00350 Real fB = fT2*afV[2]; 00351 Real fC = 1.0+fB*afV[2]; 00352 00353 if ( bIdentity ) 00354 { 00355 kL[0][0] = 1.0; 00356 kL[0][1] = kL[1][0] = 0.0; 00357 kL[0][2] = kL[2][0] = 0.0; 00358 kL[1][1] = fA; 00359 kL[1][2] = kL[2][1] = fB; 00360 kL[2][2] = fC; 00361 } 00362 else 00363 { 00364 for (int iRow = 0; iRow < 3; iRow++) 00365 { 00366 Real fTmp0 = kL[iRow][1]; 00367 Real fTmp1 = kL[iRow][2]; 00368 kL[iRow][1] = fA*fTmp0+fB*fTmp1; 00369 kL[iRow][2] = fB*fTmp0+fC*fTmp1; 00370 } 00371 } 00372 } 00373 } 00374 //----------------------------------------------------------------------- 00375 void Matrix3::GolubKahanStep (Matrix3& kA, Matrix3& kL, 00376 Matrix3& kR) 00377 { 00378 Real fT11 = kA[0][1]*kA[0][1]+kA[1][1]*kA[1][1]; 00379 Real fT22 = kA[1][2]*kA[1][2]+kA[2][2]*kA[2][2]; 00380 Real fT12 = kA[1][1]*kA[1][2]; 00381 Real fTrace = fT11+fT22; 00382 Real fDiff = fT11-fT22; 00383 Real fDiscr = Math::Sqrt(fDiff*fDiff+4.0*fT12*fT12); 00384 Real fRoot1 = 0.5*(fTrace+fDiscr); 00385 Real fRoot2 = 0.5*(fTrace-fDiscr); 00386 00387 // adjust right 00388 Real fY = kA[0][0] - (Math::Abs(fRoot1-fT22) <= 00389 Math::Abs(fRoot2-fT22) ? fRoot1 : fRoot2); 00390 Real fZ = kA[0][1]; 00391 Real fInvLength = Math::InvSqrt(fY*fY+fZ*fZ); 00392 Real fSin = fZ*fInvLength; 00393 Real fCos = -fY*fInvLength; 00394 00395 Real fTmp0 = kA[0][0]; 00396 Real fTmp1 = kA[0][1]; 00397 kA[0][0] = fCos*fTmp0-fSin*fTmp1; 00398 kA[0][1] = fSin*fTmp0+fCos*fTmp1; 00399 kA[1][0] = -fSin*kA[1][1]; 00400 kA[1][1] *= fCos; 00401 00402 size_t iRow; 00403 for (iRow = 0; iRow < 3; iRow++) 00404 { 00405 fTmp0 = kR[0][iRow]; 00406 fTmp1 = kR[1][iRow]; 00407 kR[0][iRow] = fCos*fTmp0-fSin*fTmp1; 00408 kR[1][iRow] = fSin*fTmp0+fCos*fTmp1; 00409 } 00410 00411 // adjust left 00412 fY = kA[0][0]; 00413 fZ = kA[1][0]; 00414 fInvLength = Math::InvSqrt(fY*fY+fZ*fZ); 00415 fSin = fZ*fInvLength; 00416 fCos = -fY*fInvLength; 00417 00418 kA[0][0] = fCos*kA[0][0]-fSin*kA[1][0]; 00419 fTmp0 = kA[0][1]; 00420 fTmp1 = kA[1][1]; 00421 kA[0][1] = fCos*fTmp0-fSin*fTmp1; 00422 kA[1][1] = fSin*fTmp0+fCos*fTmp1; 00423 kA[0][2] = -fSin*kA[1][2]; 00424 kA[1][2] *= fCos; 00425 00426 size_t iCol; 00427 for (iCol = 0; iCol < 3; iCol++) 00428 { 00429 fTmp0 = kL[iCol][0]; 00430 fTmp1 = kL[iCol][1]; 00431 kL[iCol][0] = fCos*fTmp0-fSin*fTmp1; 00432 kL[iCol][1] = fSin*fTmp0+fCos*fTmp1; 00433 } 00434 00435 // adjust right 00436 fY = kA[0][1]; 00437 fZ = kA[0][2]; 00438 fInvLength = Math::InvSqrt(fY*fY+fZ*fZ); 00439 fSin = fZ*fInvLength; 00440 fCos = -fY*fInvLength; 00441 00442 kA[0][1] = fCos*kA[0][1]-fSin*kA[0][2]; 00443 fTmp0 = kA[1][1]; 00444 fTmp1 = kA[1][2]; 00445 kA[1][1] = fCos*fTmp0-fSin*fTmp1; 00446 kA[1][2] = fSin*fTmp0+fCos*fTmp1; 00447 kA[2][1] = -fSin*kA[2][2]; 00448 kA[2][2] *= fCos; 00449 00450 for (iRow = 0; iRow < 3; iRow++) 00451 { 00452 fTmp0 = kR[1][iRow]; 00453 fTmp1 = kR[2][iRow]; 00454 kR[1][iRow] = fCos*fTmp0-fSin*fTmp1; 00455 kR[2][iRow] = fSin*fTmp0+fCos*fTmp1; 00456 } 00457 00458 // adjust left 00459 fY = kA[1][1]; 00460 fZ = kA[2][1]; 00461 fInvLength = Math::InvSqrt(fY*fY+fZ*fZ); 00462 fSin = fZ*fInvLength; 00463 fCos = -fY*fInvLength; 00464 00465 kA[1][1] = fCos*kA[1][1]-fSin*kA[2][1]; 00466 fTmp0 = kA[1][2]; 00467 fTmp1 = kA[2][2]; 00468 kA[1][2] = fCos*fTmp0-fSin*fTmp1; 00469 kA[2][2] = fSin*fTmp0+fCos*fTmp1; 00470 00471 for (iCol = 0; iCol < 3; iCol++) 00472 { 00473 fTmp0 = kL[iCol][1]; 00474 fTmp1 = kL[iCol][2]; 00475 kL[iCol][1] = fCos*fTmp0-fSin*fTmp1; 00476 kL[iCol][2] = fSin*fTmp0+fCos*fTmp1; 00477 } 00478 } 00479 //----------------------------------------------------------------------- 00480 void Matrix3::SingularValueDecomposition (Matrix3& kL, Vector3& kS, 00481 Matrix3& kR) const 00482 { 00483 // temas: currently unused 00484 //const int iMax = 16; 00485 size_t iRow, iCol; 00486 00487 Matrix3 kA = *this; 00488 Bidiagonalize(kA,kL,kR); 00489 00490 for (unsigned int i = 0; i < ms_iSvdMaxIterations; i++) 00491 { 00492 Real fTmp, fTmp0, fTmp1; 00493 Real fSin0, fCos0, fTan0; 00494 Real fSin1, fCos1, fTan1; 00495 00496 bool bTest1 = (Math::Abs(kA[0][1]) <= 00497 ms_fSvdEpsilon*(Math::Abs(kA[0][0])+Math::Abs(kA[1][1]))); 00498 bool bTest2 = (Math::Abs(kA[1][2]) <= 00499 ms_fSvdEpsilon*(Math::Abs(kA[1][1])+Math::Abs(kA[2][2]))); 00500 if ( bTest1 ) 00501 { 00502 if ( bTest2 ) 00503 { 00504 kS[0] = kA[0][0]; 00505 kS[1] = kA[1][1]; 00506 kS[2] = kA[2][2]; 00507 break; 00508 } 00509 else 00510 { 00511 // 2x2 closed form factorization 00512 fTmp = (kA[1][1]*kA[1][1] - kA[2][2]*kA[2][2] + 00513 kA[1][2]*kA[1][2])/(kA[1][2]*kA[2][2]); 00514 fTan0 = 0.5*(fTmp+Math::Sqrt(fTmp*fTmp + 4.0)); 00515 fCos0 = Math::InvSqrt(1.0+fTan0*fTan0); 00516 fSin0 = fTan0*fCos0; 00517 00518 for (iCol = 0; iCol < 3; iCol++) 00519 { 00520 fTmp0 = kL[iCol][1]; 00521 fTmp1 = kL[iCol][2]; 00522 kL[iCol][1] = fCos0*fTmp0-fSin0*fTmp1; 00523 kL[iCol][2] = fSin0*fTmp0+fCos0*fTmp1; 00524 } 00525 00526 fTan1 = (kA[1][2]-kA[2][2]*fTan0)/kA[1][1]; 00527 fCos1 = Math::InvSqrt(1.0+fTan1*fTan1); 00528 fSin1 = -fTan1*fCos1; 00529 00530 for (iRow = 0; iRow < 3; iRow++) 00531 { 00532 fTmp0 = kR[1][iRow]; 00533 fTmp1 = kR[2][iRow]; 00534 kR[1][iRow] = fCos1*fTmp0-fSin1*fTmp1; 00535 kR[2][iRow] = fSin1*fTmp0+fCos1*fTmp1; 00536 } 00537 00538 kS[0] = kA[0][0]; 00539 kS[1] = fCos0*fCos1*kA[1][1] - 00540 fSin1*(fCos0*kA[1][2]-fSin0*kA[2][2]); 00541 kS[2] = fSin0*fSin1*kA[1][1] + 00542 fCos1*(fSin0*kA[1][2]+fCos0*kA[2][2]); 00543 break; 00544 } 00545 } 00546 else 00547 { 00548 if ( bTest2 ) 00549 { 00550 // 2x2 closed form factorization 00551 fTmp = (kA[0][0]*kA[0][0] + kA[1][1]*kA[1][1] - 00552 kA[0][1]*kA[0][1])/(kA[0][1]*kA[1][1]); 00553 fTan0 = 0.5*(-fTmp+Math::Sqrt(fTmp*fTmp + 4.0)); 00554 fCos0 = Math::InvSqrt(1.0+fTan0*fTan0); 00555 fSin0 = fTan0*fCos0; 00556 00557 for (iCol = 0; iCol < 3; iCol++) 00558 { 00559 fTmp0 = kL[iCol][0]; 00560 fTmp1 = kL[iCol][1]; 00561 kL[iCol][0] = fCos0*fTmp0-fSin0*fTmp1; 00562 kL[iCol][1] = fSin0*fTmp0+fCos0*fTmp1; 00563 } 00564 00565 fTan1 = (kA[0][1]-kA[1][1]*fTan0)/kA[0][0]; 00566 fCos1 = Math::InvSqrt(1.0+fTan1*fTan1); 00567 fSin1 = -fTan1*fCos1; 00568 00569 for (iRow = 0; iRow < 3; iRow++) 00570 { 00571 fTmp0 = kR[0][iRow]; 00572 fTmp1 = kR[1][iRow]; 00573 kR[0][iRow] = fCos1*fTmp0-fSin1*fTmp1; 00574 kR[1][iRow] = fSin1*fTmp0+fCos1*fTmp1; 00575 } 00576 00577 kS[0] = fCos0*fCos1*kA[0][0] - 00578 fSin1*(fCos0*kA[0][1]-fSin0*kA[1][1]); 00579 kS[1] = fSin0*fSin1*kA[0][0] + 00580 fCos1*(fSin0*kA[0][1]+fCos0*kA[1][1]); 00581 kS[2] = kA[2][2]; 00582 break; 00583 } 00584 else 00585 { 00586 GolubKahanStep(kA,kL,kR); 00587 } 00588 } 00589 } 00590 00591 // positize diagonal 00592 for (iRow = 0; iRow < 3; iRow++) 00593 { 00594 if ( kS[iRow] < 0.0 ) 00595 { 00596 kS[iRow] = -kS[iRow]; 00597 for (iCol = 0; iCol < 3; iCol++) 00598 kR[iRow][iCol] = -kR[iRow][iCol]; 00599 } 00600 } 00601 } 00602 //----------------------------------------------------------------------- 00603 void Matrix3::SingularValueComposition (const Matrix3& kL, 00604 const Vector3& kS, const Matrix3& kR) 00605 { 00606 size_t iRow, iCol; 00607 Matrix3 kTmp; 00608 00609 // product S*R 00610 for (iRow = 0; iRow < 3; iRow++) 00611 { 00612 for (iCol = 0; iCol < 3; iCol++) 00613 kTmp[iRow][iCol] = kS[iRow]*kR[iRow][iCol]; 00614 } 00615 00616 // product L*S*R 00617 for (iRow = 0; iRow < 3; iRow++) 00618 { 00619 for (iCol = 0; iCol < 3; iCol++) 00620 { 00621 m[iRow][iCol] = 0.0; 00622 for (int iMid = 0; iMid < 3; iMid++) 00623 m[iRow][iCol] += kL[iRow][iMid]*kTmp[iMid][iCol]; 00624 } 00625 } 00626 } 00627 //----------------------------------------------------------------------- 00628 void Matrix3::Orthonormalize () 00629 { 00630 // Algorithm uses Gram-Schmidt orthogonalization. If 'this' matrix is 00631 // M = [m0|m1|m2], then orthonormal output matrix is Q = [q0|q1|q2], 00632 // 00633 // q0 = m0/|m0| 00634 // q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0| 00635 // q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1| 00636 // 00637 // where |V| indicates length of vector V and A*B indicates dot 00638 // product of vectors A and B. 00639 00640 // compute q0 00641 Real fInvLength = Math::InvSqrt(m[0][0]*m[0][0] 00642 + m[1][0]*m[1][0] + 00643 m[2][0]*m[2][0]); 00644 00645 m[0][0] *= fInvLength; 00646 m[1][0] *= fInvLength; 00647 m[2][0] *= fInvLength; 00648 00649 // compute q1 00650 Real fDot0 = 00651 m[0][0]*m[0][1] + 00652 m[1][0]*m[1][1] + 00653 m[2][0]*m[2][1]; 00654 00655 m[0][1] -= fDot0*m[0][0]; 00656 m[1][1] -= fDot0*m[1][0]; 00657 m[2][1] -= fDot0*m[2][0]; 00658 00659 fInvLength = Math::InvSqrt(m[0][1]*m[0][1] + 00660 m[1][1]*m[1][1] + 00661 m[2][1]*m[2][1]); 00662 00663 m[0][1] *= fInvLength; 00664 m[1][1] *= fInvLength; 00665 m[2][1] *= fInvLength; 00666 00667 // compute q2 00668 Real fDot1 = 00669 m[0][1]*m[0][2] + 00670 m[1][1]*m[1][2] + 00671 m[2][1]*m[2][2]; 00672 00673 fDot0 = 00674 m[0][0]*m[0][2] + 00675 m[1][0]*m[1][2] + 00676 m[2][0]*m[2][2]; 00677 00678 m[0][2] -= fDot0*m[0][0] + fDot1*m[0][1]; 00679 m[1][2] -= fDot0*m[1][0] + fDot1*m[1][1]; 00680 m[2][2] -= fDot0*m[2][0] + fDot1*m[2][1]; 00681 00682 fInvLength = Math::InvSqrt(m[0][2]*m[0][2] + 00683 m[1][2]*m[1][2] + 00684 m[2][2]*m[2][2]); 00685 00686 m[0][2] *= fInvLength; 00687 m[1][2] *= fInvLength; 00688 m[2][2] *= fInvLength; 00689 } 00690 //----------------------------------------------------------------------- 00691 void Matrix3::QDUDecomposition (Matrix3& kQ, 00692 Vector3& kD, Vector3& kU) const 00693 { 00694 // Factor M = QR = QDU where Q is orthogonal, D is diagonal, 00695 // and U is upper triangular with ones on its diagonal. Algorithm uses 00696 // Gram-Schmidt orthogonalization (the QR algorithm). 00697 // 00698 // If M = [ m0 | m1 | m2 ] and Q = [ q0 | q1 | q2 ], then 00699 // 00700 // q0 = m0/|m0| 00701 // q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0| 00702 // q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1| 00703 // 00704 // where |V| indicates length of vector V and A*B indicates dot 00705 // product of vectors A and B. The matrix R has entries 00706 // 00707 // r00 = q0*m0 r01 = q0*m1 r02 = q0*m2 00708 // r10 = 0 r11 = q1*m1 r12 = q1*m2 00709 // r20 = 0 r21 = 0 r22 = q2*m2 00710 // 00711 // so D = diag(r00,r11,r22) and U has entries u01 = r01/r00, 00712 // u02 = r02/r00, and u12 = r12/r11. 00713 00714 // Q = rotation 00715 // D = scaling 00716 // U = shear 00717 00718 // D stores the three diagonal entries r00, r11, r22 00719 // U stores the entries U[0] = u01, U[1] = u02, U[2] = u12 00720 00721 // build orthogonal matrix Q 00722 Real fInvLength = Math::InvSqrt(m[0][0]*m[0][0] 00723 + m[1][0]*m[1][0] + 00724 m[2][0]*m[2][0]); 00725 kQ[0][0] = m[0][0]*fInvLength; 00726 kQ[1][0] = m[1][0]*fInvLength; 00727 kQ[2][0] = m[2][0]*fInvLength; 00728 00729 Real fDot = kQ[0][0]*m[0][1] + kQ[1][0]*m[1][1] + 00730 kQ[2][0]*m[2][1]; 00731 kQ[0][1] = m[0][1]-fDot*kQ[0][0]; 00732 kQ[1][1] = m[1][1]-fDot*kQ[1][0]; 00733 kQ[2][1] = m[2][1]-fDot*kQ[2][0]; 00734 fInvLength = Math::InvSqrt(kQ[0][1]*kQ[0][1] + kQ[1][1]*kQ[1][1] + 00735 kQ[2][1]*kQ[2][1]); 00736 kQ[0][1] *= fInvLength; 00737 kQ[1][1] *= fInvLength; 00738 kQ[2][1] *= fInvLength; 00739 00740 fDot = kQ[0][0]*m[0][2] + kQ[1][0]*m[1][2] + 00741 kQ[2][0]*m[2][2]; 00742 kQ[0][2] = m[0][2]-fDot*kQ[0][0]; 00743 kQ[1][2] = m[1][2]-fDot*kQ[1][0]; 00744 kQ[2][2] = m[2][2]-fDot*kQ[2][0]; 00745 fDot = kQ[0][1]*m[0][2] + kQ[1][1]*m[1][2] + 00746 kQ[2][1]*m[2][2]; 00747 kQ[0][2] -= fDot*kQ[0][1]; 00748 kQ[1][2] -= fDot*kQ[1][1]; 00749 kQ[2][2] -= fDot*kQ[2][1]; 00750 fInvLength = Math::InvSqrt(kQ[0][2]*kQ[0][2] + kQ[1][2]*kQ[1][2] + 00751 kQ[2][2]*kQ[2][2]); 00752 kQ[0][2] *= fInvLength; 00753 kQ[1][2] *= fInvLength; 00754 kQ[2][2] *= fInvLength; 00755 00756 // guarantee that orthogonal matrix has determinant 1 (no reflections) 00757 Real fDet = kQ[0][0]*kQ[1][1]*kQ[2][2] + kQ[0][1]*kQ[1][2]*kQ[2][0] + 00758 kQ[0][2]*kQ[1][0]*kQ[2][1] - kQ[0][2]*kQ[1][1]*kQ[2][0] - 00759 kQ[0][1]*kQ[1][0]*kQ[2][2] - kQ[0][0]*kQ[1][2]*kQ[2][1]; 00760 00761 if ( fDet < 0.0 ) 00762 { 00763 for (size_t iRow = 0; iRow < 3; iRow++) 00764 for (size_t iCol = 0; iCol < 3; iCol++) 00765 kQ[iRow][iCol] = -kQ[iRow][iCol]; 00766 } 00767 00768 // build "right" matrix R 00769 Matrix3 kR; 00770 kR[0][0] = kQ[0][0]*m[0][0] + kQ[1][0]*m[1][0] + 00771 kQ[2][0]*m[2][0]; 00772 kR[0][1] = kQ[0][0]*m[0][1] + kQ[1][0]*m[1][1] + 00773 kQ[2][0]*m[2][1]; 00774 kR[1][1] = kQ[0][1]*m[0][1] + kQ[1][1]*m[1][1] + 00775 kQ[2][1]*m[2][1]; 00776 kR[0][2] = kQ[0][0]*m[0][2] + kQ[1][0]*m[1][2] + 00777 kQ[2][0]*m[2][2]; 00778 kR[1][2] = kQ[0][1]*m[0][2] + kQ[1][1]*m[1][2] + 00779 kQ[2][1]*m[2][2]; 00780 kR[2][2] = kQ[0][2]*m[0][2] + kQ[1][2]*m[1][2] + 00781 kQ[2][2]*m[2][2]; 00782 00783 // the scaling component 00784 kD[0] = kR[0][0]; 00785 kD[1] = kR[1][1]; 00786 kD[2] = kR[2][2]; 00787 00788 // the shear component 00789 Real fInvD0 = 1.0/kD[0]; 00790 kU[0] = kR[0][1]*fInvD0; 00791 kU[1] = kR[0][2]*fInvD0; 00792 kU[2] = kR[1][2]/kD[1]; 00793 } 00794 //----------------------------------------------------------------------- 00795 Real Matrix3::MaxCubicRoot (Real afCoeff[3]) 00796 { 00797 // Spectral norm is for A^T*A, so characteristic polynomial 00798 // P(x) = c[0]+c[1]*x+c[2]*x^2+x^3 has three positive real roots. 00799 // This yields the assertions c[0] < 0 and c[2]*c[2] >= 3*c[1]. 00800 00801 // quick out for uniform scale (triple root) 00802 const Real fOneThird = 1.0/3.0; 00803 const Real fEpsilon = 1e-06; 00804 Real fDiscr = afCoeff[2]*afCoeff[2] - 3.0*afCoeff[1]; 00805 if ( fDiscr <= fEpsilon ) 00806 return -fOneThird*afCoeff[2]; 00807 00808 // Compute an upper bound on roots of P(x). This assumes that A^T*A 00809 // has been scaled by its largest entry. 00810 Real fX = 1.0; 00811 Real fPoly = afCoeff[0]+fX*(afCoeff[1]+fX*(afCoeff[2]+fX)); 00812 if ( fPoly < 0.0 ) 00813 { 00814 // uses a matrix norm to find an upper bound on maximum root 00815 fX = Math::Abs(afCoeff[0]); 00816 Real fTmp = 1.0+Math::Abs(afCoeff[1]); 00817 if ( fTmp > fX ) 00818 fX = fTmp; 00819 fTmp = 1.0+Math::Abs(afCoeff[2]); 00820 if ( fTmp > fX ) 00821 fX = fTmp; 00822 } 00823 00824 // Newton's method to find root 00825 Real fTwoC2 = 2.0*afCoeff[2]; 00826 for (int i = 0; i < 16; i++) 00827 { 00828 fPoly = afCoeff[0]+fX*(afCoeff[1]+fX*(afCoeff[2]+fX)); 00829 if ( Math::Abs(fPoly) <= fEpsilon ) 00830 return fX; 00831 00832 Real fDeriv = afCoeff[1]+fX*(fTwoC2+3.0*fX); 00833 fX -= fPoly/fDeriv; 00834 } 00835 00836 return fX; 00837 } 00838 //----------------------------------------------------------------------- 00839 Real Matrix3::SpectralNorm () const 00840 { 00841 Matrix3 kP; 00842 size_t iRow, iCol; 00843 Real fPmax = 0.0; 00844 for (iRow = 0; iRow < 3; iRow++) 00845 { 00846 for (iCol = 0; iCol < 3; iCol++) 00847 { 00848 kP[iRow][iCol] = 0.0; 00849 for (int iMid = 0; iMid < 3; iMid++) 00850 { 00851 kP[iRow][iCol] += 00852 m[iMid][iRow]*m[iMid][iCol]; 00853 } 00854 if ( kP[iRow][iCol] > fPmax ) 00855 fPmax = kP[iRow][iCol]; 00856 } 00857 } 00858 00859 Real fInvPmax = 1.0/fPmax; 00860 for (iRow = 0; iRow < 3; iRow++) 00861 { 00862 for (iCol = 0; iCol < 3; iCol++) 00863 kP[iRow][iCol] *= fInvPmax; 00864 } 00865 00866 Real afCoeff[3]; 00867 afCoeff[0] = -(kP[0][0]*(kP[1][1]*kP[2][2]-kP[1][2]*kP[2][1]) + 00868 kP[0][1]*(kP[2][0]*kP[1][2]-kP[1][0]*kP[2][2]) + 00869 kP[0][2]*(kP[1][0]*kP[2][1]-kP[2][0]*kP[1][1])); 00870 afCoeff[1] = kP[0][0]*kP[1][1]-kP[0][1]*kP[1][0] + 00871 kP[0][0]*kP[2][2]-kP[0][2]*kP[2][0] + 00872 kP[1][1]*kP[2][2]-kP[1][2]*kP[2][1]; 00873 afCoeff[2] = -(kP[0][0]+kP[1][1]+kP[2][2]); 00874 00875 Real fRoot = MaxCubicRoot(afCoeff); 00876 Real fNorm = Math::Sqrt(fPmax*fRoot); 00877 return fNorm; 00878 } 00879 //----------------------------------------------------------------------- 00880 void Matrix3::ToAxisAngle (Vector3& rkAxis, Radian& rfRadians) const 00881 { 00882 // Let (x,y,z) be the unit-length axis and let A be an angle of rotation. 00883 // The rotation matrix is R = I + sin(A)*P + (1-cos(A))*P^2 where 00884 // I is the identity and 00885 // 00886 // +- -+ 00887 // P = | 0 -z +y | 00888 // | +z 0 -x | 00889 // | -y +x 0 | 00890 // +- -+ 00891 // 00892 // If A > 0, R represents a counterclockwise rotation about the axis in 00893 // the sense of looking from the tip of the axis vector towards the 00894 // origin. Some algebra will show that 00895 // 00896 // cos(A) = (trace(R)-1)/2 and R - R^t = 2*sin(A)*P 00897 // 00898 // In the event that A = pi, R-R^t = 0 which prevents us from extracting 00899 // the axis through P. Instead note that R = I+2*P^2 when A = pi, so 00900 // P^2 = (R-I)/2. The diagonal entries of P^2 are x^2-1, y^2-1, and 00901 // z^2-1. We can solve these for axis (x,y,z). Because the angle is pi, 00902 // it does not matter which sign you choose on the square roots. 00903 00904 Real fTrace = m[0][0] + m[1][1] + m[2][2]; 00905 Real fCos = 0.5*(fTrace-1.0); 00906 rfRadians = Math::ACos(fCos); // in [0,PI] 00907 00908 if ( rfRadians > Radian(0.0) ) 00909 { 00910 if ( rfRadians < Radian(Math::PI) ) 00911 { 00912 rkAxis.x = m[2][1]-m[1][2]; 00913 rkAxis.y = m[0][2]-m[2][0]; 00914 rkAxis.z = m[1][0]-m[0][1]; 00915 rkAxis.normalise(); 00916 } 00917 else 00918 { 00919 // angle is PI 00920 float fHalfInverse; 00921 if ( m[0][0] >= m[1][1] ) 00922 { 00923 // r00 >= r11 00924 if ( m[0][0] >= m[2][2] ) 00925 { 00926 // r00 is maximum diagonal term 00927 rkAxis.x = 0.5*Math::Sqrt(m[0][0] - 00928 m[1][1] - m[2][2] + 1.0); 00929 fHalfInverse = 0.5/rkAxis.x; 00930 rkAxis.y = fHalfInverse*m[0][1]; 00931 rkAxis.z = fHalfInverse*m[0][2]; 00932 } 00933 else 00934 { 00935 // r22 is maximum diagonal term 00936 rkAxis.z = 0.5*Math::Sqrt(m[2][2] - 00937 m[0][0] - m[1][1] + 1.0); 00938 fHalfInverse = 0.5/rkAxis.z; 00939 rkAxis.x = fHalfInverse*m[0][2]; 00940 rkAxis.y = fHalfInverse*m[1][2]; 00941 } 00942 } 00943 else 00944 { 00945 // r11 > r00 00946 if ( m[1][1] >= m[2][2] ) 00947 { 00948 // r11 is maximum diagonal term 00949 rkAxis.y = 0.5*Math::Sqrt(m[1][1] - 00950 m[0][0] - m[2][2] + 1.0); 00951 fHalfInverse = 0.5/rkAxis.y; 00952 rkAxis.x = fHalfInverse*m[0][1]; 00953 rkAxis.z = fHalfInverse*m[1][2]; 00954 } 00955 else 00956 { 00957 // r22 is maximum diagonal term 00958 rkAxis.z = 0.5*Math::Sqrt(m[2][2] - 00959 m[0][0] - m[1][1] + 1.0); 00960 fHalfInverse = 0.5/rkAxis.z; 00961 rkAxis.x = fHalfInverse*m[0][2]; 00962 rkAxis.y = fHalfInverse*m[1][2]; 00963 } 00964 } 00965 } 00966 } 00967 else 00968 { 00969 // The angle is 0 and the matrix is the identity. Any axis will 00970 // work, so just use the x-axis. 00971 rkAxis.x = 1.0; 00972 rkAxis.y = 0.0; 00973 rkAxis.z = 0.0; 00974 } 00975 } 00976 //----------------------------------------------------------------------- 00977 void Matrix3::FromAxisAngle (const Vector3& rkAxis, const Radian& fRadians) 00978 { 00979 Real fCos = Math::Cos(fRadians); 00980 Real fSin = Math::Sin(fRadians); 00981 Real fOneMinusCos = 1.0-fCos; 00982 Real fX2 = rkAxis.x*rkAxis.x; 00983 Real fY2 = rkAxis.y*rkAxis.y; 00984 Real fZ2 = rkAxis.z*rkAxis.z; 00985 Real fXYM = rkAxis.x*rkAxis.y*fOneMinusCos; 00986 Real fXZM = rkAxis.x*rkAxis.z*fOneMinusCos; 00987 Real fYZM = rkAxis.y*rkAxis.z*fOneMinusCos; 00988 Real fXSin = rkAxis.x*fSin; 00989 Real fYSin = rkAxis.y*fSin; 00990 Real fZSin = rkAxis.z*fSin; 00991 00992 m[0][0] = fX2*fOneMinusCos+fCos; 00993 m[0][1] = fXYM-fZSin; 00994 m[0][2] = fXZM+fYSin; 00995 m[1][0] = fXYM+fZSin; 00996 m[1][1] = fY2*fOneMinusCos+fCos; 00997 m[1][2] = fYZM-fXSin; 00998 m[2][0] = fXZM-fYSin; 00999 m[2][1] = fYZM+fXSin; 01000 m[2][2] = fZ2*fOneMinusCos+fCos; 01001 } 01002 //----------------------------------------------------------------------- 01003 bool Matrix3::ToEulerAnglesXYZ (Radian& rfYAngle, Radian& rfPAngle, 01004 Radian& rfRAngle) const 01005 { 01006 // rot = cy*cz -cy*sz sy 01007 // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx 01008 // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy 01009 01010 rfPAngle = Radian(Math::ASin(m[0][2])); 01011 if ( rfPAngle < Radian(Math::HALF_PI) ) 01012 { 01013 if ( rfPAngle > Radian(-Math::HALF_PI) ) 01014 { 01015 rfYAngle = Math::ATan2(-m[1][2],m[2][2]); 01016 rfRAngle = Math::ATan2(-m[0][1],m[0][0]); 01017 return true; 01018 } 01019 else 01020 { 01021 // WARNING. Not a unique solution. 01022 Radian fRmY = Math::ATan2(m[1][0],m[1][1]); 01023 rfRAngle = Radian(0.0); // any angle works 01024 rfYAngle = rfRAngle - fRmY; 01025 return false; 01026 } 01027 } 01028 else 01029 { 01030 // WARNING. Not a unique solution. 01031 Radian fRpY = Math::ATan2(m[1][0],m[1][1]); 01032 rfRAngle = Radian(0.0); // any angle works 01033 rfYAngle = fRpY - rfRAngle; 01034 return false; 01035 } 01036 } 01037 //----------------------------------------------------------------------- 01038 bool Matrix3::ToEulerAnglesXZY (Radian& rfYAngle, Radian& rfPAngle, 01039 Radian& rfRAngle) const 01040 { 01041 // rot = cy*cz -sz cz*sy 01042 // sx*sy+cx*cy*sz cx*cz -cy*sx+cx*sy*sz 01043 // -cx*sy+cy*sx*sz cz*sx cx*cy+sx*sy*sz 01044 01045 rfPAngle = Math::ASin(-m[0][1]); 01046 if ( rfPAngle < Radian(Math::HALF_PI) ) 01047 { 01048 if ( rfPAngle > Radian(-Math::HALF_PI) ) 01049 { 01050 rfYAngle = Math::ATan2(m[2][1],m[1][1]); 01051 rfRAngle = Math::ATan2(m[0][2],m[0][0]); 01052 return true; 01053 } 01054 else 01055 { 01056 // WARNING. Not a unique solution. 01057 Radian fRmY = Math::ATan2(-m[2][0],m[2][2]); 01058 rfRAngle = Radian(0.0); // any angle works 01059 rfYAngle = rfRAngle - fRmY; 01060 return false; 01061 } 01062 } 01063 else 01064 { 01065 // WARNING. Not a unique solution. 01066 Radian fRpY = Math::ATan2(-m[2][0],m[2][2]); 01067 rfRAngle = Radian(0.0); // any angle works 01068 rfYAngle = fRpY - rfRAngle; 01069 return false; 01070 } 01071 } 01072 //----------------------------------------------------------------------- 01073 bool Matrix3::ToEulerAnglesYXZ (Radian& rfYAngle, Radian& rfPAngle, 01074 Radian& rfRAngle) const 01075 { 01076 // rot = cy*cz+sx*sy*sz cz*sx*sy-cy*sz cx*sy 01077 // cx*sz cx*cz -sx 01078 // -cz*sy+cy*sx*sz cy*cz*sx+sy*sz cx*cy 01079 01080 rfPAngle = Math::ASin(-m[1][2]); 01081 if ( rfPAngle < Radian(Math::HALF_PI) ) 01082 { 01083 if ( rfPAngle > Radian(-Math::HALF_PI) ) 01084 { 01085 rfYAngle = Math::ATan2(m[0][2],m[2][2]); 01086 rfRAngle = Math::ATan2(m[1][0],m[1][1]); 01087 return true; 01088 } 01089 else 01090 { 01091 // WARNING. Not a unique solution. 01092 Radian fRmY = Math::ATan2(-m[0][1],m[0][0]); 01093 rfRAngle = Radian(0.0); // any angle works 01094 rfYAngle = rfRAngle - fRmY; 01095 return false; 01096 } 01097 } 01098 else 01099 { 01100 // WARNING. Not a unique solution. 01101 Radian fRpY = Math::ATan2(-m[0][1],m[0][0]); 01102 rfRAngle = Radian(0.0); // any angle works 01103 rfYAngle = fRpY - rfRAngle; 01104 return false; 01105 } 01106 } 01107 //----------------------------------------------------------------------- 01108 bool Matrix3::ToEulerAnglesYZX (Radian& rfYAngle, Radian& rfPAngle, 01109 Radian& rfRAngle) const 01110 { 01111 // rot = cy*cz sx*sy-cx*cy*sz cx*sy+cy*sx*sz 01112 // sz cx*cz -cz*sx 01113 // -cz*sy cy*sx+cx*sy*sz cx*cy-sx*sy*sz 01114 01115 rfPAngle = Math::ASin(m[1][0]); 01116 if ( rfPAngle < Radian(Math::HALF_PI) ) 01117 { 01118 if ( rfPAngle > Radian(-Math::HALF_PI) ) 01119 { 01120 rfYAngle = Math::ATan2(-m[2][0],m[0][0]); 01121 rfRAngle = Math::ATan2(-m[1][2],m[1][1]); 01122 return true; 01123 } 01124 else 01125 { 01126 // WARNING. Not a unique solution. 01127 Radian fRmY = Math::ATan2(m[2][1],m[2][2]); 01128 rfRAngle = Radian(0.0); // any angle works 01129 rfYAngle = rfRAngle - fRmY; 01130 return false; 01131 } 01132 } 01133 else 01134 { 01135 // WARNING. Not a unique solution. 01136 Radian fRpY = Math::ATan2(m[2][1],m[2][2]); 01137 rfRAngle = Radian(0.0); // any angle works 01138 rfYAngle = fRpY - rfRAngle; 01139 return false; 01140 } 01141 } 01142 //----------------------------------------------------------------------- 01143 bool Matrix3::ToEulerAnglesZXY (Radian& rfYAngle, Radian& rfPAngle, 01144 Radian& rfRAngle) const 01145 { 01146 // rot = cy*cz-sx*sy*sz -cx*sz cz*sy+cy*sx*sz 01147 // cz*sx*sy+cy*sz cx*cz -cy*cz*sx+sy*sz 01148 // -cx*sy sx cx*cy 01149 01150 rfPAngle = Math::ASin(m[2][1]); 01151 if ( rfPAngle < Radian(Math::HALF_PI) ) 01152 { 01153 if ( rfPAngle > Radian(-Math::HALF_PI) ) 01154 { 01155 rfYAngle = Math::ATan2(-m[0][1],m[1][1]); 01156 rfRAngle = Math::ATan2(-m[2][0],m[2][2]); 01157 return true; 01158 } 01159 else 01160 { 01161 // WARNING. Not a unique solution. 01162 Radian fRmY = Math::ATan2(m[0][2],m[0][0]); 01163 rfRAngle = Radian(0.0); // any angle works 01164 rfYAngle = rfRAngle - fRmY; 01165 return false; 01166 } 01167 } 01168 else 01169 { 01170 // WARNING. Not a unique solution. 01171 Radian fRpY = Math::ATan2(m[0][2],m[0][0]); 01172 rfRAngle = Radian(0.0); // any angle works 01173 rfYAngle = fRpY - rfRAngle; 01174 return false; 01175 } 01176 } 01177 //----------------------------------------------------------------------- 01178 bool Matrix3::ToEulerAnglesZYX (Radian& rfYAngle, Radian& rfPAngle, 01179 Radian& rfRAngle) const 01180 { 01181 // rot = cy*cz cz*sx*sy-cx*sz cx*cz*sy+sx*sz 01182 // cy*sz cx*cz+sx*sy*sz -cz*sx+cx*sy*sz 01183 // -sy cy*sx cx*cy 01184 01185 rfPAngle = Math::ASin(-m[2][0]); 01186 if ( rfPAngle < Radian(Math::HALF_PI) ) 01187 { 01188 if ( rfPAngle > Radian(-Math::HALF_PI) ) 01189 { 01190 rfYAngle = Math::ATan2(m[1][0],m[0][0]); 01191 rfRAngle = Math::ATan2(m[2][1],m[2][2]); 01192 return true; 01193 } 01194 else 01195 { 01196 // WARNING. Not a unique solution. 01197 Radian fRmY = Math::ATan2(-m[0][1],m[0][2]); 01198 rfRAngle = Radian(0.0); // any angle works 01199 rfYAngle = rfRAngle - fRmY; 01200 return false; 01201 } 01202 } 01203 else 01204 { 01205 // WARNING. Not a unique solution. 01206 Radian fRpY = Math::ATan2(-m[0][1],m[0][2]); 01207 rfRAngle = Radian(0.0); // any angle works 01208 rfYAngle = fRpY - rfRAngle; 01209 return false; 01210 } 01211 } 01212 //----------------------------------------------------------------------- 01213 void Matrix3::FromEulerAnglesXYZ (const Radian& fYAngle, const Radian& fPAngle, 01214 const Radian& fRAngle) 01215 { 01216 Real fCos, fSin; 01217 01218 fCos = Math::Cos(fYAngle); 01219 fSin = Math::Sin(fYAngle); 01220 Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos); 01221 01222 fCos = Math::Cos(fPAngle); 01223 fSin = Math::Sin(fPAngle); 01224 Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos); 01225 01226 fCos = Math::Cos(fRAngle); 01227 fSin = Math::Sin(fRAngle); 01228 Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0); 01229 01230 *this = kXMat*(kYMat*kZMat); 01231 } 01232 //----------------------------------------------------------------------- 01233 void Matrix3::FromEulerAnglesXZY (const Radian& fYAngle, const Radian& fPAngle, 01234 const Radian& fRAngle) 01235 { 01236 Real fCos, fSin; 01237 01238 fCos = Math::Cos(fYAngle); 01239 fSin = Math::Sin(fYAngle); 01240 Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos); 01241 01242 fCos = Math::Cos(fPAngle); 01243 fSin = Math::Sin(fPAngle); 01244 Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0); 01245 01246 fCos = Math::Cos(fRAngle); 01247 fSin = Math::Sin(fRAngle); 01248 Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos); 01249 01250 *this = kXMat*(kZMat*kYMat); 01251 } 01252 //----------------------------------------------------------------------- 01253 void Matrix3::FromEulerAnglesYXZ (const Radian& fYAngle, const Radian& fPAngle, 01254 const Radian& fRAngle) 01255 { 01256 Real fCos, fSin; 01257 01258 fCos = Math::Cos(fYAngle); 01259 fSin = Math::Sin(fYAngle); 01260 Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos); 01261 01262 fCos = Math::Cos(fPAngle); 01263 fSin = Math::Sin(fPAngle); 01264 Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos); 01265 01266 fCos = Math::Cos(fRAngle); 01267 fSin = Math::Sin(fRAngle); 01268 Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0); 01269 01270 *this = kYMat*(kXMat*kZMat); 01271 } 01272 //----------------------------------------------------------------------- 01273 void Matrix3::FromEulerAnglesYZX (const Radian& fYAngle, const Radian& fPAngle, 01274 const Radian& fRAngle) 01275 { 01276 Real fCos, fSin; 01277 01278 fCos = Math::Cos(fYAngle); 01279 fSin = Math::Sin(fYAngle); 01280 Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos); 01281 01282 fCos = Math::Cos(fPAngle); 01283 fSin = Math::Sin(fPAngle); 01284 Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0); 01285 01286 fCos = Math::Cos(fRAngle); 01287 fSin = Math::Sin(fRAngle); 01288 Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos); 01289 01290 *this = kYMat*(kZMat*kXMat); 01291 } 01292 //----------------------------------------------------------------------- 01293 void Matrix3::FromEulerAnglesZXY (const Radian& fYAngle, const Radian& fPAngle, 01294 const Radian& fRAngle) 01295 { 01296 Real fCos, fSin; 01297 01298 fCos = Math::Cos(fYAngle); 01299 fSin = Math::Sin(fYAngle); 01300 Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0); 01301 01302 fCos = Math::Cos(fPAngle); 01303 fSin = Math::Sin(fPAngle); 01304 Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos); 01305 01306 fCos = Math::Cos(fRAngle); 01307 fSin = Math::Sin(fRAngle); 01308 Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos); 01309 01310 *this = kZMat*(kXMat*kYMat); 01311 } 01312 //----------------------------------------------------------------------- 01313 void Matrix3::FromEulerAnglesZYX (const Radian& fYAngle, const Radian& fPAngle, 01314 const Radian& fRAngle) 01315 { 01316 Real fCos, fSin; 01317 01318 fCos = Math::Cos(fYAngle); 01319 fSin = Math::Sin(fYAngle); 01320 Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0); 01321 01322 fCos = Math::Cos(fPAngle); 01323 fSin = Math::Sin(fPAngle); 01324 Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos); 01325 01326 fCos = Math::Cos(fRAngle); 01327 fSin = Math::Sin(fRAngle); 01328 Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos); 01329 01330 *this = kZMat*(kYMat*kXMat); 01331 } 01332 //----------------------------------------------------------------------- 01333 void Matrix3::Tridiagonal (Real afDiag[3], Real afSubDiag[3]) 01334 { 01335 // Householder reduction T = Q^t M Q 01336 // Input: 01337 // mat, symmetric 3x3 matrix M 01338 // Output: 01339 // mat, orthogonal matrix Q 01340 // diag, diagonal entries of T 01341 // subd, subdiagonal entries of T (T is symmetric) 01342 01343 Real fA = m[0][0]; 01344 Real fB = m[0][1]; 01345 Real fC = m[0][2]; 01346 Real fD = m[1][1]; 01347 Real fE = m[1][2]; 01348 Real fF = m[2][2]; 01349 01350 afDiag[0] = fA; 01351 afSubDiag[2] = 0.0; 01352 if ( Math::Abs(fC) >= EPSILON ) 01353 { 01354 Real fLength = Math::Sqrt(fB*fB+fC*fC); 01355 Real fInvLength = 1.0/fLength; 01356 fB *= fInvLength; 01357 fC *= fInvLength; 01358 Real fQ = 2.0*fB*fE+fC*(fF-fD); 01359 afDiag[1] = fD+fC*fQ; 01360 afDiag[2] = fF-fC*fQ; 01361 afSubDiag[0] = fLength; 01362 afSubDiag[1] = fE-fB*fQ; 01363 m[0][0] = 1.0; 01364 m[0][1] = 0.0; 01365 m[0][2] = 0.0; 01366 m[1][0] = 0.0; 01367 m[1][1] = fB; 01368 m[1][2] = fC; 01369 m[2][0] = 0.0; 01370 m[2][1] = fC; 01371 m[2][2] = -fB; 01372 } 01373 else 01374 { 01375 afDiag[1] = fD; 01376 afDiag[2] = fF; 01377 afSubDiag[0] = fB; 01378 afSubDiag[1] = fE; 01379 m[0][0] = 1.0; 01380 m[0][1] = 0.0; 01381 m[0][2] = 0.0; 01382 m[1][0] = 0.0; 01383 m[1][1] = 1.0; 01384 m[1][2] = 0.0; 01385 m[2][0] = 0.0; 01386 m[2][1] = 0.0; 01387 m[2][2] = 1.0; 01388 } 01389 } 01390 //----------------------------------------------------------------------- 01391 bool Matrix3::QLAlgorithm (Real afDiag[3], Real afSubDiag[3]) 01392 { 01393 // QL iteration with implicit shifting to reduce matrix from tridiagonal 01394 // to diagonal 01395 01396 for (size_t i0 = 0; i0 < 3; i0++) 01397 { 01398 const unsigned int iMaxIter = 32; 01399 unsigned int iIter; 01400 for (iIter = 0; iIter < iMaxIter; iIter++) 01401 { 01402 size_t i1; 01403 for (i1 = i0; i1 <= 1; i1++) 01404 { 01405 Real fSum = Math::Abs(afDiag[i1]) + 01406 Math::Abs(afDiag[i1+1]); 01407 if ( Math::Abs(afSubDiag[i1]) + fSum == fSum ) 01408 break; 01409 } 01410 if ( i1 == i0 ) 01411 break; 01412 01413 Real fTmp0 = (afDiag[i0+1]-afDiag[i0])/(2.0*afSubDiag[i0]); 01414 Real fTmp1 = Math::Sqrt(fTmp0*fTmp0+1.0); 01415 if ( fTmp0 < 0.0 ) 01416 fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0-fTmp1); 01417 else 01418 fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0+fTmp1); 01419 Real fSin = 1.0; 01420 Real fCos = 1.0; 01421 Real fTmp2 = 0.0; 01422 for (size_t i2 = i1-1; i2 >= i0; i2--) 01423 { 01424 Real fTmp3 = fSin*afSubDiag[i2]; 01425 Real fTmp4 = fCos*afSubDiag[i2]; 01426 if ( Math::Abs(fTmp3) >= Math::Abs(fTmp0) ) 01427 { 01428 fCos = fTmp0/fTmp3; 01429 fTmp1 = Math::Sqrt(fCos*fCos+1.0); 01430 afSubDiag[i2+1] = fTmp3*fTmp1; 01431 fSin = 1.0/fTmp1; 01432 fCos *= fSin; 01433 } 01434 else 01435 { 01436 fSin = fTmp3/fTmp0; 01437 fTmp1 = Math::Sqrt(fSin*fSin+1.0); 01438 afSubDiag[i2+1] = fTmp0*fTmp1; 01439 fCos = 1.0/fTmp1; 01440 fSin *= fCos; 01441 } 01442 fTmp0 = afDiag[i2+1]-fTmp2; 01443 fTmp1 = (afDiag[i2]-fTmp0)*fSin+2.0*fTmp4*fCos; 01444 fTmp2 = fSin*fTmp1; 01445 afDiag[i2+1] = fTmp0+fTmp2; 01446 fTmp0 = fCos*fTmp1-fTmp4; 01447 01448 for (size_t iRow = 0; iRow < 3; iRow++) 01449 { 01450 fTmp3 = m[iRow][i2+1]; 01451 m[iRow][i2+1] = fSin*m[iRow][i2] + 01452 fCos*fTmp3; 01453 m[iRow][i2] = fCos*m[iRow][i2] - 01454 fSin*fTmp3; 01455 } 01456 } 01457 afDiag[i0] -= fTmp2; 01458 afSubDiag[i0] = fTmp0; 01459 afSubDiag[i1] = 0.0; 01460 } 01461 01462 if ( iIter == iMaxIter ) 01463 { 01464 // should not get here under normal circumstances 01465 return false; 01466 } 01467 } 01468 01469 return true; 01470 } 01471 //----------------------------------------------------------------------- 01472 void Matrix3::EigenSolveSymmetric (Real afEigenvalue[3], 01473 Vector3 akEigenvector[3]) const 01474 { 01475 Matrix3 kMatrix = *this; 01476 Real afSubDiag[3]; 01477 kMatrix.Tridiagonal(afEigenvalue,afSubDiag); 01478 kMatrix.QLAlgorithm(afEigenvalue,afSubDiag); 01479 01480 for (size_t i = 0; i < 3; i++) 01481 { 01482 akEigenvector[i][0] = kMatrix[0][i]; 01483 akEigenvector[i][1] = kMatrix[1][i]; 01484 akEigenvector[i][2] = kMatrix[2][i]; 01485 } 01486 01487 // make eigenvectors form a right--handed system 01488 Vector3 kCross = akEigenvector[1].crossProduct(akEigenvector[2]); 01489 Real fDet = akEigenvector[0].dotProduct(kCross); 01490 if ( fDet < 0.0 ) 01491 { 01492 akEigenvector[2][0] = - akEigenvector[2][0]; 01493 akEigenvector[2][1] = - akEigenvector[2][1]; 01494 akEigenvector[2][2] = - akEigenvector[2][2]; 01495 } 01496 } 01497 //----------------------------------------------------------------------- 01498 void Matrix3::TensorProduct (const Vector3& rkU, const Vector3& rkV, 01499 Matrix3& rkProduct) 01500 { 01501 for (size_t iRow = 0; iRow < 3; iRow++) 01502 { 01503 for (size_t iCol = 0; iCol < 3; iCol++) 01504 rkProduct[iRow][iCol] = rkU[iRow]*rkV[iCol]; 01505 } 01506 } 01507 //----------------------------------------------------------------------- 01508 }
Copyright © 2002-2003 by The OGRE Team
Last modified Sun Nov 28 19:48:33 2004