/////////////////////////////////////////////////////////////////////////// // // 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 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 bool extractScaling (const Matrix44 &mat, Vec3 &scl, bool exc = true); template Matrix44 sansScaling (const Matrix44 &mat, bool exc = true); template bool removeScaling (Matrix44 &mat, bool exc = true); template bool extractScalingAndShear (const Matrix44 &mat, Vec3 &scl, Vec3 &shr, bool exc = true); template Matrix44 sansScalingAndShear (const Matrix44 &mat, bool exc = true); template void sansScalingAndShear (Matrix44 &result, const Matrix44 &mat, bool exc = true); template bool removeScalingAndShear (Matrix44 &mat, bool exc = true); template bool extractAndRemoveScalingAndShear (Matrix44 &mat, Vec3 &scl, Vec3 &shr, bool exc = true); template void extractEulerXYZ (const Matrix44 &mat, Vec3 &rot); template void extractEulerZYX (const Matrix44 &mat, Vec3 &rot); template Quat extractQuat (const Matrix44 &mat); template bool extractSHRT (const Matrix44 &mat, Vec3 &s, Vec3 &h, Vec3 &r, Vec3 &t, bool exc /*= true*/, typename Euler::Order rOrder); template bool extractSHRT (const Matrix44 &mat, Vec3 &s, Vec3 &h, Vec3 &r, Vec3 &t, bool exc = true); template bool extractSHRT (const Matrix44 &mat, Vec3 &s, Vec3 &h, Euler &r, Vec3 &t, bool exc = true); // // Internal utility function. // template bool checkForZeroScaleInRow (const T &scl, const Vec3 &row, bool exc = true); template Matrix44 outerProduct ( const Vec4 &a, const Vec4 &b); // // Returns a matrix that rotates "fromDirection" vector to "toDirection" // vector. // template Matrix44 rotationMatrix (const Vec3 &fromDirection, const Vec3 &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 Matrix44 rotationMatrixWithUpDir (const Vec3 &fromDir, const Vec3 &toDir, const Vec3 &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 void alignZAxisWithTargetDir (Matrix44 &result, Vec3 targetDir, Vec3 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 Matrix44 computeLocalFrame( const Vec3& p, const Vec3& xDir, const Vec3& 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 Matrix44 addOffset( const Matrix44& inMat, const Vec3& tOffset, const Vec3& rOffset, const Vec3& sOffset, const Vec3& 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 Matrix44 computeRSMatrix( bool keepRotateA, bool keepScaleA, const Matrix44& A, const Matrix44& B); //---------------------------------------------------------------------- // // Declarations for 3x3 matrix. // template bool extractScaling (const Matrix33 &mat, Vec2 &scl, bool exc = true); template Matrix33 sansScaling (const Matrix33 &mat, bool exc = true); template bool removeScaling (Matrix33 &mat, bool exc = true); template bool extractScalingAndShear (const Matrix33 &mat, Vec2 &scl, T &h, bool exc = true); template Matrix33 sansScalingAndShear (const Matrix33 &mat, bool exc = true); template bool removeScalingAndShear (Matrix33 &mat, bool exc = true); template bool extractAndRemoveScalingAndShear (Matrix33 &mat, Vec2 &scl, T &shr, bool exc = true); template void extractEuler (const Matrix33 &mat, T &rot); template bool extractSHRT (const Matrix33 &mat, Vec2 &s, T &h, T &r, Vec2 &t, bool exc = true); template bool checkForZeroScaleInRow (const T &scl, const Vec2 &row, bool exc = true); template Matrix33 outerProduct ( const Vec3 &a, const Vec3 &b); //----------------------------------------------------------------------------- // Implementation for 4x4 Matrix //------------------------------ template bool extractScaling (const Matrix44 &mat, Vec3 &scl, bool exc) { Vec3 shr; Matrix44 M (mat); if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) return false; return true; } template Matrix44 sansScaling (const Matrix44 &mat, bool exc) { Vec3 scl; Vec3 shr; Vec3 rot; Vec3 tran; if (! extractSHRT (mat, scl, shr, rot, tran, exc)) return mat; Matrix44 M; M.translate (tran); M.rotate (rot); M.shear (shr); return M; } template bool removeScaling (Matrix44 &mat, bool exc) { Vec3 scl; Vec3 shr; Vec3 rot; Vec3 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 bool extractScalingAndShear (const Matrix44 &mat, Vec3 &scl, Vec3 &shr, bool exc) { Matrix44 M (mat); if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) return false; return true; } template Matrix44 sansScalingAndShear (const Matrix44 &mat, bool exc) { Vec3 scl; Vec3 shr; Matrix44 M (mat); if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) return mat; return M; } template void sansScalingAndShear (Matrix44 &result, const Matrix44 &mat, bool exc) { Vec3 scl; Vec3 shr; if (! extractAndRemoveScalingAndShear (result, scl, shr, exc)) result = mat; } template bool removeScalingAndShear (Matrix44 &mat, bool exc) { Vec3 scl; Vec3 shr; if (! extractAndRemoveScalingAndShear (mat, scl, shr, exc)) return false; return true; } template bool extractAndRemoveScalingAndShear (Matrix44 &mat, Vec3 &scl, Vec3 &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 row[3]; row[0] = Vec3 (mat[0][0], mat[0][1], mat[0][2]); row[1] = Vec3 (mat[1][0], mat[1][1], mat[1][2]); row[2] = Vec3 (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 void extractEulerXYZ (const Matrix44 &mat, Vec3 &rot) { // // Normalize the local x, y and z axes to remove scaling. // Vec3 i (mat[0][0], mat[0][1], mat[0][2]); Vec3 j (mat[1][0], mat[1][1], mat[1][2]); Vec3 k (mat[2][0], mat[2][1], mat[2][2]); i.normalize(); j.normalize(); k.normalize(); Matrix44 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::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 N; N.rotate (Vec3 (-rot.x, 0, 0)); N = N * M; // // Extract the other two angles, rot.y and rot.z, from N. // T cy = Math::sqrt (N[0][0]*N[0][0] + N[0][1]*N[0][1]); rot.y = Math::atan2 (-N[0][2], cy); rot.z = Math::atan2 (-N[1][0], N[1][1]); } template void extractEulerZYX (const Matrix44 &mat, Vec3 &rot) { // // Normalize the local x, y and z axes to remove scaling. // Vec3 i (mat[0][0], mat[0][1], mat[0][2]); Vec3 j (mat[1][0], mat[1][1], mat[1][2]); Vec3 k (mat[2][0], mat[2][1], mat[2][2]); i.normalize(); j.normalize(); k.normalize(); Matrix44 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::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 N; N.rotate (Vec3 (0, 0, -rot.x)); N = N * M; // // Extract the other two angles, rot.y and rot.z, from N. // T cy = Math::sqrt (N[2][2]*N[2][2] + N[2][1]*N[2][1]); rot.y = -Math::atan2 (-N[2][0], cy); rot.z = -Math::atan2 (-N[1][2], N[1][1]); } template Quat extractQuat (const Matrix44 &mat) { Matrix44 rot; T tr, s; T q[4]; int i, j, k; Quat 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::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::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 bool extractSHRT (const Matrix44 &mat, Vec3 &s, Vec3 &h, Vec3 &r, Vec3 &t, bool exc /* = true */ , typename Euler::Order rOrder /* = Euler::XYZ */ ) { Matrix44 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::XYZ) { IMATH_INTERNAL_NAMESPACE::Euler eXYZ (r, IMATH_INTERNAL_NAMESPACE::Euler::XYZ); IMATH_INTERNAL_NAMESPACE::Euler e (eXYZ, rOrder); r = e.toXYZVector (); } return true; } template bool extractSHRT (const Matrix44 &mat, Vec3 &s, Vec3 &h, Vec3 &r, Vec3 &t, bool exc) { return extractSHRT(mat, s, h, r, t, exc, IMATH_INTERNAL_NAMESPACE::Euler::XYZ); } template bool extractSHRT (const Matrix44 &mat, Vec3 &s, Vec3 &h, Euler &r, Vec3 &t, bool exc /* = true */) { return extractSHRT (mat, s, h, r, t, exc, r.order ()); } template bool checkForZeroScaleInRow (const T& scl, const Vec3 &row, bool exc /* = true */ ) { for (int i = 0; i < 3; i++) { if ((abs (scl) < 1 && abs (row[i]) >= limits::max() * abs (scl))) { if (exc) throw IMATH_INTERNAL_NAMESPACE::ZeroScaleExc ("Cannot remove zero scaling " "from matrix."); else return false; } } return true; } template Matrix44 outerProduct (const Vec4 &a, const Vec4 &b ) { return Matrix44 (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 Matrix44 rotationMatrix (const Vec3 &from, const Vec3 &to) { Quat q; q.setRotation(from, to); return q.toMatrix44(); } template Matrix44 rotationMatrixWithUpDir (const Vec3 &fromDir, const Vec3 &toDir, const Vec3 &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 (); else { Matrix44 zAxis2FromDir( IMATH_INTERNAL_NAMESPACE::UNINITIALIZED ); alignZAxisWithTargetDir (zAxis2FromDir, fromDir, Vec3 (0, 1, 0)); Matrix44 fromDir2zAxis = zAxis2FromDir.transposed (); Matrix44 zAxis2ToDir( IMATH_INTERNAL_NAMESPACE::UNINITIALIZED ); alignZAxisWithTargetDir (zAxis2ToDir, toDir, upDir); return fromDir2zAxis * zAxis2ToDir; } } template void alignZAxisWithTargetDir (Matrix44 &result, Vec3 targetDir, Vec3 upDir) { // // Ensure that the target direction is non-zero. // if ( targetDir.length () == 0 ) targetDir = Vec3 (0, 0, 1); // // Ensure that the up direction is non-zero. // if ( upDir.length () == 0 ) upDir = Vec3 (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 (1, 0, 0)); if (upDir.length() == 0) upDir = targetDir.cross(Vec3 (0, 0, 1)); } // // Compute the x-, y-, and z-axis vectors of the new coordinate system. // Vec3 targetPerpDir = upDir.cross (targetDir); Vec3 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 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 Matrix44 computeLocalFrame( const Vec3& p, const Vec3& xDir, const Vec3& normal) { Vec3 _xDir(xDir); Vec3 x = _xDir.normalize(); Vec3 y = (normal % x).normalize(); Vec3 z = (x % y).normalize(); Matrix44 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 Matrix44 addOffset( const Matrix44& inMat, const Vec3& tOffset, const Vec3& rOffset, const Vec3& sOffset, const Matrix44& ref) { Matrix44 O; Vec3 _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 S; S.scale (sOffset); Matrix44 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 Matrix44 computeRSMatrix( bool keepRotateA, bool keepScaleA, const Matrix44& A, const Matrix44& B) { Vec3 as, ah, ar, at; extractSHRT (A, as, ah, ar, at); Vec3 bs, bh, br, bt; extractSHRT (B, bs, bh, br, bt); if (!keepRotateA) ar = br; if (!keepScaleA) as = bs; Matrix44 mat; mat.makeIdentity(); mat.translate (at); mat.rotate (ar); mat.scale (as); return mat; } //----------------------------------------------------------------------------- // Implementation for 3x3 Matrix //------------------------------ template bool extractScaling (const Matrix33 &mat, Vec2 &scl, bool exc) { T shr; Matrix33 M (mat); if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) return false; return true; } template Matrix33 sansScaling (const Matrix33 &mat, bool exc) { Vec2 scl; T shr; T rot; Vec2 tran; if (! extractSHRT (mat, scl, shr, rot, tran, exc)) return mat; Matrix33 M; M.translate (tran); M.rotate (rot); M.shear (shr); return M; } template bool removeScaling (Matrix33 &mat, bool exc) { Vec2 scl; T shr; T rot; Vec2 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 bool extractScalingAndShear (const Matrix33 &mat, Vec2 &scl, T &shr, bool exc) { Matrix33 M (mat); if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) return false; return true; } template Matrix33 sansScalingAndShear (const Matrix33 &mat, bool exc) { Vec2 scl; T shr; Matrix33 M (mat); if (! extractAndRemoveScalingAndShear (M, scl, shr, exc)) return mat; return M; } template bool removeScalingAndShear (Matrix33 &mat, bool exc) { Vec2 scl; T shr; if (! extractAndRemoveScalingAndShear (mat, scl, shr, exc)) return false; return true; } template bool extractAndRemoveScalingAndShear (Matrix33 &mat, Vec2 &scl, T &shr, bool exc) { Vec2 row[2]; row[0] = Vec2 (mat[0][0], mat[0][1]); row[1] = Vec2 (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 void extractEuler (const Matrix33 &mat, T &rot) { // // Normalize the local x and y axes to remove scaling. // Vec2 i (mat[0][0], mat[0][1]); Vec2 j (mat[1][0], mat[1][1]); i.normalize(); j.normalize(); // // Extract the angle, rot. // rot = - Math::atan2 (j[0], i[0]); } template bool extractSHRT (const Matrix33 &mat, Vec2 &s, T &h, T &r, Vec2 &t, bool exc) { Matrix33 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 bool checkForZeroScaleInRow (const T& scl, const Vec2 &row, bool exc /* = true */ ) { for (int i = 0; i < 2; i++) { if ((abs (scl) < 1 && abs (row[i]) >= limits::max() * abs (scl))) { if (exc) throw IMATH_INTERNAL_NAMESPACE::ZeroScaleExc ( "Cannot remove zero scaling from matrix."); else return false; } } return true; } template Matrix33 outerProduct (const Vec3 &a, const Vec3 &b ) { return Matrix33 (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 IMATH_INTERNAL_NAMESPACE::M44d procrustesRotationAndTranslation (const IMATH_INTERNAL_NAMESPACE::Vec3* A, // From these const IMATH_INTERNAL_NAMESPACE::Vec3* B, // To these const T* weights, const size_t numPoints, const bool doScaling = false); // Unweighted: template IMATH_INTERNAL_NAMESPACE::M44d procrustesRotationAndTranslation (const IMATH_INTERNAL_NAMESPACE::Vec3* A, const IMATH_INTERNAL_NAMESPACE::Vec3* 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 void jacobiSVD (const IMATH_INTERNAL_NAMESPACE::Matrix33& A, IMATH_INTERNAL_NAMESPACE::Matrix33& U, IMATH_INTERNAL_NAMESPACE::Vec3& S, IMATH_INTERNAL_NAMESPACE::Matrix33& V, const T tol = IMATH_INTERNAL_NAMESPACE::limits::epsilon(), const bool forcePositiveDeterminant = false); template void jacobiSVD (const IMATH_INTERNAL_NAMESPACE::Matrix44& A, IMATH_INTERNAL_NAMESPACE::Matrix44& U, IMATH_INTERNAL_NAMESPACE::Vec4& S, IMATH_INTERNAL_NAMESPACE::Matrix44& V, const T tol = IMATH_INTERNAL_NAMESPACE::limits::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 void jacobiEigenSolver (Matrix33& A, Vec3& S, Matrix33& V, const T tol); template inline void jacobiEigenSolver (Matrix33& A, Vec3& S, Matrix33& V) { jacobiEigenSolver(A,S,V,limits::epsilon()); } template void jacobiEigenSolver (Matrix44& A, Vec4& S, Matrix44& V, const T tol); template inline void jacobiEigenSolver (Matrix44& A, Vec4& S, Matrix44& V) { jacobiEigenSolver(A,S,V,limits::epsilon()); } // Compute a eigenvector corresponding to the abs max/min eigenvalue // of a real symmetric matrix using Jacobi transformation. template void maxEigenVector (TM& A, TV& S); template void minEigenVector (TM& A, TV& S); IMATH_INTERNAL_NAMESPACE_HEADER_EXIT #endif // INCLUDED_IMATHMATRIXALGO_H