589 lines
18 KiB
C++
589 lines
18 KiB
C++
// Copyright Epic Games, Inc. All Rights Reserved.
|
|
|
|
#pragma once
|
|
|
|
#include "Math/Quat.h" // Engine types
|
|
#include "VectorTypes.h"
|
|
#include "MatrixTypes.h"
|
|
#include "IndexTypes.h"
|
|
|
|
// ported from geometry3Sharp Quaternion
|
|
|
|
namespace UE
|
|
{
|
|
namespace Geometry
|
|
{
|
|
|
|
using namespace UE::Math;
|
|
|
|
|
|
template<typename RealType>
|
|
struct TQuaternion
|
|
{
|
|
union
|
|
{
|
|
struct
|
|
{
|
|
// note: in Wm5 version, this is a 4-element arraY stored in order (w,x,y,z).
|
|
RealType X;
|
|
RealType Y;
|
|
RealType Z;
|
|
RealType W;
|
|
};
|
|
|
|
UE_DEPRECATED(all, "For internal use only")
|
|
RealType XYZW[4];
|
|
};
|
|
|
|
TQuaternion();
|
|
TQuaternion(RealType X, RealType Y, RealType Z, RealType W);
|
|
explicit TQuaternion(const RealType* Values);
|
|
PRAGMA_DISABLE_DEPRECATION_WARNINGS
|
|
TQuaternion(const TQuaternion& Copy) = default;
|
|
TQuaternion& operator=(const TQuaternion& Copy) = default;
|
|
PRAGMA_ENABLE_DEPRECATION_WARNINGS
|
|
template<typename RealType2>
|
|
explicit TQuaternion(const TQuaternion<RealType2>& Copy);
|
|
TQuaternion(const TVector<RealType>& Axis, RealType Angle, bool bAngleIsDegrees);
|
|
TQuaternion(const TVector<RealType>& From, const TVector<RealType>& To);
|
|
TQuaternion(const TQuaternion<RealType>& From, const TQuaternion<RealType>& To, RealType InterpT);
|
|
TQuaternion(const TMatrix3<RealType>& RotationMatrix);
|
|
|
|
void SetAxisAngleD(const TVector<RealType>& Axis, RealType AngleDeg);
|
|
void SetAxisAngleR(const TVector<RealType>& Axis, RealType AngleRad);
|
|
void SetFromTo(const TVector<RealType>& From, const TVector<RealType>& To);
|
|
void SetToSlerp(TQuaternion<RealType> From, TQuaternion<RealType> To, RealType InterpT);
|
|
void SetFromRotationMatrix(const TMatrix3<RealType>& RotationMatrix);
|
|
|
|
static TQuaternion<RealType> Zero() { return TQuaternion<RealType>((RealType)0, (RealType)0, (RealType)0, (RealType)0); }
|
|
static TQuaternion<RealType> Identity() { return TQuaternion<RealType>((RealType)0, (RealType)0, (RealType)0, (RealType)1); }
|
|
|
|
RealType& operator[](int i)
|
|
{
|
|
PRAGMA_DISABLE_DEPRECATION_WARNINGS
|
|
return XYZW[i];
|
|
PRAGMA_ENABLE_DEPRECATION_WARNINGS
|
|
}
|
|
const RealType& operator[](int i) const
|
|
{
|
|
PRAGMA_DISABLE_DEPRECATION_WARNINGS
|
|
return XYZW[i];
|
|
PRAGMA_ENABLE_DEPRECATION_WARNINGS
|
|
}
|
|
|
|
// Test whether quaternions represent the same rotation w/in a tolerance. (Note: Considers the negative representation of the same rotation as equal.)
|
|
bool EpsilonEqual(const TQuaternion<RealType>& Other, RealType Tolerance = TMathUtil<RealType>::ZeroTolerance) const;
|
|
// Test whether the quaternion is EpsilonEqual to the identity quaternion (i.e., W=1 or W=-1, within tolerance)
|
|
bool IsIdentity(RealType Tolerance = TMathUtil<RealType>::ZeroTolerance) const
|
|
{
|
|
return EpsilonEqual(Identity(), Tolerance);
|
|
}
|
|
|
|
RealType Length() const { return (RealType)sqrt(X*X + Y*Y + Z*Z + W*W); }
|
|
RealType SquaredLength() const { return X*X + Y*Y + Z*Z + W*W; }
|
|
|
|
TVector<RealType> AxisX() const;
|
|
TVector<RealType> AxisY() const;
|
|
TVector<RealType> AxisZ() const;
|
|
void GetAxes(TVector<RealType>& X, TVector<RealType>& Y, TVector<RealType>& Z) const;
|
|
|
|
RealType Normalize(const RealType epsilon = 0);
|
|
TQuaternion<RealType> Normalized(const RealType epsilon = 0) const;
|
|
|
|
RealType Dot(const TQuaternion<RealType>& Other) const;
|
|
|
|
TQuaternion<RealType> Inverse() const;
|
|
TVector<RealType> InverseMultiply(const TVector<RealType>& Other) const;
|
|
|
|
TMatrix3<RealType> ToRotationMatrix() const;
|
|
|
|
constexpr TQuaternion<RealType> operator-() const
|
|
{
|
|
return TQuaternion<RealType>(-X, -Y, -Z, -W);
|
|
}
|
|
|
|
|
|
// available for porting:
|
|
// SetFromRotationMatrix(FMatrix3f)
|
|
|
|
|
|
explicit inline operator FQuat4f() const
|
|
{
|
|
FQuat4f Quat;
|
|
Quat.X = (float)X;
|
|
Quat.Y = (float)Y;
|
|
Quat.Z = (float)Z;
|
|
Quat.W = (float)W;
|
|
return Quat;
|
|
}
|
|
explicit inline operator FQuat4d() const
|
|
{
|
|
FQuat4d Quat;
|
|
Quat.X = (double)X;
|
|
Quat.Y = (double)Y;
|
|
Quat.Z = (double)Z;
|
|
Quat.W = (double)W;
|
|
return Quat;
|
|
}
|
|
explicit inline operator FRotator() const
|
|
{
|
|
return ((FQuat)*this).Rotator();
|
|
}
|
|
explicit inline TQuaternion(const FQuat4f& Quat)
|
|
{
|
|
X = (RealType)Quat.X;
|
|
Y = (RealType)Quat.Y;
|
|
Z = (RealType)Quat.Z;
|
|
W = (RealType)Quat.W;
|
|
}
|
|
explicit inline TQuaternion(const FQuat4d& Quat)
|
|
{
|
|
X = (RealType)Quat.X;
|
|
Y = (RealType)Quat.Y;
|
|
Z = (RealType)Quat.Z;
|
|
W = (RealType)Quat.W;
|
|
}
|
|
explicit inline TQuaternion(const FRotator& Rotator)
|
|
{
|
|
FQuat4d Quat(Rotator);
|
|
X = (RealType)Quat.X;
|
|
Y = (RealType)Quat.Y;
|
|
Z = (RealType)Quat.Z;
|
|
W = (RealType)Quat.W;
|
|
}
|
|
explicit inline operator UE::Math::TVector4<RealType>() const
|
|
{
|
|
return UE::Math::TVector4<RealType>(X,Y,Z,W);
|
|
}
|
|
};
|
|
|
|
|
|
|
|
typedef TQuaternion<float> FQuaternionf;
|
|
typedef TQuaternion<double> FQuaterniond;
|
|
|
|
|
|
|
|
|
|
template<typename RealType>
|
|
TQuaternion<RealType>::TQuaternion()
|
|
{
|
|
X = Y = Z = 0; W = 1;
|
|
}
|
|
|
|
template<typename RealType>
|
|
TQuaternion<RealType>::TQuaternion(RealType x, RealType y, RealType z, RealType w)
|
|
{
|
|
X = x;
|
|
Y = y;
|
|
Z = z;
|
|
W = w;
|
|
}
|
|
|
|
template<typename RealType>
|
|
TQuaternion<RealType>::TQuaternion(const RealType* Values)
|
|
{
|
|
X = Values[0];
|
|
Y = Values[1];
|
|
Z = Values[2];
|
|
W = Values[3];
|
|
}
|
|
|
|
template<typename RealType>
|
|
template<typename RealType2>
|
|
TQuaternion<RealType>::TQuaternion(const TQuaternion<RealType2>& Copy)
|
|
{
|
|
X = (RealType)Copy.X;
|
|
Y = (RealType)Copy.Y;
|
|
Z = (RealType)Copy.Z;
|
|
W = (RealType)Copy.W;
|
|
}
|
|
|
|
template<typename RealType>
|
|
TQuaternion<RealType>::TQuaternion(const TVector<RealType>& Axis, RealType Angle, bool bAngleIsDegrees)
|
|
{
|
|
X = Y = Z = 0; W = 1;
|
|
SetAxisAngleR(Axis, Angle * (bAngleIsDegrees ? TMathUtil<RealType>::DegToRad : (RealType)1));
|
|
}
|
|
|
|
template<typename RealType>
|
|
TQuaternion<RealType>::TQuaternion(const TVector<RealType>& From, const TVector<RealType>& To)
|
|
{
|
|
X = Y = Z = 0; W = 1;
|
|
SetFromTo(From, To);
|
|
}
|
|
|
|
template<typename RealType>
|
|
TQuaternion<RealType>::TQuaternion(const TQuaternion<RealType>& From, const TQuaternion<RealType>& To, RealType InterpT)
|
|
{
|
|
X = Y = Z = 0; W = 1;
|
|
SetToSlerp(From, To, InterpT);
|
|
}
|
|
|
|
template<typename RealType>
|
|
TQuaternion<RealType>::TQuaternion(const TMatrix3<RealType>& RotationMatrix)
|
|
{
|
|
X = Y = Z = 0; W = 1;
|
|
SetFromRotationMatrix(RotationMatrix);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
template<typename RealType>
|
|
RealType TQuaternion<RealType>::Normalize(const RealType Epsilon)
|
|
{
|
|
RealType length = Length();
|
|
if (length > Epsilon)
|
|
{
|
|
RealType invLength = ((RealType)1) / length;
|
|
X *= invLength;
|
|
Y *= invLength;
|
|
Z *= invLength;
|
|
W *= invLength;
|
|
return invLength;
|
|
}
|
|
X = Y = Z = W = (RealType)0;
|
|
return 0;
|
|
}
|
|
|
|
template<typename RealType>
|
|
TQuaternion<RealType> TQuaternion<RealType>::Normalized(const RealType Epsilon) const
|
|
{
|
|
RealType length = Length();
|
|
if (length > Epsilon)
|
|
{
|
|
RealType invLength = ((RealType)1) / length;
|
|
return TQuaternion<RealType>(X*invLength, Y*invLength, Z*invLength, W*invLength);
|
|
}
|
|
return Zero();
|
|
}
|
|
|
|
template<typename RealType>
|
|
RealType TQuaternion<RealType>::Dot(const TQuaternion<RealType>& Other) const
|
|
{
|
|
return X * Other.X + Y * Other.Y + Z * Other.Z + W * Other.W;
|
|
}
|
|
|
|
|
|
template<typename RealType>
|
|
TQuaternion<RealType> operator*(const TQuaternion<RealType>& A, const TQuaternion<RealType>& B)
|
|
{
|
|
RealType W = A.W * B.W - A.X * B.X - A.Y * B.Y - A.Z * B.Z;
|
|
RealType X = A.W * B.X + A.X * B.W + A.Y * B.Z - A.Z * B.Y;
|
|
RealType Y = A.W * B.Y + A.Y * B.W + A.Z * B.X - A.X * B.Z;
|
|
RealType Z = A.W * B.Z + A.Z * B.W + A.X * B.Y - A.Y * B.X;
|
|
return TQuaternion<RealType>(X, Y, Z, W);
|
|
}
|
|
|
|
|
|
template<typename RealType>
|
|
TQuaternion<RealType> operator*(RealType Scalar, const TQuaternion<RealType>& Q)
|
|
{
|
|
return TQuaternion<RealType>(Scalar * Q.X, Scalar * Q.Y, Scalar * Q.Z, Scalar * Q.W);
|
|
}
|
|
|
|
template<typename RealType>
|
|
TQuaternion<RealType> operator*(const TQuaternion<RealType>& Q, RealType Scalar)
|
|
{
|
|
return TQuaternion<RealType>(Scalar * Q.X, Scalar * Q.Y, Scalar * Q.Z, Scalar * Q.W);
|
|
}
|
|
|
|
|
|
template<typename RealType>
|
|
TQuaternion<RealType> operator+(const TQuaternion<RealType>& A, const TQuaternion<RealType>& B)
|
|
{
|
|
return TQuaternion<RealType>(A.X + B.X, A.Y + B.Y, A.Z + B.Z, A.W + B.W);
|
|
}
|
|
|
|
|
|
template<typename RealType>
|
|
TQuaternion<RealType> operator -(const TQuaternion<RealType>& A, const TQuaternion<RealType>& B)
|
|
{
|
|
return TQuaternion<RealType>(A.X - B.X, A.Y - B.Y, A.Z - B.Z, A.W - B.W);
|
|
}
|
|
|
|
template<typename RealType>
|
|
TVector<RealType> operator*(const TQuaternion<RealType>& Q, const UE::Math::TVector<RealType>& V)
|
|
{
|
|
//return q.ToRotationMatrix() * v;
|
|
// inline-expansion of above:
|
|
RealType twoX = (RealType)2*Q.X; RealType twoY = (RealType)2*Q.Y; RealType twoZ = (RealType)2*Q.Z;
|
|
RealType twoWX = twoX * Q.W; RealType twoWY = twoY * Q.W; RealType twoWZ = twoZ * Q.W;
|
|
RealType twoXX = twoX * Q.X; RealType twoXY = twoY * Q.X; RealType twoXZ = twoZ * Q.X;
|
|
RealType twoYY = twoY * Q.Y; RealType twoYZ = twoZ * Q.Y; RealType twoZZ = twoZ * Q.Z;
|
|
return TVector<RealType>(
|
|
V.X * ((RealType)1 - (twoYY + twoZZ)) + V.Y * (twoXY - twoWZ) + V.Z * (twoXZ + twoWY),
|
|
V.X * (twoXY + twoWZ) + V.Y * ((RealType)1 - (twoXX + twoZZ)) + V.Z * (twoYZ - twoWX),
|
|
V.X * (twoXZ - twoWY) + V.Y * (twoYZ + twoWX) + V.Z * ((RealType)1 - (twoXX + twoYY))); ;
|
|
}
|
|
|
|
|
|
template<typename RealType>
|
|
TVector<RealType> TQuaternion<RealType>::InverseMultiply(const TVector<RealType>& V) const
|
|
{
|
|
RealType norm = SquaredLength();
|
|
if (norm > 0)
|
|
{
|
|
RealType invNorm = (RealType)1 / norm;
|
|
RealType qX = -X * invNorm, qY = -Y * invNorm, qZ = -Z * invNorm, qW = W * invNorm;
|
|
RealType twoX = (RealType)2 * qX; RealType twoY = (RealType)2 * qY; RealType twoZ = (RealType)2 * qZ;
|
|
RealType twoWX = twoX * qW; RealType twoWY = twoY * qW; RealType twoWZ = twoZ * qW;
|
|
RealType twoXX = twoX * qX; RealType twoXY = twoY * qX; RealType twoXZ = twoZ * qX;
|
|
RealType twoYY = twoY * qY; RealType twoYZ = twoZ * qY; RealType twoZZ = twoZ * qZ;
|
|
return TVector<RealType>(
|
|
V.X * ((RealType)1 - (twoYY + twoZZ)) + V.Y * (twoXY - twoWZ) + V.Z * (twoXZ + twoWY),
|
|
V.X * (twoXY + twoWZ) + V.Y * ((RealType)1 - (twoXX + twoZZ)) + V.Z * (twoYZ - twoWX),
|
|
V.X * (twoXZ - twoWY) + V.Y * (twoYZ + twoWX) + V.Z * ((RealType)1 - (twoXX + twoYY)));
|
|
}
|
|
return TVector<RealType>::Zero();
|
|
}
|
|
|
|
|
|
template<typename RealType>
|
|
TVector<RealType> TQuaternion<RealType>::AxisX() const
|
|
{
|
|
RealType twoY = (RealType)2 * Y; RealType twoZ = (RealType)2 * Z;
|
|
RealType twoWY = twoY * W; RealType twoWZ = twoZ * W;
|
|
RealType twoXY = twoY * X; RealType twoXZ = twoZ * X;
|
|
RealType twoYY = twoY * Y; RealType twoZZ = twoZ * Z;
|
|
return TVector<RealType>((RealType)1 - (twoYY + twoZZ), twoXY + twoWZ, twoXZ - twoWY);
|
|
}
|
|
|
|
template<typename RealType>
|
|
TVector<RealType> TQuaternion<RealType>::AxisY() const
|
|
{
|
|
RealType twoX = (RealType)2 * X; RealType twoY = (RealType)2 * Y; RealType twoZ = (RealType)2 * Z;
|
|
RealType twoWX = twoX * W; RealType twoWZ = twoZ * W; RealType twoXX = twoX * X;
|
|
RealType twoXY = twoY * X; RealType twoYZ = twoZ * Y; RealType twoZZ = twoZ * Z;
|
|
return TVector<RealType>(twoXY - twoWZ, (RealType)1 - (twoXX + twoZZ), twoYZ + twoWX);
|
|
}
|
|
|
|
template<typename RealType>
|
|
TVector<RealType> TQuaternion<RealType>::AxisZ() const
|
|
{
|
|
RealType twoX = (RealType)2 * X; RealType twoY = (RealType)2 * Y; RealType twoZ = (RealType)2 * Z;
|
|
RealType twoWX = twoX * W; RealType twoWY = twoY * W; RealType twoXX = twoX * X;
|
|
RealType twoXZ = twoZ * X; RealType twoYY = twoY * Y; RealType twoYZ = twoZ * Y;
|
|
return TVector<RealType>(twoXZ + twoWY, twoYZ - twoWX, (RealType)1 - (twoXX + twoYY));
|
|
}
|
|
|
|
template<typename RealType>
|
|
void TQuaternion<RealType>::GetAxes(TVector<RealType>& XOut, TVector<RealType>& YOut, TVector<RealType>& ZOut) const
|
|
{
|
|
RealType twoX = (RealType)2 * X; RealType twoY = (RealType)2 * Y; RealType twoZ = (RealType)2 * Z;
|
|
RealType twoWX = twoX * W; RealType twoWY = twoY * W; RealType twoWZ = twoZ * W;
|
|
RealType twoXX = twoX * X; RealType twoXY = twoY * X; RealType twoXZ = twoZ * X;
|
|
RealType twoYY = twoY * Y; RealType twoYZ = twoZ * Y; RealType twoZZ = twoZ * Z;
|
|
XOut = TVector<RealType>((RealType)1 - (twoYY + twoZZ), twoXY + twoWZ, twoXZ - twoWY);
|
|
YOut = TVector<RealType>(twoXY - twoWZ, (RealType)1 - (twoXX + twoZZ), twoYZ + twoWX);
|
|
ZOut = TVector<RealType>(twoXZ + twoWY, twoYZ - twoWX, (RealType)1 - (twoXX + twoYY));
|
|
}
|
|
|
|
|
|
template<typename RealType>
|
|
TQuaternion<RealType> TQuaternion<RealType>::Inverse() const
|
|
{
|
|
RealType norm = SquaredLength();
|
|
if (norm > (RealType)0)
|
|
{
|
|
RealType invNorm = (RealType)1 / norm;
|
|
return TQuaternion<RealType>(
|
|
-X * invNorm, -Y * invNorm, -Z * invNorm, W * invNorm);
|
|
}
|
|
return TQuaternion<RealType>::Zero();
|
|
}
|
|
|
|
|
|
template<typename RealType>
|
|
void TQuaternion<RealType>::SetAxisAngleD(const TVector<RealType>& Axis, RealType AngleDeg)
|
|
{
|
|
SetAxisAngleR(Axis, TMathUtil<RealType>::DegToRad * AngleDeg);
|
|
}
|
|
|
|
template<typename RealType>
|
|
void TQuaternion<RealType>::SetAxisAngleR(const TVector<RealType>& Axis, RealType AngleRad)
|
|
{
|
|
RealType halfAngle = (RealType)0.5 * AngleRad;
|
|
RealType sn = (RealType)sin(halfAngle);
|
|
W = (RealType)cos(halfAngle);
|
|
X = (sn * Axis.X);
|
|
Y = (sn * Axis.Y);
|
|
Z = (sn * Axis.Z);
|
|
}
|
|
|
|
|
|
|
|
// this function can take non-normalized vectors vFrom and vTo (normalizes internally)
|
|
template<typename RealType>
|
|
void TQuaternion<RealType>::SetFromTo(const TVector<RealType>& From, const TVector<RealType>& To)
|
|
{
|
|
// [TODO] this page seems to have optimized version:
|
|
// http://lolengine.net/blog/2013/09/18/beautiful-maths-quaternion-from-vectors
|
|
|
|
// [RMS] not ideal to explicitly normalize here, but if we don't,
|
|
// output TQuaternion is not normalized and this causes problems,
|
|
// eg like drift if we do repeated SetFromTo()
|
|
TVector<RealType> from = UE::Geometry::Normalized(From), to = UE::Geometry::Normalized(To);
|
|
TVector<RealType> bisector = UE::Geometry::Normalized(from + to, TMathUtil<RealType>::ZeroTolerance);
|
|
W = from.Dot(bisector);
|
|
if (W != 0)
|
|
{
|
|
TVector<RealType> cross = from.Cross(bisector);
|
|
X = cross.X;
|
|
Y = cross.Y;
|
|
Z = cross.Z;
|
|
}
|
|
else
|
|
{
|
|
RealType invLength;
|
|
if ( (RealType)fabs(from.X) >= (RealType)fabs(from.Y))
|
|
{
|
|
// V1.X or V1.Z is the largest magnitude component.
|
|
invLength = ((RealType)1 / (RealType)sqrt(from.X * from.X + from.Z * from.Z));
|
|
X = -from.Z * invLength;
|
|
Y = 0;
|
|
Z = +from.X * invLength;
|
|
}
|
|
else
|
|
{
|
|
// V1.Y or V1.Z is the largest magnitude component.
|
|
invLength = ((RealType)1 / (RealType)sqrt(from.Y * from.Y + from.Z * from.Z));
|
|
X = 0;
|
|
Y = +from.Z * invLength;
|
|
Z = -from.Y * invLength;
|
|
}
|
|
}
|
|
Normalize(); // just to be safe...
|
|
}
|
|
|
|
|
|
|
|
template<typename RealType>
|
|
void TQuaternion<RealType>::SetToSlerp( TQuaternion<RealType> From, TQuaternion<RealType> To, RealType InterpT)
|
|
{
|
|
From.Normalize();
|
|
To.Normalize();
|
|
RealType cs = From.Dot(To);
|
|
|
|
// Q and -Q are equivalent, but if we try to Slerp between them we will get nonsense instead of
|
|
// just returning Q. Depending on how the Quaternion was constructed it is possible
|
|
// that the sign flips. So flip it back.
|
|
if (cs < (RealType)-0.99)
|
|
{
|
|
From = -From;
|
|
}
|
|
|
|
RealType angle = TMathUtil<RealType>::ACos(cs);
|
|
if (TMathUtil<RealType>::Abs(angle) >= TMathUtil<RealType>::ZeroTolerance)
|
|
{
|
|
RealType sn = TMathUtil<RealType>::Sin(angle);
|
|
RealType invSn = (RealType)1 / sn;
|
|
RealType tAngle = InterpT * angle;
|
|
RealType coeff0 = TMathUtil<RealType>::Sin(angle - tAngle) * invSn;
|
|
RealType coeff1 = TMathUtil<RealType>::Sin(tAngle) * invSn;
|
|
X = coeff0 * From.X + coeff1 * To.X;
|
|
Y = coeff0 * From.Y + coeff1 * To.Y;
|
|
Z = coeff0 * From.Z + coeff1 * To.Z;
|
|
W = coeff0 * From.W + coeff1 * To.W;
|
|
}
|
|
else
|
|
{
|
|
X = From.X;
|
|
Y = From.Y;
|
|
Z = From.Z;
|
|
W = From.W;
|
|
}
|
|
|
|
Normalize(); // be safe
|
|
}
|
|
|
|
|
|
|
|
template<typename RealType>
|
|
void TQuaternion<RealType>::SetFromRotationMatrix(const TMatrix3<RealType>& RotationMatrix)
|
|
{
|
|
// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
|
|
// article "TQuaternion Calculus and Fast Animation".
|
|
FIndex3i next(1, 2, 0);
|
|
|
|
RealType trace = RotationMatrix(0,0) + RotationMatrix(1,1) + RotationMatrix(2, 2);
|
|
RealType root;
|
|
|
|
if (trace > 0)
|
|
{
|
|
// |w| > 1/2, may as well choose w > 1/2
|
|
root = (RealType)sqrt(trace + (RealType)1); // 2w
|
|
W = ((RealType)0.5) * root;
|
|
root = ((RealType)0.5) / root; // 1/(4w)
|
|
X = (RotationMatrix(2, 1) - RotationMatrix(1, 2)) * root;
|
|
Y = (RotationMatrix(0, 2) - RotationMatrix(2, 0)) * root;
|
|
Z = (RotationMatrix(1, 0) - RotationMatrix(0, 1)) * root;
|
|
}
|
|
else
|
|
{
|
|
// |w| <= 1/2
|
|
int i = 0;
|
|
if (RotationMatrix(1, 1) > RotationMatrix(0, 0))
|
|
{
|
|
i = 1;
|
|
}
|
|
if (RotationMatrix(2, 2) > RotationMatrix(i, i))
|
|
{
|
|
i = 2;
|
|
}
|
|
int j = next[i];
|
|
int k = next[j];
|
|
|
|
root = (RealType)sqrt(RotationMatrix(i, i) - RotationMatrix(j, j) - RotationMatrix(k, k) + (RealType)1);
|
|
|
|
TVector<RealType> quat(X, Y, Z);
|
|
quat[i] = ((RealType)0.5) * root;
|
|
root = ((RealType)0.5) / root;
|
|
W = (RotationMatrix(k, j) - RotationMatrix(j, k)) * root;
|
|
quat[j] = (RotationMatrix(j, i) + RotationMatrix(i, j)) * root;
|
|
quat[k] = (RotationMatrix(k, i) + RotationMatrix(i, k)) * root;
|
|
X = quat.X; Y = quat.Y; Z = quat.Z;
|
|
}
|
|
|
|
Normalize(); // we prefer normalized TQuaternions...
|
|
}
|
|
|
|
|
|
|
|
|
|
template<typename RealType>
|
|
TMatrix3<RealType> TQuaternion<RealType>::ToRotationMatrix() const
|
|
{
|
|
RealType twoX = 2 * X; RealType twoY = 2 * Y; RealType twoZ = 2 * Z;
|
|
RealType twoWX = twoX * W; RealType twoWY = twoY * W; RealType twoWZ = twoZ * W;
|
|
RealType twoXX = twoX * X; RealType twoXY = twoY * X; RealType twoXZ = twoZ * X;
|
|
RealType twoYY = twoY * Y; RealType twoYZ = twoZ * Y; RealType twoZZ = twoZ * Z;
|
|
TMatrix3<RealType> m = TMatrix3<RealType>::Zero();
|
|
m.Row0 = TVector<RealType>(1 - (twoYY + twoZZ), twoXY - twoWZ, twoXZ + twoWY);
|
|
m.Row1 = TVector<RealType>(twoXY + twoWZ, 1 - (twoXX + twoZZ), twoYZ - twoWX);
|
|
m.Row2 = TVector<RealType>(twoXZ - twoWY, twoYZ + twoWX, 1 - (twoXX + twoYY));
|
|
return m;
|
|
}
|
|
|
|
|
|
|
|
template<typename RealType>
|
|
bool TQuaternion<RealType>::EpsilonEqual(const TQuaternion<RealType>& Other, RealType Tolerance) const
|
|
{
|
|
return
|
|
((RealType)fabs(X - Other.X) <= Tolerance &&
|
|
(RealType)fabs(Y - Other.Y) <= Tolerance &&
|
|
(RealType)fabs(Z - Other.Z) <= Tolerance &&
|
|
(RealType)fabs(W - Other.W) <= Tolerance)
|
|
||
|
|
((RealType)fabs(X + Other.X) <= Tolerance &&
|
|
(RealType)fabs(Y + Other.Y) <= Tolerance &&
|
|
(RealType)fabs(Z + Other.Z) <= Tolerance &&
|
|
(RealType)fabs(W + Other.W) <= Tolerance);
|
|
}
|
|
|
|
|
|
} // end namespace UE::Geometry
|
|
} // end namespace UE
|