Changeset 13041 for OpenSceneGraph/trunk/src/osg/MatrixDecomposition.cpp
- Timestamp:
- 03/21/12 18:36:20 (15 months ago)
- Files:
-
- 1 modified
Legend:
- Unmodified
- Added
- Removed
-
OpenSceneGraph/trunk/src/osg/MatrixDecomposition.cpp
r9639 r13041 46 46 enum QuatPart {X, Y, Z, W}; 47 47 typedef double _HMatrix[4][4]; 48 typedef Quat HVect; // Homogeneous 3D vector 48 typedef Quat HVect; // Homogeneous 3D vector 49 49 typedef struct 50 50 { 51 51 osg::Vec4d t; // Translation Component; 52 52 Quat q; // Essential Rotation 53 Quat u; //Stretch rotation 54 HVect k; //Sign of determinant 55 double f; // Sign of determinant 53 Quat u; //Stretch rotation 54 HVect k; //Sign of determinant 55 double f; // Sign of determinant 56 56 } _affineParts; 57 57 … … 344 344 int i, j; 345 345 346 mat_tpose(Mk,=,M,3); 346 mat_tpose(Mk,=,M,3); 347 347 M_one = norm_one(Mk); M_inf = norm_inf(Mk); 348 348 … … 353 353 if (det==0.0) 354 354 { 355 do_rank2(Mk, MadjTk, Mk); 355 do_rank2(Mk, MadjTk, Mk); 356 356 break; 357 357 } 358 358 359 MadjT_one = norm_one(MadjTk); 359 MadjT_one = norm_one(MadjTk); 360 360 MadjT_inf = norm_inf(MadjTk); 361 361 … … 363 363 g1 = gamma*0.5; 364 364 g2 = 0.5/(gamma*det); 365 matrixCopy(Ek,=,Mk,3); 366 matBinop(Mk,=,g1*Mk,+,g2*MadjTk,3); 365 matrixCopy(Ek,=,Mk,3); 366 matBinop(Mk,=,g1*Mk,+,g2*MadjTk,3); 367 367 mat_copy(Ek,-=,Mk,3); 368 368 E_one = norm_one(Ek); 369 M_one = norm_one(Mk); 369 M_one = norm_one(Mk); 370 370 M_inf = norm_inf(Mk); 371 371 … … 375 375 mat_mult(Mk, M, S); mat_pad(S); 376 376 377 for (i=0; i<3; i++) 377 for (i=0; i<3; i++) 378 378 for (j=i; j<3; j++) 379 379 S[i][j] = S[j][i] = 0.5*(S[i][j]+S[j][i]); … … 457 457 458 458 if (ka[X]==ka[Y]) { 459 if (ka[X]==ka[Z]) 460 turn = W; 459 if (ka[X]==ka[Z]) 460 turn = W; 461 461 else turn = Z; 462 462 } 463 463 else { 464 if (ka[X]==ka[Z]) 465 turn = Y; 466 else if (ka[Y]==ka[Z]) 464 if (ka[X]==ka[Z]) 465 turn = Y; 466 else if (ka[Y]==ka[Z]) 467 467 turn = X; 468 468 } … … 490 490 491 491 if (mag[0]>mag[1]) { 492 if (mag[0]>mag[2]) 493 win = 0; 492 if (mag[0]>mag[2]) 493 win = 0; 494 494 else win = 2; 495 495 } 496 496 else { 497 if (mag[1]>mag[2]) win = 1; 497 if (mag[1]>mag[2]) win = 1; 498 498 else win = 2; 499 499 } … … 509 509 p = Qt_Mul(p, Qt_(0.0,0.0,-qp.z/t,qp.w/t)); 510 510 p = Qt_Mul(qtoz, Qt_Conj(p)); 511 } 511 } 512 512 else { 513 513 double qa[4], pa[4]; … … 525 525 526 526 /* Find two largest components, indices in hi and lo */ 527 if (qa[0]>qa[1]) lo = 0; 527 if (qa[0]>qa[1]) lo = 0; 528 528 else lo = 1; 529 529 530 if (qa[2]>qa[3]) hi = 2; 530 if (qa[2]>qa[3]) hi = 2; 531 531 else hi = 3; 532 532 … … 538 538 hi ^= lo; lo ^= hi; hi ^= lo; 539 539 } 540 } 540 } 541 541 else { 542 542 if (qa[hi^1]>qa[lo]) lo = hi^1; … … 550 550 {int i; for (i=0; i<4; i++) pa[i] = sgn(neg[i], 0.5);} 551 551 cycle(ka,par); 552 } 552 } 553 553 else {/*big*/ pa[hi] = sgn(neg[hi],1.0);} 554 554 } else { 555 555 if (two>big) { /*two*/ 556 pa[hi] = sgn(neg[hi],SQRTHALF); 556 pa[hi] = sgn(neg[hi],SQRTHALF); 557 557 pa[lo] = sgn(neg[lo], SQRTHALF); 558 558 if (lo>hi) { … … 560 560 } 561 561 if (hi==W) { 562 hi = "\001\002\000"[lo]; 562 hi = "\001\002\000"[lo]; 563 563 lo = 3-hi-lo; 564 564 } 565 565 swap(ka,hi,lo); 566 } 567 else {/*big*/ 566 } 567 else {/*big*/ 568 568 pa[hi] = sgn(neg[hi],1.0); 569 569 } … … 610 610 611 611 double mul = 1.0; 612 if (parts.t[MatrixDecomposition::W] != 0.0) 612 if (parts.t[MatrixDecomposition::W] != 0.0) 613 613 mul = 1.0 / parts.t[MatrixDecomposition::W]; 614 614 … … 620 620 621 621 mul = 1.0; 622 if (parts.k.w != 0.0) 622 if (parts.k.w != 0.0) 623 623 mul = 1.0 / parts.k.w; 624 624 … … 664 664 665 665 double mul = 1.0; 666 if (parts.t[MatrixDecomposition::W] != 0.0) 666 if (parts.t[MatrixDecomposition::W] != 0.0) 667 667 mul = 1.0 / parts.t[MatrixDecomposition::W]; 668 668 … … 674 674 675 675 mul = 1.0; 676 if (parts.k.w != 0.0) 676 if (parts.k.w != 0.0) 677 677 mul = 1.0 / parts.k.w; 678 678
