PM/DYT
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
abcd6828aa
DYT/Tool/OpenSceneGraph-3.6.5/include/Imath/ImathMatrixAlgo.h

1518 lines
46 KiB
C
Raw Normal View History

use oe ro replace oc
2024-12-24 23:49:36 +00:00
//
// 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 <math.h>
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 <class T> bool extractScaling (const Matrix44<T>& mat, Vec3<T>& 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 <class T> Matrix44<T> sansScaling (const Matrix44<T>& 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 <class T> bool removeScaling (Matrix44<T>& 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 <class T> bool extractScalingAndShear (const Matrix44<T>& mat, Vec3<T>& scl, Vec3<T>& 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 <class T> Matrix44<T> sansScalingAndShear (const Matrix44<T>& 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 <class T>
void sansScalingAndShear (Matrix44<T>& result, const Matrix44<T>& 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 <class T> bool removeScalingAndShear (Matrix44<T>& 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 <class T>
bool
extractAndRemoveScalingAndShear (Matrix44<T>& mat, Vec3<T>& scl, Vec3<T>& 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 <class T> void extractEulerXYZ (const Matrix44<T>& mat, Vec3<T>& 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 <class T> void extractEulerZYX (const Matrix44<T>& mat, Vec3<T>& 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 <class T> Quat<T> extractQuat (const Matrix44<T>& 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 <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);
/// 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 <class T>
bool extractSHRT (const Matrix44<T>& mat,
Vec3<T>& s,
Vec3<T>& h,
Vec3<T>& r,
Vec3<T>& 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 <class T>
bool extractSHRT (const Matrix44<T>& mat,
Vec3<T>& s,
Vec3<T>& h,
Euler<T>& r,
Vec3<T>& 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 <class T> bool checkForZeroScaleInRow (const T& scl, const Vec3<T>& row, bool exc = true);
/// Return the 4x4 outer product two 4-vectors
template <class T> Matrix44<T> outerProduct (const Vec4<T>& a, const Vec4<T>& b);
///
/// Return a 4x4 matrix that rotates the vector `fromDirection` to `toDirection`
///
template <class T>
Matrix44<T> rotationMatrix (const Vec3<T>& fromDirection, const Vec3<T>& 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 <class T>
Matrix44<T>
rotationMatrixWithUpDir (const Vec3<T>& fromDir, const Vec3<T>& toDir, const Vec3<T>& 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 <class T>
void alignZAxisWithTargetDir (Matrix44<T>& result, Vec3<T> targetDir, Vec3<T> 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 <class T>
Matrix44<T> computeLocalFrame (const Vec3<T>& p, const Vec3<T>& xDir, const Vec3<T>& 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 <class T>
Matrix44<T> addOffset (const Matrix44<T>& inMat,
const Vec3<T>& tOffset,
const Vec3<T>& rOffset,
const Vec3<T>& sOffset,
const Vec3<T>& ref);
/// Compute 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 <class T>
Matrix44<T>
computeRSMatrix (bool keepRotateA, bool keepScaleA, const Matrix44<T>& A, const Matrix44<T>& 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 <class T> bool extractScaling (const Matrix33<T>& mat, Vec2<T>& 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 <class T> Matrix33<T> sansScaling (const Matrix33<T>& 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 <class T> bool removeScaling (Matrix33<T>& 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 <class T>
bool extractScalingAndShear (const Matrix33<T>& mat, Vec2<T>& 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 <class T> Matrix33<T> sansScalingAndShear (const Matrix33<T>& 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 <class T> bool removeScalingAndShear (Matrix33<T>& 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 <class T>
bool extractAndRemoveScalingAndShear (Matrix33<T>& mat, Vec2<T>& 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 <class T> void extractEuler (const Matrix22<T>& 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 <class T> void extractEuler (const Matrix33<T>& 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 <class T>
bool extractSHRT (const Matrix33<T>& mat, Vec2<T>& s, T& h, T& r, Vec2<T>& 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 <class T> bool checkForZeroScaleInRow (const T& scl, const Vec2<T>& row, bool exc = true);
/// Return the 3xe outer product two 3-vectors
template <class T> Matrix33<T> outerProduct (const Vec3<T>& a, const Vec3<T>& b);
//------------------------------
// Implementation for 4x4 Matrix
//------------------------------
template <class T>
bool
extractScaling (const Matrix44<T>& mat, Vec3<T>& scl, bool exc)
{
Vec3<T> shr;
Matrix44<T> M (mat);
if (!extractAndRemoveScalingAndShear (M, scl, shr, exc))
return false;
return true;
}
template <class T>
Matrix44<T>
sansScaling (const Matrix44<T>& mat, bool exc)
{
Vec3<T> scl;
Vec3<T> shr;
Vec3<T> rot;
Vec3<T> tran;
if (!extractSHRT (mat, scl, shr, rot, tran, exc))
return mat;
Matrix44<T> M;
M.translate (tran);
M.rotate (rot);
M.shear (shr);
return M;
}
template <class T>
bool
removeScaling (Matrix44<T>& mat, bool exc)
{
Vec3<T> scl;
Vec3<T> shr;
Vec3<T> rot;
Vec3<T> tran;
if (!extractSHRT (mat, scl, shr, rot, tran, exc))
return false;
mat.makeIdentity();
mat.translate (tran);
mat.rotate (rot);
mat.shear (shr);
return true;
}
template <class T>
bool
extractScalingAndShear (const Matrix44<T>& mat, Vec3<T>& scl, Vec3<T>& shr, bool exc)
{
Matrix44<T> M (mat);
if (!extractAndRemoveScalingAndShear (M, scl, shr, exc))
return false;
return true;
}
template <class T>
Matrix44<T>
sansScalingAndShear (const Matrix44<T>& mat, bool exc)
{
Vec3<T> scl;
Vec3<T> shr;
Matrix44<T> M (mat);
if (!extractAndRemoveScalingAndShear (M, scl, shr, exc))
return mat;
return M;
}
template <class T>
void
sansScalingAndShear (Matrix44<T>& result, const Matrix44<T>& mat, bool exc)
{
Vec3<T> scl;
Vec3<T> shr;
if (!extractAndRemoveScalingAndShear (result, scl, shr, exc))
result = mat;
}
template <class T>
bool
removeScalingAndShear (Matrix44<T>& mat, bool exc)
{
Vec3<T> scl;
Vec3<T> shr;
if (!extractAndRemoveScalingAndShear (mat, scl, shr, exc))
return false;
return true;
}
template <class T>
bool
extractAndRemoveScalingAndShear (Matrix44<T>& mat, Vec3<T>& scl, Vec3<T>& shr, bool exc)
{
//
// This implementation follows the technique described in the paper by
// Spencer W. Thomas in the Graphics Gems II article: "Decomposing a
// Matrix into Simple Transformations", p. 320.
//
Vec3<T> row[3];
row[0] = Vec3<T> (mat[0][0], mat[0][1], mat[0][2]);
row[1] = Vec3<T> (mat[1][0], mat[1][1], mat[1][2]);
row[2] = Vec3<T> (mat[2][0], mat[2][1], mat[2][2]);
T maxVal = 0;
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
if (IMATH_INTERNAL_NAMESPACE::abs (row[i][j]) > maxVal)
maxVal = IMATH_INTERNAL_NAMESPACE::abs (row[i][j]);
//
// We normalize the 3x3 matrix here.
// It was noticed that this can improve numerical stability significantly,
// especially when many of the upper 3x3 matrix's coefficients are very
// close to zero; we correct for this step at the end by multiplying the
// scaling factors by maxVal at the end (shear and rotation are not
// affected by the normalization).
if (maxVal != 0)
{
for (int i = 0; i < 3; i++)
if (!checkForZeroScaleInRow (maxVal, row[i], exc))
return false;
else
row[i] /= maxVal;
}
// Compute X scale factor.
scl.x = row[0].length();
if (!checkForZeroScaleInRow (scl.x, row[0], exc))
return false;
// Normalize first row.
row[0] /= scl.x;
// An XY shear factor will shear the X coord. as the Y coord. changes.
// There are 6 combinations (XY, XZ, YZ, YX, ZX, ZY), although we only
// extract the first 3 because we can effect the last 3 by shearing in
// XY, XZ, YZ combined rotations and scales.
//
// shear matrix < 1, YX, ZX, 0,
// XY, 1, ZY, 0,
// XZ, YZ, 1, 0,
// 0, 0, 0, 1 >
// Compute XY shear factor and make 2nd row orthogonal to 1st.
shr[0] = row[0].dot (row[1]);
row[1] -= shr[0] * row[0];
// Now, compute Y scale.
scl.y = row[1].length();
if (!checkForZeroScaleInRow (scl.y, row[1], exc))
return false;
// Normalize 2nd row and correct the XY shear factor for Y scaling.
row[1] /= scl.y;
shr[0] /= scl.y;
// Compute XZ and YZ shears, orthogonalize 3rd row.
shr[1] = row[0].dot (row[2]);
row[2] -= shr[1] * row[0];
shr[2] = row[1].dot (row[2]);
row[2] -= shr[2] * row[1];
// Next, get Z scale.
scl.z = row[2].length();
if (!checkForZeroScaleInRow (scl.z, row[2], exc))
return false;
// Normalize 3rd row and correct the XZ and YZ shear factors for Z scaling.
row[2] /= scl.z;
shr[1] /= scl.z;
shr[2] /= scl.z;
// At this point, the upper 3x3 matrix in mat is orthonormal.
// Check for a coordinate system flip. If the determinant
// is less than zero, then negate the matrix and the scaling factors.
if (row[0].dot (row[1].cross (row[2])) < 0)
for (int i = 0; i < 3; i++)
{
scl[i] *= -1;
row[i] *= -1;
}
// Copy over the orthonormal rows into the returned matrix.
// The upper 3x3 matrix in mat is now a rotation matrix.
for (int i = 0; i < 3; i++)
{
mat[i][0] = row[i][0];
mat[i][1] = row[i][1];
mat[i][2] = row[i][2];
}
// Correct the scaling factors for the normalization step that we
// performed above; shear and rotation are not affected by the
// normalization.
scl *= maxVal;
return true;
}
template <class T>
void
extractEulerXYZ (const Matrix44<T>& mat, Vec3<T>& rot)
{
//
// Normalize the local x, y and z axes to remove scaling.
//
Vec3<T> i (mat[0][0], mat[0][1], mat[0][2]);
Vec3<T> j (mat[1][0], mat[1][1], mat[1][2]);
Vec3<T> k (mat[2][0], mat[2][1], mat[2][2]);
i.normalize();
j.normalize();
k.normalize();
Matrix44<T> M (i[0], i[1], i[2], 0, j[0], j[1], j[2], 0, k[0], k[1], k[2], 0, 0, 0, 0, 1);
//
// Extract the first angle, rot.x.
//
rot.x = 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)))
{
if (exc)
throw std::domain_error ("Cannot remove zero scaling "
"from matrix.");
else
return false;
}
}
return true;
}
template <class T>
Matrix44<T>
outerProduct (const Vec4<T>& a, const Vec4<T>& b)
{
return Matrix44<T> (a.x * b.x,
a.x * b.y,
a.x * b.z,
a.x * b.w,
a.y * b.x,
a.y * b.y,
a.y * b.z,
a.x * b.w,
a.z * b.x,
a.z * b.y,
a.z * b.z,
a.x * b.w,
a.w * b.x,
a.w * b.y,
a.w * b.z,
a.w * b.w);
}
template <class T>
Matrix44<T>
rotationMatrix (const Vec3<T>& from, const Vec3<T>& to)
{
Quat<T> q;
q.setRotation (from, to);
return q.toMatrix44();
}
template <class T>
Matrix44<T>
rotationMatrixWithUpDir (const Vec3<T>& fromDir, const Vec3<T>& toDir, const Vec3<T>& upDir)
{
//
// The goal is to obtain a rotation matrix that takes
// "fromDir" to "toDir". We do this in two steps and
// compose the resulting rotation matrices;
// (a) rotate "fromDir" into the z-axis
// (b) rotate the z-axis into "toDir"
//
// The from direction must be non-zero; but we allow zero to and up dirs.
if (fromDir.length() == 0)
return Matrix44<T>();
else
{
Matrix44<T> zAxis2FromDir (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;
}
/// 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 <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 *= T(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;
if (!extractSHRT (A, as, ah, ar, at))
throw std::domain_error ("degenerate A matrix in computeRSMatrix");
Vec3<T> 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<T> mat;
mat.makeIdentity();
mat.translate (at);
mat.rotate (ar);
mat.scale (as);
return mat;
}
//-----------------------------------------------------------------------------
// Implementation for 3x3 Matrix
//------------------------------
template <class T>
bool
extractScaling (const Matrix33<T>& mat, Vec2<T>& scl, bool exc)
{
T shr;
Matrix33<T> M (mat);
if (!extractAndRemoveScalingAndShear (M, scl, shr, exc))
return false;
return true;
}
template <class T>
Matrix33<T>
sansScaling (const Matrix33<T>& mat, bool exc)
{
Vec2<T> scl;
T shr;
T rot;
Vec2<T> tran;
if (!extractSHRT (mat, scl, shr, rot, tran, exc))
return mat;
Matrix33<T> M;
M.translate (tran);
M.rotate (rot);
M.shear (shr);
return M;
}
template <class T>
bool
removeScaling (Matrix33<T>& mat, bool exc)
{
Vec2<T> scl;
T shr;
T rot;
Vec2<T> tran;
if (!extractSHRT (mat, scl, shr, rot, tran, exc))
return false;
mat.makeIdentity();
mat.translate (tran);
mat.rotate (rot);
mat.shear (shr);
return true;
}
template <class T>
bool
extractScalingAndShear (const Matrix33<T>& mat, Vec2<T>& scl, T& shr, bool exc)
{
Matrix33<T> M (mat);
if (!extractAndRemoveScalingAndShear (M, scl, shr, exc))
return false;
return true;
}
template <class T>
Matrix33<T>
sansScalingAndShear (const Matrix33<T>& mat, bool exc)
{
Vec2<T> scl;
T shr;
Matrix33<T> M (mat);
if (!extractAndRemoveScalingAndShear (M, scl, shr, exc))
return mat;
return M;
}
template <class T>
bool
removeScalingAndShear (Matrix33<T>& mat, bool exc)
{
Vec2<T> scl;
T shr;
if (!extractAndRemoveScalingAndShear (mat, scl, shr, exc))
return false;
return true;
}
template <class T>
bool
extractAndRemoveScalingAndShear (Matrix33<T>& mat, Vec2<T>& scl, T& shr, bool exc)
{
Vec2<T> row[2];
row[0] = Vec2<T> (mat[0][0], mat[0][1]);
row[1] = Vec2<T> (mat[1][0], mat[1][1]);
T maxVal = 0;
for (int i = 0; i < 2; i++)
for (int j = 0; j < 2; j++)
if (IMATH_INTERNAL_NAMESPACE::abs (row[i][j]) > maxVal)
maxVal = IMATH_INTERNAL_NAMESPACE::abs (row[i][j]);
//
// We normalize the 2x2 matrix here.
// It was noticed that this can improve numerical stability significantly,
// especially when many of the upper 2x2 matrix's coefficients are very
// close to zero; we correct for this step at the end by multiplying the
// scaling factors by maxVal at the end (shear and rotation are not
// affected by the normalization).
if (maxVal != 0)
{
for (int i = 0; i < 2; i++)
if (!checkForZeroScaleInRow (maxVal, row[i], exc))
return false;
else
row[i] /= maxVal;
}
// Compute X scale factor.
scl.x = row[0].length();
if (!checkForZeroScaleInRow (scl.x, row[0], exc))
return false;
// Normalize first row.
row[0] /= scl.x;
// An XY shear factor will shear the X coord. as the Y coord. changes.
// There are 2 combinations (XY, YX), although we only extract the XY
// shear factor because we can effect the an YX shear factor by
// shearing in XY combined with rotations and scales.
//
// shear matrix < 1, YX, 0,
// XY, 1, 0,
// 0, 0, 1 >
// Compute XY shear factor and make 2nd row orthogonal to 1st.
shr = row[0].dot (row[1]);
row[1] -= shr * row[0];
// Now, compute Y scale.
scl.y = row[1].length();
if (!checkForZeroScaleInRow (scl.y, row[1], exc))
return false;
// Normalize 2nd row and correct the XY shear factor for Y scaling.
row[1] /= scl.y;
shr /= scl.y;
// At this point, the upper 2x2 matrix in mat is orthonormal.
// Check for a coordinate system flip. If the determinant
// is -1, then flip the rotation matrix and adjust the scale(Y)
// and shear(XY) factors to compensate.
if (row[0][0] * row[1][1] - row[0][1] * row[1][0] < 0)
{
row[1][0] *= -1;
row[1][1] *= -1;
scl[1] *= -1;
shr *= -1;
}
// Copy over the orthonormal rows into the returned matrix.
// The upper 2x2 matrix in mat is now a rotation matrix.
for (int i = 0; i < 2; i++)
{
mat[i][0] = row[i][0];
mat[i][1] = row[i][1];
}
scl *= maxVal;
return true;
}
template <class T>
void
extractEuler (const 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;
}
/// @cond Doxygen_Suppress
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)))
{
if (exc)
throw std::domain_error ("Cannot remove zero scaling from matrix.");
else
return false;
}
}
return true;
}
/// @endcond
template <class T>
Matrix33<T>
outerProduct (const Vec3<T>& a, const Vec3<T>& b)
{
return Matrix33<T> (a.x * b.x,
a.x * b.y,
a.x * b.z,
a.y * b.x,
a.y * b.y,
a.y * b.z,
a.z * b.x,
a.z * b.y,
a.z * b.z);
}
/// Computes the translation and rotation that brings the 'from' points
/// as close as possible to the 'to' points under the Frobenius norm.
/// To be more specific, let x be the matrix of 'from' points and y be
/// the matrix of 'to' points, we want to find the matrix A of the form
/// [ R t ]
/// [ 0 1 ]
/// that minimizes
/// || (A*x - y)^T * W * (A*x - y) ||_F
/// If doScaling is true, then a uniform scale is allowed also.
/// @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 <typename T>
M44d
procrustesRotationAndTranslation (const Vec3<T>* A,
const Vec3<T>* 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 <typename T>
M44d
procrustesRotationAndTranslation (const Vec3<T>* A,
const Vec3<T>* B,
const size_t numPoints,
const bool doScaling = false);
/// Compute the SVD of a 3x3 matrix using Jacobi transformations. This method
/// should be quite accurate (competitive with LAPACK) even for poorly
/// conditioned matrices, and because it has been written specifically for the
/// 3x3/4x4 case it is much faster than calling out to LAPACK.
///
/// The SVD of a 3x3/4x4 matrix A is defined as follows:
/// A = U * S * V^T
/// where S is the diagonal matrix of singular values and both U and V are
/// orthonormal. By convention, the entries S are all positive and sorted from
/// the largest to the smallest. However, some uses of this function may
/// require that the matrix U*V^T have positive determinant; in this case, we
/// may make the smallest singular value negative to ensure that this is
/// satisfied.
///
/// Currently only available for single- and double-precision matrices.
template <typename T>
void jacobiSVD (const Matrix33<T>& A,
Matrix33<T>& U,
Vec3<T>& S,
Matrix33<T>& V,
const T tol = std::numeric_limits<T>::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 <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);
/// 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 <typename T>
void jacobiEigenSolver (Matrix33<T>& A, Vec3<T>& S, Matrix33<T>& 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 <typename T>
inline void
jacobiEigenSolver (Matrix33<T>& A, Vec3<T>& S, Matrix33<T>& V)
{
jacobiEigenSolver (A, S, V, std::numeric_limits<T>::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 <typename T>
void jacobiEigenSolver (Matrix44<T>& A, Vec4<T>& S, Matrix44<T>& 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 <typename T>
inline void
jacobiEigenSolver (Matrix44<T>& A, Vec4<T>& S, Matrix44<T>& V)
{
jacobiEigenSolver (A, S, V, std::numeric_limits<T>::epsilon());
}
/// Compute a eigenvector corresponding to the abs max eigenvalue
/// of a real symmetric matrix using Jacobi transformation.
template <typename TM, typename TV> void maxEigenVector (TM& A, TV& S);
/// Compute a eigenvector corresponding to the abs min eigenvalue
/// of a real symmetric matrix using Jacobi transformation.
template <typename TM, typename TV> void minEigenVector (TM& A, TV& S);
IMATH_INTERNAL_NAMESPACE_HEADER_EXIT
#endif // INCLUDED_IMATHMATRIXALGO_H
Reference in New Issue Copy Permalink