1 /* 2 ----------------------------------------------------------------------------- 3 This source file is part of OGRE 4 (Object-oriented Graphics Rendering Engine) 5 For the latest info, see http://www.ogre3d.org/ 6 7 Copyright (c) 2000-2014 Torus Knot Software Ltd 8 9 Permission is hereby granted, free of charge, to any person obtaining a copy 10 of this software and associated documentation files (the "Software"), to deal 11 in the Software without restriction, including without limitation the rights 12 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 13 copies of the Software, and to permit persons to whom the Software is 14 furnished to do so, subject to the following conditions: 15 16 The above copyright notice and this permission notice shall be included in 17 all copies or substantial portions of the Software. 18 19 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 20 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 21 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 22 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 23 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 24 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 25 THE SOFTWARE. 26 ----------------------------------------------------------------------------- 27 */ 28 #include "OgreStableHeaders.h" 29 30 // Adapted from Matrix math by Wild Magic http://www.geometrictools.com/ 31 32 namespace Ogre 33 { 34 const Real Matrix3::EPSILON = 1e-06; 35 const Matrix3 Matrix3::ZERO(0,0,0,0,0,0,0,0,0); 36 const Matrix3 Matrix3::IDENTITY(1,0,0,0,1,0,0,0,1); 37 const unsigned int Matrix3::msSvdMaxIterations = 32; 38 39 //----------------------------------------------------------------------- GetColumn(size_t iCol) const40 Vector3 Matrix3::GetColumn (size_t iCol) const 41 { 42 assert( iCol < 3 ); 43 return Vector3(m[0][iCol],m[1][iCol], 44 m[2][iCol]); 45 } 46 //----------------------------------------------------------------------- SetColumn(size_t iCol,const Vector3 & vec)47 void Matrix3::SetColumn(size_t iCol, const Vector3& vec) 48 { 49 assert( iCol < 3 ); 50 m[0][iCol] = vec.x; 51 m[1][iCol] = vec.y; 52 m[2][iCol] = vec.z; 53 54 } 55 //----------------------------------------------------------------------- FromAxes(const Vector3 & xAxis,const Vector3 & yAxis,const Vector3 & zAxis)56 void Matrix3::FromAxes(const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis) 57 { 58 SetColumn(0,xAxis); 59 SetColumn(1,yAxis); 60 SetColumn(2,zAxis); 61 62 } 63 64 //----------------------------------------------------------------------- operator ==(const Matrix3 & rkMatrix) const65 bool Matrix3::operator== (const Matrix3& rkMatrix) const 66 { 67 for (size_t iRow = 0; iRow < 3; iRow++) 68 { 69 for (size_t iCol = 0; iCol < 3; iCol++) 70 { 71 if ( m[iRow][iCol] != rkMatrix.m[iRow][iCol] ) 72 return false; 73 } 74 } 75 76 return true; 77 } 78 //----------------------------------------------------------------------- operator +(const Matrix3 & rkMatrix) const79 Matrix3 Matrix3::operator+ (const Matrix3& rkMatrix) const 80 { 81 Matrix3 kSum; 82 for (size_t iRow = 0; iRow < 3; iRow++) 83 { 84 for (size_t iCol = 0; iCol < 3; iCol++) 85 { 86 kSum.m[iRow][iCol] = m[iRow][iCol] + 87 rkMatrix.m[iRow][iCol]; 88 } 89 } 90 return kSum; 91 } 92 //----------------------------------------------------------------------- operator -(const Matrix3 & rkMatrix) const93 Matrix3 Matrix3::operator- (const Matrix3& rkMatrix) const 94 { 95 Matrix3 kDiff; 96 for (size_t iRow = 0; iRow < 3; iRow++) 97 { 98 for (size_t iCol = 0; iCol < 3; iCol++) 99 { 100 kDiff.m[iRow][iCol] = m[iRow][iCol] - 101 rkMatrix.m[iRow][iCol]; 102 } 103 } 104 return kDiff; 105 } 106 //----------------------------------------------------------------------- operator *(const Matrix3 & rkMatrix) const107 Matrix3 Matrix3::operator* (const Matrix3& rkMatrix) const 108 { 109 Matrix3 kProd; 110 for (size_t iRow = 0; iRow < 3; iRow++) 111 { 112 for (size_t iCol = 0; iCol < 3; iCol++) 113 { 114 kProd.m[iRow][iCol] = 115 m[iRow][0]*rkMatrix.m[0][iCol] + 116 m[iRow][1]*rkMatrix.m[1][iCol] + 117 m[iRow][2]*rkMatrix.m[2][iCol]; 118 } 119 } 120 return kProd; 121 } 122 //----------------------------------------------------------------------- operator *(const Vector3 & rkPoint,const Matrix3 & rkMatrix)123 Vector3 operator* (const Vector3& rkPoint, const Matrix3& rkMatrix) 124 { 125 Vector3 kProd; 126 for (size_t iRow = 0; iRow < 3; iRow++) 127 { 128 kProd[iRow] = 129 rkPoint[0]*rkMatrix.m[0][iRow] + 130 rkPoint[1]*rkMatrix.m[1][iRow] + 131 rkPoint[2]*rkMatrix.m[2][iRow]; 132 } 133 return kProd; 134 } 135 //----------------------------------------------------------------------- operator -() const136 Matrix3 Matrix3::operator- () const 137 { 138 Matrix3 kNeg; 139 for (size_t iRow = 0; iRow < 3; iRow++) 140 { 141 for (size_t iCol = 0; iCol < 3; iCol++) 142 kNeg[iRow][iCol] = -m[iRow][iCol]; 143 } 144 return kNeg; 145 } 146 //----------------------------------------------------------------------- operator *(Real fScalar) const147 Matrix3 Matrix3::operator* (Real fScalar) const 148 { 149 Matrix3 kProd; 150 for (size_t iRow = 0; iRow < 3; iRow++) 151 { 152 for (size_t iCol = 0; iCol < 3; iCol++) 153 kProd[iRow][iCol] = fScalar*m[iRow][iCol]; 154 } 155 return kProd; 156 } 157 //----------------------------------------------------------------------- operator *(Real fScalar,const Matrix3 & rkMatrix)158 Matrix3 operator* (Real fScalar, const Matrix3& rkMatrix) 159 { 160 Matrix3 kProd; 161 for (size_t iRow = 0; iRow < 3; iRow++) 162 { 163 for (size_t iCol = 0; iCol < 3; iCol++) 164 kProd[iRow][iCol] = fScalar*rkMatrix.m[iRow][iCol]; 165 } 166 return kProd; 167 } 168 //----------------------------------------------------------------------- Transpose() const169 Matrix3 Matrix3::Transpose () const 170 { 171 Matrix3 kTranspose; 172 for (size_t iRow = 0; iRow < 3; iRow++) 173 { 174 for (size_t iCol = 0; iCol < 3; iCol++) 175 kTranspose[iRow][iCol] = m[iCol][iRow]; 176 } 177 return kTranspose; 178 } 179 //----------------------------------------------------------------------- Inverse(Matrix3 & rkInverse,Real fTolerance) const180 bool Matrix3::Inverse (Matrix3& rkInverse, Real fTolerance) const 181 { 182 // Invert a 3x3 using cofactors. This is about 8 times faster than 183 // the Numerical Recipes code which uses Gaussian elimination. 184 185 rkInverse[0][0] = m[1][1]*m[2][2] - 186 m[1][2]*m[2][1]; 187 rkInverse[0][1] = m[0][2]*m[2][1] - 188 m[0][1]*m[2][2]; 189 rkInverse[0][2] = m[0][1]*m[1][2] - 190 m[0][2]*m[1][1]; 191 rkInverse[1][0] = m[1][2]*m[2][0] - 192 m[1][0]*m[2][2]; 193 rkInverse[1][1] = m[0][0]*m[2][2] - 194 m[0][2]*m[2][0]; 195 rkInverse[1][2] = m[0][2]*m[1][0] - 196 m[0][0]*m[1][2]; 197 rkInverse[2][0] = m[1][0]*m[2][1] - 198 m[1][1]*m[2][0]; 199 rkInverse[2][1] = m[0][1]*m[2][0] - 200 m[0][0]*m[2][1]; 201 rkInverse[2][2] = m[0][0]*m[1][1] - 202 m[0][1]*m[1][0]; 203 204 Real fDet = 205 m[0][0]*rkInverse[0][0] + 206 m[0][1]*rkInverse[1][0]+ 207 m[0][2]*rkInverse[2][0]; 208 209 if ( Math::Abs(fDet) <= fTolerance ) 210 return false; 211 212 Real fInvDet = 1.0f/fDet; 213 for (size_t iRow = 0; iRow < 3; iRow++) 214 { 215 for (size_t iCol = 0; iCol < 3; iCol++) 216 rkInverse[iRow][iCol] *= fInvDet; 217 } 218 219 return true; 220 } 221 //----------------------------------------------------------------------- Inverse(Real fTolerance) const222 Matrix3 Matrix3::Inverse (Real fTolerance) const 223 { 224 Matrix3 kInverse = Matrix3::ZERO; 225 Inverse(kInverse,fTolerance); 226 return kInverse; 227 } 228 //----------------------------------------------------------------------- Determinant() const229 Real Matrix3::Determinant () const 230 { 231 Real fCofactor00 = m[1][1]*m[2][2] - 232 m[1][2]*m[2][1]; 233 Real fCofactor10 = m[1][2]*m[2][0] - 234 m[1][0]*m[2][2]; 235 Real fCofactor20 = m[1][0]*m[2][1] - 236 m[1][1]*m[2][0]; 237 238 Real fDet = 239 m[0][0]*fCofactor00 + 240 m[0][1]*fCofactor10 + 241 m[0][2]*fCofactor20; 242 243 return fDet; 244 } 245 //----------------------------------------------------------------------- Bidiagonalize(Matrix3 & kA,Matrix3 & kL,Matrix3 & kR)246 void Matrix3::Bidiagonalize (Matrix3& kA, Matrix3& kL, 247 Matrix3& kR) 248 { 249 Real afV[3], afW[3]; 250 Real fLength, fSign, fT1, fInvT1, fT2; 251 bool bIdentity; 252 253 // map first column to (*,0,0) 254 fLength = Math::Sqrt(kA[0][0]*kA[0][0] + kA[1][0]*kA[1][0] + 255 kA[2][0]*kA[2][0]); 256 if ( fLength > 0.0 ) 257 { 258 fSign = (kA[0][0] > 0.0f ? 1.0f : -1.0f); 259 fT1 = kA[0][0] + fSign*fLength; 260 fInvT1 = 1.0f/fT1; 261 afV[1] = kA[1][0]*fInvT1; 262 afV[2] = kA[2][0]*fInvT1; 263 264 fT2 = -2.0f/(1.0f+afV[1]*afV[1]+afV[2]*afV[2]); 265 afW[0] = fT2*(kA[0][0]+kA[1][0]*afV[1]+kA[2][0]*afV[2]); 266 afW[1] = fT2*(kA[0][1]+kA[1][1]*afV[1]+kA[2][1]*afV[2]); 267 afW[2] = fT2*(kA[0][2]+kA[1][2]*afV[1]+kA[2][2]*afV[2]); 268 kA[0][0] += afW[0]; 269 kA[0][1] += afW[1]; 270 kA[0][2] += afW[2]; 271 kA[1][1] += afV[1]*afW[1]; 272 kA[1][2] += afV[1]*afW[2]; 273 kA[2][1] += afV[2]*afW[1]; 274 kA[2][2] += afV[2]*afW[2]; 275 276 kL[0][0] = 1.0f+fT2; 277 kL[0][1] = kL[1][0] = fT2*afV[1]; 278 kL[0][2] = kL[2][0] = fT2*afV[2]; 279 kL[1][1] = 1.0f+fT2*afV[1]*afV[1]; 280 kL[1][2] = kL[2][1] = fT2*afV[1]*afV[2]; 281 kL[2][2] = 1.0f+fT2*afV[2]*afV[2]; 282 bIdentity = false; 283 } 284 else 285 { 286 kL = Matrix3::IDENTITY; 287 bIdentity = true; 288 } 289 290 // map first row to (*,*,0) 291 fLength = Math::Sqrt(kA[0][1]*kA[0][1]+kA[0][2]*kA[0][2]); 292 if ( fLength > 0.0 ) 293 { 294 fSign = (kA[0][1] > 0.0f ? 1.0f : -1.0f); 295 fT1 = kA[0][1] + fSign*fLength; 296 afV[2] = kA[0][2]/fT1; 297 298 fT2 = -2.0f/(1.0f+afV[2]*afV[2]); 299 afW[0] = fT2*(kA[0][1]+kA[0][2]*afV[2]); 300 afW[1] = fT2*(kA[1][1]+kA[1][2]*afV[2]); 301 afW[2] = fT2*(kA[2][1]+kA[2][2]*afV[2]); 302 kA[0][1] += afW[0]; 303 kA[1][1] += afW[1]; 304 kA[1][2] += afW[1]*afV[2]; 305 kA[2][1] += afW[2]; 306 kA[2][2] += afW[2]*afV[2]; 307 308 kR[0][0] = 1.0; 309 kR[0][1] = kR[1][0] = 0.0; 310 kR[0][2] = kR[2][0] = 0.0; 311 kR[1][1] = 1.0f+fT2; 312 kR[1][2] = kR[2][1] = fT2*afV[2]; 313 kR[2][2] = 1.0f+fT2*afV[2]*afV[2]; 314 } 315 else 316 { 317 kR = Matrix3::IDENTITY; 318 } 319 320 // map second column to (*,*,0) 321 fLength = Math::Sqrt(kA[1][1]*kA[1][1]+kA[2][1]*kA[2][1]); 322 if ( fLength > 0.0 ) 323 { 324 fSign = (kA[1][1] > 0.0f ? 1.0f : -1.0f); 325 fT1 = kA[1][1] + fSign*fLength; 326 afV[2] = kA[2][1]/fT1; 327 328 fT2 = -2.0f/(1.0f+afV[2]*afV[2]); 329 afW[1] = fT2*(kA[1][1]+kA[2][1]*afV[2]); 330 afW[2] = fT2*(kA[1][2]+kA[2][2]*afV[2]); 331 kA[1][1] += afW[1]; 332 kA[1][2] += afW[2]; 333 kA[2][2] += afV[2]*afW[2]; 334 335 Real fA = 1.0f+fT2; 336 Real fB = fT2*afV[2]; 337 Real fC = 1.0f+fB*afV[2]; 338 339 if ( bIdentity ) 340 { 341 kL[0][0] = 1.0; 342 kL[0][1] = kL[1][0] = 0.0; 343 kL[0][2] = kL[2][0] = 0.0; 344 kL[1][1] = fA; 345 kL[1][2] = kL[2][1] = fB; 346 kL[2][2] = fC; 347 } 348 else 349 { 350 for (int iRow = 0; iRow < 3; iRow++) 351 { 352 Real fTmp0 = kL[iRow][1]; 353 Real fTmp1 = kL[iRow][2]; 354 kL[iRow][1] = fA*fTmp0+fB*fTmp1; 355 kL[iRow][2] = fB*fTmp0+fC*fTmp1; 356 } 357 } 358 } 359 } 360 //----------------------------------------------------------------------- GolubKahanStep(Matrix3 & kA,Matrix3 & kL,Matrix3 & kR)361 void Matrix3::GolubKahanStep (Matrix3& kA, Matrix3& kL, 362 Matrix3& kR) 363 { 364 Real fT11 = kA[0][1]*kA[0][1]+kA[1][1]*kA[1][1]; 365 Real fT22 = kA[1][2]*kA[1][2]+kA[2][2]*kA[2][2]; 366 Real fT12 = kA[1][1]*kA[1][2]; 367 Real fTrace = fT11+fT22; 368 Real fDiff = fT11-fT22; 369 Real fDiscr = Math::Sqrt(fDiff*fDiff+4.0f*fT12*fT12); 370 Real fRoot1 = 0.5f*(fTrace+fDiscr); 371 Real fRoot2 = 0.5f*(fTrace-fDiscr); 372 373 // adjust right 374 Real fY = kA[0][0] - (Math::Abs(fRoot1-fT22) <= 375 Math::Abs(fRoot2-fT22) ? fRoot1 : fRoot2); 376 Real fZ = kA[0][1]; 377 Real fInvLength = Math::InvSqrt(fY*fY+fZ*fZ); 378 Real fSin = fZ*fInvLength; 379 Real fCos = -fY*fInvLength; 380 381 Real fTmp0 = kA[0][0]; 382 Real fTmp1 = kA[0][1]; 383 kA[0][0] = fCos*fTmp0-fSin*fTmp1; 384 kA[0][1] = fSin*fTmp0+fCos*fTmp1; 385 kA[1][0] = -fSin*kA[1][1]; 386 kA[1][1] *= fCos; 387 388 size_t iRow; 389 for (iRow = 0; iRow < 3; iRow++) 390 { 391 fTmp0 = kR[0][iRow]; 392 fTmp1 = kR[1][iRow]; 393 kR[0][iRow] = fCos*fTmp0-fSin*fTmp1; 394 kR[1][iRow] = fSin*fTmp0+fCos*fTmp1; 395 } 396 397 // adjust left 398 fY = kA[0][0]; 399 fZ = kA[1][0]; 400 fInvLength = Math::InvSqrt(fY*fY+fZ*fZ); 401 fSin = fZ*fInvLength; 402 fCos = -fY*fInvLength; 403 404 kA[0][0] = fCos*kA[0][0]-fSin*kA[1][0]; 405 fTmp0 = kA[0][1]; 406 fTmp1 = kA[1][1]; 407 kA[0][1] = fCos*fTmp0-fSin*fTmp1; 408 kA[1][1] = fSin*fTmp0+fCos*fTmp1; 409 kA[0][2] = -fSin*kA[1][2]; 410 kA[1][2] *= fCos; 411 412 size_t iCol; 413 for (iCol = 0; iCol < 3; iCol++) 414 { 415 fTmp0 = kL[iCol][0]; 416 fTmp1 = kL[iCol][1]; 417 kL[iCol][0] = fCos*fTmp0-fSin*fTmp1; 418 kL[iCol][1] = fSin*fTmp0+fCos*fTmp1; 419 } 420 421 // adjust right 422 fY = kA[0][1]; 423 fZ = kA[0][2]; 424 fInvLength = Math::InvSqrt(fY*fY+fZ*fZ); 425 fSin = fZ*fInvLength; 426 fCos = -fY*fInvLength; 427 428 kA[0][1] = fCos*kA[0][1]-fSin*kA[0][2]; 429 fTmp0 = kA[1][1]; 430 fTmp1 = kA[1][2]; 431 kA[1][1] = fCos*fTmp0-fSin*fTmp1; 432 kA[1][2] = fSin*fTmp0+fCos*fTmp1; 433 kA[2][1] = -fSin*kA[2][2]; 434 kA[2][2] *= fCos; 435 436 for (iRow = 0; iRow < 3; iRow++) 437 { 438 fTmp0 = kR[1][iRow]; 439 fTmp1 = kR[2][iRow]; 440 kR[1][iRow] = fCos*fTmp0-fSin*fTmp1; 441 kR[2][iRow] = fSin*fTmp0+fCos*fTmp1; 442 } 443 444 // adjust left 445 fY = kA[1][1]; 446 fZ = kA[2][1]; 447 fInvLength = Math::InvSqrt(fY*fY+fZ*fZ); 448 fSin = fZ*fInvLength; 449 fCos = -fY*fInvLength; 450 451 kA[1][1] = fCos*kA[1][1]-fSin*kA[2][1]; 452 fTmp0 = kA[1][2]; 453 fTmp1 = kA[2][2]; 454 kA[1][2] = fCos*fTmp0-fSin*fTmp1; 455 kA[2][2] = fSin*fTmp0+fCos*fTmp1; 456 457 for (iCol = 0; iCol < 3; iCol++) 458 { 459 fTmp0 = kL[iCol][1]; 460 fTmp1 = kL[iCol][2]; 461 kL[iCol][1] = fCos*fTmp0-fSin*fTmp1; 462 kL[iCol][2] = fSin*fTmp0+fCos*fTmp1; 463 } 464 } 465 //----------------------------------------------------------------------- SingularValueDecomposition(Matrix3 & kL,Vector3 & kS,Matrix3 & kR) const466 void Matrix3::SingularValueDecomposition (Matrix3& kL, Vector3& kS, 467 Matrix3& kR) const 468 { 469 // temas: currently unused 470 //const int iMax = 16; 471 size_t iRow, iCol; 472 473 Matrix3 kA = *this; 474 Bidiagonalize(kA,kL,kR); 475 476 // Compute 'threshold = multiplier*epsilon*|kA|' as the threshold for 477 // diagonal entries effectively zero; that is, |d| <= |threshold| 478 // implies that d is (effectively) zero. We will use the L2-norm |kA|. 479 Real norm = Math::Sqrt(kA[0][0] * kA[0][0] + kA[0][1] * kA[0][1] + 480 kA[1][1] * kA[1][1] + kA[1][2] * kA[1][2] + kA[2][2] * kA[2][2]); 481 Real const multiplier = (Real)8; 482 Real const epsilon = std::numeric_limits<Real>::epsilon(); 483 Real const threshold = multiplier * epsilon * norm; 484 485 for (unsigned int i = 0; i < msSvdMaxIterations; i++) 486 { 487 Real fTmp, fTmp0, fTmp1; 488 Real fSin0, fCos0, fTan0; 489 Real fSin1, fCos1, fTan1; 490 491 bool bTest1 = (Math::Abs(kA[0][1]) <= threshold); 492 bool bTest2 = (Math::Abs(kA[1][2]) <= threshold); 493 if ( bTest1 ) 494 { 495 if ( bTest2 ) 496 { 497 kS[0] = kA[0][0]; 498 kS[1] = kA[1][1]; 499 kS[2] = kA[2][2]; 500 break; 501 } 502 else 503 { 504 // 2x2 closed form factorization 505 fTmp = (kA[1][1]*kA[1][1] - kA[2][2]*kA[2][2] + 506 kA[1][2]*kA[1][2])/(kA[1][2]*kA[2][2]); 507 fTan0 = 0.5f*(fTmp+Math::Sqrt(fTmp*fTmp + 4.0f)); 508 fCos0 = Math::InvSqrt(1.0f+fTan0*fTan0); 509 fSin0 = fTan0*fCos0; 510 511 for (iCol = 0; iCol < 3; iCol++) 512 { 513 fTmp0 = kL[iCol][1]; 514 fTmp1 = kL[iCol][2]; 515 kL[iCol][1] = fCos0*fTmp0-fSin0*fTmp1; 516 kL[iCol][2] = fSin0*fTmp0+fCos0*fTmp1; 517 } 518 519 fTan1 = (kA[1][2]-kA[2][2]*fTan0)/kA[1][1]; 520 fCos1 = Math::InvSqrt(1.0f+fTan1*fTan1); 521 fSin1 = -fTan1*fCos1; 522 523 for (iRow = 0; iRow < 3; iRow++) 524 { 525 fTmp0 = kR[1][iRow]; 526 fTmp1 = kR[2][iRow]; 527 kR[1][iRow] = fCos1*fTmp0-fSin1*fTmp1; 528 kR[2][iRow] = fSin1*fTmp0+fCos1*fTmp1; 529 } 530 531 kS[0] = kA[0][0]; 532 kS[1] = fCos0*fCos1*kA[1][1] - 533 fSin1*(fCos0*kA[1][2]-fSin0*kA[2][2]); 534 kS[2] = fSin0*fSin1*kA[1][1] + 535 fCos1*(fSin0*kA[1][2]+fCos0*kA[2][2]); 536 break; 537 } 538 } 539 else 540 { 541 if ( bTest2 ) 542 { 543 // 2x2 closed form factorization 544 fTmp = (kA[0][0]*kA[0][0] - kA[1][1]*kA[1][1] + 545 kA[0][1]*kA[0][1])/(kA[0][1]*kA[1][1]); 546 fTan0 = 0.5f*(fTmp+Math::Sqrt(fTmp*fTmp + 4.0f)); 547 fCos0 = Math::InvSqrt(1.0f+fTan0*fTan0); 548 fSin0 = fTan0*fCos0; 549 550 for (iCol = 0; iCol < 3; iCol++) 551 { 552 fTmp0 = kL[iCol][0]; 553 fTmp1 = kL[iCol][1]; 554 kL[iCol][0] = fCos0*fTmp0-fSin0*fTmp1; 555 kL[iCol][1] = fSin0*fTmp0+fCos0*fTmp1; 556 } 557 558 fTan1 = (kA[0][1]-kA[1][1]*fTan0)/kA[0][0]; 559 fCos1 = Math::InvSqrt(1.0f+fTan1*fTan1); 560 fSin1 = -fTan1*fCos1; 561 562 for (iRow = 0; iRow < 3; iRow++) 563 { 564 fTmp0 = kR[0][iRow]; 565 fTmp1 = kR[1][iRow]; 566 kR[0][iRow] = fCos1*fTmp0-fSin1*fTmp1; 567 kR[1][iRow] = fSin1*fTmp0+fCos1*fTmp1; 568 } 569 570 kS[0] = fCos0*fCos1*kA[0][0] - 571 fSin1*(fCos0*kA[0][1]-fSin0*kA[1][1]); 572 kS[1] = fSin0*fSin1*kA[0][0] + 573 fCos1*(fSin0*kA[0][1]+fCos0*kA[1][1]); 574 kS[2] = kA[2][2]; 575 break; 576 } 577 else 578 { 579 GolubKahanStep(kA,kL,kR); 580 } 581 } 582 } 583 584 // positize diagonal 585 for (iRow = 0; iRow < 3; iRow++) 586 { 587 if ( kS[iRow] < 0.0 ) 588 { 589 kS[iRow] = -kS[iRow]; 590 for (iCol = 0; iCol < 3; iCol++) 591 kR[iRow][iCol] = -kR[iRow][iCol]; 592 } 593 } 594 } 595 //----------------------------------------------------------------------- SingularValueComposition(const Matrix3 & kL,const Vector3 & kS,const Matrix3 & kR)596 void Matrix3::SingularValueComposition (const Matrix3& kL, 597 const Vector3& kS, const Matrix3& kR) 598 { 599 size_t iRow, iCol; 600 Matrix3 kTmp; 601 602 // product S*R 603 for (iRow = 0; iRow < 3; iRow++) 604 { 605 for (iCol = 0; iCol < 3; iCol++) 606 kTmp[iRow][iCol] = kS[iRow]*kR[iRow][iCol]; 607 } 608 609 // product L*S*R 610 for (iRow = 0; iRow < 3; iRow++) 611 { 612 for (iCol = 0; iCol < 3; iCol++) 613 { 614 m[iRow][iCol] = 0.0; 615 for (int iMid = 0; iMid < 3; iMid++) 616 m[iRow][iCol] += kL[iRow][iMid]*kTmp[iMid][iCol]; 617 } 618 } 619 } 620 //----------------------------------------------------------------------- Orthonormalize()621 void Matrix3::Orthonormalize () 622 { 623 // Algorithm uses Gram-Schmidt orthogonalization. If 'this' matrix is 624 // M = [m0|m1|m2], then orthonormal output matrix is Q = [q0|q1|q2], 625 // 626 // q0 = m0/|m0| 627 // q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0| 628 // q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1| 629 // 630 // where |V| indicates length of vector V and A*B indicates dot 631 // product of vectors A and B. 632 633 // compute q0 634 Real fInvLength = Math::InvSqrt(m[0][0]*m[0][0] 635 + m[1][0]*m[1][0] + 636 m[2][0]*m[2][0]); 637 638 m[0][0] *= fInvLength; 639 m[1][0] *= fInvLength; 640 m[2][0] *= fInvLength; 641 642 // compute q1 643 Real fDot0 = 644 m[0][0]*m[0][1] + 645 m[1][0]*m[1][1] + 646 m[2][0]*m[2][1]; 647 648 m[0][1] -= fDot0*m[0][0]; 649 m[1][1] -= fDot0*m[1][0]; 650 m[2][1] -= fDot0*m[2][0]; 651 652 fInvLength = Math::InvSqrt(m[0][1]*m[0][1] + 653 m[1][1]*m[1][1] + 654 m[2][1]*m[2][1]); 655 656 m[0][1] *= fInvLength; 657 m[1][1] *= fInvLength; 658 m[2][1] *= fInvLength; 659 660 // compute q2 661 Real fDot1 = 662 m[0][1]*m[0][2] + 663 m[1][1]*m[1][2] + 664 m[2][1]*m[2][2]; 665 666 fDot0 = 667 m[0][0]*m[0][2] + 668 m[1][0]*m[1][2] + 669 m[2][0]*m[2][2]; 670 671 m[0][2] -= fDot0*m[0][0] + fDot1*m[0][1]; 672 m[1][2] -= fDot0*m[1][0] + fDot1*m[1][1]; 673 m[2][2] -= fDot0*m[2][0] + fDot1*m[2][1]; 674 675 fInvLength = Math::InvSqrt(m[0][2]*m[0][2] + 676 m[1][2]*m[1][2] + 677 m[2][2]*m[2][2]); 678 679 m[0][2] *= fInvLength; 680 m[1][2] *= fInvLength; 681 m[2][2] *= fInvLength; 682 } 683 //----------------------------------------------------------------------- QDUDecomposition(Matrix3 & kQ,Vector3 & kD,Vector3 & kU) const684 void Matrix3::QDUDecomposition (Matrix3& kQ, 685 Vector3& kD, Vector3& kU) const 686 { 687 // Factor M = QR = QDU where Q is orthogonal, D is diagonal, 688 // and U is upper triangular with ones on its diagonal. Algorithm uses 689 // Gram-Schmidt orthogonalization (the QR algorithm). 690 // 691 // If M = [ m0 | m1 | m2 ] and Q = [ q0 | q1 | q2 ], then 692 // 693 // q0 = m0/|m0| 694 // q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0| 695 // q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1| 696 // 697 // where |V| indicates length of vector V and A*B indicates dot 698 // product of vectors A and B. The matrix R has entries 699 // 700 // r00 = q0*m0 r01 = q0*m1 r02 = q0*m2 701 // r10 = 0 r11 = q1*m1 r12 = q1*m2 702 // r20 = 0 r21 = 0 r22 = q2*m2 703 // 704 // so D = diag(r00,r11,r22) and U has entries u01 = r01/r00, 705 // u02 = r02/r00, and u12 = r12/r11. 706 707 // Q = rotation 708 // D = scaling 709 // U = shear 710 711 // D stores the three diagonal entries r00, r11, r22 712 // U stores the entries U[0] = u01, U[1] = u02, U[2] = u12 713 714 // build orthogonal matrix Q 715 Real fInvLength = Math::InvSqrt(m[0][0]*m[0][0] + m[1][0]*m[1][0] + m[2][0]*m[2][0]); 716 717 kQ[0][0] = m[0][0]*fInvLength; 718 kQ[1][0] = m[1][0]*fInvLength; 719 kQ[2][0] = m[2][0]*fInvLength; 720 721 Real fDot = kQ[0][0]*m[0][1] + kQ[1][0]*m[1][1] + 722 kQ[2][0]*m[2][1]; 723 kQ[0][1] = m[0][1]-fDot*kQ[0][0]; 724 kQ[1][1] = m[1][1]-fDot*kQ[1][0]; 725 kQ[2][1] = m[2][1]-fDot*kQ[2][0]; 726 fInvLength = Math::InvSqrt(kQ[0][1]*kQ[0][1] + kQ[1][1]*kQ[1][1] + kQ[2][1]*kQ[2][1]); 727 728 kQ[0][1] *= fInvLength; 729 kQ[1][1] *= fInvLength; 730 kQ[2][1] *= fInvLength; 731 732 fDot = kQ[0][0]*m[0][2] + kQ[1][0]*m[1][2] + 733 kQ[2][0]*m[2][2]; 734 kQ[0][2] = m[0][2]-fDot*kQ[0][0]; 735 kQ[1][2] = m[1][2]-fDot*kQ[1][0]; 736 kQ[2][2] = m[2][2]-fDot*kQ[2][0]; 737 fDot = kQ[0][1]*m[0][2] + kQ[1][1]*m[1][2] + 738 kQ[2][1]*m[2][2]; 739 kQ[0][2] -= fDot*kQ[0][1]; 740 kQ[1][2] -= fDot*kQ[1][1]; 741 kQ[2][2] -= fDot*kQ[2][1]; 742 fInvLength = Math::InvSqrt(kQ[0][2]*kQ[0][2] + kQ[1][2]*kQ[1][2] + kQ[2][2]*kQ[2][2]); 743 744 kQ[0][2] *= fInvLength; 745 kQ[1][2] *= fInvLength; 746 kQ[2][2] *= fInvLength; 747 748 // guarantee that orthogonal matrix has determinant 1 (no reflections) 749 Real fDet = kQ[0][0]*kQ[1][1]*kQ[2][2] + kQ[0][1]*kQ[1][2]*kQ[2][0] + 750 kQ[0][2]*kQ[1][0]*kQ[2][1] - kQ[0][2]*kQ[1][1]*kQ[2][0] - 751 kQ[0][1]*kQ[1][0]*kQ[2][2] - kQ[0][0]*kQ[1][2]*kQ[2][1]; 752 753 if ( fDet < 0.0 ) 754 { 755 for (size_t iRow = 0; iRow < 3; iRow++) 756 for (size_t iCol = 0; iCol < 3; iCol++) 757 kQ[iRow][iCol] = -kQ[iRow][iCol]; 758 } 759 760 // build "right" matrix R 761 Matrix3 kR; 762 kR[0][0] = kQ[0][0]*m[0][0] + kQ[1][0]*m[1][0] + 763 kQ[2][0]*m[2][0]; 764 kR[0][1] = kQ[0][0]*m[0][1] + kQ[1][0]*m[1][1] + 765 kQ[2][0]*m[2][1]; 766 kR[1][1] = kQ[0][1]*m[0][1] + kQ[1][1]*m[1][1] + 767 kQ[2][1]*m[2][1]; 768 kR[0][2] = kQ[0][0]*m[0][2] + kQ[1][0]*m[1][2] + 769 kQ[2][0]*m[2][2]; 770 kR[1][2] = kQ[0][1]*m[0][2] + kQ[1][1]*m[1][2] + 771 kQ[2][1]*m[2][2]; 772 kR[2][2] = kQ[0][2]*m[0][2] + kQ[1][2]*m[1][2] + 773 kQ[2][2]*m[2][2]; 774 775 // the scaling component 776 kD[0] = kR[0][0]; 777 kD[1] = kR[1][1]; 778 kD[2] = kR[2][2]; 779 780 // the shear component 781 Real fInvD0 = 1.0f/kD[0]; 782 kU[0] = kR[0][1]*fInvD0; 783 kU[1] = kR[0][2]*fInvD0; 784 kU[2] = kR[1][2]/kD[1]; 785 } 786 //----------------------------------------------------------------------- MaxCubicRoot(Real afCoeff[3])787 Real Matrix3::MaxCubicRoot (Real afCoeff[3]) 788 { 789 // Spectral norm is for A^T*A, so characteristic polynomial 790 // P(x) = c[0]+c[1]*x+c[2]*x^2+x^3 has three positive real roots. 791 // This yields the assertions c[0] < 0 and c[2]*c[2] >= 3*c[1]. 792 793 // quick out for uniform scale (triple root) 794 const Real fOneThird = 1.0/3.0; 795 const Real fEpsilon = 1e-06; 796 Real fDiscr = afCoeff[2]*afCoeff[2] - 3.0f*afCoeff[1]; 797 if ( fDiscr <= fEpsilon ) 798 return -fOneThird*afCoeff[2]; 799 800 // Compute an upper bound on roots of P(x). This assumes that A^T*A 801 // has been scaled by its largest entry. 802 Real fX = 1.0; 803 Real fPoly = afCoeff[0]+fX*(afCoeff[1]+fX*(afCoeff[2]+fX)); 804 if ( fPoly < 0.0 ) 805 { 806 // uses a matrix norm to find an upper bound on maximum root 807 fX = Math::Abs(afCoeff[0]); 808 Real fTmp = 1.0f+Math::Abs(afCoeff[1]); 809 if ( fTmp > fX ) 810 fX = fTmp; 811 fTmp = 1.0f+Math::Abs(afCoeff[2]); 812 if ( fTmp > fX ) 813 fX = fTmp; 814 } 815 816 // Newton's method to find root 817 Real fTwoC2 = 2.0f*afCoeff[2]; 818 for (int i = 0; i < 16; i++) 819 { 820 fPoly = afCoeff[0]+fX*(afCoeff[1]+fX*(afCoeff[2]+fX)); 821 if ( Math::Abs(fPoly) <= fEpsilon ) 822 return fX; 823 824 Real fDeriv = afCoeff[1]+fX*(fTwoC2+3.0f*fX); 825 fX -= fPoly/fDeriv; 826 } 827 828 return fX; 829 } 830 //----------------------------------------------------------------------- SpectralNorm() const831 Real Matrix3::SpectralNorm () const 832 { 833 Matrix3 kP; 834 size_t iRow, iCol; 835 Real fPmax = 0.0; 836 for (iRow = 0; iRow < 3; iRow++) 837 { 838 for (iCol = 0; iCol < 3; iCol++) 839 { 840 kP[iRow][iCol] = 0.0; 841 for (int iMid = 0; iMid < 3; iMid++) 842 { 843 kP[iRow][iCol] += 844 m[iMid][iRow]*m[iMid][iCol]; 845 } 846 if ( kP[iRow][iCol] > fPmax ) 847 fPmax = kP[iRow][iCol]; 848 } 849 } 850 851 Real fInvPmax = 1.0f/fPmax; 852 for (iRow = 0; iRow < 3; iRow++) 853 { 854 for (iCol = 0; iCol < 3; iCol++) 855 kP[iRow][iCol] *= fInvPmax; 856 } 857 858 Real afCoeff[3]; 859 afCoeff[0] = -(kP[0][0]*(kP[1][1]*kP[2][2]-kP[1][2]*kP[2][1]) + 860 kP[0][1]*(kP[2][0]*kP[1][2]-kP[1][0]*kP[2][2]) + 861 kP[0][2]*(kP[1][0]*kP[2][1]-kP[2][0]*kP[1][1])); 862 afCoeff[1] = kP[0][0]*kP[1][1]-kP[0][1]*kP[1][0] + 863 kP[0][0]*kP[2][2]-kP[0][2]*kP[2][0] + 864 kP[1][1]*kP[2][2]-kP[1][2]*kP[2][1]; 865 afCoeff[2] = -(kP[0][0]+kP[1][1]+kP[2][2]); 866 867 Real fRoot = MaxCubicRoot(afCoeff); 868 Real fNorm = Math::Sqrt(fPmax*fRoot); 869 return fNorm; 870 } 871 //----------------------------------------------------------------------- ToAngleAxis(Vector3 & rkAxis,Radian & rfRadians) const872 void Matrix3::ToAngleAxis (Vector3& rkAxis, Radian& rfRadians) const 873 { 874 // Let (x,y,z) be the unit-length axis and let A be an angle of rotation. 875 // The rotation matrix is R = I + sin(A)*P + (1-cos(A))*P^2 where 876 // I is the identity and 877 // 878 // +- -+ 879 // P = | 0 -z +y | 880 // | +z 0 -x | 881 // | -y +x 0 | 882 // +- -+ 883 // 884 // If A > 0, R represents a counterclockwise rotation about the axis in 885 // the sense of looking from the tip of the axis vector towards the 886 // origin. Some algebra will show that 887 // 888 // cos(A) = (trace(R)-1)/2 and R - R^t = 2*sin(A)*P 889 // 890 // In the event that A = pi, R-R^t = 0 which prevents us from extracting 891 // the axis through P. Instead note that R = I+2*P^2 when A = pi, so 892 // P^2 = (R-I)/2. The diagonal entries of P^2 are x^2-1, y^2-1, and 893 // z^2-1. We can solve these for axis (x,y,z). Because the angle is pi, 894 // it does not matter which sign you choose on the square roots. 895 896 Real fTrace = m[0][0] + m[1][1] + m[2][2]; 897 Real fCos = 0.5f*(fTrace-1.0f); 898 rfRadians = Math::ACos(fCos); // in [0,PI] 899 900 if ( rfRadians > Radian(0.0) ) 901 { 902 if ( rfRadians < Radian(Math::PI) ) 903 { 904 rkAxis.x = m[2][1]-m[1][2]; 905 rkAxis.y = m[0][2]-m[2][0]; 906 rkAxis.z = m[1][0]-m[0][1]; 907 rkAxis.normalise(); 908 } 909 else 910 { 911 // angle is PI 912 float fHalfInverse; 913 if ( m[0][0] >= m[1][1] ) 914 { 915 // r00 >= r11 916 if ( m[0][0] >= m[2][2] ) 917 { 918 // r00 is maximum diagonal term 919 rkAxis.x = 0.5f*Math::Sqrt(m[0][0] - 920 m[1][1] - m[2][2] + 1.0f); 921 fHalfInverse = 0.5f/rkAxis.x; 922 rkAxis.y = fHalfInverse*m[0][1]; 923 rkAxis.z = fHalfInverse*m[0][2]; 924 } 925 else 926 { 927 // r22 is maximum diagonal term 928 rkAxis.z = 0.5f*Math::Sqrt(m[2][2] - 929 m[0][0] - m[1][1] + 1.0f); 930 fHalfInverse = 0.5f/rkAxis.z; 931 rkAxis.x = fHalfInverse*m[0][2]; 932 rkAxis.y = fHalfInverse*m[1][2]; 933 } 934 } 935 else 936 { 937 // r11 > r00 938 if ( m[1][1] >= m[2][2] ) 939 { 940 // r11 is maximum diagonal term 941 rkAxis.y = 0.5f*Math::Sqrt(m[1][1] - 942 m[0][0] - m[2][2] + 1.0f); 943 fHalfInverse = 0.5f/rkAxis.y; 944 rkAxis.x = fHalfInverse*m[0][1]; 945 rkAxis.z = fHalfInverse*m[1][2]; 946 } 947 else 948 { 949 // r22 is maximum diagonal term 950 rkAxis.z = 0.5f*Math::Sqrt(m[2][2] - 951 m[0][0] - m[1][1] + 1.0f); 952 fHalfInverse = 0.5f/rkAxis.z; 953 rkAxis.x = fHalfInverse*m[0][2]; 954 rkAxis.y = fHalfInverse*m[1][2]; 955 } 956 } 957 } 958 } 959 else 960 { 961 // The angle is 0 and the matrix is the identity. Any axis will 962 // work, so just use the x-axis. 963 rkAxis.x = 1.0; 964 rkAxis.y = 0.0; 965 rkAxis.z = 0.0; 966 } 967 } 968 //----------------------------------------------------------------------- FromAngleAxis(const Vector3 & rkAxis,const Radian & fRadians)969 void Matrix3::FromAngleAxis (const Vector3& rkAxis, const Radian& fRadians) 970 { 971 Real fCos = Math::Cos(fRadians); 972 Real fSin = Math::Sin(fRadians); 973 Real fOneMinusCos = 1.0f-fCos; 974 Real fX2 = rkAxis.x*rkAxis.x; 975 Real fY2 = rkAxis.y*rkAxis.y; 976 Real fZ2 = rkAxis.z*rkAxis.z; 977 Real fXYM = rkAxis.x*rkAxis.y*fOneMinusCos; 978 Real fXZM = rkAxis.x*rkAxis.z*fOneMinusCos; 979 Real fYZM = rkAxis.y*rkAxis.z*fOneMinusCos; 980 Real fXSin = rkAxis.x*fSin; 981 Real fYSin = rkAxis.y*fSin; 982 Real fZSin = rkAxis.z*fSin; 983 984 m[0][0] = fX2*fOneMinusCos+fCos; 985 m[0][1] = fXYM-fZSin; 986 m[0][2] = fXZM+fYSin; 987 m[1][0] = fXYM+fZSin; 988 m[1][1] = fY2*fOneMinusCos+fCos; 989 m[1][2] = fYZM-fXSin; 990 m[2][0] = fXZM-fYSin; 991 m[2][1] = fYZM+fXSin; 992 m[2][2] = fZ2*fOneMinusCos+fCos; 993 } 994 //----------------------------------------------------------------------- ToEulerAnglesXYZ(Radian & rfYAngle,Radian & rfPAngle,Radian & rfRAngle) const995 bool Matrix3::ToEulerAnglesXYZ (Radian& rfYAngle, Radian& rfPAngle, 996 Radian& rfRAngle) const 997 { 998 // rot = cy*cz -cy*sz sy 999 // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx 1000 // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy 1001 1002 rfPAngle = Radian(Math::ASin(m[0][2])); 1003 if ( rfPAngle < Radian(Math::HALF_PI) ) 1004 { 1005 if ( rfPAngle > Radian(-Math::HALF_PI) ) 1006 { 1007 rfYAngle = Math::ATan2(-m[1][2],m[2][2]); 1008 rfRAngle = Math::ATan2(-m[0][1],m[0][0]); 1009 return true; 1010 } 1011 else 1012 { 1013 // WARNING. Not a unique solution. 1014 Radian fRmY = Math::ATan2(m[1][0],m[1][1]); 1015 rfRAngle = Radian(0.0); // any angle works 1016 rfYAngle = rfRAngle - fRmY; 1017 return false; 1018 } 1019 } 1020 else 1021 { 1022 // WARNING. Not a unique solution. 1023 Radian fRpY = Math::ATan2(m[1][0],m[1][1]); 1024 rfRAngle = Radian(0.0); // any angle works 1025 rfYAngle = fRpY - rfRAngle; 1026 return false; 1027 } 1028 } 1029 //----------------------------------------------------------------------- ToEulerAnglesXZY(Radian & rfYAngle,Radian & rfPAngle,Radian & rfRAngle) const1030 bool Matrix3::ToEulerAnglesXZY (Radian& rfYAngle, Radian& rfPAngle, 1031 Radian& rfRAngle) const 1032 { 1033 // rot = cy*cz -sz cz*sy 1034 // sx*sy+cx*cy*sz cx*cz -cy*sx+cx*sy*sz 1035 // -cx*sy+cy*sx*sz cz*sx cx*cy+sx*sy*sz 1036 1037 rfPAngle = Math::ASin(-m[0][1]); 1038 if ( rfPAngle < Radian(Math::HALF_PI) ) 1039 { 1040 if ( rfPAngle > Radian(-Math::HALF_PI) ) 1041 { 1042 rfYAngle = Math::ATan2(m[2][1],m[1][1]); 1043 rfRAngle = Math::ATan2(m[0][2],m[0][0]); 1044 return true; 1045 } 1046 else 1047 { 1048 // WARNING. Not a unique solution. 1049 Radian fRmY = Math::ATan2(-m[2][0],m[2][2]); 1050 rfRAngle = Radian(0.0); // any angle works 1051 rfYAngle = rfRAngle - fRmY; 1052 return false; 1053 } 1054 } 1055 else 1056 { 1057 // WARNING. Not a unique solution. 1058 Radian fRpY = Math::ATan2(-m[2][0],m[2][2]); 1059 rfRAngle = Radian(0.0); // any angle works 1060 rfYAngle = fRpY - rfRAngle; 1061 return false; 1062 } 1063 } 1064 //----------------------------------------------------------------------- ToEulerAnglesYXZ(Radian & rfYAngle,Radian & rfPAngle,Radian & rfRAngle) const1065 bool Matrix3::ToEulerAnglesYXZ (Radian& rfYAngle, Radian& rfPAngle, 1066 Radian& rfRAngle) const 1067 { 1068 // rot = cy*cz+sx*sy*sz cz*sx*sy-cy*sz cx*sy 1069 // cx*sz cx*cz -sx 1070 // -cz*sy+cy*sx*sz cy*cz*sx+sy*sz cx*cy 1071 1072 rfPAngle = Math::ASin(-m[1][2]); 1073 if ( rfPAngle < Radian(Math::HALF_PI) ) 1074 { 1075 if ( rfPAngle > Radian(-Math::HALF_PI) ) 1076 { 1077 rfYAngle = Math::ATan2(m[0][2],m[2][2]); 1078 rfRAngle = Math::ATan2(m[1][0],m[1][1]); 1079 return true; 1080 } 1081 else 1082 { 1083 // WARNING. Not a unique solution. 1084 Radian fRmY = Math::ATan2(-m[0][1],m[0][0]); 1085 rfRAngle = Radian(0.0); // any angle works 1086 rfYAngle = rfRAngle - fRmY; 1087 return false; 1088 } 1089 } 1090 else 1091 { 1092 // WARNING. Not a unique solution. 1093 Radian fRpY = Math::ATan2(-m[0][1],m[0][0]); 1094 rfRAngle = Radian(0.0); // any angle works 1095 rfYAngle = fRpY - rfRAngle; 1096 return false; 1097 } 1098 } 1099 //----------------------------------------------------------------------- ToEulerAnglesYZX(Radian & rfYAngle,Radian & rfPAngle,Radian & rfRAngle) const1100 bool Matrix3::ToEulerAnglesYZX (Radian& rfYAngle, Radian& rfPAngle, 1101 Radian& rfRAngle) const 1102 { 1103 // rot = cy*cz sx*sy-cx*cy*sz cx*sy+cy*sx*sz 1104 // sz cx*cz -cz*sx 1105 // -cz*sy cy*sx+cx*sy*sz cx*cy-sx*sy*sz 1106 1107 rfPAngle = Math::ASin(m[1][0]); 1108 if ( rfPAngle < Radian(Math::HALF_PI) ) 1109 { 1110 if ( rfPAngle > Radian(-Math::HALF_PI) ) 1111 { 1112 rfYAngle = Math::ATan2(-m[2][0],m[0][0]); 1113 rfRAngle = Math::ATan2(-m[1][2],m[1][1]); 1114 return true; 1115 } 1116 else 1117 { 1118 // WARNING. Not a unique solution. 1119 Radian fRmY = Math::ATan2(m[2][1],m[2][2]); 1120 rfRAngle = Radian(0.0); // any angle works 1121 rfYAngle = rfRAngle - fRmY; 1122 return false; 1123 } 1124 } 1125 else 1126 { 1127 // WARNING. Not a unique solution. 1128 Radian fRpY = Math::ATan2(m[2][1],m[2][2]); 1129 rfRAngle = Radian(0.0); // any angle works 1130 rfYAngle = fRpY - rfRAngle; 1131 return false; 1132 } 1133 } 1134 //----------------------------------------------------------------------- ToEulerAnglesZXY(Radian & rfYAngle,Radian & rfPAngle,Radian & rfRAngle) const1135 bool Matrix3::ToEulerAnglesZXY (Radian& rfYAngle, Radian& rfPAngle, 1136 Radian& rfRAngle) const 1137 { 1138 // rot = cy*cz-sx*sy*sz -cx*sz cz*sy+cy*sx*sz 1139 // cz*sx*sy+cy*sz cx*cz -cy*cz*sx+sy*sz 1140 // -cx*sy sx cx*cy 1141 1142 rfPAngle = Math::ASin(m[2][1]); 1143 if ( rfPAngle < Radian(Math::HALF_PI) ) 1144 { 1145 if ( rfPAngle > Radian(-Math::HALF_PI) ) 1146 { 1147 rfYAngle = Math::ATan2(-m[0][1],m[1][1]); 1148 rfRAngle = Math::ATan2(-m[2][0],m[2][2]); 1149 return true; 1150 } 1151 else 1152 { 1153 // WARNING. Not a unique solution. 1154 Radian fRmY = Math::ATan2(m[0][2],m[0][0]); 1155 rfRAngle = Radian(0.0); // any angle works 1156 rfYAngle = rfRAngle - fRmY; 1157 return false; 1158 } 1159 } 1160 else 1161 { 1162 // WARNING. Not a unique solution. 1163 Radian fRpY = Math::ATan2(m[0][2],m[0][0]); 1164 rfRAngle = Radian(0.0); // any angle works 1165 rfYAngle = fRpY - rfRAngle; 1166 return false; 1167 } 1168 } 1169 //----------------------------------------------------------------------- ToEulerAnglesZYX(Radian & rfYAngle,Radian & rfPAngle,Radian & rfRAngle) const1170 bool Matrix3::ToEulerAnglesZYX (Radian& rfYAngle, Radian& rfPAngle, 1171 Radian& rfRAngle) const 1172 { 1173 // rot = cy*cz cz*sx*sy-cx*sz cx*cz*sy+sx*sz 1174 // cy*sz cx*cz+sx*sy*sz -cz*sx+cx*sy*sz 1175 // -sy cy*sx cx*cy 1176 1177 rfPAngle = Math::ASin(-m[2][0]); 1178 if ( rfPAngle < Radian(Math::HALF_PI) ) 1179 { 1180 if ( rfPAngle > Radian(-Math::HALF_PI) ) 1181 { 1182 rfYAngle = Math::ATan2(m[1][0],m[0][0]); 1183 rfRAngle = Math::ATan2(m[2][1],m[2][2]); 1184 return true; 1185 } 1186 else 1187 { 1188 // WARNING. Not a unique solution. 1189 Radian fRmY = Math::ATan2(-m[0][1],m[0][2]); 1190 rfRAngle = Radian(0.0); // any angle works 1191 rfYAngle = rfRAngle - fRmY; 1192 return false; 1193 } 1194 } 1195 else 1196 { 1197 // WARNING. Not a unique solution. 1198 Radian fRpY = Math::ATan2(-m[0][1],m[0][2]); 1199 rfRAngle = Radian(0.0); // any angle works 1200 rfYAngle = fRpY - rfRAngle; 1201 return false; 1202 } 1203 } 1204 //----------------------------------------------------------------------- FromEulerAnglesXYZ(const Radian & fYAngle,const Radian & fPAngle,const Radian & fRAngle)1205 void Matrix3::FromEulerAnglesXYZ (const Radian& fYAngle, const Radian& fPAngle, 1206 const Radian& fRAngle) 1207 { 1208 Real fCos, fSin; 1209 1210 fCos = Math::Cos(fYAngle); 1211 fSin = Math::Sin(fYAngle); 1212 Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos); 1213 1214 fCos = Math::Cos(fPAngle); 1215 fSin = Math::Sin(fPAngle); 1216 Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos); 1217 1218 fCos = Math::Cos(fRAngle); 1219 fSin = Math::Sin(fRAngle); 1220 Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0); 1221 1222 *this = kXMat*(kYMat*kZMat); 1223 } 1224 //----------------------------------------------------------------------- FromEulerAnglesXZY(const Radian & fYAngle,const Radian & fPAngle,const Radian & fRAngle)1225 void Matrix3::FromEulerAnglesXZY (const Radian& fYAngle, const Radian& fPAngle, 1226 const Radian& fRAngle) 1227 { 1228 Real fCos, fSin; 1229 1230 fCos = Math::Cos(fYAngle); 1231 fSin = Math::Sin(fYAngle); 1232 Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos); 1233 1234 fCos = Math::Cos(fPAngle); 1235 fSin = Math::Sin(fPAngle); 1236 Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0); 1237 1238 fCos = Math::Cos(fRAngle); 1239 fSin = Math::Sin(fRAngle); 1240 Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos); 1241 1242 *this = kXMat*(kZMat*kYMat); 1243 } 1244 //----------------------------------------------------------------------- FromEulerAnglesYXZ(const Radian & fYAngle,const Radian & fPAngle,const Radian & fRAngle)1245 void Matrix3::FromEulerAnglesYXZ (const Radian& fYAngle, const Radian& fPAngle, 1246 const Radian& fRAngle) 1247 { 1248 Real fCos, fSin; 1249 1250 fCos = Math::Cos(fYAngle); 1251 fSin = Math::Sin(fYAngle); 1252 Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos); 1253 1254 fCos = Math::Cos(fPAngle); 1255 fSin = Math::Sin(fPAngle); 1256 Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos); 1257 1258 fCos = Math::Cos(fRAngle); 1259 fSin = Math::Sin(fRAngle); 1260 Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0); 1261 1262 *this = kYMat*(kXMat*kZMat); 1263 } 1264 //----------------------------------------------------------------------- FromEulerAnglesYZX(const Radian & fYAngle,const Radian & fPAngle,const Radian & fRAngle)1265 void Matrix3::FromEulerAnglesYZX (const Radian& fYAngle, const Radian& fPAngle, 1266 const Radian& fRAngle) 1267 { 1268 Real fCos, fSin; 1269 1270 fCos = Math::Cos(fYAngle); 1271 fSin = Math::Sin(fYAngle); 1272 Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos); 1273 1274 fCos = Math::Cos(fPAngle); 1275 fSin = Math::Sin(fPAngle); 1276 Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0); 1277 1278 fCos = Math::Cos(fRAngle); 1279 fSin = Math::Sin(fRAngle); 1280 Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos); 1281 1282 *this = kYMat*(kZMat*kXMat); 1283 } 1284 //----------------------------------------------------------------------- FromEulerAnglesZXY(const Radian & fYAngle,const Radian & fPAngle,const Radian & fRAngle)1285 void Matrix3::FromEulerAnglesZXY (const Radian& fYAngle, const Radian& fPAngle, 1286 const Radian& fRAngle) 1287 { 1288 Real fCos, fSin; 1289 1290 fCos = Math::Cos(fYAngle); 1291 fSin = Math::Sin(fYAngle); 1292 Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0); 1293 1294 fCos = Math::Cos(fPAngle); 1295 fSin = Math::Sin(fPAngle); 1296 Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos); 1297 1298 fCos = Math::Cos(fRAngle); 1299 fSin = Math::Sin(fRAngle); 1300 Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos); 1301 1302 *this = kZMat*(kXMat*kYMat); 1303 } 1304 //----------------------------------------------------------------------- FromEulerAnglesZYX(const Radian & fYAngle,const Radian & fPAngle,const Radian & fRAngle)1305 void Matrix3::FromEulerAnglesZYX (const Radian& fYAngle, const Radian& fPAngle, 1306 const Radian& fRAngle) 1307 { 1308 Real fCos, fSin; 1309 1310 fCos = Math::Cos(fYAngle); 1311 fSin = Math::Sin(fYAngle); 1312 Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0); 1313 1314 fCos = Math::Cos(fPAngle); 1315 fSin = Math::Sin(fPAngle); 1316 Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos); 1317 1318 fCos = Math::Cos(fRAngle); 1319 fSin = Math::Sin(fRAngle); 1320 Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos); 1321 1322 *this = kZMat*(kYMat*kXMat); 1323 } 1324 //----------------------------------------------------------------------- Tridiagonal(Real afDiag[3],Real afSubDiag[3])1325 void Matrix3::Tridiagonal (Real afDiag[3], Real afSubDiag[3]) 1326 { 1327 // Householder reduction T = Q^t M Q 1328 // Input: 1329 // mat, symmetric 3x3 matrix M 1330 // Output: 1331 // mat, orthogonal matrix Q 1332 // diag, diagonal entries of T 1333 // subd, subdiagonal entries of T (T is symmetric) 1334 1335 Real fA = m[0][0]; 1336 Real fB = m[0][1]; 1337 Real fC = m[0][2]; 1338 Real fD = m[1][1]; 1339 Real fE = m[1][2]; 1340 Real fF = m[2][2]; 1341 1342 afDiag[0] = fA; 1343 afSubDiag[2] = 0.0; 1344 if ( Math::Abs(fC) >= EPSILON ) 1345 { 1346 Real fLength = Math::Sqrt(fB*fB+fC*fC); 1347 Real fInvLength = 1.0f/fLength; 1348 fB *= fInvLength; 1349 fC *= fInvLength; 1350 Real fQ = 2.0f*fB*fE+fC*(fF-fD); 1351 afDiag[1] = fD+fC*fQ; 1352 afDiag[2] = fF-fC*fQ; 1353 afSubDiag[0] = fLength; 1354 afSubDiag[1] = fE-fB*fQ; 1355 m[0][0] = 1.0; 1356 m[0][1] = 0.0; 1357 m[0][2] = 0.0; 1358 m[1][0] = 0.0; 1359 m[1][1] = fB; 1360 m[1][2] = fC; 1361 m[2][0] = 0.0; 1362 m[2][1] = fC; 1363 m[2][2] = -fB; 1364 } 1365 else 1366 { 1367 afDiag[1] = fD; 1368 afDiag[2] = fF; 1369 afSubDiag[0] = fB; 1370 afSubDiag[1] = fE; 1371 m[0][0] = 1.0; 1372 m[0][1] = 0.0; 1373 m[0][2] = 0.0; 1374 m[1][0] = 0.0; 1375 m[1][1] = 1.0; 1376 m[1][2] = 0.0; 1377 m[2][0] = 0.0; 1378 m[2][1] = 0.0; 1379 m[2][2] = 1.0; 1380 } 1381 } 1382 //----------------------------------------------------------------------- QLAlgorithm(Real afDiag[3],Real afSubDiag[3])1383 bool Matrix3::QLAlgorithm (Real afDiag[3], Real afSubDiag[3]) 1384 { 1385 // QL iteration with implicit shifting to reduce matrix from tridiagonal 1386 // to diagonal 1387 1388 for (int i0 = 0; i0 < 3; i0++) 1389 { 1390 const unsigned int iMaxIter = 32; 1391 unsigned int iIter; 1392 for (iIter = 0; iIter < iMaxIter; iIter++) 1393 { 1394 int i1; 1395 for (i1 = i0; i1 <= 1; i1++) 1396 { 1397 Real fSum = Math::Abs(afDiag[i1]) + 1398 Math::Abs(afDiag[i1+1]); 1399 if ( Math::Abs(afSubDiag[i1]) + fSum == fSum ) 1400 break; 1401 } 1402 if ( i1 == i0 ) 1403 break; 1404 1405 Real fTmp0 = (afDiag[i0+1]-afDiag[i0])/(2.0f*afSubDiag[i0]); 1406 Real fTmp1 = Math::Sqrt(fTmp0*fTmp0+1.0f); 1407 if ( fTmp0 < 0.0 ) 1408 fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0-fTmp1); 1409 else 1410 fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0+fTmp1); 1411 Real fSin = 1.0; 1412 Real fCos = 1.0; 1413 Real fTmp2 = 0.0; 1414 for (int i2 = i1-1; i2 >= i0; i2--) 1415 { 1416 Real fTmp3 = fSin*afSubDiag[i2]; 1417 Real fTmp4 = fCos*afSubDiag[i2]; 1418 if ( Math::Abs(fTmp3) >= Math::Abs(fTmp0) ) 1419 { 1420 fCos = fTmp0/fTmp3; 1421 fTmp1 = Math::Sqrt(fCos*fCos+1.0f); 1422 afSubDiag[i2+1] = fTmp3*fTmp1; 1423 fSin = 1.0f/fTmp1; 1424 fCos *= fSin; 1425 } 1426 else 1427 { 1428 fSin = fTmp3/fTmp0; 1429 fTmp1 = Math::Sqrt(fSin*fSin+1.0f); 1430 afSubDiag[i2+1] = fTmp0*fTmp1; 1431 fCos = 1.0f/fTmp1; 1432 fSin *= fCos; 1433 } 1434 fTmp0 = afDiag[i2+1]-fTmp2; 1435 fTmp1 = (afDiag[i2]-fTmp0)*fSin+2.0f*fTmp4*fCos; 1436 fTmp2 = fSin*fTmp1; 1437 afDiag[i2+1] = fTmp0+fTmp2; 1438 fTmp0 = fCos*fTmp1-fTmp4; 1439 1440 for (int iRow = 0; iRow < 3; iRow++) 1441 { 1442 fTmp3 = m[iRow][i2+1]; 1443 m[iRow][i2+1] = fSin*m[iRow][i2] + 1444 fCos*fTmp3; 1445 m[iRow][i2] = fCos*m[iRow][i2] - 1446 fSin*fTmp3; 1447 } 1448 } 1449 afDiag[i0] -= fTmp2; 1450 afSubDiag[i0] = fTmp0; 1451 afSubDiag[i1] = 0.0; 1452 } 1453 1454 if ( iIter == iMaxIter ) 1455 { 1456 // should not get here under normal circumstances 1457 return false; 1458 } 1459 } 1460 1461 return true; 1462 } 1463 //----------------------------------------------------------------------- EigenSolveSymmetric(Real afEigenvalue[3],Vector3 akEigenvector[3]) const1464 void Matrix3::EigenSolveSymmetric (Real afEigenvalue[3], 1465 Vector3 akEigenvector[3]) const 1466 { 1467 Matrix3 kMatrix = *this; 1468 Real afSubDiag[3]; 1469 kMatrix.Tridiagonal(afEigenvalue,afSubDiag); 1470 kMatrix.QLAlgorithm(afEigenvalue,afSubDiag); 1471 1472 for (size_t i = 0; i < 3; i++) 1473 { 1474 akEigenvector[i][0] = kMatrix[0][i]; 1475 akEigenvector[i][1] = kMatrix[1][i]; 1476 akEigenvector[i][2] = kMatrix[2][i]; 1477 } 1478 1479 // make eigenvectors form a right--handed system 1480 Vector3 kCross = akEigenvector[1].crossProduct(akEigenvector[2]); 1481 Real fDet = akEigenvector[0].dotProduct(kCross); 1482 if ( fDet < 0.0 ) 1483 { 1484 akEigenvector[2][0] = - akEigenvector[2][0]; 1485 akEigenvector[2][1] = - akEigenvector[2][1]; 1486 akEigenvector[2][2] = - akEigenvector[2][2]; 1487 } 1488 } 1489 //----------------------------------------------------------------------- TensorProduct(const Vector3 & rkU,const Vector3 & rkV,Matrix3 & rkProduct)1490 void Matrix3::TensorProduct (const Vector3& rkU, const Vector3& rkV, 1491 Matrix3& rkProduct) 1492 { 1493 for (size_t iRow = 0; iRow < 3; iRow++) 1494 { 1495 for (size_t iCol = 0; iCol < 3; iCol++) 1496 rkProduct[iRow][iCol] = rkU[iRow]*rkV[iCol]; 1497 } 1498 } 1499 //----------------------------------------------------------------------- 1500 } 1501