PBRT
|
00001 00002 // 00003 // Copyright (c) 2002, Industrial Light & Magic, a division of Lucas 00004 // Digital Ltd. LLC 00005 // 00006 // All rights reserved. 00007 // 00008 // Redistribution and use in source and binary forms, with or without 00009 // modification, are permitted provided that the following conditions are 00010 // met: 00011 // * Redistributions of source code must retain the above copyright 00012 // notice, this list of conditions and the following disclaimer. 00013 // * Redistributions in binary form must reproduce the above 00014 // copyright notice, this list of conditions and the following disclaimer 00015 // in the documentation and/or other materials provided with the 00016 // distribution. 00017 // * Neither the name of Industrial Light & Magic nor the names of 00018 // its contributors may be used to endorse or promote products derived 00019 // from this software without specific prior written permission. 00020 // 00021 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 00024 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 00025 // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 00026 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 00027 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 00028 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 00029 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00030 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 00031 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00032 // 00034 00035 00036 #ifndef INCLUDED_IMATHMATRIXALGO_H 00037 #define INCLUDED_IMATHMATRIXALGO_H 00038 00039 //------------------------------------------------------------------------- 00040 // 00041 // This file contains algorithms applied to or in conjunction with 00042 // transformation matrices (Imath::Matrix33 and Imath::Matrix44). 00043 // The assumption made is that these functions are called much less 00044 // often than the basic point functions or these functions require 00045 // more support classes. 00046 // 00047 // This file also defines a few predefined constant matrices. 00048 // 00049 //------------------------------------------------------------------------- 00050 00051 #include "ImathMatrix.h" 00052 #include "ImathQuat.h" 00053 #include "ImathEuler.h" 00054 #include "ImathExc.h" 00055 #include "ImathVec.h" 00056 #include <math.h> 00057 00058 00059 #ifdef OPENEXR_DLL 00060 #ifdef IMATH_EXPORTS 00061 #define IMATH_EXPORT_CONST extern __declspec(dllexport) 00062 #else 00063 #define IMATH_EXPORT_CONST extern __declspec(dllimport) 00064 #endif 00065 #else 00066 #define IMATH_EXPORT_CONST extern const 00067 #endif 00068 00069 00070 namespace Imath { 00071 00072 //------------------ 00073 // Identity matrices 00074 //------------------ 00075 00076 IMATH_EXPORT_CONST M33f identity33f; 00077 IMATH_EXPORT_CONST M44f identity44f; 00078 IMATH_EXPORT_CONST M33d identity33d; 00079 IMATH_EXPORT_CONST M44d identity44d; 00080 00081 //---------------------------------------------------------------------- 00082 // Extract scale, shear, rotation, and translation values from a matrix: 00083 // 00084 // Notes: 00085 // 00086 // This implementation follows the technique described in the paper by 00087 // Spencer W. Thomas in the Graphics Gems II article: "Decomposing a 00088 // Matrix into Simple Transformations", p. 320. 00089 // 00090 // - Some of the functions below have an optional exc parameter 00091 // that determines the functions' behavior when the matrix' 00092 // scaling is very close to zero: 00093 // 00094 // If exc is true, the functions throw an Imath::ZeroScale exception. 00095 // 00096 // If exc is false: 00097 // 00098 // extractScaling (m, s) returns false, s is invalid 00099 // sansScaling (m) returns m 00100 // removeScaling (m) returns false, m is unchanged 00101 // sansScalingAndShear (m) returns m 00102 // removeScalingAndShear (m) returns false, m is unchanged 00103 // extractAndRemoveScalingAndShear (m, s, h) 00104 // returns false, m is unchanged, 00105 // (sh) are invalid 00106 // checkForZeroScaleInRow () returns false 00107 // extractSHRT (m, s, h, r, t) returns false, (shrt) are invalid 00108 // 00109 // - Functions extractEuler(), extractEulerXYZ() and extractEulerZYX() 00110 // assume that the matrix does not include shear or non-uniform scaling, 00111 // but they do not examine the matrix to verify this assumption. 00112 // Matrices with shear or non-uniform scaling are likely to produce 00113 // meaningless results. Therefore, you should use the 00114 // removeScalingAndShear() routine, if necessary, prior to calling 00115 // extractEuler...() . 00116 // 00117 // - All functions assume that the matrix does not include perspective 00118 // transformation(s), but they do not examine the matrix to verify 00119 // this assumption. Matrices with perspective transformations are 00120 // likely to produce meaningless results. 00121 // 00122 //---------------------------------------------------------------------- 00123 00124 00125 // 00126 // Declarations for 4x4 matrix. 00127 // 00128 00129 template <class T> bool extractScaling 00130 (const Matrix44<T> &mat, 00131 Vec3<T> &scl, 00132 bool exc = true); 00133 00134 template <class T> Matrix44<T> sansScaling (const Matrix44<T> &mat, 00135 bool exc = true); 00136 00137 template <class T> bool removeScaling 00138 (Matrix44<T> &mat, 00139 bool exc = true); 00140 00141 template <class T> bool extractScalingAndShear 00142 (const Matrix44<T> &mat, 00143 Vec3<T> &scl, 00144 Vec3<T> &shr, 00145 bool exc = true); 00146 00147 template <class T> Matrix44<T> sansScalingAndShear 00148 (const Matrix44<T> &mat, 00149 bool exc = true); 00150 00151 template <class T> bool removeScalingAndShear 00152 (Matrix44<T> &mat, 00153 bool exc = true); 00154 00155 template <class T> bool extractAndRemoveScalingAndShear 00156 (Matrix44<T> &mat, 00157 Vec3<T> &scl, 00158 Vec3<T> &shr, 00159 bool exc = true); 00160 00161 template <class T> void extractEulerXYZ 00162 (const Matrix44<T> &mat, 00163 Vec3<T> &rot); 00164 00165 template <class T> void extractEulerZYX 00166 (const Matrix44<T> &mat, 00167 Vec3<T> &rot); 00168 00169 template <class T> Quat<T> extractQuat (const Matrix44<T> &mat); 00170 00171 template <class T> bool extractSHRT 00172 (const Matrix44<T> &mat, 00173 Vec3<T> &s, 00174 Vec3<T> &h, 00175 Vec3<T> &r, 00176 Vec3<T> &t, 00177 bool exc /*= true*/, 00178 typename Euler<T>::Order rOrder); 00179 00180 template <class T> bool extractSHRT 00181 (const Matrix44<T> &mat, 00182 Vec3<T> &s, 00183 Vec3<T> &h, 00184 Vec3<T> &r, 00185 Vec3<T> &t, 00186 bool exc = true); 00187 00188 template <class T> bool extractSHRT 00189 (const Matrix44<T> &mat, 00190 Vec3<T> &s, 00191 Vec3<T> &h, 00192 Euler<T> &r, 00193 Vec3<T> &t, 00194 bool exc = true); 00195 00196 // 00197 // Internal utility function. 00198 // 00199 00200 template <class T> bool checkForZeroScaleInRow 00201 (const T &scl, 00202 const Vec3<T> &row, 00203 bool exc = true); 00204 00205 // 00206 // Returns a matrix that rotates "fromDirection" vector to "toDirection" 00207 // vector. 00208 // 00209 00210 template <class T> Matrix44<T> rotationMatrix (const Vec3<T> &fromDirection, 00211 const Vec3<T> &toDirection); 00212 00213 00214 00215 // 00216 // Returns a matrix that rotates the "fromDir" vector 00217 // so that it points towards "toDir". You may also 00218 // specify that you want the up vector to be pointing 00219 // in a certain direction "upDir". 00220 // 00221 00222 template <class T> Matrix44<T> rotationMatrixWithUpDir 00223 (const Vec3<T> &fromDir, 00224 const Vec3<T> &toDir, 00225 const Vec3<T> &upDir); 00226 00227 00228 // 00229 // Returns a matrix that rotates the z-axis so that it 00230 // points towards "targetDir". You must also specify 00231 // that you want the up vector to be pointing in a 00232 // certain direction "upDir". 00233 // 00234 // Notes: The following degenerate cases are handled: 00235 // (a) when the directions given by "toDir" and "upDir" 00236 // are parallel or opposite; 00237 // (the direction vectors must have a non-zero cross product) 00238 // (b) when any of the given direction vectors have zero length 00239 // 00240 00241 template <class T> Matrix44<T> alignZAxisWithTargetDir 00242 (Vec3<T> targetDir, 00243 Vec3<T> upDir); 00244 00245 00246 //---------------------------------------------------------------------- 00247 00248 00249 // 00250 // Declarations for 3x3 matrix. 00251 // 00252 00253 00254 template <class T> bool extractScaling 00255 (const Matrix33<T> &mat, 00256 Vec2<T> &scl, 00257 bool exc = true); 00258 00259 template <class T> Matrix33<T> sansScaling (const Matrix33<T> &mat, 00260 bool exc = true); 00261 00262 template <class T> bool removeScaling 00263 (Matrix33<T> &mat, 00264 bool exc = true); 00265 00266 template <class T> bool extractScalingAndShear 00267 (const Matrix33<T> &mat, 00268 Vec2<T> &scl, 00269 T &h, 00270 bool exc = true); 00271 00272 template <class T> Matrix33<T> sansScalingAndShear 00273 (const Matrix33<T> &mat, 00274 bool exc = true); 00275 00276 template <class T> bool removeScalingAndShear 00277 (Matrix33<T> &mat, 00278 bool exc = true); 00279 00280 template <class T> bool extractAndRemoveScalingAndShear 00281 (Matrix33<T> &mat, 00282 Vec2<T> &scl, 00283 T &shr, 00284 bool exc = true); 00285 00286 template <class T> void extractEuler 00287 (const Matrix33<T> &mat, 00288 T &rot); 00289 00290 template <class T> bool extractSHRT (const Matrix33<T> &mat, 00291 Vec2<T> &s, 00292 T &h, 00293 T &r, 00294 Vec2<T> &t, 00295 bool exc = true); 00296 00297 template <class T> bool checkForZeroScaleInRow 00298 (const T &scl, 00299 const Vec2<T> &row, 00300 bool exc = true); 00301 00302 00303 00304 00305 //----------------------------------------------------------------------------- 00306 // Implementation for 4x4 Matrix 00307 //------------------------------ 00308 00309 00310 template <class T> 00311 bool 00312 extractScaling (const Matrix44<T> &mat, Vec3<T> &scl, bool exc) 00313 { 00314 Vec3<T> shr; 00315 Matrix44<T> M (mat); 00316 00317 if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) 00318 return false; 00319 00320 return true; 00321 } 00322 00323 00324 template <class T> 00325 Matrix44<T> 00326 sansScaling (const Matrix44<T> &mat, bool exc) 00327 { 00328 Vec3<T> scl; 00329 Vec3<T> shr; 00330 Vec3<T> rot; 00331 Vec3<T> tran; 00332 00333 if (! extractSHRT (mat, scl, shr, rot, tran, exc)) 00334 return mat; 00335 00336 Matrix44<T> M; 00337 00338 M.translate (tran); 00339 M.rotate (rot); 00340 M.shear (shr); 00341 00342 return M; 00343 } 00344 00345 00346 template <class T> 00347 bool 00348 removeScaling (Matrix44<T> &mat, bool exc) 00349 { 00350 Vec3<T> scl; 00351 Vec3<T> shr; 00352 Vec3<T> rot; 00353 Vec3<T> tran; 00354 00355 if (! extractSHRT (mat, scl, shr, rot, tran, exc)) 00356 return false; 00357 00358 mat.makeIdentity (); 00359 mat.translate (tran); 00360 mat.rotate (rot); 00361 mat.shear (shr); 00362 00363 return true; 00364 } 00365 00366 00367 template <class T> 00368 bool 00369 extractScalingAndShear (const Matrix44<T> &mat, 00370 Vec3<T> &scl, Vec3<T> &shr, bool exc) 00371 { 00372 Matrix44<T> M (mat); 00373 00374 if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) 00375 return false; 00376 00377 return true; 00378 } 00379 00380 00381 template <class T> 00382 Matrix44<T> 00383 sansScalingAndShear (const Matrix44<T> &mat, bool exc) 00384 { 00385 Vec3<T> scl; 00386 Vec3<T> shr; 00387 Matrix44<T> M (mat); 00388 00389 if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) 00390 return mat; 00391 00392 return M; 00393 } 00394 00395 00396 template <class T> 00397 bool 00398 removeScalingAndShear (Matrix44<T> &mat, bool exc) 00399 { 00400 Vec3<T> scl; 00401 Vec3<T> shr; 00402 00403 if (! extractAndRemoveScalingAndShear (mat, scl, shr, exc)) 00404 return false; 00405 00406 return true; 00407 } 00408 00409 00410 template <class T> 00411 bool 00412 extractAndRemoveScalingAndShear (Matrix44<T> &mat, 00413 Vec3<T> &scl, Vec3<T> &shr, bool exc) 00414 { 00415 // 00416 // This implementation follows the technique described in the paper by 00417 // Spencer W. Thomas in the Graphics Gems II article: "Decomposing a 00418 // Matrix into Simple Transformations", p. 320. 00419 // 00420 00421 Vec3<T> row[3]; 00422 00423 row[0] = Vec3<T> (mat[0][0], mat[0][1], mat[0][2]); 00424 row[1] = Vec3<T> (mat[1][0], mat[1][1], mat[1][2]); 00425 row[2] = Vec3<T> (mat[2][0], mat[2][1], mat[2][2]); 00426 00427 T maxVal = 0; 00428 for (int i=0; i < 3; i++) 00429 for (int j=0; j < 3; j++) 00430 if (Imath::abs (row[i][j]) > maxVal) 00431 maxVal = Imath::abs (row[i][j]); 00432 00433 // 00434 // We normalize the 3x3 matrix here. 00435 // It was noticed that this can improve numerical stability significantly, 00436 // especially when many of the upper 3x3 matrix's coefficients are very 00437 // close to zero; we correct for this step at the end by multiplying the 00438 // scaling factors by maxVal at the end (shear and rotation are not 00439 // affected by the normalization). 00440 00441 if (maxVal != 0) 00442 { 00443 for (int i=0; i < 3; i++) 00444 if (! checkForZeroScaleInRow (maxVal, row[i], exc)) 00445 return false; 00446 else 00447 row[i] /= maxVal; 00448 } 00449 00450 // Compute X scale factor. 00451 scl.x = row[0].length (); 00452 if (! checkForZeroScaleInRow (scl.x, row[0], exc)) 00453 return false; 00454 00455 // Normalize first row. 00456 row[0] /= scl.x; 00457 00458 // An XY shear factor will shear the X coord. as the Y coord. changes. 00459 // There are 6 combinations (XY, XZ, YZ, YX, ZX, ZY), although we only 00460 // extract the first 3 because we can effect the last 3 by shearing in 00461 // XY, XZ, YZ combined rotations and scales. 00462 // 00463 // shear matrix < 1, YX, ZX, 0, 00464 // XY, 1, ZY, 0, 00465 // XZ, YZ, 1, 0, 00466 // 0, 0, 0, 1 > 00467 00468 // Compute XY shear factor and make 2nd row orthogonal to 1st. 00469 shr[0] = row[0].dot (row[1]); 00470 row[1] -= shr[0] * row[0]; 00471 00472 // Now, compute Y scale. 00473 scl.y = row[1].length (); 00474 if (! checkForZeroScaleInRow (scl.y, row[1], exc)) 00475 return false; 00476 00477 // Normalize 2nd row and correct the XY shear factor for Y scaling. 00478 row[1] /= scl.y; 00479 shr[0] /= scl.y; 00480 00481 // Compute XZ and YZ shears, orthogonalize 3rd row. 00482 shr[1] = row[0].dot (row[2]); 00483 row[2] -= shr[1] * row[0]; 00484 shr[2] = row[1].dot (row[2]); 00485 row[2] -= shr[2] * row[1]; 00486 00487 // Next, get Z scale. 00488 scl.z = row[2].length (); 00489 if (! checkForZeroScaleInRow (scl.z, row[2], exc)) 00490 return false; 00491 00492 // Normalize 3rd row and correct the XZ and YZ shear factors for Z scaling. 00493 row[2] /= scl.z; 00494 shr[1] /= scl.z; 00495 shr[2] /= scl.z; 00496 00497 // At this point, the upper 3x3 matrix in mat is orthonormal. 00498 // Check for a coordinate system flip. If the determinant 00499 // is less than zero, then negate the matrix and the scaling factors. 00500 if (row[0].dot (row[1].cross (row[2])) < 0) 00501 for (int i=0; i < 3; i++) 00502 { 00503 scl[i] *= -1; 00504 row[i] *= -1; 00505 } 00506 00507 // Copy over the orthonormal rows into the returned matrix. 00508 // The upper 3x3 matrix in mat is now a rotation matrix. 00509 for (int i=0; i < 3; i++) 00510 { 00511 mat[i][0] = row[i][0]; 00512 mat[i][1] = row[i][1]; 00513 mat[i][2] = row[i][2]; 00514 } 00515 00516 // Correct the scaling factors for the normalization step that we 00517 // performed above; shear and rotation are not affected by the 00518 // normalization. 00519 scl *= maxVal; 00520 00521 return true; 00522 } 00523 00524 00525 template <class T> 00526 void 00527 extractEulerXYZ (const Matrix44<T> &mat, Vec3<T> &rot) 00528 { 00529 // 00530 // Normalize the local x, y and z axes to remove scaling. 00531 // 00532 00533 Vec3<T> i (mat[0][0], mat[0][1], mat[0][2]); 00534 Vec3<T> j (mat[1][0], mat[1][1], mat[1][2]); 00535 Vec3<T> k (mat[2][0], mat[2][1], mat[2][2]); 00536 00537 i.normalize(); 00538 j.normalize(); 00539 k.normalize(); 00540 00541 Matrix44<T> M (i[0], i[1], i[2], 0, 00542 j[0], j[1], j[2], 0, 00543 k[0], k[1], k[2], 0, 00544 0, 0, 0, 1); 00545 00546 00547 // 00548 // Extract the first angle, rot.x. 00549 // 00550 00551 rot.x = Math<T>::atan2 (M[1][2], M[2][2]); 00552 00553 // 00554 // Remove the rot.x rotation from M, so that the remaining 00555 // rotation, N, is only around two axes, and gimbal lock 00556 // cannot occur. 00557 // 00558 Matrix44<T> N; 00559 N.rotate (Vec3<T> (-rot.x, 0, 0)); 00560 00561 N = N * M; 00562 00563 // Extract the other two angles, rot.y and rot.z, from N. 00564 // 00565 00566 T cy = Math<T>::sqrt (N[0][0]*N[0][0] + N[0][1]*N[0][1]); 00567 rot.y = Math<T>::atan2 (-N[0][2], cy); 00568 rot.z = Math<T>::atan2 (-N[1][0], N[1][1]); 00569 00570 } 00571 00572 00573 template <class T> 00574 void 00575 extractEulerZYX (const Matrix44<T> &mat, Vec3<T> &rot) 00576 { 00577 // 00578 // Normalize the local x, y and z axes to remove scaling. 00579 // 00580 00581 Vec3<T> i (mat[0][0], mat[0][1], mat[0][2]); 00582 Vec3<T> j (mat[1][0], mat[1][1], mat[1][2]); 00583 Vec3<T> k (mat[2][0], mat[2][1], mat[2][2]); 00584 00585 i.normalize(); 00586 j.normalize(); 00587 k.normalize(); 00588 00589 Matrix44<T> M (i[0], i[1], i[2], 0, 00590 j[0], j[1], j[2], 0, 00591 k[0], k[1], k[2], 0, 00592 0, 0, 0, 1); 00593 00594 // 00595 // Extract the first angle, rot.x. 00596 // 00597 00598 rot.x = -Math<T>::atan2 (M[1][0], M[0][0]); 00599 00600 // 00601 // Remove the x rotation from M, so that the remaining 00602 // rotation, N, is only around two axes, and gimbal lock 00603 // cannot occur. 00604 // 00605 00606 Matrix44<T> N; 00607 N.rotate (Vec3<T> (0, 0, -rot.x)); 00608 N = N * M; 00609 00610 // 00611 // Extract the other two angles, rot.y and rot.z, from N. 00612 // 00613 00614 T cy = Math<T>::sqrt (N[2][2]*N[2][2] + N[2][1]*N[2][1]); 00615 rot.y = -Math<T>::atan2 (-N[2][0], cy); 00616 rot.z = -Math<T>::atan2 (-N[1][2], N[1][1]); 00617 } 00618 00619 00620 template <class T> 00621 Quat<T> 00622 extractQuat (const Matrix44<T> &mat) 00623 { 00624 Matrix44<T> rot; 00625 00626 T tr, s; 00627 T q[4]; 00628 int i, j, k; 00629 Quat<T> quat; 00630 00631 int nxt[3] = {1, 2, 0}; 00632 tr = mat[0][0] + mat[1][1] + mat[2][2]; 00633 00634 // check the diagonal 00635 if (tr > 0.0) { 00636 s = Math<T>::sqrt (tr + 1.0); 00637 quat.r = s / 2.0; 00638 s = 0.5 / s; 00639 00640 quat.v.x = (mat[1][2] - mat[2][1]) * s; 00641 quat.v.y = (mat[2][0] - mat[0][2]) * s; 00642 quat.v.z = (mat[0][1] - mat[1][0]) * s; 00643 } 00644 else { 00645 // diagonal is negative 00646 i = 0; 00647 if (mat[1][1] > mat[0][0]) 00648 i=1; 00649 if (mat[2][2] > mat[i][i]) 00650 i=2; 00651 00652 j = nxt[i]; 00653 k = nxt[j]; 00654 s = Math<T>::sqrt ((mat[i][i] - (mat[j][j] + mat[k][k])) + 1.0); 00655 00656 q[i] = s * 0.5; 00657 if (s != 0.0) 00658 s = 0.5 / s; 00659 00660 q[3] = (mat[j][k] - mat[k][j]) * s; 00661 q[j] = (mat[i][j] + mat[j][i]) * s; 00662 q[k] = (mat[i][k] + mat[k][i]) * s; 00663 00664 quat.v.x = q[0]; 00665 quat.v.y = q[1]; 00666 quat.v.z = q[2]; 00667 quat.r = q[3]; 00668 } 00669 00670 return quat; 00671 } 00672 00673 template <class T> 00674 bool 00675 extractSHRT (const Matrix44<T> &mat, 00676 Vec3<T> &s, 00677 Vec3<T> &h, 00678 Vec3<T> &r, 00679 Vec3<T> &t, 00680 bool exc /* = true */ , 00681 typename Euler<T>::Order rOrder /* = Euler<T>::XYZ */ ) 00682 { 00683 Matrix44<T> rot; 00684 00685 rot = mat; 00686 if (! extractAndRemoveScalingAndShear (rot, s, h, exc)) 00687 return false; 00688 00689 extractEulerXYZ (rot, r); 00690 00691 t.x = mat[3][0]; 00692 t.y = mat[3][1]; 00693 t.z = mat[3][2]; 00694 00695 if (rOrder != Euler<T>::XYZ) 00696 { 00697 Imath::Euler<T> eXYZ (r, Imath::Euler<T>::XYZ); 00698 Imath::Euler<T> e (eXYZ, rOrder); 00699 r = e.toXYZVector (); 00700 } 00701 00702 return true; 00703 } 00704 00705 template <class T> 00706 bool 00707 extractSHRT (const Matrix44<T> &mat, 00708 Vec3<T> &s, 00709 Vec3<T> &h, 00710 Vec3<T> &r, 00711 Vec3<T> &t, 00712 bool exc) 00713 { 00714 return extractSHRT(mat, s, h, r, t, exc, Imath::Euler<T>::XYZ); 00715 } 00716 00717 template <class T> 00718 bool 00719 extractSHRT (const Matrix44<T> &mat, 00720 Vec3<T> &s, 00721 Vec3<T> &h, 00722 Euler<T> &r, 00723 Vec3<T> &t, 00724 bool exc /* = true */) 00725 { 00726 return extractSHRT (mat, s, h, r, t, exc, r.order ()); 00727 } 00728 00729 00730 template <class T> 00731 bool 00732 checkForZeroScaleInRow (const T& scl, 00733 const Vec3<T> &row, 00734 bool exc /* = true */ ) 00735 { 00736 for (int i = 0; i < 3; i++) 00737 { 00738 if ((abs (scl) < 1 && abs (row[i]) >= limits<T>::max() * abs (scl))) 00739 { 00740 if (exc) 00741 throw Imath::ZeroScaleExc ("Cannot remove zero scaling " 00742 "from matrix."); 00743 else 00744 return false; 00745 } 00746 } 00747 00748 return true; 00749 } 00750 00751 00752 template <class T> 00753 Matrix44<T> 00754 rotationMatrix (const Vec3<T> &from, const Vec3<T> &to) 00755 { 00756 Quat<T> q; 00757 q.setRotation(from, to); 00758 return q.toMatrix44(); 00759 } 00760 00761 00762 template <class T> 00763 Matrix44<T> 00764 rotationMatrixWithUpDir (const Vec3<T> &fromDir, 00765 const Vec3<T> &toDir, 00766 const Vec3<T> &upDir) 00767 { 00768 // 00769 // The goal is to obtain a rotation matrix that takes 00770 // "fromDir" to "toDir". We do this in two steps and 00771 // compose the resulting rotation matrices; 00772 // (a) rotate "fromDir" into the z-axis 00773 // (b) rotate the z-axis into "toDir" 00774 // 00775 00776 // The from direction must be non-zero; but we allow zero to and up dirs. 00777 if (fromDir.length () == 0) 00778 return Matrix44<T> (); 00779 00780 else 00781 { 00782 Matrix44<T> zAxis2FromDir = alignZAxisWithTargetDir 00783 (fromDir, Vec3<T> (0, 1, 0)); 00784 00785 Matrix44<T> fromDir2zAxis = zAxis2FromDir.transposed (); 00786 00787 Matrix44<T> zAxis2ToDir = alignZAxisWithTargetDir (toDir, upDir); 00788 00789 return fromDir2zAxis * zAxis2ToDir; 00790 } 00791 } 00792 00793 00794 template <class T> 00795 Matrix44<T> 00796 alignZAxisWithTargetDir (Vec3<T> targetDir, Vec3<T> upDir) 00797 { 00798 // 00799 // Ensure that the target direction is non-zero. 00800 // 00801 00802 if ( targetDir.length () == 0 ) 00803 targetDir = Vec3<T> (0, 0, 1); 00804 00805 // 00806 // Ensure that the up direction is non-zero. 00807 // 00808 00809 if ( upDir.length () == 0 ) 00810 upDir = Vec3<T> (0, 1, 0); 00811 00812 // 00813 // Check for degeneracies. If the upDir and targetDir are parallel 00814 // or opposite, then compute a new, arbitrary up direction that is 00815 // not parallel or opposite to the targetDir. 00816 // 00817 00818 if (upDir.cross (targetDir).length () == 0) 00819 { 00820 upDir = targetDir.cross (Vec3<T> (1, 0, 0)); 00821 if (upDir.length() == 0) 00822 upDir = targetDir.cross(Vec3<T> (0, 0, 1)); 00823 } 00824 00825 // 00826 // Compute the x-, y-, and z-axis vectors of the new coordinate system. 00827 // 00828 00829 Vec3<T> targetPerpDir = upDir.cross (targetDir); 00830 Vec3<T> targetUpDir = targetDir.cross (targetPerpDir); 00831 00832 // 00833 // Rotate the x-axis into targetPerpDir (row 0), 00834 // rotate the y-axis into targetUpDir (row 1), 00835 // rotate the z-axis into targetDir (row 2). 00836 // 00837 00838 Vec3<T> row[3]; 00839 row[0] = targetPerpDir.normalized (); 00840 row[1] = targetUpDir .normalized (); 00841 row[2] = targetDir .normalized (); 00842 00843 Matrix44<T> mat ( row[0][0], row[0][1], row[0][2], 0, 00844 row[1][0], row[1][1], row[1][2], 0, 00845 row[2][0], row[2][1], row[2][2], 0, 00846 0, 0, 0, 1 ); 00847 00848 return mat; 00849 } 00850 00851 00852 00853 //----------------------------------------------------------------------------- 00854 // Implementation for 3x3 Matrix 00855 //------------------------------ 00856 00857 00858 template <class T> 00859 bool 00860 extractScaling (const Matrix33<T> &mat, Vec2<T> &scl, bool exc) 00861 { 00862 T shr; 00863 Matrix33<T> M (mat); 00864 00865 if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) 00866 return false; 00867 00868 return true; 00869 } 00870 00871 00872 template <class T> 00873 Matrix33<T> 00874 sansScaling (const Matrix33<T> &mat, bool exc) 00875 { 00876 Vec2<T> scl; 00877 T shr; 00878 T rot; 00879 Vec2<T> tran; 00880 00881 if (! extractSHRT (mat, scl, shr, rot, tran, exc)) 00882 return mat; 00883 00884 Matrix33<T> M; 00885 00886 M.translate (tran); 00887 M.rotate (rot); 00888 M.shear (shr); 00889 00890 return M; 00891 } 00892 00893 00894 template <class T> 00895 bool 00896 removeScaling (Matrix33<T> &mat, bool exc) 00897 { 00898 Vec2<T> scl; 00899 T shr; 00900 T rot; 00901 Vec2<T> tran; 00902 00903 if (! extractSHRT (mat, scl, shr, rot, tran, exc)) 00904 return false; 00905 00906 mat.makeIdentity (); 00907 mat.translate (tran); 00908 mat.rotate (rot); 00909 mat.shear (shr); 00910 00911 return true; 00912 } 00913 00914 00915 template <class T> 00916 bool 00917 extractScalingAndShear (const Matrix33<T> &mat, Vec2<T> &scl, T &shr, bool exc) 00918 { 00919 Matrix33<T> M (mat); 00920 00921 if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) 00922 return false; 00923 00924 return true; 00925 } 00926 00927 00928 template <class T> 00929 Matrix33<T> 00930 sansScalingAndShear (const Matrix33<T> &mat, bool exc) 00931 { 00932 Vec2<T> scl; 00933 T shr; 00934 Matrix33<T> M (mat); 00935 00936 if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) 00937 return mat; 00938 00939 return M; 00940 } 00941 00942 00943 template <class T> 00944 bool 00945 removeScalingAndShear (Matrix33<T> &mat, bool exc) 00946 { 00947 Vec2<T> scl; 00948 T shr; 00949 00950 if (! extractAndRemoveScalingAndShear (mat, scl, shr, exc)) 00951 return false; 00952 00953 return true; 00954 } 00955 00956 template <class T> 00957 bool 00958 extractAndRemoveScalingAndShear (Matrix33<T> &mat, 00959 Vec2<T> &scl, T &shr, bool exc) 00960 { 00961 Vec2<T> row[2]; 00962 00963 row[0] = Vec2<T> (mat[0][0], mat[0][1]); 00964 row[1] = Vec2<T> (mat[1][0], mat[1][1]); 00965 00966 T maxVal = 0; 00967 for (int i=0; i < 2; i++) 00968 for (int j=0; j < 2; j++) 00969 if (Imath::abs (row[i][j]) > maxVal) 00970 maxVal = Imath::abs (row[i][j]); 00971 00972 // 00973 // We normalize the 2x2 matrix here. 00974 // It was noticed that this can improve numerical stability significantly, 00975 // especially when many of the upper 2x2 matrix's coefficients are very 00976 // close to zero; we correct for this step at the end by multiplying the 00977 // scaling factors by maxVal at the end (shear and rotation are not 00978 // affected by the normalization). 00979 00980 if (maxVal != 0) 00981 { 00982 for (int i=0; i < 2; i++) 00983 if (! checkForZeroScaleInRow (maxVal, row[i], exc)) 00984 return false; 00985 else 00986 row[i] /= maxVal; 00987 } 00988 00989 // Compute X scale factor. 00990 scl.x = row[0].length (); 00991 if (! checkForZeroScaleInRow (scl.x, row[0], exc)) 00992 return false; 00993 00994 // Normalize first row. 00995 row[0] /= scl.x; 00996 00997 // An XY shear factor will shear the X coord. as the Y coord. changes. 00998 // There are 2 combinations (XY, YX), although we only extract the XY 00999 // shear factor because we can effect the an YX shear factor by 01000 // shearing in XY combined with rotations and scales. 01001 // 01002 // shear matrix < 1, YX, 0, 01003 // XY, 1, 0, 01004 // 0, 0, 1 > 01005 01006 // Compute XY shear factor and make 2nd row orthogonal to 1st. 01007 shr = row[0].dot (row[1]); 01008 row[1] -= shr * row[0]; 01009 01010 // Now, compute Y scale. 01011 scl.y = row[1].length (); 01012 if (! checkForZeroScaleInRow (scl.y, row[1], exc)) 01013 return false; 01014 01015 // Normalize 2nd row and correct the XY shear factor for Y scaling. 01016 row[1] /= scl.y; 01017 shr /= scl.y; 01018 01019 // At this point, the upper 2x2 matrix in mat is orthonormal. 01020 // Check for a coordinate system flip. If the determinant 01021 // is -1, then flip the rotation matrix and adjust the scale(Y) 01022 // and shear(XY) factors to compensate. 01023 if (row[0][0] * row[1][1] - row[0][1] * row[1][0] < 0) 01024 { 01025 row[1][0] *= -1; 01026 row[1][1] *= -1; 01027 scl[1] *= -1; 01028 shr *= -1; 01029 } 01030 01031 // Copy over the orthonormal rows into the returned matrix. 01032 // The upper 2x2 matrix in mat is now a rotation matrix. 01033 for (int i=0; i < 2; i++) 01034 { 01035 mat[i][0] = row[i][0]; 01036 mat[i][1] = row[i][1]; 01037 } 01038 01039 scl *= maxVal; 01040 01041 return true; 01042 } 01043 01044 01045 template <class T> 01046 void 01047 extractEuler (const Matrix33<T> &mat, T &rot) 01048 { 01049 // 01050 // Normalize the local x and y axes to remove scaling. 01051 // 01052 01053 Vec2<T> i (mat[0][0], mat[0][1]); 01054 Vec2<T> j (mat[1][0], mat[1][1]); 01055 01056 i.normalize(); 01057 j.normalize(); 01058 01059 // 01060 // Extract the angle, rot. 01061 // 01062 01063 rot = - Math<T>::atan2 (j[0], i[0]); 01064 } 01065 01066 01067 template <class T> 01068 bool 01069 extractSHRT (const Matrix33<T> &mat, 01070 Vec2<T> &s, 01071 T &h, 01072 T &r, 01073 Vec2<T> &t, 01074 bool exc) 01075 { 01076 Matrix33<T> rot; 01077 01078 rot = mat; 01079 if (! extractAndRemoveScalingAndShear (rot, s, h, exc)) 01080 return false; 01081 01082 extractEuler (rot, r); 01083 01084 t.x = mat[2][0]; 01085 t.y = mat[2][1]; 01086 01087 return true; 01088 } 01089 01090 01091 template <class T> 01092 bool 01093 checkForZeroScaleInRow (const T& scl, 01094 const Vec2<T> &row, 01095 bool exc /* = true */ ) 01096 { 01097 for (int i = 0; i < 2; i++) 01098 { 01099 if ((abs (scl) < 1 && abs (row[i]) >= limits<T>::max() * abs (scl))) 01100 { 01101 if (exc) 01102 throw Imath::ZeroScaleExc ("Cannot remove zero scaling " 01103 "from matrix."); 01104 else 01105 return false; 01106 } 01107 } 01108 01109 return true; 01110 } 01111 01112 01113 } // namespace Imath 01114 01115 #endif