rfreeimage 0.1.0

data/vendor/FreeImage/Source/OpenEXR/Imath/ImathMatrixAlgo.h

@@ -0,0 +1,1425 @@
+///////////////////////////////////////////////////////////////////////////
+//
+// Copyright (c) 2002-2012, Industrial Light & Magic, a division of Lucas
+// Digital Ltd. LLC
+// 
+// All rights reserved.
+// 
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+// *       Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// *       Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// *       Neither the name of Industrial Light & Magic nor the names of
+// its contributors may be used to endorse or promote products derived
+// from this software without specific prior written permission. 
+// 
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+///////////////////////////////////////////////////////////////////////////
+
+
+#ifndef INCLUDED_IMATHMATRIXALGO_H
+#define INCLUDED_IMATHMATRIXALGO_H
+
+//-------------------------------------------------------------------------
+//
+//  This file contains algorithms applied to or in conjunction with
+//  transformation matrices (Imath::Matrix33 and Imath::Matrix44).
+//  The assumption made is that these functions are called much less
+//  often than the basic point functions or these functions require
+//  more support classes.
+//
+//  This file also defines a few predefined constant matrices.
+//
+//-------------------------------------------------------------------------
+
+#include "ImathExport.h"
+#include "ImathMatrix.h"
+#include "ImathQuat.h"
+#include "ImathEuler.h"
+#include "ImathExc.h"
+#include "ImathVec.h"
+#include "ImathLimits.h"
+#include "ImathNamespace.h"
+#include <math.h>
+
+IMATH_INTERNAL_NAMESPACE_HEADER_ENTER
+
+//------------------
+// Identity matrices
+//------------------
+
+IMATH_EXPORT_CONST M33f identity33f;
+IMATH_EXPORT_CONST M44f identity44f;
+IMATH_EXPORT_CONST M33d identity33d;
+IMATH_EXPORT_CONST M44d identity44d;
+
+//----------------------------------------------------------------------
+// Extract scale, shear, rotation, and translation values from a matrix:
+// 
+// Notes:
+//
+// This implementation follows the technique described in the paper by
+// Spencer W. Thomas in the Graphics Gems II article: "Decomposing a 
+// Matrix into Simple Transformations", p. 320.
+//
+// - Some of the functions below have an optional exc parameter
+//   that determines the functions' behavior when the matrix'
+//   scaling is very close to zero:
+//
+//   If exc is true, the functions throw an Imath::ZeroScale exception.
+//
+//   If exc is false:
+//
+//      extractScaling (m, s)            returns false, s is invalid
+//	sansScaling (m)		         returns m
+//	removeScaling (m)	         returns false, m is unchanged
+//      sansScalingAndShear (m)          returns m
+//      removeScalingAndShear (m)        returns false, m is unchanged
+//      extractAndRemoveScalingAndShear (m, s, h)  
+//                                       returns false, m is unchanged, 
+//                                                      (sh) are invalid
+//      checkForZeroScaleInRow ()        returns false
+//	extractSHRT (m, s, h, r, t)      returns false, (shrt) are invalid
+//
+// - Functions extractEuler(), extractEulerXYZ() and extractEulerZYX() 
+//   assume that the matrix does not include shear or non-uniform scaling, 
+//   but they do not examine the matrix to verify this assumption.  
+//   Matrices with shear or non-uniform scaling are likely to produce 
+//   meaningless results.  Therefore, you should use the 
+//   removeScalingAndShear() routine, if necessary, prior to calling
+//   extractEuler...() .
+//
+// - All functions assume that the matrix does not include perspective
+//   transformation(s), but they do not examine the matrix to verify 
+//   this assumption.  Matrices with perspective transformations are 
+//   likely to produce meaningless results.
+//
+//----------------------------------------------------------------------
+
+
+//
+// Declarations for 4x4 matrix.
+//
+
+template <class T>  bool        extractScaling 
+                                            (const Matrix44<T> &mat,
+					     Vec3<T> &scl,
+					     bool exc = true);
+  
+template <class T>  Matrix44<T> sansScaling (const Matrix44<T> &mat, 
+					     bool exc = true);
+
+template <class T>  bool        removeScaling 
+                                            (Matrix44<T> &mat, 
+					     bool exc = true);
+
+template <class T>  bool        extractScalingAndShear 
+                                            (const Matrix44<T> &mat,
+					     Vec3<T> &scl,
+					     Vec3<T> &shr,
+					     bool exc = true);
+  
+template <class T>  Matrix44<T> sansScalingAndShear 
+                                            (const Matrix44<T> &mat, 
+					     bool exc = true);
+
+template <class T>  void        sansScalingAndShear 
+                                            (Matrix44<T> &result,
+                                             const Matrix44<T> &mat, 
+					     bool exc = true);
+
+template <class T>  bool        removeScalingAndShear 
+                                            (Matrix44<T> &mat,
+					     bool exc = true);
+
+template <class T>  bool        extractAndRemoveScalingAndShear
+                                            (Matrix44<T> &mat,
+					     Vec3<T>     &scl,
+					     Vec3<T>     &shr,
+					     bool exc = true);
+
+template <class T>  void	extractEulerXYZ 
+                                            (const Matrix44<T> &mat,
+					     Vec3<T> &rot);
+
+template <class T>  void	extractEulerZYX 
+                                            (const Matrix44<T> &mat,
+					     Vec3<T> &rot);
+
+template <class T>  Quat<T>	extractQuat (const Matrix44<T> &mat);
+
+template <class T>  bool	extractSHRT 
+                                    (const Matrix44<T> &mat,
+				     Vec3<T> &s,
+				     Vec3<T> &h,
+				     Vec3<T> &r,
+				     Vec3<T> &t,
+				     bool exc /*= true*/,
+				     typename Euler<T>::Order rOrder);
+
+template <class T>  bool	extractSHRT 
+                                    (const Matrix44<T> &mat,
+				     Vec3<T> &s,
+				     Vec3<T> &h,
+				     Vec3<T> &r,
+				     Vec3<T> &t,
+				     bool exc = true);
+
+template <class T>  bool	extractSHRT 
+                                    (const Matrix44<T> &mat,
+				     Vec3<T> &s,
+				     Vec3<T> &h,
+				     Euler<T> &r,
+				     Vec3<T> &t,
+				     bool exc = true);
+
+//
+// Internal utility function.
+//
+
+template <class T>  bool	checkForZeroScaleInRow
+                                            (const T       &scl, 
+					     const Vec3<T> &row,
+					     bool exc = true);
+
+template <class T>  Matrix44<T> outerProduct
+                                            ( const Vec4<T> &a,
+                                              const Vec4<T> &b);
+
+
+//
+// Returns a matrix that rotates "fromDirection" vector to "toDirection"
+// vector.
+//
+
+template <class T> Matrix44<T>	rotationMatrix (const Vec3<T> &fromDirection,
+						const Vec3<T> &toDirection);
+
+
+
+//
+// Returns a matrix that rotates the "fromDir" vector 
+// so that it points towards "toDir".  You may also 
+// specify that you want the up vector to be pointing 
+// in a certain direction "upDir".
+//
+
+template <class T> Matrix44<T>	rotationMatrixWithUpDir 
+                                            (const Vec3<T> &fromDir,
+					     const Vec3<T> &toDir,
+					     const Vec3<T> &upDir);
+
+
+//
+// Constructs a matrix that rotates the z-axis so that it 
+// points towards "targetDir".  You must also specify 
+// that you want the up vector to be pointing in a 
+// certain direction "upDir".
+//
+// Notes: The following degenerate cases are handled:
+//        (a) when the directions given by "toDir" and "upDir" 
+//            are parallel or opposite;
+//            (the direction vectors must have a non-zero cross product)
+//        (b) when any of the given direction vectors have zero length
+//
+
+template <class T> void	alignZAxisWithTargetDir 
+                                            (Matrix44<T> &result,
+                                             Vec3<T>      targetDir, 
+					     Vec3<T>      upDir);
+
+
+// Compute an orthonormal direct frame from : a position, an x axis direction and a normal to the y axis
+// If the x axis and normal are perpendicular, then the normal will have the same direction as the z axis.
+// Inputs are : 
+//     -the position of the frame
+//     -the x axis direction of the frame
+//     -a normal to the y axis of the frame
+// Return is the orthonormal frame
+template <class T> Matrix44<T> computeLocalFrame( const Vec3<T>& p,
+                                                  const Vec3<T>& xDir,
+                                                  const Vec3<T>& normal);
+
+// Add a translate/rotate/scale offset to an input frame
+// and put it in another frame of reference
+// Inputs are :
+//     - input frame
+//     - translate offset
+//     - rotate    offset in degrees
+//     - scale     offset
+//     - frame of reference
+// Output is the offsetted frame
+template <class T> Matrix44<T> addOffset( const Matrix44<T>& inMat,
+                                          const Vec3<T>&     tOffset,
+                                          const Vec3<T>&     rOffset,
+                                          const Vec3<T>&     sOffset,
+                                          const Vec3<T>&     ref);
+
+// Compute Translate/Rotate/Scale matrix from matrix A with the Rotate/Scale of Matrix B
+// Inputs are :
+//      -keepRotateA : if true keep rotate from matrix A, use B otherwise
+//      -keepScaleA  : if true keep scale  from matrix A, use B otherwise
+//      -Matrix A
+//      -Matrix B
+// Return Matrix A with tweaked rotation/scale
+template <class T> Matrix44<T> computeRSMatrix( bool               keepRotateA,
+                                                bool               keepScaleA, 
+                                                const Matrix44<T>& A,
+                                                const Matrix44<T>& B);
+
+
+//----------------------------------------------------------------------
+
+
+// 
+// Declarations for 3x3 matrix.
+//
+
+ 
+template <class T>  bool        extractScaling 
+                                            (const Matrix33<T> &mat,
+					     Vec2<T> &scl,
+					     bool exc = true);
+  
+template <class T>  Matrix33<T> sansScaling (const Matrix33<T> &mat, 
+					     bool exc = true);
+
+template <class T>  bool        removeScaling 
+                                            (Matrix33<T> &mat, 
+					     bool exc = true);
+
+template <class T>  bool        extractScalingAndShear 
+                                            (const Matrix33<T> &mat,
+					     Vec2<T> &scl,
+					     T &h,
+					     bool exc = true);
+  
+template <class T>  Matrix33<T> sansScalingAndShear 
+                                            (const Matrix33<T> &mat, 
+					     bool exc = true);
+
+template <class T>  bool        removeScalingAndShear 
+                                            (Matrix33<T> &mat,
+					     bool exc = true);
+
+template <class T>  bool        extractAndRemoveScalingAndShear
+                                            (Matrix33<T> &mat,
+					     Vec2<T>     &scl,
+					     T           &shr,
+					     bool exc = true);
+
+template <class T>  void	extractEuler
+                                            (const Matrix33<T> &mat,
+					     T       &rot);
+
+template <class T>  bool	extractSHRT (const Matrix33<T> &mat,
+					     Vec2<T> &s,
+					     T       &h,
+					     T       &r,
+					     Vec2<T> &t,
+					     bool exc = true);
+
+template <class T>  bool	checkForZeroScaleInRow
+                                            (const T       &scl, 
+					     const Vec2<T> &row,
+					     bool exc = true);
+
+template <class T>  Matrix33<T> outerProduct
+                                            ( const Vec3<T> &a,
+                                              const Vec3<T> &b);
+
+
+//-----------------------------------------------------------------------------
+// Implementation for 4x4 Matrix
+//------------------------------
+
+
+template <class T>
+bool
+extractScaling (const Matrix44<T> &mat, Vec3<T> &scl, bool exc)
+{
+    Vec3<T> shr;
+    Matrix44<T> M (mat);
+
+    if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
+	return false;
+    
+    return true;
+}
+
+
+template <class T>
+Matrix44<T>
+sansScaling (const Matrix44<T> &mat, bool exc)
+{
+    Vec3<T> scl;
+    Vec3<T> shr;
+    Vec3<T> rot;
+    Vec3<T> tran;
+
+    if (! extractSHRT (mat, scl, shr, rot, tran, exc))
+	return mat;
+
+    Matrix44<T> M;
+    
+    M.translate (tran);
+    M.rotate (rot);
+    M.shear (shr);
+
+    return M;
+}
+
+
+template <class T>
+bool
+removeScaling (Matrix44<T> &mat, bool exc)
+{
+    Vec3<T> scl;
+    Vec3<T> shr;
+    Vec3<T> rot;
+    Vec3<T> tran;
+
+    if (! extractSHRT (mat, scl, shr, rot, tran, exc))
+	return false;
+
+    mat.makeIdentity ();
+    mat.translate (tran);
+    mat.rotate (rot);
+    mat.shear (shr);
+
+    return true;
+}
+
+
+template <class T>
+bool
+extractScalingAndShear (const Matrix44<T> &mat, 
+			Vec3<T> &scl, Vec3<T> &shr, bool exc)
+{
+    Matrix44<T> M (mat);
+
+    if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
+	return false;
+    
+    return true;
+}
+
+
+template <class T>
+Matrix44<T>
+sansScalingAndShear (const Matrix44<T> &mat, bool exc)
+{
+    Vec3<T> scl;
+    Vec3<T> shr;
+    Matrix44<T> M (mat);
+
+    if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
+	return mat;
+    
+    return M;
+}
+
+
+template <class T>
+void
+sansScalingAndShear (Matrix44<T> &result, const Matrix44<T> &mat, bool exc)
+{
+    Vec3<T> scl;
+    Vec3<T> shr;
+
+    if (! extractAndRemoveScalingAndShear (result, scl, shr, exc))
+	result = mat;
+}
+
+
+template <class T>
+bool
+removeScalingAndShear (Matrix44<T> &mat, bool exc)
+{
+    Vec3<T> scl;
+    Vec3<T> shr;
+
+    if (! extractAndRemoveScalingAndShear (mat, scl, shr, exc))
+	return false;
+    
+    return true;
+}
+
+
+template <class T>
+bool
+extractAndRemoveScalingAndShear (Matrix44<T> &mat, 
+				 Vec3<T> &scl, Vec3<T> &shr, bool exc)
+{
+    //
+    // This implementation follows the technique described in the paper by
+    // Spencer W. Thomas in the Graphics Gems II article: "Decomposing a 
+    // Matrix into Simple Transformations", p. 320.
+    //
+
+    Vec3<T> row[3];
+
+    row[0] = Vec3<T> (mat[0][0], mat[0][1], mat[0][2]);
+    row[1] = Vec3<T> (mat[1][0], mat[1][1], mat[1][2]);
+    row[2] = Vec3<T> (mat[2][0], mat[2][1], mat[2][2]);
+    
+    T maxVal = 0;
+    for (int i=0; i < 3; i++)
+	for (int j=0; j < 3; j++)
+	    if (IMATH_INTERNAL_NAMESPACE::abs (row[i][j]) > maxVal)
+		maxVal = IMATH_INTERNAL_NAMESPACE::abs (row[i][j]);
+
+    //
+    // We normalize the 3x3 matrix here.
+    // It was noticed that this can improve numerical stability significantly,
+    // especially when many of the upper 3x3 matrix's coefficients are very
+    // close to zero; we correct for this step at the end by multiplying the 
+    // scaling factors by maxVal at the end (shear and rotation are not 
+    // affected by the normalization).
+
+    if (maxVal != 0)
+    {
+	for (int i=0; i < 3; i++)
+	    if (! checkForZeroScaleInRow (maxVal, row[i], exc))
+		return false;
+	    else
+		row[i] /= maxVal;
+    }
+
+    // Compute X scale factor. 
+    scl.x = row[0].length ();
+    if (! checkForZeroScaleInRow (scl.x, row[0], exc))
+	return false;
+
+    // Normalize first row.
+    row[0] /= scl.x;
+
+    // An XY shear factor will shear the X coord. as the Y coord. changes.
+    // There are 6 combinations (XY, XZ, YZ, YX, ZX, ZY), although we only
+    // extract the first 3 because we can effect the last 3 by shearing in
+    // XY, XZ, YZ combined rotations and scales.
+    //
+    // shear matrix <   1,  YX,  ZX,  0,
+    //                 XY,   1,  ZY,  0,
+    //                 XZ,  YZ,   1,  0,
+    //                  0,   0,   0,  1 >
+
+    // Compute XY shear factor and make 2nd row orthogonal to 1st.
+    shr[0]  = row[0].dot (row[1]);
+    row[1] -= shr[0] * row[0];
+
+    // Now, compute Y scale.
+    scl.y = row[1].length ();
+    if (! checkForZeroScaleInRow (scl.y, row[1], exc))
+	return false;
+
+    // Normalize 2nd row and correct the XY shear factor for Y scaling.
+    row[1] /= scl.y; 
+    shr[0] /= scl.y;
+
+    // Compute XZ and YZ shears, orthogonalize 3rd row.
+    shr[1]  = row[0].dot (row[2]);
+    row[2] -= shr[1] * row[0];
+    shr[2]  = row[1].dot (row[2]);
+    row[2] -= shr[2] * row[1];
+
+    // Next, get Z scale.
+    scl.z = row[2].length ();
+    if (! checkForZeroScaleInRow (scl.z, row[2], exc))
+	return false;
+
+    // Normalize 3rd row and correct the XZ and YZ shear factors for Z scaling.
+    row[2] /= scl.z;
+    shr[1] /= scl.z;
+    shr[2] /= scl.z;
+
+    // At this point, the upper 3x3 matrix in mat is orthonormal.
+    // Check for a coordinate system flip. If the determinant
+    // is less than zero, then negate the matrix and the scaling factors.
+    if (row[0].dot (row[1].cross (row[2])) < 0)
+	for (int  i=0; i < 3; i++)
+	{
+	    scl[i] *= -1;
+	    row[i] *= -1;
+	}
+
+    // Copy over the orthonormal rows into the returned matrix.
+    // The upper 3x3 matrix in mat is now a rotation matrix.
+    for (int i=0; i < 3; i++)
+    {
+	mat[i][0] = row[i][0]; 
+	mat[i][1] = row[i][1]; 
+	mat[i][2] = row[i][2];
+    }
+
+    // Correct the scaling factors for the normalization step that we 
+    // performed above; shear and rotation are not affected by the 
+    // normalization.
+    scl *= maxVal;
+
+    return true;
+}
+
+
+template <class T>
+void
+extractEulerXYZ (const Matrix44<T> &mat, Vec3<T> &rot)
+{
+    //
+    // Normalize the local x, y and z axes to remove scaling.
+    //
+
+    Vec3<T> i (mat[0][0], mat[0][1], mat[0][2]);
+    Vec3<T> j (mat[1][0], mat[1][1], mat[1][2]);
+    Vec3<T> k (mat[2][0], mat[2][1], mat[2][2]);
+
+    i.normalize();
+    j.normalize();
+    k.normalize();
+
+    Matrix44<T> M (i[0], i[1], i[2], 0, 
+		   j[0], j[1], j[2], 0, 
+		   k[0], k[1], k[2], 0, 
+		   0,    0,    0,    1);
+
+    //
+    // Extract the first angle, rot.x.
+    // 
+
+    rot.x = Math<T>::atan2 (M[1][2], M[2][2]);
+
+    //
+    // Remove the rot.x rotation from M, so that the remaining
+    // rotation, N, is only around two axes, and gimbal lock
+    // cannot occur.
+    //
+
+    Matrix44<T> N;
+    N.rotate (Vec3<T> (-rot.x, 0, 0));
+    N = N * M;
+
+    //
+    // Extract the other two angles, rot.y and rot.z, from N.
+    //
+
+    T cy = Math<T>::sqrt (N[0][0]*N[0][0] + N[0][1]*N[0][1]);
+    rot.y = Math<T>::atan2 (-N[0][2], cy);
+    rot.z = Math<T>::atan2 (-N[1][0], N[1][1]);
+}
+
+
+template <class T>
+void
+extractEulerZYX (const Matrix44<T> &mat, Vec3<T> &rot)
+{
+    //
+    // Normalize the local x, y and z axes to remove scaling.
+    //
+
+    Vec3<T> i (mat[0][0], mat[0][1], mat[0][2]);
+    Vec3<T> j (mat[1][0], mat[1][1], mat[1][2]);
+    Vec3<T> k (mat[2][0], mat[2][1], mat[2][2]);
+
+    i.normalize();
+    j.normalize();
+    k.normalize();
+
+    Matrix44<T> M (i[0], i[1], i[2], 0, 
+		   j[0], j[1], j[2], 0, 
+		   k[0], k[1], k[2], 0, 
+		   0,    0,    0,    1);
+
+    //
+    // Extract the first angle, rot.x.
+    // 
+
+    rot.x = -Math<T>::atan2 (M[1][0], M[0][0]);
+
+    //
+    // Remove the x rotation from M, so that the remaining
+    // rotation, N, is only around two axes, and gimbal lock
+    // cannot occur.
+    //
+
+    Matrix44<T> N;
+    N.rotate (Vec3<T> (0, 0, -rot.x));
+    N = N * M;
+
+    //
+    // Extract the other two angles, rot.y and rot.z, from N.
+    //
+
+    T cy = Math<T>::sqrt (N[2][2]*N[2][2] + N[2][1]*N[2][1]);
+    rot.y = -Math<T>::atan2 (-N[2][0], cy);
+    rot.z = -Math<T>::atan2 (-N[1][2], N[1][1]);
+}
+
+
+template <class T>
+Quat<T>
+extractQuat (const Matrix44<T> &mat)
+{
+  Matrix44<T> rot;
+
+  T        tr, s;
+  T         q[4];
+  int    i, j, k;
+  Quat<T>   quat;
+
+  int nxt[3] = {1, 2, 0};
+  tr = mat[0][0] + mat[1][1] + mat[2][2];
+
+  // check the diagonal
+  if (tr > 0.0) {
+     s = Math<T>::sqrt (tr + T(1.0));
+     quat.r = s / T(2.0);
+     s = T(0.5) / s;
+
+     quat.v.x = (mat[1][2] - mat[2][1]) * s;
+     quat.v.y = (mat[2][0] - mat[0][2]) * s;
+     quat.v.z = (mat[0][1] - mat[1][0]) * s;
+  } 
+  else {      
+     // diagonal is negative
+     i = 0;
+     if (mat[1][1] > mat[0][0]) 
+        i=1;
+     if (mat[2][2] > mat[i][i]) 
+        i=2;
+    
+     j = nxt[i];
+     k = nxt[j];
+     s = Math<T>::sqrt ((mat[i][i] - (mat[j][j] + mat[k][k])) + T(1.0));
+      
+     q[i] = s * T(0.5);
+     if (s != T(0.0)) 
+        s = T(0.5) / s;
+
+     q[3] = (mat[j][k] - mat[k][j]) * s;
+     q[j] = (mat[i][j] + mat[j][i]) * s;
+     q[k] = (mat[i][k] + mat[k][i]) * s;
+
+     quat.v.x = q[0];
+     quat.v.y = q[1];
+     quat.v.z = q[2];
+     quat.r = q[3];
+ }
+
+  return quat;
+}
+
+template <class T>
+bool 
+extractSHRT (const Matrix44<T> &mat,
+	     Vec3<T> &s,
+	     Vec3<T> &h,
+	     Vec3<T> &r,
+	     Vec3<T> &t,
+	     bool exc /* = true */ ,
+	     typename Euler<T>::Order rOrder /* = Euler<T>::XYZ */ )
+{
+    Matrix44<T> rot;
+
+    rot = mat;
+    if (! extractAndRemoveScalingAndShear (rot, s, h, exc))
+	return false;
+
+    extractEulerXYZ (rot, r);
+
+    t.x = mat[3][0];
+    t.y = mat[3][1];
+    t.z = mat[3][2];
+
+    if (rOrder != Euler<T>::XYZ)
+    {
+	IMATH_INTERNAL_NAMESPACE::Euler<T> eXYZ (r, IMATH_INTERNAL_NAMESPACE::Euler<T>::XYZ);
+	IMATH_INTERNAL_NAMESPACE::Euler<T> e (eXYZ, rOrder);
+	r = e.toXYZVector ();
+    }
+
+    return true;
+}
+
+template <class T>
+bool 
+extractSHRT (const Matrix44<T> &mat,
+	     Vec3<T> &s,
+	     Vec3<T> &h,
+	     Vec3<T> &r,
+	     Vec3<T> &t,
+	     bool exc)
+{
+    return extractSHRT(mat, s, h, r, t, exc, IMATH_INTERNAL_NAMESPACE::Euler<T>::XYZ);
+}
+
+template <class T>
+bool 
+extractSHRT (const Matrix44<T> &mat,
+	     Vec3<T> &s,
+	     Vec3<T> &h,
+	     Euler<T> &r,
+	     Vec3<T> &t,
+	     bool exc /* = true */)
+{
+    return extractSHRT (mat, s, h, r, t, exc, r.order ());
+}
+
+
+template <class T> 
+bool		
+checkForZeroScaleInRow (const T& scl, 
+			const Vec3<T> &row,
+			bool exc /* = true */ )
+{
+    for (int i = 0; i < 3; i++)
+    {
+	if ((abs (scl) < 1 && abs (row[i]) >= limits<T>::max() * abs (scl)))
+	{
+	    if (exc)
+		throw IMATH_INTERNAL_NAMESPACE::ZeroScaleExc ("Cannot remove zero scaling "
+					   "from matrix.");
+	    else
+		return false;
+	}
+    }
+
+    return true;
+}
+
+template <class T>
+Matrix44<T>
+outerProduct (const Vec4<T> &a, const Vec4<T> &b )
+{
+    return Matrix44<T> (a.x*b.x, a.x*b.y, a.x*b.z, a.x*b.w,
+                        a.y*b.x, a.y*b.y, a.y*b.z, a.x*b.w,
+                        a.z*b.x, a.z*b.y, a.z*b.z, a.x*b.w,
+                        a.w*b.x, a.w*b.y, a.w*b.z, a.w*b.w);
+}
+
+template <class T>
+Matrix44<T>
+rotationMatrix (const Vec3<T> &from, const Vec3<T> &to)
+{
+    Quat<T> q;
+    q.setRotation(from, to);
+    return q.toMatrix44();
+}
+
+
+template <class T>
+Matrix44<T>	
+rotationMatrixWithUpDir (const Vec3<T> &fromDir,
+			 const Vec3<T> &toDir,
+			 const Vec3<T> &upDir)
+{
+    //
+    // The goal is to obtain a rotation matrix that takes 
+    // "fromDir" to "toDir".  We do this in two steps and 
+    // compose the resulting rotation matrices; 
+    //    (a) rotate "fromDir" into the z-axis
+    //    (b) rotate the z-axis into "toDir"
+    //
+
+    // The from direction must be non-zero; but we allow zero to and up dirs.
+    if (fromDir.length () == 0)
+	return Matrix44<T> ();
+
+    else
+    {
+	Matrix44<T> zAxis2FromDir( IMATH_INTERNAL_NAMESPACE::UNINITIALIZED );
+	alignZAxisWithTargetDir (zAxis2FromDir, fromDir, Vec3<T> (0, 1, 0));
+
+	Matrix44<T> fromDir2zAxis  = zAxis2FromDir.transposed ();
+	
+	Matrix44<T> zAxis2ToDir( IMATH_INTERNAL_NAMESPACE::UNINITIALIZED );
+	alignZAxisWithTargetDir (zAxis2ToDir, toDir, upDir);
+
+	return fromDir2zAxis * zAxis2ToDir;
+    }
+}
+
+
+template <class T>
+void
+alignZAxisWithTargetDir (Matrix44<T> &result, Vec3<T> targetDir, Vec3<T> upDir)
+{
+    //
+    // Ensure that the target direction is non-zero.
+    //
+
+    if ( targetDir.length () == 0 )
+	targetDir = Vec3<T> (0, 0, 1);
+
+    //
+    // Ensure that the up direction is non-zero.
+    //
+
+    if ( upDir.length () == 0 )
+	upDir = Vec3<T> (0, 1, 0);
+
+    //
+    // Check for degeneracies.  If the upDir and targetDir are parallel 
+    // or opposite, then compute a new, arbitrary up direction that is
+    // not parallel or opposite to the targetDir.
+    //
+
+    if (upDir.cross (targetDir).length () == 0)
+    {
+	upDir = targetDir.cross (Vec3<T> (1, 0, 0));
+	if (upDir.length() == 0)
+	    upDir = targetDir.cross(Vec3<T> (0, 0, 1));
+    }
+
+    //
+    // Compute the x-, y-, and z-axis vectors of the new coordinate system.
+    //
+
+    Vec3<T> targetPerpDir = upDir.cross (targetDir);    
+    Vec3<T> targetUpDir   = targetDir.cross (targetPerpDir);
+    
+    //
+    // Rotate the x-axis into targetPerpDir (row 0),
+    // rotate the y-axis into targetUpDir   (row 1),
+    // rotate the z-axis into targetDir     (row 2).
+    //
+    
+    Vec3<T> row[3];
+    row[0] = targetPerpDir.normalized ();
+    row[1] = targetUpDir  .normalized ();
+    row[2] = targetDir    .normalized ();
+    
+    result.x[0][0] = row[0][0];
+    result.x[0][1] = row[0][1];
+    result.x[0][2] = row[0][2];
+    result.x[0][3] = (T)0;
+ 
+    result.x[1][0] = row[1][0];
+    result.x[1][1] = row[1][1];
+    result.x[1][2] = row[1][2];
+    result.x[1][3] = (T)0;
+ 
+    result.x[2][0] = row[2][0];
+    result.x[2][1] = row[2][1];
+    result.x[2][2] = row[2][2];
+    result.x[2][3] = (T)0;
+ 
+    result.x[3][0] = (T)0;
+    result.x[3][1] = (T)0;
+    result.x[3][2] = (T)0;
+    result.x[3][3] = (T)1;
+}
+
+
+// Compute an orthonormal direct frame from : a position, an x axis direction and a normal to the y axis
+// If the x axis and normal are perpendicular, then the normal will have the same direction as the z axis.
+// Inputs are : 
+//     -the position of the frame
+//     -the x axis direction of the frame
+//     -a normal to the y axis of the frame
+// Return is the orthonormal frame
+template <class T>
+Matrix44<T>
+computeLocalFrame( const Vec3<T>& p,
+                   const Vec3<T>& xDir,
+                   const Vec3<T>& normal)
+{
+    Vec3<T> _xDir(xDir);
+    Vec3<T> x = _xDir.normalize();
+    Vec3<T> y = (normal % x).normalize();
+    Vec3<T> z = (x % y).normalize();
+
+    Matrix44<T> L;
+    L[0][0] = x[0];
+    L[0][1] = x[1];
+    L[0][2] = x[2];
+    L[0][3] = 0.0;
+
+    L[1][0] = y[0];
+    L[1][1] = y[1];
+    L[1][2] = y[2];
+    L[1][3] = 0.0;
+
+    L[2][0] = z[0];
+    L[2][1] = z[1];
+    L[2][2] = z[2];
+    L[2][3] = 0.0;
+
+    L[3][0] = p[0];
+    L[3][1] = p[1];
+    L[3][2] = p[2];
+    L[3][3] = 1.0;
+    
+    return L;
+}
+
+// Add a translate/rotate/scale offset to an input frame
+// and put it in another frame of reference
+// Inputs are :
+//     - input frame
+//     - translate offset
+//     - rotate    offset in degrees
+//     - scale     offset
+//     - frame of reference
+// Output is the offsetted frame
+template <class T>
+Matrix44<T>
+addOffset( const Matrix44<T>& inMat,
+           const Vec3<T>&     tOffset,
+           const Vec3<T>&     rOffset,
+           const Vec3<T>&     sOffset,
+           const Matrix44<T>& ref)
+{
+    Matrix44<T> O;
+
+    Vec3<T> _rOffset(rOffset);
+    _rOffset *= M_PI / 180.0;
+    O.rotate (_rOffset);
+
+    O[3][0] = tOffset[0];
+    O[3][1] = tOffset[1];
+    O[3][2] = tOffset[2];
+
+    Matrix44<T> S;
+    S.scale (sOffset);
+
+    Matrix44<T> X = S * O * inMat * ref;
+
+    return X;
+}
+
+// Compute Translate/Rotate/Scale matrix from matrix A with the Rotate/Scale of Matrix B
+// Inputs are :
+//      -keepRotateA : if true keep rotate from matrix A, use B otherwise
+//      -keepScaleA  : if true keep scale  from matrix A, use B otherwise
+//      -Matrix A
+//      -Matrix B
+// Return Matrix A with tweaked rotation/scale
+template <class T>
+Matrix44<T>
+computeRSMatrix( bool               keepRotateA,
+                 bool               keepScaleA, 
+                 const Matrix44<T>& A, 
+                 const Matrix44<T>& B)
+{
+    Vec3<T> as, ah, ar, at;
+    extractSHRT (A, as, ah, ar, at);
+    
+    Vec3<T> bs, bh, br, bt;
+    extractSHRT (B, bs, bh, br, bt);
+
+    if (!keepRotateA)
+        ar = br;
+
+    if (!keepScaleA)
+        as = bs;
+
+    Matrix44<T> mat;
+    mat.makeIdentity();
+    mat.translate (at);
+    mat.rotate (ar);
+    mat.scale (as);
+    
+    return mat;
+}
+
+
+
+//-----------------------------------------------------------------------------
+// Implementation for 3x3 Matrix
+//------------------------------
+
+
+template <class T>
+bool
+extractScaling (const Matrix33<T> &mat, Vec2<T> &scl, bool exc)
+{
+    T shr;
+    Matrix33<T> M (mat);
+
+    if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
+	return false;
+
+    return true;
+}
+
+
+template <class T>
+Matrix33<T>
+sansScaling (const Matrix33<T> &mat, bool exc)
+{
+    Vec2<T> scl;
+    T shr;
+    T rot;
+    Vec2<T> tran;
+
+    if (! extractSHRT (mat, scl, shr, rot, tran, exc))
+	return mat;
+
+    Matrix33<T> M;
+    
+    M.translate (tran);
+    M.rotate (rot);
+    M.shear (shr);
+
+    return M;
+}
+
+
+template <class T>
+bool
+removeScaling (Matrix33<T> &mat, bool exc)
+{
+    Vec2<T> scl;
+    T shr;
+    T rot;
+    Vec2<T> tran;
+
+    if (! extractSHRT (mat, scl, shr, rot, tran, exc))
+	return false;
+
+    mat.makeIdentity ();
+    mat.translate (tran);
+    mat.rotate (rot);
+    mat.shear (shr);
+
+    return true;
+}
+
+
+template <class T>
+bool
+extractScalingAndShear (const Matrix33<T> &mat, Vec2<T> &scl, T &shr, bool exc)
+{
+    Matrix33<T> M (mat);
+
+    if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
+	return false;
+
+    return true;
+}
+
+
+template <class T>
+Matrix33<T>
+sansScalingAndShear (const Matrix33<T> &mat, bool exc)
+{
+    Vec2<T> scl;
+    T shr;
+    Matrix33<T> M (mat);
+
+    if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
+	return mat;
+    
+    return M;
+}
+
+
+template <class T>
+bool
+removeScalingAndShear (Matrix33<T> &mat, bool exc)
+{
+    Vec2<T> scl;
+    T shr;
+
+    if (! extractAndRemoveScalingAndShear (mat, scl, shr, exc))
+	return false;
+    
+    return true;
+}
+
+template <class T>
+bool
+extractAndRemoveScalingAndShear (Matrix33<T> &mat, 
+				 Vec2<T> &scl, T &shr, bool exc)
+{
+    Vec2<T> row[2];
+
+    row[0] = Vec2<T> (mat[0][0], mat[0][1]);
+    row[1] = Vec2<T> (mat[1][0], mat[1][1]);
+    
+    T maxVal = 0;
+    for (int i=0; i < 2; i++)
+	for (int j=0; j < 2; j++)
+	    if (IMATH_INTERNAL_NAMESPACE::abs (row[i][j]) > maxVal)
+		maxVal = IMATH_INTERNAL_NAMESPACE::abs (row[i][j]);
+
+    //
+    // We normalize the 2x2 matrix here.
+    // It was noticed that this can improve numerical stability significantly,
+    // especially when many of the upper 2x2 matrix's coefficients are very
+    // close to zero; we correct for this step at the end by multiplying the 
+    // scaling factors by maxVal at the end (shear and rotation are not 
+    // affected by the normalization).
+
+    if (maxVal != 0)
+    {
+	for (int i=0; i < 2; i++)
+	    if (! checkForZeroScaleInRow (maxVal, row[i], exc))
+		return false;
+	    else
+		row[i] /= maxVal;
+    }
+
+    // Compute X scale factor. 
+    scl.x = row[0].length ();
+    if (! checkForZeroScaleInRow (scl.x, row[0], exc))
+	return false;
+
+    // Normalize first row.
+    row[0] /= scl.x;
+
+    // An XY shear factor will shear the X coord. as the Y coord. changes.
+    // There are 2 combinations (XY, YX), although we only extract the XY 
+    // shear factor because we can effect the an YX shear factor by 
+    // shearing in XY combined with rotations and scales.
+    //
+    // shear matrix <   1,  YX,  0,
+    //                 XY,   1,  0,
+    //                  0,   0,  1 >
+
+    // Compute XY shear factor and make 2nd row orthogonal to 1st.
+    shr     = row[0].dot (row[1]);
+    row[1] -= shr * row[0];
+
+    // Now, compute Y scale.
+    scl.y = row[1].length ();
+    if (! checkForZeroScaleInRow (scl.y, row[1], exc))
+	return false;
+
+    // Normalize 2nd row and correct the XY shear factor for Y scaling.
+    row[1] /= scl.y; 
+    shr    /= scl.y;
+
+    // At this point, the upper 2x2 matrix in mat is orthonormal.
+    // Check for a coordinate system flip. If the determinant
+    // is -1, then flip the rotation matrix and adjust the scale(Y) 
+    // and shear(XY) factors to compensate.
+    if (row[0][0] * row[1][1] - row[0][1] * row[1][0] < 0)
+    {
+	row[1][0] *= -1;
+	row[1][1] *= -1;
+	scl[1] *= -1;
+	shr *= -1;
+    }
+
+    // Copy over the orthonormal rows into the returned matrix.
+    // The upper 2x2 matrix in mat is now a rotation matrix.
+    for (int i=0; i < 2; i++)
+    {
+	mat[i][0] = row[i][0]; 
+	mat[i][1] = row[i][1]; 
+    }
+
+    scl *= maxVal;
+
+    return true;
+}
+
+
+template <class T>
+void
+extractEuler (const Matrix33<T> &mat, T &rot)
+{
+    //
+    // Normalize the local x and y axes to remove scaling.
+    //
+
+    Vec2<T> i (mat[0][0], mat[0][1]);
+    Vec2<T> j (mat[1][0], mat[1][1]);
+
+    i.normalize();
+    j.normalize();
+
+    //
+    // Extract the angle, rot.
+    // 
+
+    rot = - Math<T>::atan2 (j[0], i[0]);
+}
+
+
+template <class T>
+bool 
+extractSHRT (const Matrix33<T> &mat,
+	     Vec2<T> &s,
+	     T       &h,
+	     T       &r,
+	     Vec2<T> &t,
+	     bool    exc)
+{
+    Matrix33<T> rot;
+
+    rot = mat;
+    if (! extractAndRemoveScalingAndShear (rot, s, h, exc))
+	return false;
+
+    extractEuler (rot, r);
+
+    t.x = mat[2][0];
+    t.y = mat[2][1];
+
+    return true;
+}
+
+
+template <class T> 
+bool		
+checkForZeroScaleInRow (const T& scl, 
+                        const Vec2<T> &row,
+                        bool exc /* = true */ )
+{
+    for (int i = 0; i < 2; i++)
+    {
+        if ((abs (scl) < 1 && abs (row[i]) >= limits<T>::max() * abs (scl)))
+        {
+            if (exc)
+                throw IMATH_INTERNAL_NAMESPACE::ZeroScaleExc (
+                        "Cannot remove zero scaling from matrix.");
+            else
+                return false;
+        }
+    }
+
+    return true;
+}
+
+
+template <class T>
+Matrix33<T>
+outerProduct (const Vec3<T> &a, const Vec3<T> &b )
+{
+    return Matrix33<T> (a.x*b.x, a.x*b.y, a.x*b.z,
+                        a.y*b.x, a.y*b.y, a.y*b.z,
+                        a.z*b.x, a.z*b.y, a.z*b.z );
+}
+
+
+// Computes the translation and rotation that brings the 'from' points
+// as close as possible to the 'to' points under the Frobenius norm.  
+// To be more specific, let x be the matrix of 'from' points and y be
+// the matrix of 'to' points, we want to find the matrix A of the form
+//    [ R t ]
+//    [ 0 1 ]
+// that minimizes
+//     || (A*x - y)^T * W * (A*x - y) ||_F
+// If doScaling is true, then a uniform scale is allowed also.
+template <typename T>
+IMATH_INTERNAL_NAMESPACE::M44d
+procrustesRotationAndTranslation (const IMATH_INTERNAL_NAMESPACE::Vec3<T>* A,  // From these
+                                  const IMATH_INTERNAL_NAMESPACE::Vec3<T>* B,  // To these
+                                  const T* weights, 
+                                  const size_t numPoints,
+                                  const bool doScaling = false);
+
+// Unweighted:
+template <typename T>
+IMATH_INTERNAL_NAMESPACE::M44d
+procrustesRotationAndTranslation (const IMATH_INTERNAL_NAMESPACE::Vec3<T>* A, 
+                                  const IMATH_INTERNAL_NAMESPACE::Vec3<T>* B, 
+                                  const size_t numPoints,
+                                  const bool doScaling = false);
+
+// Compute the SVD of a 3x3 matrix using Jacobi transformations.  This method
+// should be quite accurate (competitive with LAPACK) even for poorly
+// conditioned matrices, and because it has been written specifically for the
+// 3x3/4x4 case it is much faster than calling out to LAPACK.  
+//
+// The SVD of a 3x3/4x4 matrix A is defined as follows:
+//     A = U * S * V^T
+// where S is the diagonal matrix of singular values and both U and V are
+// orthonormal.  By convention, the entries S are all positive and sorted from
+// the largest to the smallest.  However, some uses of this function may
+// require that the matrix U*V^T have positive determinant; in this case, we
+// may make the smallest singular value negative to ensure that this is
+// satisfied.  
+//
+// Currently only available for single- and double-precision matrices.
+template <typename T>
+void
+jacobiSVD (const IMATH_INTERNAL_NAMESPACE::Matrix33<T>& A,
+           IMATH_INTERNAL_NAMESPACE::Matrix33<T>& U,
+           IMATH_INTERNAL_NAMESPACE::Vec3<T>& S,
+           IMATH_INTERNAL_NAMESPACE::Matrix33<T>& V,
+           const T tol = IMATH_INTERNAL_NAMESPACE::limits<T>::epsilon(),
+           const bool forcePositiveDeterminant = false);
+
+template <typename T>
+void
+jacobiSVD (const IMATH_INTERNAL_NAMESPACE::Matrix44<T>& A,
+           IMATH_INTERNAL_NAMESPACE::Matrix44<T>& U,
+           IMATH_INTERNAL_NAMESPACE::Vec4<T>& S,
+           IMATH_INTERNAL_NAMESPACE::Matrix44<T>& V,
+           const T tol = IMATH_INTERNAL_NAMESPACE::limits<T>::epsilon(),
+           const bool forcePositiveDeterminant = false);
+
+// Compute the eigenvalues (S) and the eigenvectors (V) of
+// a real symmetric matrix using Jacobi transformation.
+//
+// Jacobi transformation of a 3x3/4x4 matrix A outputs S and V:
+// 	A = V * S * V^T
+// where V is orthonormal and S is the diagonal matrix of eigenvalues.
+// Input matrix A must be symmetric. A is also modified during
+// the computation so that upper diagonal entries of A become zero. 
+//
+template <typename T>
+void
+jacobiEigenSolver (Matrix33<T>& A,
+                   Vec3<T>& S,
+                   Matrix33<T>& V,
+                   const T tol);
+
+template <typename T>
+inline
+void
+jacobiEigenSolver (Matrix33<T>& A,
+                   Vec3<T>& S,
+                   Matrix33<T>& V)
+{
+    jacobiEigenSolver(A,S,V,limits<T>::epsilon());
+}
+
+template <typename T>
+void
+jacobiEigenSolver (Matrix44<T>& A,
+                   Vec4<T>& S,
+                   Matrix44<T>& V,
+                   const T tol);
+
+template <typename T>
+inline
+void
+jacobiEigenSolver (Matrix44<T>& A,
+                   Vec4<T>& S,
+                   Matrix44<T>& V)
+{
+    jacobiEigenSolver(A,S,V,limits<T>::epsilon());
+}
+
+// Compute a eigenvector corresponding to the abs max/min eigenvalue
+// of a real symmetric matrix using Jacobi transformation.
+template <typename TM, typename TV>
+void
+maxEigenVector (TM& A, TV& S);
+template <typename TM, typename TV>
+void
+minEigenVector (TM& A, TV& S);
+
+IMATH_INTERNAL_NAMESPACE_HEADER_EXIT
+
+#endif // INCLUDED_IMATHMATRIXALGO_H