You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
824 lines
30 KiB
824 lines
30 KiB
// David Eberly, Geometric Tools, Redmond WA 98052
|
|
// Copyright (c) 1998-2021
|
|
// Distributed under the Boost Software License, Version 1.0.
|
|
// https://www.boost.org/LICENSE_1_0.txt
|
|
// https://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
|
|
// Version: 4.0.2020.01.10
|
|
|
|
#pragma once
|
|
|
|
#include <Mathematics/Logger.h>
|
|
#include <Mathematics/Matrix3x3.h>
|
|
#include <Mathematics/Matrix4x4.h>
|
|
#include <Mathematics/Rotation.h>
|
|
|
|
// Transforms when GTE_USE_MAT_VEC is defined in the preprocessor symbols.
|
|
//
|
|
// The transform is Y = M*X+T, where M is a 3-by-3 matrix and T is a 3x1
|
|
// translation. In most cases, M = R, a rotation matrix, or M = R*S,
|
|
// where R is a rotation matrix and S is a diagonal matrix whose diagonal
|
|
// entries are positive scales. To support modeling packages that allow
|
|
// general affine transforms, M can be any invertible 3x3 matrix. The vector
|
|
// X is transformed in the "forward" direction to Y. The "inverse" direction
|
|
// transforms Y to X, namely X = M^{-1}*(Y-T) in the general case. In the
|
|
// special case of M = R*S, the inverse direction is X = S^{-1}*R^t*(Y-T),
|
|
// where S^{-1} is the diagonal matrix whose diagonal entries are the
|
|
// reciprocoals of those of S and where R^t is the transpose of R. For SIMD
|
|
// support of matrix-vector and matrix-matrix multiplications, a homogeneous
|
|
// matrix H = {{M,T},{0,1}} is stored by this class. The forward transform is
|
|
// {Y,1} = H*{X,1} and the inverse transform is {X,1} = H^{-1}*{Y,1}.
|
|
|
|
// Transforms when GTE_USE_MAT_VEC is NOT defined in the preprocessor symbols.
|
|
//
|
|
// The transform is Y = T + X*M, where M is a 3-by-3 matrix and T is a 1x3
|
|
// translation. In most cases, M = R, a rotation matrix, or M = S*R,
|
|
// where R is a rotation matrix and S is a diagonal matrix whose diagonal
|
|
// entries are positive scales. To support modeling packages that allow
|
|
// general affine transforms, M can be any invertible 3x3 matrix. The vector
|
|
// X is transformed in the "forward" direction to Y. The "inverse" direction
|
|
// transforms Y to X, namely X = (Y-T)*M^{-1} in the general case. In the
|
|
// special case of M = S*R, the inverse direction is X = (Y-T)*R^t*S^{-1},
|
|
// where S^{-1} is the diagonal matrix whose diagonal entries are the
|
|
// reciprocoals of those of S and where R^t is the transpose of R. For SIMD
|
|
// support of matrix-vector and matrix-matrix multiplications, a homogeneous
|
|
// matrix H = {{M,0},{T,1}} is stored by this class. The forward transform is
|
|
// {Y,1} = {X,1}*H and the inverse transform is {X,1} = {Y,1}*H^{-1}.
|
|
|
|
// With either multiplication convention, a matrix M = R*S (GTE_USE_MAT_VEC)
|
|
// or a matrix M = S*R (!GTE_USE_VEC_MAT) is referred to as an "RS-matrix".
|
|
// The class does not provide a member function to compute the inverse of a
|
|
// transform: 'Transform GetInverse() const'. If one were to add this,
|
|
// be aware that the inverse of an RS-matrix is not generally an RS-matrix;
|
|
// that is, the inverse of R*S is S^{-1}*R^t which cannot always be factored
|
|
// as S^{-1} * R^t = R' * S'. You would need to SetMatrix using S^{-1}*R^t
|
|
// as the input.
|
|
|
|
namespace gte
|
|
{
|
|
template <typename Real>
|
|
class Transform
|
|
{
|
|
public:
|
|
// The default constructor produces the identity transformation. The
|
|
// default copy constructor and assignment operator are generated by
|
|
// the compiler.
|
|
Transform()
|
|
:
|
|
mTranslate{ (Real)0, (Real)0, (Real)0, (Real)1 },
|
|
mScale{ (Real)1, (Real)1, (Real)1, (Real)1 },
|
|
mIsIdentity(true),
|
|
mIsRSMatrix(true),
|
|
mIsUniformScale(true),
|
|
mInverseNeedsUpdate(false)
|
|
{
|
|
mHMatrix.MakeIdentity();
|
|
mInvHMatrix.MakeIdentity();
|
|
mMatrix.MakeIdentity();
|
|
}
|
|
|
|
// Implicit conversion.
|
|
inline operator Matrix4x4<Real> const& () const
|
|
{
|
|
return GetHMatrix();
|
|
}
|
|
|
|
// Set the transformation to the identity matrix.
|
|
void MakeIdentity()
|
|
{
|
|
mMatrix.MakeIdentity();
|
|
mTranslate = { (Real)0, (Real)0, (Real)0, (Real)1 };
|
|
mScale = { (Real)1, (Real)1, (Real)1, (Real)1 };
|
|
mIsIdentity = true;
|
|
mIsRSMatrix = true;
|
|
mIsUniformScale = true;
|
|
UpdateHMatrix();
|
|
}
|
|
|
|
// Set the transformation to have scales of 1.
|
|
void MakeUnitScale()
|
|
{
|
|
LogAssert(mIsRSMatrix, "Transform is not rotation-scale.");
|
|
mScale = { (Real)1, (Real)1, (Real)1, (Real)1 };
|
|
mIsUniformScale = true;
|
|
UpdateHMatrix();
|
|
}
|
|
|
|
// Hints about the structure of the transformation.
|
|
|
|
// M = I
|
|
inline bool IsIdentity() const
|
|
{
|
|
return mIsIdentity;
|
|
}
|
|
|
|
// R*S (GTE_USE_MAT_VEC defined) or S*R (GTE_USE_MAT_VEC not defined)
|
|
inline bool IsRSMatrix() const
|
|
{
|
|
return mIsRSMatrix;
|
|
}
|
|
|
|
// RS-matrix with S = c*I
|
|
inline bool IsUniformScale() const
|
|
{
|
|
return mIsRSMatrix && mIsUniformScale;
|
|
}
|
|
|
|
// Member access.
|
|
// (1) The Set* functions set the is-identity hint to false.
|
|
// (2) The SetRotate function sets the is-rsmatrix hint to true. If this
|
|
// hint is false, GetRotate triggers an assertion in debug mode.
|
|
// (3) The SetMatrix function sets the is-rsmatrix and is-uniform-scale
|
|
// hints to false.
|
|
// (4) The SetScale function sets the is-uniform-scale hint to false.
|
|
// The SetUniformScale function sets the is-uniform-scale hint to
|
|
// true. If this hint is false, GetUniformScale triggers an assertion
|
|
// in debug mode.
|
|
// (5) All Set* functions set the inverse-needs-update to true. When
|
|
// GetHInverse is called, the inverse must be computed in this case
|
|
// and the inverse-needs-update is reset to false.
|
|
|
|
// {{R,0},{0,1}}
|
|
void SetRotation(Matrix4x4<Real> const& rotate)
|
|
{
|
|
mMatrix = rotate;
|
|
mIsIdentity = false;
|
|
mIsRSMatrix = true;
|
|
UpdateHMatrix();
|
|
}
|
|
|
|
// {{M,0},{0,1}}
|
|
void SetMatrix(Matrix4x4<Real> const& matrix)
|
|
{
|
|
mMatrix = matrix;
|
|
mIsIdentity = false;
|
|
mIsRSMatrix = false;
|
|
mIsUniformScale = false;
|
|
UpdateHMatrix();
|
|
}
|
|
|
|
void SetTranslation(Real x0, Real x1, Real x2)
|
|
{
|
|
mTranslate = Vector4<Real>{ x0, x1, x2, (Real)1 };
|
|
mIsIdentity = false;
|
|
UpdateHMatrix();
|
|
}
|
|
|
|
inline void SetTranslation(Vector3<Real> const& translate)
|
|
{
|
|
SetTranslation(translate[0], translate[1], translate[2]);
|
|
}
|
|
|
|
inline void SetTranslation(Vector4<Real> const& translate)
|
|
{
|
|
SetTranslation(translate[0], translate[1], translate[2]);
|
|
}
|
|
|
|
void SetScale(Real s0, Real s1, Real s2)
|
|
{
|
|
LogAssert(mIsRSMatrix, "Transform is not rotation-scale.");
|
|
LogAssert(s0 != (Real)0 && s1 != (Real)0 && s2 != (Real)0, "Scales must be nonzero.");
|
|
mScale = { s0, s1, s2, (Real)1 };
|
|
mIsIdentity = false;
|
|
mIsUniformScale = false;
|
|
UpdateHMatrix();
|
|
}
|
|
|
|
inline void SetScale(Vector3<Real> const& scale)
|
|
{
|
|
SetScale(scale[0], scale[1], scale[2]);
|
|
}
|
|
|
|
inline void SetScale(Vector4<Real> const& scale)
|
|
{
|
|
SetScale(scale[0], scale[1], scale[2]);
|
|
}
|
|
|
|
void SetUniformScale(Real scale)
|
|
{
|
|
LogAssert(mIsRSMatrix, "Transform is not rotation-scale.");
|
|
LogAssert(scale != (Real)0, "Scale must be nonzero.");
|
|
mScale = { scale, scale, scale, (Real)1 };
|
|
mIsIdentity = false;
|
|
mIsUniformScale = true;
|
|
UpdateHMatrix();
|
|
}
|
|
|
|
// {{R,0},{0,1}}
|
|
Matrix4x4<Real> const& GetRotation() const
|
|
{
|
|
LogAssert(mIsRSMatrix, "Transform is not rotation-scale.");
|
|
return mMatrix;
|
|
}
|
|
|
|
// {{M,0},{0,1}}
|
|
inline Matrix4x4<Real> const& GetMatrix() const
|
|
{
|
|
return mMatrix;
|
|
}
|
|
|
|
// (x,y,z)
|
|
inline Vector3<Real> GetTranslation() const
|
|
{
|
|
return Vector3<Real>{ mTranslate[0], mTranslate[1], mTranslate[2] };
|
|
}
|
|
|
|
// (x,y,z,0)
|
|
inline Vector4<Real> GetTranslationW0() const
|
|
{
|
|
return Vector4<Real>{ mTranslate[0], mTranslate[1], mTranslate[2], (Real)0 };
|
|
}
|
|
|
|
// (x,y,z,1)
|
|
inline Vector4<Real> GetTranslationW1() const
|
|
{
|
|
return Vector4<Real>{ mTranslate[0], mTranslate[1], mTranslate[2], (Real)1 };
|
|
}
|
|
|
|
// (s0,s1,s2)
|
|
Vector3<Real> GetScale() const
|
|
{
|
|
LogAssert(mIsRSMatrix, "Transform is not rotation-scale.");
|
|
return Vector3<Real>{ mScale[0], mScale[1], mScale[2] };
|
|
}
|
|
|
|
// (s0,s1,s2,1)
|
|
Vector4<Real> GetScaleW1() const
|
|
{
|
|
LogAssert(mIsRSMatrix, "Transform is not rotation-scale.");
|
|
return Vector4<Real>{ mScale[0], mScale[1], mScale[2], (Real)1 };
|
|
}
|
|
|
|
Real GetUniformScale() const
|
|
{
|
|
LogAssert(mIsRSMatrix, "Transform is not rotation-scale.");
|
|
LogAssert(mIsUniformScale, "Transform is not uniform scale.");
|
|
return mScale[0];
|
|
}
|
|
|
|
// Alternate representations to set/get the rotation.
|
|
|
|
// Set/get from 3x3 matrices.
|
|
void SetRotation(Matrix3x3<Real> const& rotate)
|
|
{
|
|
mMatrix.MakeIdentity();
|
|
for (int r = 0; r < 3; ++r)
|
|
{
|
|
for (int c = 0; c < 3; ++c)
|
|
{
|
|
mMatrix(r, c) = rotate(r, c);
|
|
}
|
|
}
|
|
|
|
mIsIdentity = false;
|
|
mIsRSMatrix = true;
|
|
UpdateHMatrix();
|
|
}
|
|
|
|
void GetRotation(Matrix3x3<Real>& rotate) const
|
|
{
|
|
LogAssert(mIsRSMatrix, "Transform is not rotation-scale.");
|
|
for (int r = 0; r < 3; ++r)
|
|
{
|
|
for (int c = 0; c < 3; ++c)
|
|
{
|
|
rotate(r, c) = mMatrix(r, c);
|
|
}
|
|
}
|
|
}
|
|
|
|
// The quaternion is unit length.
|
|
void SetRotation(Quaternion<Real> const& q)
|
|
{
|
|
mMatrix = Rotation<4, Real>(q);
|
|
mIsIdentity = false;
|
|
mIsRSMatrix = true;
|
|
UpdateHMatrix();
|
|
}
|
|
|
|
void GetRotation(Quaternion<Real>& q) const
|
|
{
|
|
LogAssert(mIsRSMatrix, "Transform is not rotation-scale.");
|
|
q = Rotation<4, Real>(mMatrix);
|
|
}
|
|
|
|
// The axis is unit length and the angle is in radians.
|
|
void SetRotation(AxisAngle<3, Real> const& axisAngle)
|
|
{
|
|
AxisAngle<4, Real> aa4(HLift(axisAngle.axis, (Real)1), axisAngle.angle);
|
|
mMatrix = Rotation<4, Real>(aa4);
|
|
mIsIdentity = false;
|
|
mIsRSMatrix = true;
|
|
UpdateHMatrix();
|
|
}
|
|
|
|
void GetRotation(AxisAngle<3, Real>& axisAngle) const
|
|
{
|
|
LogAssert(mIsRSMatrix, "Transform is not rotation-scale.");
|
|
AxisAngle<4, Real> aa4 = Rotation<4, Real>(mMatrix);
|
|
axisAngle.axis = HProject(aa4.axis);
|
|
axisAngle.angle = aa4.angle;
|
|
}
|
|
|
|
void SetRotation(AxisAngle<4, Real> const& axisAngle)
|
|
{
|
|
mMatrix = Rotation<4, Real>(axisAngle);
|
|
mIsIdentity = false;
|
|
mIsRSMatrix = true;
|
|
UpdateHMatrix();
|
|
}
|
|
|
|
void GetRotation(AxisAngle<4, Real>& axisAngle) const
|
|
{
|
|
LogAssert(mIsRSMatrix, "Transform is not rotation-scale.");
|
|
axisAngle = Rotation<4, Real>(mMatrix);
|
|
}
|
|
|
|
// The Euler angles are in radians. The GetEulerAngles function
|
|
// expects the eulerAngles.axis[] values to be set to the axis order
|
|
// you want.
|
|
void SetRotation(EulerAngles<Real> const& eulerAngles)
|
|
{
|
|
mMatrix = Rotation<4, Real>(eulerAngles);
|
|
mIsIdentity = false;
|
|
mIsRSMatrix = true;
|
|
UpdateHMatrix();
|
|
}
|
|
|
|
void GetRotation(EulerAngles<Real>& eulerAngles) const
|
|
{
|
|
LogAssert(mIsRSMatrix, "Transform is not rotation-scale.");
|
|
eulerAngles = Rotation<4, Real>(mMatrix)(eulerAngles.axis[0],
|
|
eulerAngles.axis[1], eulerAngles.axis[2]);
|
|
}
|
|
|
|
// For M = R*S or M = S*R, the largest value of S in absolute value is
|
|
// returned. For general M, the max-row-sum norm is returned when
|
|
// GTE_USE_MAT_VEC is defined or the max-col-sum norm is returned when
|
|
// the GTE_USE_MAT_VEC is not defined, which is a reasonable measure
|
|
// of maximum scale of the transformation.
|
|
Real GetNorm() const
|
|
{
|
|
Real sum0, sum1, sum2;
|
|
|
|
if (mIsRSMatrix)
|
|
{
|
|
// A RS matrix (GTE_USE_MAT_VEC defined) or an SR matrix
|
|
// (GTE_USE_MAT_VEC is not defined).
|
|
sum0 = std::fabs(mScale[0]);
|
|
sum1 = std::fabs(mScale[1]);
|
|
sum2 = std::fabs(mScale[2]);
|
|
}
|
|
else
|
|
{
|
|
// The spectral norm (the maximum absolute value of the
|
|
// eigenvalues) is smaller or equal to this norm. Therefore,
|
|
// this function returns an approximation to the maximum
|
|
// scale.
|
|
|
|
#if defined(GTE_USE_MAT_VEC)
|
|
// Use the max-row-sum matrix norm.
|
|
sum0 = std::fabs(mMatrix(0, 0)) + std::fabs(mMatrix(0, 1)) + std::fabs(mMatrix(0, 2));
|
|
sum1 = std::fabs(mMatrix(1, 0)) + std::fabs(mMatrix(1, 1)) + std::fabs(mMatrix(1, 2));
|
|
sum2 = std::fabs(mMatrix(2, 0)) + std::fabs(mMatrix(2, 1)) + std::fabs(mMatrix(2, 2));
|
|
#else
|
|
// Use the max-col-sum matrix norm.
|
|
sum0 = std::fabs(mMatrix(0, 0)) + std::fabs(mMatrix(1, 0)) + std::fabs(mMatrix(2, 0));
|
|
sum1 = std::fabs(mMatrix(0, 1)) + std::fabs(mMatrix(1, 1)) + std::fabs(mMatrix(2, 1));
|
|
sum2 = std::fabs(mMatrix(0, 2)) + std::fabs(mMatrix(1, 2)) + std::fabs(mMatrix(2, 2));
|
|
#endif
|
|
}
|
|
|
|
return std::max(std::max(sum0, sum1), sum2);
|
|
}
|
|
|
|
// Get the homogeneous matrix (composite of all channels).
|
|
inline Matrix4x4<Real> const& GetHMatrix() const
|
|
{
|
|
return mHMatrix;
|
|
}
|
|
|
|
// Get the inverse homogeneous matrix, recomputing it when necessary.
|
|
// GTE_USE_MAT_VEC
|
|
// H = {{M,T},{0,1}}, then H^{-1} = {{M^{-1},-M^{-1}*T},{0,1}}
|
|
// GTE_USE_VEC_MAT
|
|
// H = {{M,0},{T,1}}, then H^{-1} = {{M^{-1},0},{-M^{-1}*T,1}}
|
|
Matrix4x4<Real> const& GetHInverse() const
|
|
{
|
|
if (mInverseNeedsUpdate)
|
|
{
|
|
if (mIsIdentity)
|
|
{
|
|
mInvHMatrix.MakeIdentity();
|
|
}
|
|
else
|
|
{
|
|
if (mIsRSMatrix)
|
|
{
|
|
if (mIsUniformScale)
|
|
{
|
|
Real invScale = (Real)1 / mScale[0];
|
|
#if defined(GTE_USE_MAT_VEC)
|
|
mInvHMatrix(0, 0) = invScale * mMatrix(0, 0);
|
|
mInvHMatrix(0, 1) = invScale * mMatrix(1, 0);
|
|
mInvHMatrix(0, 2) = invScale * mMatrix(2, 0);
|
|
mInvHMatrix(1, 0) = invScale * mMatrix(0, 1);
|
|
mInvHMatrix(1, 1) = invScale * mMatrix(1, 1);
|
|
mInvHMatrix(1, 2) = invScale * mMatrix(2, 1);
|
|
mInvHMatrix(2, 0) = invScale * mMatrix(0, 2);
|
|
mInvHMatrix(2, 1) = invScale * mMatrix(1, 2);
|
|
mInvHMatrix(2, 2) = invScale * mMatrix(2, 2);
|
|
#else
|
|
mInvHMatrix(0, 0) = mMatrix(0, 0) * invScale;
|
|
mInvHMatrix(0, 1) = mMatrix(1, 0) * invScale;
|
|
mInvHMatrix(0, 2) = mMatrix(2, 0) * invScale;
|
|
mInvHMatrix(1, 0) = mMatrix(0, 1) * invScale;
|
|
mInvHMatrix(1, 1) = mMatrix(1, 1) * invScale;
|
|
mInvHMatrix(1, 2) = mMatrix(2, 1) * invScale;
|
|
mInvHMatrix(2, 0) = mMatrix(0, 2) * invScale;
|
|
mInvHMatrix(2, 1) = mMatrix(1, 2) * invScale;
|
|
mInvHMatrix(2, 2) = mMatrix(2, 2) * invScale;
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
// Replace 3 reciprocals by 6 multiplies and
|
|
// 1 reciprocal.
|
|
Real s01 = mScale[0] * mScale[1];
|
|
Real s02 = mScale[0] * mScale[2];
|
|
Real s12 = mScale[1] * mScale[2];
|
|
Real invs012 = (Real)1 / (s01 * mScale[2]);
|
|
Real invS0 = s12 * invs012;
|
|
Real invS1 = s02 * invs012;
|
|
Real invS2 = s01 * invs012;
|
|
#if defined(GTE_USE_MAT_VEC)
|
|
mInvHMatrix(0, 0) = invS0 * mMatrix(0, 0);
|
|
mInvHMatrix(0, 1) = invS0 * mMatrix(1, 0);
|
|
mInvHMatrix(0, 2) = invS0 * mMatrix(2, 0);
|
|
mInvHMatrix(1, 0) = invS1 * mMatrix(0, 1);
|
|
mInvHMatrix(1, 1) = invS1 * mMatrix(1, 1);
|
|
mInvHMatrix(1, 2) = invS1 * mMatrix(2, 1);
|
|
mInvHMatrix(2, 0) = invS2 * mMatrix(0, 2);
|
|
mInvHMatrix(2, 1) = invS2 * mMatrix(1, 2);
|
|
mInvHMatrix(2, 2) = invS2 * mMatrix(2, 2);
|
|
#else
|
|
mInvHMatrix(0, 0) = mMatrix(0, 0) * invS0;
|
|
mInvHMatrix(0, 1) = mMatrix(1, 0) * invS1;
|
|
mInvHMatrix(0, 2) = mMatrix(2, 0) * invS2;
|
|
mInvHMatrix(1, 0) = mMatrix(0, 1) * invS0;
|
|
mInvHMatrix(1, 1) = mMatrix(1, 1) * invS1;
|
|
mInvHMatrix(1, 2) = mMatrix(2, 1) * invS2;
|
|
mInvHMatrix(2, 0) = mMatrix(0, 2) * invS0;
|
|
mInvHMatrix(2, 1) = mMatrix(1, 2) * invS1;
|
|
mInvHMatrix(2, 2) = mMatrix(2, 2) * invS2;
|
|
#endif
|
|
}
|
|
}
|
|
else
|
|
{
|
|
Invert3x3(mHMatrix, mInvHMatrix);
|
|
}
|
|
|
|
#if defined(GTE_USE_MAT_VEC)
|
|
mInvHMatrix(0, 3) = -(
|
|
mInvHMatrix(0, 0) * mTranslate[0] +
|
|
mInvHMatrix(0, 1) * mTranslate[1] +
|
|
mInvHMatrix(0, 2) * mTranslate[2]
|
|
);
|
|
|
|
mInvHMatrix(1, 3) = -(
|
|
mInvHMatrix(1, 0) * mTranslate[0] +
|
|
mInvHMatrix(1, 1) * mTranslate[1] +
|
|
mInvHMatrix(1, 2) * mTranslate[2]
|
|
);
|
|
|
|
mInvHMatrix(2, 3) = -(
|
|
mInvHMatrix(2, 0) * mTranslate[0] +
|
|
mInvHMatrix(2, 1) * mTranslate[1] +
|
|
mInvHMatrix(2, 2) * mTranslate[2]
|
|
);
|
|
|
|
// The last row of mHMatrix is always (0,0,0,1) for an
|
|
// affine transformation, so it is set once in the
|
|
// constructor. It is not necessary to reset it here.
|
|
#else
|
|
mInvHMatrix(3, 0) = -(
|
|
mInvHMatrix(0, 0) * mTranslate[0] +
|
|
mInvHMatrix(1, 0) * mTranslate[1] +
|
|
mInvHMatrix(2, 0) * mTranslate[2]
|
|
);
|
|
|
|
mInvHMatrix(3, 1) = -(
|
|
mInvHMatrix(0, 1) * mTranslate[0] +
|
|
mInvHMatrix(1, 1) * mTranslate[1] +
|
|
mInvHMatrix(2, 1) * mTranslate[2]
|
|
);
|
|
|
|
mInvHMatrix(3, 2) = -(
|
|
mInvHMatrix(0, 2) * mTranslate[0] +
|
|
mInvHMatrix(1, 2) * mTranslate[1] +
|
|
mInvHMatrix(2, 2) * mTranslate[2]
|
|
);
|
|
|
|
// The last column of mHMatrix is always (0,0,0,1) for an
|
|
// affine transformation, so it is set once in the
|
|
// constructor. It is not necessary to reset it here.
|
|
#endif
|
|
}
|
|
|
|
mInverseNeedsUpdate = false;
|
|
}
|
|
|
|
return mInvHMatrix;
|
|
}
|
|
|
|
// Invert the transform. If possible, the channels are properly
|
|
// assigned. For example, if the input has mIsRSMatrix equal to
|
|
// 'true', then the inverse also has mIsRSMatrix equal to 'true'
|
|
// and the inverse's mMatrix is a rotation matrix and mScale is
|
|
// set accordingly.
|
|
Transform Inverse() const
|
|
{
|
|
Transform inverse;
|
|
|
|
if (mIsIdentity)
|
|
{
|
|
inverse.MakeIdentity();
|
|
}
|
|
else
|
|
{
|
|
if (mIsRSMatrix && mIsUniformScale)
|
|
{
|
|
inverse.SetRotation(Transpose(GetRotation()));
|
|
inverse.SetUniformScale(1.0f / GetUniformScale());
|
|
}
|
|
else
|
|
{
|
|
Matrix4x4<Real> invUpper;
|
|
Invert3x3(GetMatrix(), invUpper);
|
|
inverse.SetMatrix(invUpper);
|
|
}
|
|
Vector4<Real> trn = -GetTranslationW0();
|
|
inverse.SetTranslation(inverse.GetMatrix() * trn);
|
|
}
|
|
|
|
mInverseNeedsUpdate = true;
|
|
return inverse;
|
|
}
|
|
|
|
// The identity transformation.
|
|
static Transform Identity()
|
|
{
|
|
static Transform identity;
|
|
return identity;
|
|
}
|
|
|
|
private:
|
|
// Fill in the entries of mHMatrix whenever one of the components
|
|
// mMatrix, mTranslate, or mScale changes.
|
|
void UpdateHMatrix()
|
|
{
|
|
if (mIsIdentity)
|
|
{
|
|
mHMatrix.MakeIdentity();
|
|
}
|
|
else
|
|
{
|
|
if (mIsRSMatrix)
|
|
{
|
|
#if defined(GTE_USE_MAT_VEC)
|
|
mHMatrix(0, 0) = mMatrix(0, 0) * mScale[0];
|
|
mHMatrix(0, 1) = mMatrix(0, 1) * mScale[1];
|
|
mHMatrix(0, 2) = mMatrix(0, 2) * mScale[2];
|
|
mHMatrix(1, 0) = mMatrix(1, 0) * mScale[0];
|
|
mHMatrix(1, 1) = mMatrix(1, 1) * mScale[1];
|
|
mHMatrix(1, 2) = mMatrix(1, 2) * mScale[2];
|
|
mHMatrix(2, 0) = mMatrix(2, 0) * mScale[0];
|
|
mHMatrix(2, 1) = mMatrix(2, 1) * mScale[1];
|
|
mHMatrix(2, 2) = mMatrix(2, 2) * mScale[2];
|
|
#else
|
|
mHMatrix(0, 0) = mScale[0] * mMatrix(0, 0);
|
|
mHMatrix(0, 1) = mScale[0] * mMatrix(0, 1);
|
|
mHMatrix(0, 2) = mScale[0] * mMatrix(0, 2);
|
|
mHMatrix(1, 0) = mScale[1] * mMatrix(1, 0);
|
|
mHMatrix(1, 1) = mScale[1] * mMatrix(1, 1);
|
|
mHMatrix(1, 2) = mScale[1] * mMatrix(1, 2);
|
|
mHMatrix(2, 0) = mScale[2] * mMatrix(2, 0);
|
|
mHMatrix(2, 1) = mScale[2] * mMatrix(2, 1);
|
|
mHMatrix(2, 2) = mScale[2] * mMatrix(2, 2);
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
mHMatrix(0, 0) = mMatrix(0, 0);
|
|
mHMatrix(0, 1) = mMatrix(0, 1);
|
|
mHMatrix(0, 2) = mMatrix(0, 2);
|
|
mHMatrix(1, 0) = mMatrix(1, 0);
|
|
mHMatrix(1, 1) = mMatrix(1, 1);
|
|
mHMatrix(1, 2) = mMatrix(1, 2);
|
|
mHMatrix(2, 0) = mMatrix(2, 0);
|
|
mHMatrix(2, 1) = mMatrix(2, 1);
|
|
mHMatrix(2, 2) = mMatrix(2, 2);
|
|
}
|
|
|
|
#if defined(GTE_USE_MAT_VEC)
|
|
mHMatrix(0, 3) = mTranslate[0];
|
|
mHMatrix(1, 3) = mTranslate[1];
|
|
mHMatrix(2, 3) = mTranslate[2];
|
|
|
|
// The last row of mHMatrix is always (0,0,0,1) for an affine
|
|
// transformation, so it is set once in the constructor. It
|
|
// is not necessary to reset it here.
|
|
#else
|
|
mHMatrix(3, 0) = mTranslate[0];
|
|
mHMatrix(3, 1) = mTranslate[1];
|
|
mHMatrix(3, 2) = mTranslate[2];
|
|
|
|
// The last column of mHMatrix is always (0,0,0,1) for an
|
|
// affine transformation, so it is set once in the
|
|
// constructor. It is not necessary to reset it here.
|
|
#endif
|
|
}
|
|
|
|
mInverseNeedsUpdate = true;
|
|
}
|
|
|
|
// Invert the 3x3 upper-left block of the input matrix.
|
|
static void Invert3x3(Matrix4x4<Real> const& mat, Matrix4x4<Real>& invMat)
|
|
{
|
|
// Compute the adjoint of M (3x3).
|
|
invMat(0, 0) = mat(1, 1) * mat(2, 2) - mat(1, 2) * mat(2, 1);
|
|
invMat(0, 1) = mat(0, 2) * mat(2, 1) - mat(0, 1) * mat(2, 2);
|
|
invMat(0, 2) = mat(0, 1) * mat(1, 2) - mat(0, 2) * mat(1, 1);
|
|
invMat(0, 3) = 0.0f;
|
|
invMat(1, 0) = mat(1, 2) * mat(2, 0) - mat(1, 0) * mat(2, 2);
|
|
invMat(1, 1) = mat(0, 0) * mat(2, 2) - mat(0, 2) * mat(2, 0);
|
|
invMat(1, 2) = mat(0, 2) * mat(1, 0) - mat(0, 0) * mat(1, 2);
|
|
invMat(1, 3) = 0.0f;
|
|
invMat(2, 0) = mat(1, 0) * mat(2, 1) - mat(1, 1) * mat(2, 0);
|
|
invMat(2, 1) = mat(0, 1) * mat(2, 0) - mat(0, 0) * mat(2, 1);
|
|
invMat(2, 2) = mat(0, 0) * mat(1, 1) - mat(0, 1) * mat(1, 0);
|
|
invMat(2, 3) = 0.0f;
|
|
invMat(3, 0) = 0.0f;
|
|
invMat(3, 1) = 0.0f;
|
|
invMat(3, 2) = 0.0f;
|
|
invMat(3, 3) = 1.0f;
|
|
|
|
// Compute the reciprocal of the determinant of M.
|
|
Real invDet = (Real)1 / (
|
|
mat(0, 0) * invMat(0, 0) +
|
|
mat(0, 1) * invMat(1, 0) +
|
|
mat(0, 2) * invMat(2, 0)
|
|
);
|
|
|
|
// inverse(M) = adjoint(M)/determinant(M).
|
|
invMat(0, 0) *= invDet;
|
|
invMat(0, 1) *= invDet;
|
|
invMat(0, 2) *= invDet;
|
|
invMat(1, 0) *= invDet;
|
|
invMat(1, 1) *= invDet;
|
|
invMat(1, 2) *= invDet;
|
|
invMat(2, 0) *= invDet;
|
|
invMat(2, 1) *= invDet;
|
|
invMat(2, 2) *= invDet;
|
|
}
|
|
|
|
// The full 4x4 homogeneous matrix H and its inverse H^{-1}, stored
|
|
// according to the conventions (see GetHInverse description). The
|
|
// inverse is computed only on demand.
|
|
Matrix4x4<Real> mHMatrix;
|
|
mutable Matrix4x4<Real> mInvHMatrix;
|
|
|
|
Matrix4x4<Real> mMatrix; // M (general) or R (rotation)
|
|
Vector4<Real> mTranslate; // T
|
|
Vector4<Real> mScale; // S
|
|
bool mIsIdentity, mIsRSMatrix, mIsUniformScale;
|
|
mutable bool mInverseNeedsUpdate;
|
|
};
|
|
|
|
// Compute M*V.
|
|
template <typename Real>
|
|
Vector4<Real> operator*(Transform<Real> const& M, Vector4<Real> const& V)
|
|
{
|
|
return M.GetHMatrix() * V;
|
|
}
|
|
|
|
// Compute V^T*M.
|
|
template <typename Real>
|
|
Vector4<Real> operator*(Vector4<Real> const& V, Transform<Real> const& M)
|
|
{
|
|
return V * M.GetHMatrix();
|
|
}
|
|
|
|
// Compute A*B.
|
|
template <typename Real>
|
|
Transform<Real> operator*(Transform<Real> const& A, Transform<Real> const& B)
|
|
{
|
|
if (A.IsIdentity())
|
|
{
|
|
return B;
|
|
}
|
|
|
|
if (B.IsIdentity())
|
|
{
|
|
return A;
|
|
}
|
|
|
|
Transform<Real> product;
|
|
|
|
if (A.IsRSMatrix() && B.IsRSMatrix())
|
|
{
|
|
#if defined(GTE_USE_MAT_VEC)
|
|
if (A.IsUniformScale())
|
|
{
|
|
product.SetRotation(A.GetRotation() * B.GetRotation());
|
|
|
|
product.SetTranslation(A.GetUniformScale() * (
|
|
A.GetRotation() * B.GetTranslationW0()) +
|
|
A.GetTranslationW1());
|
|
|
|
if (B.IsUniformScale())
|
|
{
|
|
product.SetUniformScale(A.GetUniformScale() * B.GetUniformScale());
|
|
}
|
|
else
|
|
{
|
|
product.SetScale(A.GetUniformScale() * B.GetScale());
|
|
}
|
|
|
|
return product;
|
|
}
|
|
#else
|
|
if (B.IsUniformScale())
|
|
{
|
|
product.SetRotation(A.GetRotation() * B.GetRotation());
|
|
|
|
product.SetTranslation(B.GetUniformScale() * (
|
|
A.GetTranslationW0() * B.GetRotation()) +
|
|
B.GetTranslationW1());
|
|
|
|
if (A.IsUniformScale())
|
|
{
|
|
product.SetUniformScale(A.GetUniformScale() * B.GetUniformScale());
|
|
}
|
|
else
|
|
{
|
|
product.SetScale(A.GetScale() * B.GetUniformScale());
|
|
}
|
|
|
|
return product;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
// In all remaining cases, the matrix cannot be written as R*S*X+T.
|
|
Matrix4x4<Real> matMA;
|
|
if (A.IsRSMatrix())
|
|
{
|
|
#if defined(GTE_USE_MAT_VEC)
|
|
matMA = MultiplyMD(A.GetRotation(), A.GetScaleW1());
|
|
#else
|
|
matMA = MultiplyDM(A.GetScaleW1(), A.GetRotation());
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
matMA = A.GetMatrix();
|
|
}
|
|
|
|
Matrix4x4<Real> matMB;
|
|
if (B.IsRSMatrix())
|
|
{
|
|
#if defined(GTE_USE_MAT_VEC)
|
|
matMB = MultiplyMD(B.GetRotation(), B.GetScaleW1());
|
|
#else
|
|
matMB = MultiplyDM(B.GetScaleW1(), B.GetRotation());
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
matMB = B.GetMatrix();
|
|
}
|
|
|
|
product.SetMatrix(matMA * matMB);
|
|
#if defined(GTE_USE_MAT_VEC)
|
|
product.SetTranslation(matMA * B.GetTranslationW0() +
|
|
A.GetTranslationW1());
|
|
#else
|
|
product.SetTranslation(A.GetTranslationW0() * matMB +
|
|
B.GetTranslationW1());
|
|
#endif
|
|
return product;
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Matrix4x4<Real> operator*(Matrix4x4<Real> const& A, Transform<Real> const& B)
|
|
{
|
|
return A * B.GetHMatrix();
|
|
}
|
|
|
|
template <typename Real>
|
|
inline Matrix4x4<Real> operator*(Transform<Real> const& A, Matrix4x4<Real> const& B)
|
|
{
|
|
return A.GetHMatrix()* B;
|
|
}
|
|
}
|
|
|