ImathMatrixAlgo.h

//
// 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_ATOMSMATHMATRIXALGO_H
#define INCLUDED_ATOMSMATHMATRIXALGO_H
 
#include <AtomsMath/ImathEuler.h>
#include <AtomsMath/ImathExport.h>
#include <AtomsMath/ImathMatrix.h>
#include <AtomsMath/ImathNamespace.h>
#include <AtomsMath/ImathQuat.h>
#include <AtomsMath/ImathVec.h>
#include <math.h>
 
ATOMSMATH_INTERNAL_NAMESPACE_HEADER_ENTER
 
//------------------
// Identity matrices
//------------------
 
ATOMSMATH_EXPORT_CONST M22f identity22f;
ATOMSMATH_EXPORT_CONST M33f identity33f;
ATOMSMATH_EXPORT_CONST M44f identity44f;
ATOMSMATH_EXPORT_CONST M22d identity22d;
ATOMSMATH_EXPORT_CONST M33d identity33d;
ATOMSMATH_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.
//
 
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);
 
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);
 
template <class T>
Matrix44<T> rotationMatrix (const Vec3<T>& fromDirection, const Vec3<T>& toDirection);
 
template <class T>
Matrix44<T>
rotationMatrixWithUpDir (const Vec3<T>& fromDir, const Vec3<T>& toDir, const Vec3<T>& upDir);
 
template <class T>
void alignZAxisWithTargetDir (Matrix44<T>& result, Vec3<T> targetDir, Vec3<T> upDir);
 
template <class T>
Matrix44<T> computeLocalFrame (const Vec3<T>& p, const Vec3<T>& xDir, const Vec3<T>& normal);
 
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);
 
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& shr, 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 Matrix22<T>& mat, T& rot);
 
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 (ATOMSMATH_INTERNAL_NAMESPACE::abs (row[i][j]) > maxVal)
                maxVal = ATOMSMATH_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 = 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<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  = 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 <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 = -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<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  = 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 <class T>
Quat<T>
extractQuat (const Matrix44<T>& mat)
{
    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      = 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 <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)
    {
        Euler<T> eXYZ (r, Euler<T>::XYZ);
        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, 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]) >= std::numeric_limits<T>::max() * abs (scl)))
        {
            #ifdef ATOMS_USE_EXCEPTIONS
            if (exc)
                throw std::domain_error ("Cannot remove zero scaling "
                                         "from matrix.");
            else
            #endif
                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 (UNINITIALIZED);
        alignZAxisWithTargetDir (zAxis2FromDir, fromDir, Vec3<T> (0, 1, 0));
 
        Matrix44<T> fromDir2zAxis = zAxis2FromDir.transposed();
 
        Matrix44<T> zAxis2ToDir (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;
}
 
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 (ATOMSMATH_INTERNAL_NAMESPACE::abs (row[i][j]) > maxVal)
                maxVal = ATOMSMATH_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 Matrix22<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 = -std::atan2 (j[0], i[0]);
}
 
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 = -std::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]) >= std::numeric_limits<T>::max() * abs (scl)))
        {
            #ifdef ATOMS_USE_EXCEPTIONS
            if (exc)
                throw std::domain_error ("Cannot remove zero scaling from matrix.");
            else
            #endif
                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);
}
 
template <typename T>
M44d
procrustesRotationAndTranslation (const Vec3<T>* A,
                                  const Vec3<T>* B,
                                  const T* weights,
                                  const size_t numPoints,
                                  const bool doScaling = false);
 
template <typename T>
M44d
procrustesRotationAndTranslation (const Vec3<T>* A,
                                  const Vec3<T>* B,
                                  const size_t numPoints,
                                  const bool doScaling = false);
 
template <typename T>
void jacobiSVD (const Matrix33<T>& A,
                Matrix33<T>& U,
                Vec3<T>& S,
                Matrix33<T>& V,
                const T tol = std::numeric_limits<T>::epsilon(),
                const bool forcePositiveDeterminant = false);
 
template <typename T>
void jacobiSVD (const Matrix44<T>& A,
                Matrix44<T>& U,
                Vec4<T>& S,
                Matrix44<T>& V,
                const T tol = std::numeric_limits<T>::epsilon(),
                const bool forcePositiveDeterminant = false);
 
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, std::numeric_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, std::numeric_limits<T>::epsilon());
}
 
template <typename TM, typename TV> void maxEigenVector (TM& A, TV& S);
 
template <typename TM, typename TV> void minEigenVector (TM& A, TV& S);
 
ATOMSMATH_INTERNAL_NAMESPACE_HEADER_EXIT
 
#endif // INCLUDED_ATOMSMATHMATRIXALGO_H