/src/FreeImage/Source/OpenEXR/Imath/ImathMatrixAlgo.h
C++ Header | 1115 lines | 629 code | 234 blank | 252 comment | 64 complexity | 151e74894f916f81a6dfef581006074c MD5 | raw file
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 <math.h> 57 58 59#ifdef OPENEXR_DLL 60 #ifdef IMATH_EXPORTS 61 #define IMATH_EXPORT_CONST extern __declspec(dllexport) 62 #else 63 #define IMATH_EXPORT_CONST extern __declspec(dllimport) 64 #endif 65#else 66 #define IMATH_EXPORT_CONST extern const 67#endif 68 69 70namespace Imath { 71 72//------------------ 73// Identity matrices 74//------------------ 75 76IMATH_EXPORT_CONST M33f identity33f; 77IMATH_EXPORT_CONST M44f identity44f; 78IMATH_EXPORT_CONST M33d identity33d; 79IMATH_EXPORT_CONST M44d identity44d; 80 81//---------------------------------------------------------------------- 82// Extract scale, shear, rotation, and translation values from a matrix: 83// 84// Notes: 85// 86// This implementation follows the technique described in the paper by 87// Spencer W. Thomas in the Graphics Gems II article: "Decomposing a 88// Matrix into Simple Transformations", p. 320. 89// 90// - Some of the functions below have an optional exc parameter 91// that determines the functions' behavior when the matrix' 92// scaling is very close to zero: 93// 94// If exc is true, the functions throw an Imath::ZeroScale exception. 95// 96// If exc is false: 97// 98// extractScaling (m, s) returns false, s is invalid 99// sansScaling (m) returns m 100// removeScaling (m) returns false, m is unchanged 101// sansScalingAndShear (m) returns m 102// removeScalingAndShear (m) returns false, m is unchanged 103// extractAndRemoveScalingAndShear (m, s, h) 104// returns false, m is unchanged, 105// (sh) are invalid 106// checkForZeroScaleInRow () returns false 107// extractSHRT (m, s, h, r, t) returns false, (shrt) are invalid 108// 109// - Functions extractEuler(), extractEulerXYZ() and extractEulerZYX() 110// assume that the matrix does not include shear or non-uniform scaling, 111// but they do not examine the matrix to verify this assumption. 112// Matrices with shear or non-uniform scaling are likely to produce 113// meaningless results. Therefore, you should use the 114// removeScalingAndShear() routine, if necessary, prior to calling 115// extractEuler...() . 116// 117// - All functions assume that the matrix does not include perspective 118// transformation(s), but they do not examine the matrix to verify 119// this assumption. Matrices with perspective transformations are 120// likely to produce meaningless results. 121// 122//---------------------------------------------------------------------- 123 124 125// 126// Declarations for 4x4 matrix. 127// 128 129template <class T> bool extractScaling 130 (const Matrix44<T> &mat, 131 Vec3<T> &scl, 132 bool exc = true); 133 134template <class T> Matrix44<T> sansScaling (const Matrix44<T> &mat, 135 bool exc = true); 136 137template <class T> bool removeScaling 138 (Matrix44<T> &mat, 139 bool exc = true); 140 141template <class T> bool extractScalingAndShear 142 (const Matrix44<T> &mat, 143 Vec3<T> &scl, 144 Vec3<T> &shr, 145 bool exc = true); 146 147template <class T> Matrix44<T> sansScalingAndShear 148 (const Matrix44<T> &mat, 149 bool exc = true); 150 151template <class T> bool removeScalingAndShear 152 (Matrix44<T> &mat, 153 bool exc = true); 154 155template <class T> bool extractAndRemoveScalingAndShear 156 (Matrix44<T> &mat, 157 Vec3<T> &scl, 158 Vec3<T> &shr, 159 bool exc = true); 160 161template <class T> void extractEulerXYZ 162 (const Matrix44<T> &mat, 163 Vec3<T> &rot); 164 165template <class T> void extractEulerZYX 166 (const Matrix44<T> &mat, 167 Vec3<T> &rot); 168 169template <class T> Quat<T> extractQuat (const Matrix44<T> &mat); 170 171template <class T> bool extractSHRT 172 (const Matrix44<T> &mat, 173 Vec3<T> &s, 174 Vec3<T> &h, 175 Vec3<T> &r, 176 Vec3<T> &t, 177 bool exc /*= true*/, 178 typename Euler<T>::Order rOrder); 179 180template <class T> bool extractSHRT 181 (const Matrix44<T> &mat, 182 Vec3<T> &s, 183 Vec3<T> &h, 184 Vec3<T> &r, 185 Vec3<T> &t, 186 bool exc = true); 187 188template <class T> bool extractSHRT 189 (const Matrix44<T> &mat, 190 Vec3<T> &s, 191 Vec3<T> &h, 192 Euler<T> &r, 193 Vec3<T> &t, 194 bool exc = true); 195 196// 197// Internal utility function. 198// 199 200template <class T> bool checkForZeroScaleInRow 201 (const T &scl, 202 const Vec3<T> &row, 203 bool exc = true); 204 205// 206// Returns a matrix that rotates "fromDirection" vector to "toDirection" 207// vector. 208// 209 210template <class T> Matrix44<T> rotationMatrix (const Vec3<T> &fromDirection, 211 const Vec3<T> &toDirection); 212 213 214 215// 216// Returns a matrix that rotates the "fromDir" vector 217// so that it points towards "toDir". You may also 218// specify that you want the up vector to be pointing 219// in a certain direction "upDir". 220// 221 222template <class T> Matrix44<T> rotationMatrixWithUpDir 223 (const Vec3<T> &fromDir, 224 const Vec3<T> &toDir, 225 const Vec3<T> &upDir); 226 227 228// 229// Returns a matrix that rotates the z-axis so that it 230// points towards "targetDir". You must also specify 231// that you want the up vector to be pointing in a 232// certain direction "upDir". 233// 234// Notes: The following degenerate cases are handled: 235// (a) when the directions given by "toDir" and "upDir" 236// are parallel or opposite; 237// (the direction vectors must have a non-zero cross product) 238// (b) when any of the given direction vectors have zero length 239// 240 241template <class T> Matrix44<T> alignZAxisWithTargetDir 242 (Vec3<T> targetDir, 243 Vec3<T> upDir); 244 245 246//---------------------------------------------------------------------- 247 248 249// 250// Declarations for 3x3 matrix. 251// 252 253 254template <class T> bool extractScaling 255 (const Matrix33<T> &mat, 256 Vec2<T> &scl, 257 bool exc = true); 258 259template <class T> Matrix33<T> sansScaling (const Matrix33<T> &mat, 260 bool exc = true); 261 262template <class T> bool removeScaling 263 (Matrix33<T> &mat, 264 bool exc = true); 265 266template <class T> bool extractScalingAndShear 267 (const Matrix33<T> &mat, 268 Vec2<T> &scl, 269 T &h, 270 bool exc = true); 271 272template <class T> Matrix33<T> sansScalingAndShear 273 (const Matrix33<T> &mat, 274 bool exc = true); 275 276template <class T> bool removeScalingAndShear 277 (Matrix33<T> &mat, 278 bool exc = true); 279 280template <class T> bool extractAndRemoveScalingAndShear 281 (Matrix33<T> &mat, 282 Vec2<T> &scl, 283 T &shr, 284 bool exc = true); 285 286template <class T> void extractEuler 287 (const Matrix33<T> &mat, 288 T &rot); 289 290template <class T> bool extractSHRT (const Matrix33<T> &mat, 291 Vec2<T> &s, 292 T &h, 293 T &r, 294 Vec2<T> &t, 295 bool exc = true); 296 297template <class T> bool checkForZeroScaleInRow 298 (const T &scl, 299 const Vec2<T> &row, 300 bool exc = true); 301 302 303 304 305//----------------------------------------------------------------------------- 306// Implementation for 4x4 Matrix 307//------------------------------ 308 309 310template <class T> 311bool 312extractScaling (const Matrix44<T> &mat, Vec3<T> &scl, bool exc) 313{ 314 Vec3<T> shr; 315 Matrix44<T> M (mat); 316 317 if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) 318 return false; 319 320 return true; 321} 322 323 324template <class T> 325Matrix44<T> 326sansScaling (const Matrix44<T> &mat, bool exc) 327{ 328 Vec3<T> scl; 329 Vec3<T> shr; 330 Vec3<T> rot; 331 Vec3<T> tran; 332 333 if (! extractSHRT (mat, scl, shr, rot, tran, exc)) 334 return mat; 335 336 Matrix44<T> M; 337 338 M.translate (tran); 339 M.rotate (rot); 340 M.shear (shr); 341 342 return M; 343} 344 345 346template <class T> 347bool 348removeScaling (Matrix44<T> &mat, bool exc) 349{ 350 Vec3<T> scl; 351 Vec3<T> shr; 352 Vec3<T> rot; 353 Vec3<T> tran; 354 355 if (! extractSHRT (mat, scl, shr, rot, tran, exc)) 356 return false; 357 358 mat.makeIdentity (); 359 mat.translate (tran); 360 mat.rotate (rot); 361 mat.shear (shr); 362 363 return true; 364} 365 366 367template <class T> 368bool 369extractScalingAndShear (const Matrix44<T> &mat, 370 Vec3<T> &scl, Vec3<T> &shr, bool exc) 371{ 372 Matrix44<T> M (mat); 373 374 if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) 375 return false; 376 377 return true; 378} 379 380 381template <class T> 382Matrix44<T> 383sansScalingAndShear (const Matrix44<T> &mat, bool exc) 384{ 385 Vec3<T> scl; 386 Vec3<T> shr; 387 Matrix44<T> M (mat); 388 389 if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) 390 return mat; 391 392 return M; 393} 394 395 396template <class T> 397bool 398removeScalingAndShear (Matrix44<T> &mat, bool exc) 399{ 400 Vec3<T> scl; 401 Vec3<T> shr; 402 403 if (! extractAndRemoveScalingAndShear (mat, scl, shr, exc)) 404 return false; 405 406 return true; 407} 408 409 410template <class T> 411bool 412extractAndRemoveScalingAndShear (Matrix44<T> &mat, 413 Vec3<T> &scl, Vec3<T> &shr, bool exc) 414{ 415 // 416 // This implementation follows the technique described in the paper by 417 // Spencer W. Thomas in the Graphics Gems II article: "Decomposing a 418 // Matrix into Simple Transformations", p. 320. 419 // 420 421 Vec3<T> row[3]; 422 423 row[0] = Vec3<T> (mat[0][0], mat[0][1], mat[0][2]); 424 row[1] = Vec3<T> (mat[1][0], mat[1][1], mat[1][2]); 425 row[2] = Vec3<T> (mat[2][0], mat[2][1], mat[2][2]); 426 427 T maxVal = 0; 428 for (int i=0; i < 3; i++) 429 for (int j=0; j < 3; j++) 430 if (Imath::abs (row[i][j]) > maxVal) 431 maxVal = Imath::abs (row[i][j]); 432 433 // 434 // We normalize the 3x3 matrix here. 435 // It was noticed that this can improve numerical stability significantly, 436 // especially when many of the upper 3x3 matrix's coefficients are very 437 // close to zero; we correct for this step at the end by multiplying the 438 // scaling factors by maxVal at the end (shear and rotation are not 439 // affected by the normalization). 440 441 if (maxVal != 0) 442 { 443 for (int i=0; i < 3; i++) 444 if (! checkForZeroScaleInRow (maxVal, row[i], exc)) 445 return false; 446 else 447 row[i] /= maxVal; 448 } 449 450 // Compute X scale factor. 451 scl.x = row[0].length (); 452 if (! checkForZeroScaleInRow (scl.x, row[0], exc)) 453 return false; 454 455 // Normalize first row. 456 row[0] /= scl.x; 457 458 // An XY shear factor will shear the X coord. as the Y coord. changes. 459 // There are 6 combinations (XY, XZ, YZ, YX, ZX, ZY), although we only 460 // extract the first 3 because we can effect the last 3 by shearing in 461 // XY, XZ, YZ combined rotations and scales. 462 // 463 // shear matrix < 1, YX, ZX, 0, 464 // XY, 1, ZY, 0, 465 // XZ, YZ, 1, 0, 466 // 0, 0, 0, 1 > 467 468 // Compute XY shear factor and make 2nd row orthogonal to 1st. 469 shr[0] = row[0].dot (row[1]); 470 row[1] -= shr[0] * row[0]; 471 472 // Now, compute Y scale. 473 scl.y = row[1].length (); 474 if (! checkForZeroScaleInRow (scl.y, row[1], exc)) 475 return false; 476 477 // Normalize 2nd row and correct the XY shear factor for Y scaling. 478 row[1] /= scl.y; 479 shr[0] /= scl.y; 480 481 // Compute XZ and YZ shears, orthogonalize 3rd row. 482 shr[1] = row[0].dot (row[2]); 483 row[2] -= shr[1] * row[0]; 484 shr[2] = row[1].dot (row[2]); 485 row[2] -= shr[2] * row[1]; 486 487 // Next, get Z scale. 488 scl.z = row[2].length (); 489 if (! checkForZeroScaleInRow (scl.z, row[2], exc)) 490 return false; 491 492 // Normalize 3rd row and correct the XZ and YZ shear factors for Z scaling. 493 row[2] /= scl.z; 494 shr[1] /= scl.z; 495 shr[2] /= scl.z; 496 497 // At this point, the upper 3x3 matrix in mat is orthonormal. 498 // Check for a coordinate system flip. If the determinant 499 // is less than zero, then negate the matrix and the scaling factors. 500 if (row[0].dot (row[1].cross (row[2])) < 0) 501 for (int i=0; i < 3; i++) 502 { 503 scl[i] *= -1; 504 row[i] *= -1; 505 } 506 507 // Copy over the orthonormal rows into the returned matrix. 508 // The upper 3x3 matrix in mat is now a rotation matrix. 509 for (int i=0; i < 3; i++) 510 { 511 mat[i][0] = row[i][0]; 512 mat[i][1] = row[i][1]; 513 mat[i][2] = row[i][2]; 514 } 515 516 // Correct the scaling factors for the normalization step that we 517 // performed above; shear and rotation are not affected by the 518 // normalization. 519 scl *= maxVal; 520 521 return true; 522} 523 524 525template <class T> 526void 527extractEulerXYZ (const Matrix44<T> &mat, Vec3<T> &rot) 528{ 529 // 530 // Normalize the local x, y and z axes to remove scaling. 531 // 532 533 Vec3<T> i (mat[0][0], mat[0][1], mat[0][2]); 534 Vec3<T> j (mat[1][0], mat[1][1], mat[1][2]); 535 Vec3<T> k (mat[2][0], mat[2][1], mat[2][2]); 536 537 i.normalize(); 538 j.normalize(); 539 k.normalize(); 540 541 Matrix44<T> M (i[0], i[1], i[2], 0, 542 j[0], j[1], j[2], 0, 543 k[0], k[1], k[2], 0, 544 0, 0, 0, 1); 545 546 547 // 548 // Extract the first angle, rot.x. 549 // 550 551 rot.x = Math<T>::atan2 (M[1][2], M[2][2]); 552 553 // 554 // Remove the rot.x rotation from M, so that the remaining 555 // rotation, N, is only around two axes, and gimbal lock 556 // cannot occur. 557 // 558 Matrix44<T> N; 559 N.rotate (Vec3<T> (-rot.x, 0, 0)); 560 561 N = N * M; 562 563 // Extract the other two angles, rot.y and rot.z, from N. 564 // 565 566 T cy = Math<T>::sqrt (N[0][0]*N[0][0] + N[0][1]*N[0][1]); 567 rot.y = Math<T>::atan2 (-N[0][2], cy); 568 rot.z = Math<T>::atan2 (-N[1][0], N[1][1]); 569 570} 571 572 573template <class T> 574void 575extractEulerZYX (const Matrix44<T> &mat, Vec3<T> &rot) 576{ 577 // 578 // Normalize the local x, y and z axes to remove scaling. 579 // 580 581 Vec3<T> i (mat[0][0], mat[0][1], mat[0][2]); 582 Vec3<T> j (mat[1][0], mat[1][1], mat[1][2]); 583 Vec3<T> k (mat[2][0], mat[2][1], mat[2][2]); 584 585 i.normalize(); 586 j.normalize(); 587 k.normalize(); 588 589 Matrix44<T> M (i[0], i[1], i[2], 0, 590 j[0], j[1], j[2], 0, 591 k[0], k[1], k[2], 0, 592 0, 0, 0, 1); 593 594 // 595 // Extract the first angle, rot.x. 596 // 597 598 rot.x = -Math<T>::atan2 (M[1][0], M[0][0]); 599 600 // 601 // Remove the x rotation from M, so that the remaining 602 // rotation, N, is only around two axes, and gimbal lock 603 // cannot occur. 604 // 605 606 Matrix44<T> N; 607 N.rotate (Vec3<T> (0, 0, -rot.x)); 608 N = N * M; 609 610 // 611 // Extract the other two angles, rot.y and rot.z, from N. 612 // 613 614 T cy = Math<T>::sqrt (N[2][2]*N[2][2] + N[2][1]*N[2][1]); 615 rot.y = -Math<T>::atan2 (-N[2][0], cy); 616 rot.z = -Math<T>::atan2 (-N[1][2], N[1][1]); 617} 618 619 620template <class T> 621Quat<T> 622extractQuat (const Matrix44<T> &mat) 623{ 624 Matrix44<T> rot; 625 626 T tr, s; 627 T q[4]; 628 int i, j, k; 629 Quat<T> quat; 630 631 int nxt[3] = {1, 2, 0}; 632 tr = mat[0][0] + mat[1][1] + mat[2][2]; 633 634 // check the diagonal 635 if (tr > 0.0) { 636 s = Math<T>::sqrt (tr + 1.0); 637 quat.r = s / 2.0; 638 s = 0.5 / s; 639 640 quat.v.x = (mat[1][2] - mat[2][1]) * s; 641 quat.v.y = (mat[2][0] - mat[0][2]) * s; 642 quat.v.z = (mat[0][1] - mat[1][0]) * s; 643 } 644 else { 645 // diagonal is negative 646 i = 0; 647 if (mat[1][1] > mat[0][0]) 648 i=1; 649 if (mat[2][2] > mat[i][i]) 650 i=2; 651 652 j = nxt[i]; 653 k = nxt[j]; 654 s = Math<T>::sqrt ((mat[i][i] - (mat[j][j] + mat[k][k])) + 1.0); 655 656 q[i] = s * 0.5; 657 if (s != 0.0) 658 s = 0.5 / s; 659 660 q[3] = (mat[j][k] - mat[k][j]) * s; 661 q[j] = (mat[i][j] + mat[j][i]) * s; 662 q[k] = (mat[i][k] + mat[k][i]) * s; 663 664 quat.v.x = q[0]; 665 quat.v.y = q[1]; 666 quat.v.z = q[2]; 667 quat.r = q[3]; 668 } 669 670 return quat; 671} 672 673template <class T> 674bool 675extractSHRT (const Matrix44<T> &mat, 676 Vec3<T> &s, 677 Vec3<T> &h, 678 Vec3<T> &r, 679 Vec3<T> &t, 680 bool exc /* = true */ , 681 typename Euler<T>::Order rOrder /* = Euler<T>::XYZ */ ) 682{ 683 Matrix44<T> rot; 684 685 rot = mat; 686 if (! extractAndRemoveScalingAndShear (rot, s, h, exc)) 687 return false; 688 689 extractEulerXYZ (rot, r); 690 691 t.x = mat[3][0]; 692 t.y = mat[3][1]; 693 t.z = mat[3][2]; 694 695 if (rOrder != Euler<T>::XYZ) 696 { 697 Imath::Euler<T> eXYZ (r, Imath::Euler<T>::XYZ); 698 Imath::Euler<T> e (eXYZ, rOrder); 699 r = e.toXYZVector (); 700 } 701 702 return true; 703} 704 705template <class T> 706bool 707extractSHRT (const Matrix44<T> &mat, 708 Vec3<T> &s, 709 Vec3<T> &h, 710 Vec3<T> &r, 711 Vec3<T> &t, 712 bool exc) 713{ 714 return extractSHRT(mat, s, h, r, t, exc, Imath::Euler<T>::XYZ); 715} 716 717template <class T> 718bool 719extractSHRT (const Matrix44<T> &mat, 720 Vec3<T> &s, 721 Vec3<T> &h, 722 Euler<T> &r, 723 Vec3<T> &t, 724 bool exc /* = true */) 725{ 726 return extractSHRT (mat, s, h, r, t, exc, r.order ()); 727} 728 729 730template <class T> 731bool 732checkForZeroScaleInRow (const T& scl, 733 const Vec3<T> &row, 734 bool exc /* = true */ ) 735{ 736 for (int i = 0; i < 3; i++) 737 { 738 if ((abs (scl) < 1 && abs (row[i]) >= limits<T>::max() * abs (scl))) 739 { 740 if (exc) 741 throw Imath::ZeroScaleExc ("Cannot remove zero scaling " 742 "from matrix."); 743 else 744 return false; 745 } 746 } 747 748 return true; 749} 750 751 752template <class T> 753Matrix44<T> 754rotationMatrix (const Vec3<T> &from, const Vec3<T> &to) 755{ 756 Quat<T> q; 757 q.setRotation(from, to); 758 return q.toMatrix44(); 759} 760 761 762template <class T> 763Matrix44<T> 764rotationMatrixWithUpDir (const Vec3<T> &fromDir, 765 const Vec3<T> &toDir, 766 const Vec3<T> &upDir) 767{ 768 // 769 // The goal is to obtain a rotation matrix that takes 770 // "fromDir" to "toDir". We do this in two steps and 771 // compose the resulting rotation matrices; 772 // (a) rotate "fromDir" into the z-axis 773 // (b) rotate the z-axis into "toDir" 774 // 775 776 // The from direction must be non-zero; but we allow zero to and up dirs. 777 if (fromDir.length () == 0) 778 return Matrix44<T> (); 779 780 else 781 { 782 Matrix44<T> zAxis2FromDir = alignZAxisWithTargetDir 783 (fromDir, Vec3<T> (0, 1, 0)); 784 785 Matrix44<T> fromDir2zAxis = zAxis2FromDir.transposed (); 786 787 Matrix44<T> zAxis2ToDir = alignZAxisWithTargetDir (toDir, upDir); 788 789 return fromDir2zAxis * zAxis2ToDir; 790 } 791} 792 793 794template <class T> 795Matrix44<T> 796alignZAxisWithTargetDir (Vec3<T> targetDir, Vec3<T> upDir) 797{ 798 // 799 // Ensure that the target direction is non-zero. 800 // 801 802 if ( targetDir.length () == 0 ) 803 targetDir = Vec3<T> (0, 0, 1); 804 805 // 806 // Ensure that the up direction is non-zero. 807 // 808 809 if ( upDir.length () == 0 ) 810 upDir = Vec3<T> (0, 1, 0); 811 812 // 813 // Check for degeneracies. If the upDir and targetDir are parallel 814 // or opposite, then compute a new, arbitrary up direction that is 815 // not parallel or opposite to the targetDir. 816 // 817 818 if (upDir.cross (targetDir).length () == 0) 819 { 820 upDir = targetDir.cross (Vec3<T> (1, 0, 0)); 821 if (upDir.length() == 0) 822 upDir = targetDir.cross(Vec3<T> (0, 0, 1)); 823 } 824 825 // 826 // Compute the x-, y-, and z-axis vectors of the new coordinate system. 827 // 828 829 Vec3<T> targetPerpDir = upDir.cross (targetDir); 830 Vec3<T> targetUpDir = targetDir.cross (targetPerpDir); 831 832 // 833 // Rotate the x-axis into targetPerpDir (row 0), 834 // rotate the y-axis into targetUpDir (row 1), 835 // rotate the z-axis into targetDir (row 2). 836 // 837 838 Vec3<T> row[3]; 839 row[0] = targetPerpDir.normalized (); 840 row[1] = targetUpDir .normalized (); 841 row[2] = targetDir .normalized (); 842 843 Matrix44<T> mat ( row[0][0], row[0][1], row[0][2], 0, 844 row[1][0], row[1][1], row[1][2], 0, 845 row[2][0], row[2][1], row[2][2], 0, 846 0, 0, 0, 1 ); 847 848 return mat; 849} 850 851 852 853//----------------------------------------------------------------------------- 854// Implementation for 3x3 Matrix 855//------------------------------ 856 857 858template <class T> 859bool 860extractScaling (const Matrix33<T> &mat, Vec2<T> &scl, bool exc) 861{ 862 T shr; 863 Matrix33<T> M (mat); 864 865 if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) 866 return false; 867 868 return true; 869} 870 871 872template <class T> 873Matrix33<T> 874sansScaling (const Matrix33<T> &mat, bool exc) 875{ 876 Vec2<T> scl; 877 T shr; 878 T rot; 879 Vec2<T> tran; 880 881 if (! extractSHRT (mat, scl, shr, rot, tran, exc)) 882 return mat; 883 884 Matrix33<T> M; 885 886 M.translate (tran); 887 M.rotate (rot); 888 M.shear (shr); 889 890 return M; 891} 892 893 894template <class T> 895bool 896removeScaling (Matrix33<T> &mat, bool exc) 897{ 898 Vec2<T> scl; 899 T shr; 900 T rot; 901 Vec2<T> tran; 902 903 if (! extractSHRT (mat, scl, shr, rot, tran, exc)) 904 return false; 905 906 mat.makeIdentity (); 907 mat.translate (tran); 908 mat.rotate (rot); 909 mat.shear (shr); 910 911 return true; 912} 913 914 915template <class T> 916bool 917extractScalingAndShear (const Matrix33<T> &mat, Vec2<T> &scl, T &shr, bool exc) 918{ 919 Matrix33<T> M (mat); 920 921 if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) 922 return false; 923 924 return true; 925} 926 927 928template <class T> 929Matrix33<T> 930sansScalingAndShear (const Matrix33<T> &mat, bool exc) 931{ 932 Vec2<T> scl; 933 T shr; 934 Matrix33<T> M (mat); 935 936 if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) 937 return mat; 938 939 return M; 940} 941 942 943template <class T> 944bool 945removeScalingAndShear (Matrix33<T> &mat, bool exc) 946{ 947 Vec2<T> scl; 948 T shr; 949 950 if (! extractAndRemoveScalingAndShear (mat, scl, shr, exc)) 951 return false; 952 953 return true; 954} 955 956template <class T> 957bool 958extractAndRemoveScalingAndShear (Matrix33<T> &mat, 959 Vec2<T> &scl, T &shr, bool exc) 960{ 961 Vec2<T> row[2]; 962 963 row[0] = Vec2<T> (mat[0][0], mat[0][1]); 964 row[1] = Vec2<T> (mat[1][0], mat[1][1]); 965 966 T maxVal = 0; 967 for (int i=0; i < 2; i++) 968 for (int j=0; j < 2; j++) 969 if (Imath::abs (row[i][j]) > maxVal) 970 maxVal = Imath::abs (row[i][j]); 971 972 // 973 // We normalize the 2x2 matrix here. 974 // It was noticed that this can improve numerical stability significantly, 975 // especially when many of the upper 2x2 matrix's coefficients are very 976 // close to zero; we correct for this step at the end by multiplying the 977 // scaling factors by maxVal at the end (shear and rotation are not 978 // affected by the normalization). 979 980 if (maxVal != 0) 981 { 982 for (int i=0; i < 2; i++) 983 if (! checkForZeroScaleInRow (maxVal, row[i], exc)) 984 return false; 985 else 986 row[i] /= maxVal; 987 } 988 989 // Compute X scale factor. 990 scl.x = row[0].length (); 991 if (! checkForZeroScaleInRow (scl.x, row[0], exc)) 992 return false; 993 994 // Normalize first row. 995 row[0] /= scl.x; 996 997 // An XY shear factor will shear the X coord. as the Y coord. changes. 998 // There are 2 combinations (XY, YX), although we only extract the XY 999 // shear factor because we can effect the an YX shear factor by 1000 // shearing in XY combined with rotations and scales. 1001 // 1002 // shear matrix < 1, YX, 0, 1003 // XY, 1, 0, 1004 // 0, 0, 1 > 1005 1006 // Compute XY shear factor and make 2nd row orthogonal to 1st. 1007 shr = row[0].dot (row[1]); 1008 row[1] -= shr * row[0]; 1009 1010 // Now, compute Y scale. 1011 scl.y = row[1].length (); 1012 if (! checkForZeroScaleInRow (scl.y, row[1], exc)) 1013 return false; 1014 1015 // Normalize 2nd row and correct the XY shear factor for Y scaling. 1016 row[1] /= scl.y; 1017 shr /= scl.y; 1018 1019 // At this point, the upper 2x2 matrix in mat is orthonormal. 1020 // Check for a coordinate system flip. If the determinant 1021 // is -1, then flip the rotation matrix and adjust the scale(Y) 1022 // and shear(XY) factors to compensate. 1023 if (row[0][0] * row[1][1] - row[0][1] * row[1][0] < 0) 1024 { 1025 row[1][0] *= -1; 1026 row[1][1] *= -1; 1027 scl[1] *= -1; 1028 shr *= -1; 1029 } 1030 1031 // Copy over the orthonormal rows into the returned matrix. 1032 // The upper 2x2 matrix in mat is now a rotation matrix. 1033 for (int i=0; i < 2; i++) 1034 { 1035 mat[i][0] = row[i][0]; 1036 mat[i][1] = row[i][1]; 1037 } 1038 1039 scl *= maxVal; 1040 1041 return true; 1042} 1043 1044 1045template <class T> 1046void 1047extractEuler (const Matrix33<T> &mat, T &rot) 1048{ 1049 // 1050 // Normalize the local x and y axes to remove scaling. 1051 // 1052 1053 Vec2<T> i (mat[0][0], mat[0][1]); 1054 Vec2<T> j (mat[1][0], mat[1][1]); 1055 1056 i.normalize(); 1057 j.normalize(); 1058 1059 // 1060 // Extract the angle, rot. 1061 // 1062 1063 rot = - Math<T>::atan2 (j[0], i[0]); 1064} 1065 1066 1067template <class T> 1068bool 1069extractSHRT (const Matrix33<T> &mat, 1070 Vec2<T> &s, 1071 T &h, 1072 T &r, 1073 Vec2<T> &t, 1074 bool exc) 1075{ 1076 Matrix33<T> rot; 1077 1078 rot = mat; 1079 if (! extractAndRemoveScalingAndShear (rot, s, h, exc)) 1080 return false; 1081 1082 extractEuler (rot, r); 1083 1084 t.x = mat[2][0]; 1085 t.y = mat[2][1]; 1086 1087 return true; 1088} 1089 1090 1091template <class T> 1092bool 1093checkForZeroScaleInRow (const T& scl, 1094 const Vec2<T> &row, 1095 bool exc /* = true */ ) 1096{ 1097 for (int i = 0; i < 2; i++) 1098 { 1099 if ((abs (scl) < 1 && abs (row[i]) >= limits<T>::max() * abs (scl))) 1100 { 1101 if (exc) 1102 throw Imath::ZeroScaleExc ("Cannot remove zero scaling " 1103 "from matrix."); 1104 else 1105 return false; 1106 } 1107 } 1108 1109 return true; 1110} 1111 1112 1113} // namespace Imath 1114 1115#endif