1/////////////////////////////////////////////////////////////////////////// 2// 3// Copyright (c) 2002, Industrial Light & Magic, a division of Lucas 4// Digital Ltd. LLC 5// 6// All rights reserved. 7// 8// Redistribution and use in source and binary forms, with or without 9// modification, are permitted provided that the following conditions are 10// met: 11// * Redistributions of source code must retain the above copyright 12// notice, this list of conditions and the following disclaimer. 13// * Redistributions in binary form must reproduce the above 14// copyright notice, this list of conditions and the following disclaimer 15// in the documentation and/or other materials provided with the 16// distribution. 17// * Neither the name of Industrial Light & Magic nor the names of 18// its contributors may be used to endorse or promote products derived 19// from this software without specific prior written permission. 20// 21// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 24// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 25// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 26// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 27// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 28// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 29// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 30// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32// 33/////////////////////////////////////////////////////////////////////////// 34 35 36#ifndef INCLUDED_IMATHMATRIXALGO_H 37#define INCLUDED_IMATHMATRIXALGO_H 38 39//------------------------------------------------------------------------- 40// 41// This file contains algorithms applied to or in conjunction with 42// transformation matrices (Imath::Matrix33 and Imath::Matrix44). 43// The assumption made is that these functions are called much less 44// often than the basic point functions or these functions require 45// more support classes. 46// 47// This file also defines a few predefined constant matrices. 48// 49//------------------------------------------------------------------------- 50 51#include "ImathMatrix.h" 52#include "ImathQuat.h" 53#include "ImathEuler.h" 54#include "ImathExc.h" 55#include "ImathVec.h" 56#include "ImathLimits.h" 57#include <math.h> 58 59 60#ifdef OPENEXR_DLL 61 #ifdef IMATH_EXPORTS 62 #define IMATH_EXPORT_CONST extern __declspec(dllexport) 63 #else 64 #define IMATH_EXPORT_CONST extern __declspec(dllimport) 65 #endif 66#else 67 #define IMATH_EXPORT_CONST extern const 68#endif 69 70 71namespace Imath { 72 73//------------------ 74// Identity matrices 75//------------------ 76 77IMATH_EXPORT_CONST M33f identity33f; 78IMATH_EXPORT_CONST M44f identity44f; 79IMATH_EXPORT_CONST M33d identity33d; 80IMATH_EXPORT_CONST M44d identity44d; 81 82//---------------------------------------------------------------------- 83// Extract scale, shear, rotation, and translation values from a matrix: 84// 85// Notes: 86// 87// This implementation follows the technique described in the paper by 88// Spencer W. Thomas in the Graphics Gems II article: "Decomposing a 89// Matrix into Simple Transformations", p. 320. 90// 91// - Some of the functions below have an optional exc parameter 92// that determines the functions' behavior when the matrix' 93// scaling is very close to zero: 94// 95// If exc is true, the functions throw an Imath::ZeroScale exception. 96// 97// If exc is false: 98// 99// extractScaling (m, s) returns false, s is invalid 100// sansScaling (m) returns m 101// removeScaling (m) returns false, m is unchanged 102// sansScalingAndShear (m) returns m 103// removeScalingAndShear (m) returns false, m is unchanged 104// extractAndRemoveScalingAndShear (m, s, h) 105// returns false, m is unchanged, 106// (sh) are invalid 107// checkForZeroScaleInRow () returns false 108// extractSHRT (m, s, h, r, t) returns false, (shrt) are invalid 109// 110// - Functions extractEuler(), extractEulerXYZ() and extractEulerZYX() 111// assume that the matrix does not include shear or non-uniform scaling, 112// but they do not examine the matrix to verify this assumption. 113// Matrices with shear or non-uniform scaling are likely to produce 114// meaningless results. Therefore, you should use the 115// removeScalingAndShear() routine, if necessary, prior to calling 116// extractEuler...() . 117// 118// - All functions assume that the matrix does not include perspective 119// transformation(s), but they do not examine the matrix to verify 120// this assumption. Matrices with perspective transformations are 121// likely to produce meaningless results. 122// 123//---------------------------------------------------------------------- 124 125 126// 127// Declarations for 4x4 matrix. 128// 129 130template <class T> bool extractScaling 131 (const Matrix44<T> &mat, 132 Vec3<T> &scl, 133 bool exc = true); 134 135template <class T> Matrix44<T> sansScaling (const Matrix44<T> &mat, 136 bool exc = true); 137 138template <class T> bool removeScaling 139 (Matrix44<T> &mat, 140 bool exc = true); 141 142template <class T> bool extractScalingAndShear 143 (const Matrix44<T> &mat, 144 Vec3<T> &scl, 145 Vec3<T> &shr, 146 bool exc = true); 147 148template <class T> Matrix44<T> sansScalingAndShear 149 (const Matrix44<T> &mat, 150 bool exc = true); 151 152template <class T> void sansScalingAndShear 153 (Matrix44<T> &result, 154 const Matrix44<T> &mat, 155 bool exc = true); 156 157template <class T> bool removeScalingAndShear 158 (Matrix44<T> &mat, 159 bool exc = true); 160 161template <class T> bool extractAndRemoveScalingAndShear 162 (Matrix44<T> &mat, 163 Vec3<T> &scl, 164 Vec3<T> &shr, 165 bool exc = true); 166 167template <class T> void extractEulerXYZ 168 (const Matrix44<T> &mat, 169 Vec3<T> &rot); 170 171template <class T> void extractEulerZYX 172 (const Matrix44<T> &mat, 173 Vec3<T> &rot); 174 175template <class T> Quat<T> extractQuat (const Matrix44<T> &mat); 176 177template <class T> bool extractSHRT 178 (const Matrix44<T> &mat, 179 Vec3<T> &s, 180 Vec3<T> &h, 181 Vec3<T> &r, 182 Vec3<T> &t, 183 bool exc /*= true*/, 184 typename Euler<T>::Order rOrder); 185 186template <class T> bool extractSHRT 187 (const Matrix44<T> &mat, 188 Vec3<T> &s, 189 Vec3<T> &h, 190 Vec3<T> &r, 191 Vec3<T> &t, 192 bool exc = true); 193 194template <class T> bool extractSHRT 195 (const Matrix44<T> &mat, 196 Vec3<T> &s, 197 Vec3<T> &h, 198 Euler<T> &r, 199 Vec3<T> &t, 200 bool exc = true); 201 202// 203// Internal utility function. 204// 205 206template <class T> bool checkForZeroScaleInRow 207 (const T &scl, 208 const Vec3<T> &row, 209 bool exc = true); 210 211template <class T> Matrix44<T> outerProduct 212 ( const Vec4<T> &a, 213 const Vec4<T> &b); 214 215 216// 217// Returns a matrix that rotates "fromDirection" vector to "toDirection" 218// vector. 219// 220 221template <class T> Matrix44<T> rotationMatrix (const Vec3<T> &fromDirection, 222 const Vec3<T> &toDirection); 223 224 225 226// 227// Returns a matrix that rotates the "fromDir" vector 228// so that it points towards "toDir". You may also 229// specify that you want the up vector to be pointing 230// in a certain direction "upDir". 231// 232 233template <class T> Matrix44<T> rotationMatrixWithUpDir 234 (const Vec3<T> &fromDir, 235 const Vec3<T> &toDir, 236 const Vec3<T> &upDir); 237 238 239// 240// Constructs a matrix that rotates the z-axis so that it 241// points towards "targetDir". You must also specify 242// that you want the up vector to be pointing in a 243// certain direction "upDir". 244// 245// Notes: The following degenerate cases are handled: 246// (a) when the directions given by "toDir" and "upDir" 247// are parallel or opposite; 248// (the direction vectors must have a non-zero cross product) 249// (b) when any of the given direction vectors have zero length 250// 251 252template <class T> void alignZAxisWithTargetDir 253 (Matrix44<T> &result, 254 Vec3<T> targetDir, 255 Vec3<T> upDir); 256 257 258// Compute an orthonormal direct frame from : a position, an x axis direction and a normal to the y axis 259// If the x axis and normal are perpendicular, then the normal will have the same direction as the z axis. 260// Inputs are : 261// -the position of the frame 262// -the x axis direction of the frame 263// -a normal to the y axis of the frame 264// Return is the orthonormal frame 265template <class T> Matrix44<T> computeLocalFrame( const Vec3<T>& p, 266 const Vec3<T>& xDir, 267 const Vec3<T>& normal); 268 269// Add a translate/rotate/scale offset to an input frame 270// and put it in another frame of reference 271// Inputs are : 272// - input frame 273// - translate offset 274// - rotate offset in degrees 275// - scale offset 276// - frame of reference 277// Output is the offsetted frame 278template <class T> Matrix44<T> addOffset( const Matrix44<T>& inMat, 279 const Vec3<T>& tOffset, 280 const Vec3<T>& rOffset, 281 const Vec3<T>& sOffset, 282 const Vec3<T>& ref); 283 284// Compute Translate/Rotate/Scale matrix from matrix A with the Rotate/Scale of Matrix B 285// Inputs are : 286// -keepRotateA : if true keep rotate from matrix A, use B otherwise 287// -keepScaleA : if true keep scale from matrix A, use B otherwise 288// -Matrix A 289// -Matrix B 290// Return Matrix A with tweaked rotation/scale 291template <class T> Matrix44<T> computeRSMatrix( bool keepRotateA, 292 bool keepScaleA, 293 const Matrix44<T>& A, 294 const Matrix44<T>& B); 295 296 297//---------------------------------------------------------------------- 298 299 300// 301// Declarations for 3x3 matrix. 302// 303 304 305template <class T> bool extractScaling 306 (const Matrix33<T> &mat, 307 Vec2<T> &scl, 308 bool exc = true); 309 310template <class T> Matrix33<T> sansScaling (const Matrix33<T> &mat, 311 bool exc = true); 312 313template <class T> bool removeScaling 314 (Matrix33<T> &mat, 315 bool exc = true); 316 317template <class T> bool extractScalingAndShear 318 (const Matrix33<T> &mat, 319 Vec2<T> &scl, 320 T &h, 321 bool exc = true); 322 323template <class T> Matrix33<T> sansScalingAndShear 324 (const Matrix33<T> &mat, 325 bool exc = true); 326 327template <class T> bool removeScalingAndShear 328 (Matrix33<T> &mat, 329 bool exc = true); 330 331template <class T> bool extractAndRemoveScalingAndShear 332 (Matrix33<T> &mat, 333 Vec2<T> &scl, 334 T &shr, 335 bool exc = true); 336 337template <class T> void extractEuler 338 (const Matrix33<T> &mat, 339 T &rot); 340 341template <class T> bool extractSHRT (const Matrix33<T> &mat, 342 Vec2<T> &s, 343 T &h, 344 T &r, 345 Vec2<T> &t, 346 bool exc = true); 347 348template <class T> bool checkForZeroScaleInRow 349 (const T &scl, 350 const Vec2<T> &row, 351 bool exc = true); 352 353template <class T> Matrix33<T> outerProduct 354 ( const Vec3<T> &a, 355 const Vec3<T> &b); 356 357 358//----------------------------------------------------------------------------- 359// Implementation for 4x4 Matrix 360//------------------------------ 361 362 363template <class T> 364bool 365extractScaling (const Matrix44<T> &mat, Vec3<T> &scl, bool exc) 366{ 367 Vec3<T> shr; 368 Matrix44<T> M (mat); 369 370 if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) 371 return false; 372 373 return true; 374} 375 376 377template <class T> 378Matrix44<T> 379sansScaling (const Matrix44<T> &mat, bool exc) 380{ 381 Vec3<T> scl; 382 Vec3<T> shr; 383 Vec3<T> rot; 384 Vec3<T> tran; 385 386 if (! extractSHRT (mat, scl, shr, rot, tran, exc)) 387 return mat; 388 389 Matrix44<T> M; 390 391 M.translate (tran); 392 M.rotate (rot); 393 M.shear (shr); 394 395 return M; 396} 397 398 399template <class T> 400bool 401removeScaling (Matrix44<T> &mat, bool exc) 402{ 403 Vec3<T> scl; 404 Vec3<T> shr; 405 Vec3<T> rot; 406 Vec3<T> tran; 407 408 if (! extractSHRT (mat, scl, shr, rot, tran, exc)) 409 return false; 410 411 mat.makeIdentity (); 412 mat.translate (tran); 413 mat.rotate (rot); 414 mat.shear (shr); 415 416 return true; 417} 418 419 420template <class T> 421bool 422extractScalingAndShear (const Matrix44<T> &mat, 423 Vec3<T> &scl, Vec3<T> &shr, bool exc) 424{ 425 Matrix44<T> M (mat); 426 427 if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) 428 return false; 429 430 return true; 431} 432 433 434template <class T> 435Matrix44<T> 436sansScalingAndShear (const Matrix44<T> &mat, bool exc) 437{ 438 Vec3<T> scl; 439 Vec3<T> shr; 440 Matrix44<T> M (mat); 441 442 if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) 443 return mat; 444 445 return M; 446} 447 448 449template <class T> 450void 451sansScalingAndShear (Matrix44<T> &result, const Matrix44<T> &mat, bool exc) 452{ 453 Vec3<T> scl; 454 Vec3<T> shr; 455 456 if (! extractAndRemoveScalingAndShear (result, scl, shr, exc)) 457 result = mat; 458} 459 460 461template <class T> 462bool 463removeScalingAndShear (Matrix44<T> &mat, bool exc) 464{ 465 Vec3<T> scl; 466 Vec3<T> shr; 467 468 if (! extractAndRemoveScalingAndShear (mat, scl, shr, exc)) 469 return false; 470 471 return true; 472} 473 474 475template <class T> 476bool 477extractAndRemoveScalingAndShear (Matrix44<T> &mat, 478 Vec3<T> &scl, Vec3<T> &shr, bool exc) 479{ 480 // 481 // This implementation follows the technique described in the paper by 482 // Spencer W. Thomas in the Graphics Gems II article: "Decomposing a 483 // Matrix into Simple Transformations", p. 320. 484 // 485 486 Vec3<T> row[3]; 487 488 row[0] = Vec3<T> (mat[0][0], mat[0][1], mat[0][2]); 489 row[1] = Vec3<T> (mat[1][0], mat[1][1], mat[1][2]); 490 row[2] = Vec3<T> (mat[2][0], mat[2][1], mat[2][2]); 491 492 T maxVal = 0; 493 for (int i=0; i < 3; i++) 494 for (int j=0; j < 3; j++) 495 if (Imath::abs (row[i][j]) > maxVal) 496 maxVal = Imath::abs (row[i][j]); 497 498 // 499 // We normalize the 3x3 matrix here. 500 // It was noticed that this can improve numerical stability significantly, 501 // especially when many of the upper 3x3 matrix's coefficients are very 502 // close to zero; we correct for this step at the end by multiplying the 503 // scaling factors by maxVal at the end (shear and rotation are not 504 // affected by the normalization). 505 506 if (maxVal != 0) 507 { 508 for (int i=0; i < 3; i++) 509 if (! checkForZeroScaleInRow (maxVal, row[i], exc)) 510 return false; 511 else 512 row[i] /= maxVal; 513 } 514 515 // Compute X scale factor. 516 scl.x = row[0].length (); 517 if (! checkForZeroScaleInRow (scl.x, row[0], exc)) 518 return false; 519 520 // Normalize first row. 521 row[0] /= scl.x; 522 523 // An XY shear factor will shear the X coord. as the Y coord. changes. 524 // There are 6 combinations (XY, XZ, YZ, YX, ZX, ZY), although we only 525 // extract the first 3 because we can effect the last 3 by shearing in 526 // XY, XZ, YZ combined rotations and scales. 527 // 528 // shear matrix < 1, YX, ZX, 0, 529 // XY, 1, ZY, 0, 530 // XZ, YZ, 1, 0, 531 // 0, 0, 0, 1 > 532 533 // Compute XY shear factor and make 2nd row orthogonal to 1st. 534 shr[0] = row[0].dot (row[1]); 535 row[1] -= shr[0] * row[0]; 536 537 // Now, compute Y scale. 538 scl.y = row[1].length (); 539 if (! checkForZeroScaleInRow (scl.y, row[1], exc)) 540 return false; 541 542 // Normalize 2nd row and correct the XY shear factor for Y scaling. 543 row[1] /= scl.y; 544 shr[0] /= scl.y; 545 546 // Compute XZ and YZ shears, orthogonalize 3rd row. 547 shr[1] = row[0].dot (row[2]); 548 row[2] -= shr[1] * row[0]; 549 shr[2] = row[1].dot (row[2]); 550 row[2] -= shr[2] * row[1]; 551 552 // Next, get Z scale. 553 scl.z = row[2].length (); 554 if (! checkForZeroScaleInRow (scl.z, row[2], exc)) 555 return false; 556 557 // Normalize 3rd row and correct the XZ and YZ shear factors for Z scaling. 558 row[2] /= scl.z; 559 shr[1] /= scl.z; 560 shr[2] /= scl.z; 561 562 // At this point, the upper 3x3 matrix in mat is orthonormal. 563 // Check for a coordinate system flip. If the determinant 564 // is less than zero, then negate the matrix and the scaling factors. 565 if (row[0].dot (row[1].cross (row[2])) < 0) 566 for (int i=0; i < 3; i++) 567 { 568 scl[i] *= -1; 569 row[i] *= -1; 570 } 571 572 // Copy over the orthonormal rows into the returned matrix. 573 // The upper 3x3 matrix in mat is now a rotation matrix. 574 for (int i=0; i < 3; i++) 575 { 576 mat[i][0] = row[i][0]; 577 mat[i][1] = row[i][1]; 578 mat[i][2] = row[i][2]; 579 } 580 581 // Correct the scaling factors for the normalization step that we 582 // performed above; shear and rotation are not affected by the 583 // normalization. 584 scl *= maxVal; 585 586 return true; 587} 588 589 590template <class T> 591void 592extractEulerXYZ (const Matrix44<T> &mat, Vec3<T> &rot) 593{ 594 // 595 // Normalize the local x, y and z axes to remove scaling. 596 // 597 598 Vec3<T> i (mat[0][0], mat[0][1], mat[0][2]); 599 Vec3<T> j (mat[1][0], mat[1][1], mat[1][2]); 600 Vec3<T> k (mat[2][0], mat[2][1], mat[2][2]); 601 602 i.normalize(); 603 j.normalize(); 604 k.normalize(); 605 606 Matrix44<T> M (i[0], i[1], i[2], 0, 607 j[0], j[1], j[2], 0, 608 k[0], k[1], k[2], 0, 609 0, 0, 0, 1); 610 611 // 612 // Extract the first angle, rot.x. 613 // 614 615 rot.x = Math<T>::atan2 (M[1][2], M[2][2]); 616 617 // 618 // Remove the rot.x rotation from M, so that the remaining 619 // rotation, N, is only around two axes, and gimbal lock 620 // cannot occur. 621 // 622 623 Matrix44<T> N; 624 N.rotate (Vec3<T> (-rot.x, 0, 0)); 625 N = N * M; 626 627 // 628 // Extract the other two angles, rot.y and rot.z, from N. 629 // 630 631 T cy = Math<T>::sqrt (N[0][0]*N[0][0] + N[0][1]*N[0][1]); 632 rot.y = Math<T>::atan2 (-N[0][2], cy); 633 rot.z = Math<T>::atan2 (-N[1][0], N[1][1]); 634} 635 636 637template <class T> 638void 639extractEulerZYX (const Matrix44<T> &mat, Vec3<T> &rot) 640{ 641 // 642 // Normalize the local x, y and z axes to remove scaling. 643 // 644 645 Vec3<T> i (mat[0][0], mat[0][1], mat[0][2]); 646 Vec3<T> j (mat[1][0], mat[1][1], mat[1][2]); 647 Vec3<T> k (mat[2][0], mat[2][1], mat[2][2]); 648 649 i.normalize(); 650 j.normalize(); 651 k.normalize(); 652 653 Matrix44<T> M (i[0], i[1], i[2], 0, 654 j[0], j[1], j[2], 0, 655 k[0], k[1], k[2], 0, 656 0, 0, 0, 1); 657 658 // 659 // Extract the first angle, rot.x. 660 // 661 662 rot.x = -Math<T>::atan2 (M[1][0], M[0][0]); 663 664 // 665 // Remove the x rotation from M, so that the remaining 666 // rotation, N, is only around two axes, and gimbal lock 667 // cannot occur. 668 // 669 670 Matrix44<T> N; 671 N.rotate (Vec3<T> (0, 0, -rot.x)); 672 N = N * M; 673 674 // 675 // Extract the other two angles, rot.y and rot.z, from N. 676 // 677 678 T cy = Math<T>::sqrt (N[2][2]*N[2][2] + N[2][1]*N[2][1]); 679 rot.y = -Math<T>::atan2 (-N[2][0], cy); 680 rot.z = -Math<T>::atan2 (-N[1][2], N[1][1]); 681} 682 683 684template <class T> 685Quat<T> 686extractQuat (const Matrix44<T> &mat) 687{ 688 Matrix44<T> rot; 689 690 T tr, s; 691 T q[4]; 692 int i, j, k; 693 Quat<T> quat; 694 695 int nxt[3] = {1, 2, 0}; 696 tr = mat[0][0] + mat[1][1] + mat[2][2]; 697 698 // check the diagonal 699 if (tr > 0.0) { 700 s = Math<T>::sqrt (tr + T(1.0)); 701 quat.r = s / T(2.0); 702 s = T(0.5) / s; 703 704 quat.v.x = (mat[1][2] - mat[2][1]) * s; 705 quat.v.y = (mat[2][0] - mat[0][2]) * s; 706 quat.v.z = (mat[0][1] - mat[1][0]) * s; 707 } 708 else { 709 // diagonal is negative 710 i = 0; 711 if (mat[1][1] > mat[0][0]) 712 i=1; 713 if (mat[2][2] > mat[i][i]) 714 i=2; 715 716 j = nxt[i]; 717 k = nxt[j]; 718 s = Math<T>::sqrt ((mat[i][i] - (mat[j][j] + mat[k][k])) + T(1.0)); 719 720 q[i] = s * T(0.5); 721 if (s != T(0.0)) 722 s = T(0.5) / s; 723 724 q[3] = (mat[j][k] - mat[k][j]) * s; 725 q[j] = (mat[i][j] + mat[j][i]) * s; 726 q[k] = (mat[i][k] + mat[k][i]) * s; 727 728 quat.v.x = q[0]; 729 quat.v.y = q[1]; 730 quat.v.z = q[2]; 731 quat.r = q[3]; 732 } 733 734 return quat; 735} 736 737template <class T> 738bool 739extractSHRT (const Matrix44<T> &mat, 740 Vec3<T> &s, 741 Vec3<T> &h, 742 Vec3<T> &r, 743 Vec3<T> &t, 744 bool exc /* = true */ , 745 typename Euler<T>::Order rOrder /* = Euler<T>::XYZ */ ) 746{ 747 Matrix44<T> rot; 748 749 rot = mat; 750 if (! extractAndRemoveScalingAndShear (rot, s, h, exc)) 751 return false; 752 753 extractEulerXYZ (rot, r); 754 755 t.x = mat[3][0]; 756 t.y = mat[3][1]; 757 t.z = mat[3][2]; 758 759 if (rOrder != Euler<T>::XYZ) 760 { 761 Imath::Euler<T> eXYZ (r, Imath::Euler<T>::XYZ); 762 Imath::Euler<T> e (eXYZ, rOrder); 763 r = e.toXYZVector (); 764 } 765 766 return true; 767} 768 769template <class T> 770bool 771extractSHRT (const Matrix44<T> &mat, 772 Vec3<T> &s, 773 Vec3<T> &h, 774 Vec3<T> &r, 775 Vec3<T> &t, 776 bool exc) 777{ 778 return extractSHRT(mat, s, h, r, t, exc, Imath::Euler<T>::XYZ); 779} 780 781template <class T> 782bool 783extractSHRT (const Matrix44<T> &mat, 784 Vec3<T> &s, 785 Vec3<T> &h, 786 Euler<T> &r, 787 Vec3<T> &t, 788 bool exc /* = true */) 789{ 790 return extractSHRT (mat, s, h, r, t, exc, r.order ()); 791} 792 793 794template <class T> 795bool 796checkForZeroScaleInRow (const T& scl, 797 const Vec3<T> &row, 798 bool exc /* = true */ ) 799{ 800 for (int i = 0; i < 3; i++) 801 { 802 if ((abs (scl) < 1 && abs (row[i]) >= limits<T>::max() * abs (scl))) 803 { 804 if (exc) 805 throw Imath::ZeroScaleExc ("Cannot remove zero scaling " 806 "from matrix."); 807 else 808 return false; 809 } 810 } 811 812 return true; 813} 814 815template <class T> 816Matrix44<T> 817outerProduct (const Vec4<T> &a, const Vec4<T> &b ) 818{ 819 return Matrix44<T> (a.x*b.x, a.x*b.y, a.x*b.z, a.x*b.w, 820 a.y*b.x, a.y*b.y, a.y*b.z, a.x*b.w, 821 a.z*b.x, a.z*b.y, a.z*b.z, a.x*b.w, 822 a.w*b.x, a.w*b.y, a.w*b.z, a.w*b.w); 823} 824 825template <class T> 826Matrix44<T> 827rotationMatrix (const Vec3<T> &from, const Vec3<T> &to) 828{ 829 Quat<T> q; 830 q.setRotation(from, to); 831 return q.toMatrix44(); 832} 833 834 835template <class T> 836Matrix44<T> 837rotationMatrixWithUpDir (const Vec3<T> &fromDir, 838 const Vec3<T> &toDir, 839 const Vec3<T> &upDir) 840{ 841 // 842 // The goal is to obtain a rotation matrix that takes 843 // "fromDir" to "toDir". We do this in two steps and 844 // compose the resulting rotation matrices; 845 // (a) rotate "fromDir" into the z-axis 846 // (b) rotate the z-axis into "toDir" 847 // 848 849 // The from direction must be non-zero; but we allow zero to and up dirs. 850 if (fromDir.length () == 0) 851 return Matrix44<T> (); 852 853 else 854 { 855 Matrix44<T> zAxis2FromDir( Imath::UNINITIALIZED ); 856 alignZAxisWithTargetDir (zAxis2FromDir, fromDir, Vec3<T> (0, 1, 0)); 857 858 Matrix44<T> fromDir2zAxis = zAxis2FromDir.transposed (); 859 860 Matrix44<T> zAxis2ToDir( Imath::UNINITIALIZED ); 861 alignZAxisWithTargetDir (zAxis2ToDir, toDir, upDir); 862 863 return fromDir2zAxis * zAxis2ToDir; 864 } 865} 866 867 868template <class T> 869void 870alignZAxisWithTargetDir (Matrix44<T> &result, Vec3<T> targetDir, Vec3<T> upDir) 871{ 872 // 873 // Ensure that the target direction is non-zero. 874 // 875 876 if ( targetDir.length () == 0 ) 877 targetDir = Vec3<T> (0, 0, 1); 878 879 // 880 // Ensure that the up direction is non-zero. 881 // 882 883 if ( upDir.length () == 0 ) 884 upDir = Vec3<T> (0, 1, 0); 885 886 // 887 // Check for degeneracies. If the upDir and targetDir are parallel 888 // or opposite, then compute a new, arbitrary up direction that is 889 // not parallel or opposite to the targetDir. 890 // 891 892 if (upDir.cross (targetDir).length () == 0) 893 { 894 upDir = targetDir.cross (Vec3<T> (1, 0, 0)); 895 if (upDir.length() == 0) 896 upDir = targetDir.cross(Vec3<T> (0, 0, 1)); 897 } 898 899 // 900 // Compute the x-, y-, and z-axis vectors of the new coordinate system. 901 // 902 903 Vec3<T> targetPerpDir = upDir.cross (targetDir); 904 Vec3<T> targetUpDir = targetDir.cross (targetPerpDir); 905 906 // 907 // Rotate the x-axis into targetPerpDir (row 0), 908 // rotate the y-axis into targetUpDir (row 1), 909 // rotate the z-axis into targetDir (row 2). 910 // 911 912 Vec3<T> row[3]; 913 row[0] = targetPerpDir.normalized (); 914 row[1] = targetUpDir .normalized (); 915 row[2] = targetDir .normalized (); 916 917 result.x[0][0] = row[0][0]; 918 result.x[0][1] = row[0][1]; 919 result.x[0][2] = row[0][2]; 920 result.x[0][3] = (T)0; 921 922 result.x[1][0] = row[1][0]; 923 result.x[1][1] = row[1][1]; 924 result.x[1][2] = row[1][2]; 925 result.x[1][3] = (T)0; 926 927 result.x[2][0] = row[2][0]; 928 result.x[2][1] = row[2][1]; 929 result.x[2][2] = row[2][2]; 930 result.x[2][3] = (T)0; 931 932 result.x[3][0] = (T)0; 933 result.x[3][1] = (T)0; 934 result.x[3][2] = (T)0; 935 result.x[3][3] = (T)1; 936} 937 938 939// Compute an orthonormal direct frame from : a position, an x axis direction and a normal to the y axis 940// If the x axis and normal are perpendicular, then the normal will have the same direction as the z axis. 941// Inputs are : 942// -the position of the frame 943// -the x axis direction of the frame 944// -a normal to the y axis of the frame 945// Return is the orthonormal frame 946template <class T> 947Matrix44<T> 948computeLocalFrame( const Vec3<T>& p, 949 const Vec3<T>& xDir, 950 const Vec3<T>& normal) 951{ 952 Vec3<T> _xDir(xDir); 953 Vec3<T> x = _xDir.normalize(); 954 Vec3<T> y = (normal % x).normalize(); 955 Vec3<T> z = (x % y).normalize(); 956 957 Matrix44<T> L; 958 L[0][0] = x[0]; 959 L[0][1] = x[1]; 960 L[0][2] = x[2]; 961 L[0][3] = 0.0; 962 963 L[1][0] = y[0]; 964 L[1][1] = y[1]; 965 L[1][2] = y[2]; 966 L[1][3] = 0.0; 967 968 L[2][0] = z[0]; 969 L[2][1] = z[1]; 970 L[2][2] = z[2]; 971 L[2][3] = 0.0; 972 973 L[3][0] = p[0]; 974 L[3][1] = p[1]; 975 L[3][2] = p[2]; 976 L[3][3] = 1.0; 977 978 return L; 979} 980 981// Add a translate/rotate/scale offset to an input frame 982// and put it in another frame of reference 983// Inputs are : 984// - input frame 985// - translate offset 986// - rotate offset in degrees 987// - scale offset 988// - frame of reference 989// Output is the offsetted frame 990template <class T> 991Matrix44<T> 992addOffset( const Matrix44<T>& inMat, 993 const Vec3<T>& tOffset, 994 const Vec3<T>& rOffset, 995 const Vec3<T>& sOffset, 996 const Matrix44<T>& ref) 997{ 998 Matrix44<T> O; 999 1000 Vec3<T> _rOffset(rOffset); 1001 _rOffset *= M_PI / 180.0; 1002 O.rotate (_rOffset); 1003 1004 O[3][0] = tOffset[0]; 1005 O[3][1] = tOffset[1]; 1006 O[3][2] = tOffset[2]; 1007 1008 Matrix44<T> S; 1009 S.scale (sOffset); 1010 1011 Matrix44<T> X = S * O * inMat * ref; 1012 1013 return X; 1014} 1015 1016// Compute Translate/Rotate/Scale matrix from matrix A with the Rotate/Scale of Matrix B 1017// Inputs are : 1018// -keepRotateA : if true keep rotate from matrix A, use B otherwise 1019// -keepScaleA : if true keep scale from matrix A, use B otherwise 1020// -Matrix A 1021// -Matrix B 1022// Return Matrix A with tweaked rotation/scale 1023template <class T> 1024Matrix44<T> 1025computeRSMatrix( bool keepRotateA, 1026 bool keepScaleA, 1027 const Matrix44<T>& A, 1028 const Matrix44<T>& B) 1029{ 1030 Vec3<T> as, ah, ar, at; 1031 extractSHRT (A, as, ah, ar, at); 1032 1033 Vec3<T> bs, bh, br, bt; 1034 extractSHRT (B, bs, bh, br, bt); 1035 1036 if (!keepRotateA) 1037 ar = br; 1038 1039 if (!keepScaleA) 1040 as = bs; 1041 1042 Matrix44<T> mat; 1043 mat.makeIdentity(); 1044 mat.translate (at); 1045 mat.rotate (ar); 1046 mat.scale (as); 1047 1048 return mat; 1049} 1050 1051 1052 1053//----------------------------------------------------------------------------- 1054// Implementation for 3x3 Matrix 1055//------------------------------ 1056 1057 1058template <class T> 1059bool 1060extractScaling (const Matrix33<T> &mat, Vec2<T> &scl, bool exc) 1061{ 1062 T shr; 1063 Matrix33<T> M (mat); 1064 1065 if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) 1066 return false; 1067 1068 return true; 1069} 1070 1071 1072template <class T> 1073Matrix33<T> 1074sansScaling (const Matrix33<T> &mat, bool exc) 1075{ 1076 Vec2<T> scl; 1077 T shr; 1078 T rot; 1079 Vec2<T> tran; 1080 1081 if (! extractSHRT (mat, scl, shr, rot, tran, exc)) 1082 return mat; 1083 1084 Matrix33<T> M; 1085 1086 M.translate (tran); 1087 M.rotate (rot); 1088 M.shear (shr); 1089 1090 return M; 1091} 1092 1093 1094template <class T> 1095bool 1096removeScaling (Matrix33<T> &mat, bool exc) 1097{ 1098 Vec2<T> scl; 1099 T shr; 1100 T rot; 1101 Vec2<T> tran; 1102 1103 if (! extractSHRT (mat, scl, shr, rot, tran, exc)) 1104 return false; 1105 1106 mat.makeIdentity (); 1107 mat.translate (tran); 1108 mat.rotate (rot); 1109 mat.shear (shr); 1110 1111 return true; 1112} 1113 1114 1115template <class T> 1116bool 1117extractScalingAndShear (const Matrix33<T> &mat, Vec2<T> &scl, T &shr, bool exc) 1118{ 1119 Matrix33<T> M (mat); 1120 1121 if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) 1122 return false; 1123 1124 return true; 1125} 1126 1127 1128template <class T> 1129Matrix33<T> 1130sansScalingAndShear (const Matrix33<T> &mat, bool exc) 1131{ 1132 Vec2<T> scl; 1133 T shr; 1134 Matrix33<T> M (mat); 1135 1136 if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) 1137 return mat; 1138 1139 return M; 1140} 1141 1142 1143template <class T> 1144bool 1145removeScalingAndShear (Matrix33<T> &mat, bool exc) 1146{ 1147 Vec2<T> scl; 1148 T shr; 1149 1150 if (! extractAndRemoveScalingAndShear (mat, scl, shr, exc)) 1151 return false; 1152 1153 return true; 1154} 1155 1156template <class T> 1157bool 1158extractAndRemoveScalingAndShear (Matrix33<T> &mat, 1159 Vec2<T> &scl, T &shr, bool exc) 1160{ 1161 Vec2<T> row[2]; 1162 1163 row[0] = Vec2<T> (mat[0][0], mat[0][1]); 1164 row[1] = Vec2<T> (mat[1][0], mat[1][1]); 1165 1166 T maxVal = 0; 1167 for (int i=0; i < 2; i++) 1168 for (int j=0; j < 2; j++) 1169 if (Imath::abs (row[i][j]) > maxVal) 1170 maxVal = Imath::abs (row[i][j]); 1171 1172 // 1173 // We normalize the 2x2 matrix here. 1174 // It was noticed that this can improve numerical stability significantly, 1175 // especially when many of the upper 2x2 matrix's coefficients are very 1176 // close to zero; we correct for this step at the end by multiplying the 1177 // scaling factors by maxVal at the end (shear and rotation are not 1178 // affected by the normalization). 1179 1180 if (maxVal != 0) 1181 { 1182 for (int i=0; i < 2; i++) 1183 if (! checkForZeroScaleInRow (maxVal, row[i], exc)) 1184 return false; 1185 else 1186 row[i] /= maxVal; 1187 } 1188 1189 // Compute X scale factor. 1190 scl.x = row[0].length (); 1191 if (! checkForZeroScaleInRow (scl.x, row[0], exc)) 1192 return false; 1193 1194 // Normalize first row. 1195 row[0] /= scl.x; 1196 1197 // An XY shear factor will shear the X coord. as the Y coord. changes. 1198 // There are 2 combinations (XY, YX), although we only extract the XY 1199 // shear factor because we can effect the an YX shear factor by 1200 // shearing in XY combined with rotations and scales. 1201 // 1202 // shear matrix < 1, YX, 0, 1203 // XY, 1, 0, 1204 // 0, 0, 1 > 1205 1206 // Compute XY shear factor and make 2nd row orthogonal to 1st. 1207 shr = row[0].dot (row[1]); 1208 row[1] -= shr * row[0]; 1209 1210 // Now, compute Y scale. 1211 scl.y = row[1].length (); 1212 if (! checkForZeroScaleInRow (scl.y, row[1], exc)) 1213 return false; 1214 1215 // Normalize 2nd row and correct the XY shear factor for Y scaling. 1216 row[1] /= scl.y; 1217 shr /= scl.y; 1218 1219 // At this point, the upper 2x2 matrix in mat is orthonormal. 1220 // Check for a coordinate system flip. If the determinant 1221 // is -1, then flip the rotation matrix and adjust the scale(Y) 1222 // and shear(XY) factors to compensate. 1223 if (row[0][0] * row[1][1] - row[0][1] * row[1][0] < 0) 1224 { 1225 row[1][0] *= -1; 1226 row[1][1] *= -1; 1227 scl[1] *= -1; 1228 shr *= -1; 1229 } 1230 1231 // Copy over the orthonormal rows into the returned matrix. 1232 // The upper 2x2 matrix in mat is now a rotation matrix. 1233 for (int i=0; i < 2; i++) 1234 { 1235 mat[i][0] = row[i][0]; 1236 mat[i][1] = row[i][1]; 1237 } 1238 1239 scl *= maxVal; 1240 1241 return true; 1242} 1243 1244 1245template <class T> 1246void 1247extractEuler (const Matrix33<T> &mat, T &rot) 1248{ 1249 // 1250 // Normalize the local x and y axes to remove scaling. 1251 // 1252 1253 Vec2<T> i (mat[0][0], mat[0][1]); 1254 Vec2<T> j (mat[1][0], mat[1][1]); 1255 1256 i.normalize(); 1257 j.normalize(); 1258 1259 // 1260 // Extract the angle, rot. 1261 // 1262 1263 rot = - Math<T>::atan2 (j[0], i[0]); 1264} 1265 1266 1267template <class T> 1268bool 1269extractSHRT (const Matrix33<T> &mat, 1270 Vec2<T> &s, 1271 T &h, 1272 T &r, 1273 Vec2<T> &t, 1274 bool exc) 1275{ 1276 Matrix33<T> rot; 1277 1278 rot = mat; 1279 if (! extractAndRemoveScalingAndShear (rot, s, h, exc)) 1280 return false; 1281 1282 extractEuler (rot, r); 1283 1284 t.x = mat[2][0]; 1285 t.y = mat[2][1]; 1286 1287 return true; 1288} 1289 1290 1291template <class T> 1292bool 1293checkForZeroScaleInRow (const T& scl, 1294 const Vec2<T> &row, 1295 bool exc /* = true */ ) 1296{ 1297 for (int i = 0; i < 2; i++) 1298 { 1299 if ((abs (scl) < 1 && abs (row[i]) >= limits<T>::max() * abs (scl))) 1300 { 1301 if (exc) 1302 throw Imath::ZeroScaleExc ("Cannot remove zero scaling " 1303 "from matrix."); 1304 else 1305 return false; 1306 } 1307 } 1308 1309 return true; 1310} 1311 1312 1313template <class T> 1314Matrix33<T> 1315outerProduct (const Vec3<T> &a, const Vec3<T> &b ) 1316{ 1317 return Matrix33<T> (a.x*b.x, a.x*b.y, a.x*b.z, 1318 a.y*b.x, a.y*b.y, a.y*b.z, 1319 a.z*b.x, a.z*b.y, a.z*b.z ); 1320} 1321 1322 1323// Computes the translation and rotation that brings the 'from' points 1324// as close as possible to the 'to' points under the Frobenius norm. 1325// To be more specific, let x be the matrix of 'from' points and y be 1326// the matrix of 'to' points, we want to find the matrix A of the form 1327// [ R t ] 1328// [ 0 1 ] 1329// that minimizes 1330// || (A*x - y)^T * W * (A*x - y) ||_F 1331// If doScaling is true, then a uniform scale is allowed also. 1332template <typename T> 1333Imath::M44d 1334procrustesRotationAndTranslation (const Imath::Vec3<T>* A, // From these 1335 const Imath::Vec3<T>* B, // To these 1336 const T* weights, 1337 const size_t numPoints, 1338 const bool doScaling = false); 1339 1340// Unweighted: 1341template <typename T> 1342Imath::M44d 1343procrustesRotationAndTranslation (const Imath::Vec3<T>* A, 1344 const Imath::Vec3<T>* B, 1345 const size_t numPoints, 1346 const bool doScaling = false); 1347 1348// Compute the SVD of a 3x3 matrix using Jacobi transformations. This method 1349// should be quite accurate (competitive with LAPACK) even for poorly 1350// conditioned matrices, and because it has been written specifically for the 1351// 3x3/4x4 case it is much faster than calling out to LAPACK. 1352// 1353// The SVD of a 3x3/4x4 matrix A is defined as follows: 1354// A = U * S * V^T 1355// where S is the diagonal matrix of singular values and both U and V are 1356// orthonormal. By convention, the entries S are all positive and sorted from 1357// the largest to the smallest. However, some uses of this function may 1358// require that the matrix U*V^T have positive determinant; in this case, we 1359// may make the smallest singular value negative to ensure that this is 1360// satisfied. 1361// 1362// Currently only available for single- and double-precision matrices. 1363template <typename T> 1364void 1365jacobiSVD (const Imath::Matrix33<T>& A, 1366 Imath::Matrix33<T>& U, 1367 Imath::Vec3<T>& S, 1368 Imath::Matrix33<T>& V, 1369 const T tol = Imath::limits<T>::epsilon(), 1370 const bool forcePositiveDeterminant = false); 1371 1372template <typename T> 1373void 1374jacobiSVD (const Imath::Matrix44<T>& A, 1375 Imath::Matrix44<T>& U, 1376 Imath::Vec4<T>& S, 1377 Imath::Matrix44<T>& V, 1378 const T tol = Imath::limits<T>::epsilon(), 1379 const bool forcePositiveDeterminant = false); 1380 1381// Compute the eigenvalues (S) and the eigenvectors (V) of 1382// a real symmetric matrix using Jacobi transformation. 1383// 1384// Jacobi transformation of a 3x3/4x4 matrix A outputs S and V: 1385// A = V * S * V^T 1386// where V is orthonormal and S is the diagonal matrix of eigenvalues. 1387// Input matrix A must be symmetric. A is also modified during 1388// the computation so that upper diagonal entries of A become zero. 1389// 1390template <typename T> 1391void 1392jacobiEigenSolver (Matrix33<T>& A, 1393 Vec3<T>& S, 1394 Matrix33<T>& V, 1395 const T tol); 1396 1397template <typename T> 1398inline 1399void 1400jacobiEigenSolver (Matrix33<T>& A, 1401 Vec3<T>& S, 1402 Matrix33<T>& V) 1403{ 1404 jacobiEigenSolver(A,S,V,limits<T>::epsilon()); 1405} 1406 1407template <typename T> 1408void 1409jacobiEigenSolver (Matrix44<T>& A, 1410 Vec4<T>& S, 1411 Matrix44<T>& V, 1412 const T tol); 1413 1414template <typename T> 1415inline 1416void 1417jacobiEigenSolver (Matrix44<T>& A, 1418 Vec4<T>& S, 1419 Matrix44<T>& V) 1420{ 1421 jacobiEigenSolver(A,S,V,limits<T>::epsilon()); 1422} 1423 1424// Compute a eigenvector corresponding to the abs max/min eigenvalue 1425// of a real symmetric matrix using Jacobi transformation. 1426template <typename TM, typename TV> 1427void 1428maxEigenVector (TM& A, TV& S); 1429template <typename TM, typename TV> 1430void 1431minEigenVector (TM& A, TV& S); 1432 1433} // namespace Imath 1434 1435#endif 1436