// // SPDX-License-Identifier: BSD-3-Clause // Copyright Contributors to the OpenEXR Project. // // // // Functions operating on Matrix22, Matrix33, and Matrix44 types // // This file also defines a few predefined constant matrices. // #ifndef INCLUDED_IMATHMATRIXALGO_H #define INCLUDED_IMATHMATRIXALGO_H #include "ImathEuler.h" #include "ImathExport.h" #include "ImathMatrix.h" #include "ImathNamespace.h" #include "ImathQuat.h" #include "ImathVec.h" #include IMATH_INTERNAL_NAMESPACE_HEADER_ENTER //------------------ // Identity matrices //------------------ /// M22f identity matrix IMATH_EXPORT_CONST M22f identity22f; /// M33f identity matrix IMATH_EXPORT_CONST M33f identity33f; /// M44f identity matrix IMATH_EXPORT_CONST M44f identity44f; /// M22d identity matrix IMATH_EXPORT_CONST M22d identity22d; /// M33d identity matrix IMATH_EXPORT_CONST M33d identity33d; /// M44d identity matrix 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 a std::domain_error 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. // /// Extract the scaling component of the given 4x4 matrix. /// /// @param[in] mat The input matrix /// @param[out] scl The extracted scale, i.e. the output value /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. /// @return True if the scale could be extracted, false if the matrix is degenerate. template bool extractScaling (const Matrix44& mat, Vec3& scl, bool exc = true); /// Return the given 4x4 matrix with scaling removed. /// /// @param[in] mat The input matrix /// @param[in] exc If true, throw an exception if the scaling in `mat` template Matrix44 sansScaling (const Matrix44& mat, bool exc = true); /// Remove scaling from the given 4x4 matrix in place. Return true if the /// scale could be successfully extracted, false if the matrix is /// degenerate. // /// @param[in] mat The matrix to operate on /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. /// @return True if the scale could be extracted, false if the matrix is degenerate. template bool removeScaling (Matrix44& mat, bool exc = true); /// Extract the scaling and shear components of the given 4x4 matrix. /// Return true if the scale could be successfully extracted, false if /// the matrix is degenerate. /// /// @param[in] mat The input matrix /// @param[out] scl The extracted scale /// @param[out] shr The extracted shear /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. /// @return True if the scale could be extracted, false if the matrix is degenerate. template bool extractScalingAndShear (const Matrix44& mat, Vec3& scl, Vec3& shr, bool exc = true); /// Return the given 4x4 matrix with scaling and shear removed. /// /// @param[in] mat The input matrix /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. template Matrix44 sansScalingAndShear (const Matrix44& mat, bool exc = true); /// Extract scaling and shear from the given 4x4 matrix in-place. /// /// @param[in,out] result The output matrix /// @param[in] mat The return value if `result` is degenerate /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. template void sansScalingAndShear (Matrix44& result, const Matrix44& mat, bool exc = true); /// Remove scaling and shear from the given 4x4 matrix in place. // /// @param[in,out] mat The matrix to operate on /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. /// @return True if the scale could be extracted, false if the matrix is degenerate. template bool removeScalingAndShear (Matrix44& mat, bool exc = true); /// Remove scaling and shear from the given 4x4 matrix in place, returning /// the extracted values. // /// @param[in,out] mat The matrix to operate on /// @param[out] scl The extracted scale /// @param[out] shr The extracted shear /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. /// @return True if the scale could be extracted, false if the matrix is degenerate. template bool extractAndRemoveScalingAndShear (Matrix44& mat, Vec3& scl, Vec3& shr, bool exc = true); /// Extract the rotation from the given 4x4 matrix in the form of XYZ /// euler angles. /// /// @param[in] mat The input matrix /// @param[out] rot The extracted XYZ euler angle vector template void extractEulerXYZ (const Matrix44& mat, Vec3& rot); /// Extract the rotation from the given 4x4 matrix in the form of ZYX /// euler angles. /// /// @param[in] mat The input matrix /// @param[out] rot The extracted ZYX euler angle vector template void extractEulerZYX (const Matrix44& mat, Vec3& rot); /// Extract the rotation from the given 4x4 matrix in the form of a quaternion. /// /// @param[in] mat The input matrix /// @return The extracted quaternion template Quat extractQuat (const Matrix44& mat); /// Extract the scaling, shear, rotation, and translation components /// of the given 4x4 matrix. The values are such that: /// /// M = S * H * R * T /// /// @param[in] mat The input matrix /// @param[out] s The extracted scale /// @param[out] h The extracted shear /// @param[out] r The extracted rotation /// @param[out] t The extracted translation /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. /// @param[in] rOrder The order with which to extract the rotation /// @return True if the values could be extracted, false if the matrix is degenerate. template bool extractSHRT (const Matrix44& mat, Vec3& s, Vec3& h, Vec3& r, Vec3& t, bool exc /*= true*/, typename Euler::Order rOrder); /// Extract the scaling, shear, rotation, and translation components /// of the given 4x4 matrix. /// /// @param[in] mat The input matrix /// @param[out] s The extracted scale /// @param[out] h The extracted shear /// @param[out] r The extracted rotation, in XYZ euler angles /// @param[out] t The extracted translation /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. /// @return True if the values could be extracted, false if the matrix is degenerate. template bool extractSHRT (const Matrix44& mat, Vec3& s, Vec3& h, Vec3& r, Vec3& t, bool exc = true); /// Extract the scaling, shear, rotation, and translation components /// of the given 4x4 matrix. /// /// @param[in] mat The input matrix /// @param[out] s The extracted scale /// @param[out] h The extracted shear /// @param[out] r The extracted rotation, in Euler angles /// @param[out] t The extracted translation /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. /// @return True if the values could be extracted, false if the matrix is degenerate. template bool extractSHRT (const Matrix44& mat, Vec3& s, Vec3& h, Euler& r, Vec3& t, bool exc = true); /// Return true if the given scale can be removed from the given row /// matrix, false if `scl` is small enough that the operation would /// overflow. If `exc` is true, throw an exception on overflow. template bool checkForZeroScaleInRow (const T& scl, const Vec3& row, bool exc = true); /// Return the 4x4 outer product two 4-vectors template Matrix44 outerProduct (const Vec4& a, const Vec4& b); /// /// Return a 4x4 matrix that rotates the vector `fromDirection` to `toDirection` /// template Matrix44 rotationMatrix (const Vec3& fromDirection, const Vec3& toDirection); /// /// Return a 4x4 matrix that rotates the `fromDir` vector /// so that it points towards `toDir1. You may also /// specify that you want the up vector to be pointing /// in a certain direction 1upDir`. template Matrix44 rotationMatrixWithUpDir (const Vec3& fromDir, const Vec3& toDir, const Vec3& upDir); /// /// Construct a 4x4 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 /// /// @param[out] result The output matrix /// @param[in] targetDir The target direction vector /// @param[in] upDir The up direction vector template void alignZAxisWithTargetDir (Matrix44& result, Vec3 targetDir, Vec3 upDir); /// Compute an orthonormal direct 4x4 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. /// /// @param[in] p The position of the frame /// @param[in] xDir The x axis direction of the frame /// @param[in] normal A normal to the y axis of the frame /// @return The orthonormal frame template Matrix44 computeLocalFrame (const Vec3& p, const Vec3& xDir, const Vec3& normal); /// Add a translate/rotate/scale offset to a 4x4 input frame /// and put it in another frame of reference /// /// @param[in] inMat Input frame /// @param[in] tOffset Translation offset /// @param[in] rOffset Rotation offset in degrees /// @param[in] sOffset Scale offset /// @param[in] ref Frame of reference /// @return The offsetted frame template Matrix44 addOffset (const Matrix44& inMat, const Vec3& tOffset, const Vec3& rOffset, const Vec3& sOffset, const Vec3& ref); /// Compute 4x4 translate/rotate/scale matrix from `A` with the /// rotate/scale of `B`. /// /// @param[in] keepRotateA If true, keep rotate from matrix `A`, use `B` otherwise /// @param[in] keepScaleA If true, keep scale from matrix `A`, use `B` otherwise /// @param[in] A Matrix A /// @param[in] B 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. // /// Extract the scaling component of the given 3x3 matrix. /// /// @param[in] mat The input matrix /// @param[out] scl The extracted scale, i.e. the output value /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. /// @return True if the scale could be extracted, false if the matrix is degenerate. template bool extractScaling (const Matrix33& mat, Vec2& scl, bool exc = true); /// Return the given 3x3 matrix with scaling removed. /// /// @param[in] mat The input matrix /// @param[in] exc If true, throw an exception if the scaling in `mat` template Matrix33 sansScaling (const Matrix33& mat, bool exc = true); /// Remove scaling from the given 3x3 matrix in place. Return true if the /// scale could be successfully extracted, false if the matrix is /// degenerate. // /// @param[in] mat The matrix to operate on /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. /// @return True if the scale could be extracted, false if the matrix is degenerate. template bool removeScaling (Matrix33& mat, bool exc = true); /// Extract the scaling and shear components of the given 3x3 matrix. /// Return true if the scale could be successfully extracted, false if /// the matrix is degenerate. /// /// @param[in] mat The input matrix /// @param[out] scl The extracted scale /// @param[out] shr The extracted shear /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. /// @return True if the scale could be extracted, false if the matrix is degenerate. template bool extractScalingAndShear (const Matrix33& mat, Vec2& scl, T& shr, bool exc = true); /// Return the given 3x3 matrix with scaling and shear removed. /// /// @param[in] mat The input matrix /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. template Matrix33 sansScalingAndShear (const Matrix33& mat, bool exc = true); /// Remove scaling and shear from the given 3x3e matrix in place. // /// @param[in,out] mat The matrix to operate on /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. /// @return True if the scale could be extracted, false if the matrix is degenerate. template bool removeScalingAndShear (Matrix33& mat, bool exc = true); /// Remove scaling and shear from the given 3x3 matrix in place, returning /// the extracted values. // /// @param[in,out] mat The matrix to operate on /// @param[out] scl The extracted scale /// @param[out] shr The extracted shear /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. /// @return True if the scale could be extracted, false if the matrix is degenerate. template bool extractAndRemoveScalingAndShear (Matrix33& mat, Vec2& scl, T& shr, bool exc = true); /// Extract the rotation from the given 2x2 matrix /// /// @param[in] mat The input matrix /// @param[out] rot The extracted rotation value template void extractEuler (const Matrix22& mat, T& rot); /// Extract the rotation from the given 3x3 matrix /// /// @param[in] mat The input matrix /// @param[out] rot The extracted rotation value template void extractEuler (const Matrix33& mat, T& rot); /// Extract the scaling, shear, rotation, and translation components /// of the given 3x3 matrix. The values are such that: /// /// M = S * H * R * T /// /// @param[in] mat The input matrix /// @param[out] s The extracted scale /// @param[out] h The extracted shear /// @param[out] r The extracted rotation /// @param[out] t The extracted translation /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. /// @return True if the values could be extracted, false if the matrix is degenerate. template bool extractSHRT (const Matrix33& mat, Vec2& s, T& h, T& r, Vec2& t, bool exc = true); /// Return true if the given scale can be removed from the given row /// matrix, false if `scl` is small enough that the operation would /// overflow. If `exc` is true, throw an exception on overflow. template bool checkForZeroScaleInRow (const T& scl, const Vec2& row, bool exc = true); /// Return the 3xe outer product two 3-vectors 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 = std::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 = std::sqrt (N[0][0] * N[0][0] + N[0][1] * N[0][1]); rot.y = std::atan2 (-N[0][2], cy); rot.z = std::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 = -std::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 = std::sqrt (N[2][2] * N[2][2] + N[2][1] * N[2][1]); rot.y = -std::atan2 (-N[2][0], cy); rot.z = -std::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 = std::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 = std::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) { Euler eXYZ (r, Euler::XYZ); 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, 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]) >= std::numeric_limits::max() * abs (scl))) { if (exc) throw std::domain_error ("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 (UNINITIALIZED); alignZAxisWithTargetDir (zAxis2FromDir, fromDir, Vec3 (0, 1, 0)); Matrix44 fromDir2zAxis = zAxis2FromDir.transposed(); Matrix44 zAxis2ToDir (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. /// /// @param inMat input frame /// @param tOffset translate offset /// @param rOffset rotate offset in degrees /// @param sOffset scale offset /// @param ref Frame of reference /// @return 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 *= T(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; if (!extractSHRT (B, bs, bh, br, bt)) throw std::domain_error ("degenerate B matrix in computeRSMatrix"); 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 Matrix22& 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 = -std::atan2 (j[0], i[0]); } 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 = -std::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; } /// @cond Doxygen_Suppress 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]) >= std::numeric_limits::max() * abs (scl))) { if (exc) throw std::domain_error ("Cannot remove zero scaling from matrix."); else return false; } } return true; } /// @endcond 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. /// @param A From points /// @param B To points /// @param weights Per-point weights /// @param numPoints The number of points in `A`, `B`, and `weights` (must be equal) /// @param doScaling If true, include a scaling transformation /// @return The procrustes transformation template M44d procrustesRotationAndTranslation (const Vec3* A, const Vec3* B, const T* weights, const size_t numPoints, const bool doScaling = false); /// 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. /// @param A From points /// @param B To points /// @param numPoints The number of points in `A` and `B` (must be equal) /// @param doScaling If true, include a scaling transformation /// @return The procrustes transformation template M44d procrustesRotationAndTranslation (const Vec3* A, const 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 Matrix33& A, Matrix33& U, Vec3& S, Matrix33& V, const T tol = std::numeric_limits::epsilon(), const bool forcePositiveDeterminant = 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 Matrix44& A, Matrix44& U, Vec4& S, Matrix44& V, const T tol = std::numeric_limits::epsilon(), const bool forcePositiveDeterminant = false); /// Compute the eigenvalues (S) and the eigenvectors (V) of a real /// symmetric matrix using Jacobi transformation, using a given /// tolerance `tol`. /// /// 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); /// 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 inline void jacobiEigenSolver (Matrix33& A, Vec3& S, Matrix33& V) { jacobiEigenSolver (A, S, V, std::numeric_limits::epsilon()); } /// Compute the eigenvalues (S) and the eigenvectors (V) of a real /// symmetric matrix using Jacobi transformation, using a given /// tolerance `tol`. /// /// 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 (Matrix44& A, Vec4& S, Matrix44& V, const T tol); /// 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 inline void jacobiEigenSolver (Matrix44& A, Vec4& S, Matrix44& V) { jacobiEigenSolver (A, S, V, std::numeric_limits::epsilon()); } /// Compute a eigenvector corresponding to the abs max eigenvalue /// of a real symmetric matrix using Jacobi transformation. template void maxEigenVector (TM& A, TV& S); /// Compute a eigenvector corresponding to the abs min eigenvalue /// of a real symmetric matrix using Jacobi transformation. template void minEigenVector (TM& A, TV& S); IMATH_INTERNAL_NAMESPACE_HEADER_EXIT #endif // INCLUDED_IMATHMATRIXALGO_H