PBRT
/home/felix/UBC/projects/AdaptiveLightfieldSampling/pbrt_v2/src/3rdparty/ilmbase-1.0.2/ImathMatrixAlgo.h
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