// 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 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 explicit TQuaternion(const TQuaternion& Copy); TQuaternion(const TVector& Axis, RealType Angle, bool bAngleIsDegrees); TQuaternion(const TVector& From, const TVector& To); TQuaternion(const TQuaternion& From, const TQuaternion& To, RealType InterpT); TQuaternion(const TMatrix3& RotationMatrix); void SetAxisAngleD(const TVector& Axis, RealType AngleDeg); void SetAxisAngleR(const TVector& Axis, RealType AngleRad); void SetFromTo(const TVector& From, const TVector& To); void SetToSlerp(TQuaternion From, TQuaternion To, RealType InterpT); void SetFromRotationMatrix(const TMatrix3& RotationMatrix); static TQuaternion Zero() { return TQuaternion((RealType)0, (RealType)0, (RealType)0, (RealType)0); } static TQuaternion Identity() { return TQuaternion((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& Other, RealType Tolerance = TMathUtil::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::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 AxisX() const; TVector AxisY() const; TVector AxisZ() const; void GetAxes(TVector& X, TVector& Y, TVector& Z) const; RealType Normalize(const RealType epsilon = 0); TQuaternion Normalized(const RealType epsilon = 0) const; RealType Dot(const TQuaternion& Other) const; TQuaternion Inverse() const; TVector InverseMultiply(const TVector& Other) const; TMatrix3 ToRotationMatrix() const; constexpr TQuaternion operator-() const { return TQuaternion(-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() const { return UE::Math::TVector4(X,Y,Z,W); } }; typedef TQuaternion FQuaternionf; typedef TQuaternion FQuaterniond; template TQuaternion::TQuaternion() { X = Y = Z = 0; W = 1; } template TQuaternion::TQuaternion(RealType x, RealType y, RealType z, RealType w) { X = x; Y = y; Z = z; W = w; } template TQuaternion::TQuaternion(const RealType* Values) { X = Values[0]; Y = Values[1]; Z = Values[2]; W = Values[3]; } template template TQuaternion::TQuaternion(const TQuaternion& Copy) { X = (RealType)Copy.X; Y = (RealType)Copy.Y; Z = (RealType)Copy.Z; W = (RealType)Copy.W; } template TQuaternion::TQuaternion(const TVector& Axis, RealType Angle, bool bAngleIsDegrees) { X = Y = Z = 0; W = 1; SetAxisAngleR(Axis, Angle * (bAngleIsDegrees ? TMathUtil::DegToRad : (RealType)1)); } template TQuaternion::TQuaternion(const TVector& From, const TVector& To) { X = Y = Z = 0; W = 1; SetFromTo(From, To); } template TQuaternion::TQuaternion(const TQuaternion& From, const TQuaternion& To, RealType InterpT) { X = Y = Z = 0; W = 1; SetToSlerp(From, To, InterpT); } template TQuaternion::TQuaternion(const TMatrix3& RotationMatrix) { X = Y = Z = 0; W = 1; SetFromRotationMatrix(RotationMatrix); } template RealType TQuaternion::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 TQuaternion TQuaternion::Normalized(const RealType Epsilon) const { RealType length = Length(); if (length > Epsilon) { RealType invLength = ((RealType)1) / length; return TQuaternion(X*invLength, Y*invLength, Z*invLength, W*invLength); } return Zero(); } template RealType TQuaternion::Dot(const TQuaternion& Other) const { return X * Other.X + Y * Other.Y + Z * Other.Z + W * Other.W; } template TQuaternion operator*(const TQuaternion& A, const TQuaternion& 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(X, Y, Z, W); } template TQuaternion operator*(RealType Scalar, const TQuaternion& Q) { return TQuaternion(Scalar * Q.X, Scalar * Q.Y, Scalar * Q.Z, Scalar * Q.W); } template TQuaternion operator*(const TQuaternion& Q, RealType Scalar) { return TQuaternion(Scalar * Q.X, Scalar * Q.Y, Scalar * Q.Z, Scalar * Q.W); } template TQuaternion operator+(const TQuaternion& A, const TQuaternion& B) { return TQuaternion(A.X + B.X, A.Y + B.Y, A.Z + B.Z, A.W + B.W); } template TQuaternion operator -(const TQuaternion& A, const TQuaternion& B) { return TQuaternion(A.X - B.X, A.Y - B.Y, A.Z - B.Z, A.W - B.W); } template TVector operator*(const TQuaternion& Q, const UE::Math::TVector& 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( 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 TVector TQuaternion::InverseMultiply(const TVector& 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( 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::Zero(); } template TVector TQuaternion::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)1 - (twoYY + twoZZ), twoXY + twoWZ, twoXZ - twoWY); } template TVector TQuaternion::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(twoXY - twoWZ, (RealType)1 - (twoXX + twoZZ), twoYZ + twoWX); } template TVector TQuaternion::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(twoXZ + twoWY, twoYZ - twoWX, (RealType)1 - (twoXX + twoYY)); } template void TQuaternion::GetAxes(TVector& XOut, TVector& YOut, TVector& 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)1 - (twoYY + twoZZ), twoXY + twoWZ, twoXZ - twoWY); YOut = TVector(twoXY - twoWZ, (RealType)1 - (twoXX + twoZZ), twoYZ + twoWX); ZOut = TVector(twoXZ + twoWY, twoYZ - twoWX, (RealType)1 - (twoXX + twoYY)); } template TQuaternion TQuaternion::Inverse() const { RealType norm = SquaredLength(); if (norm > (RealType)0) { RealType invNorm = (RealType)1 / norm; return TQuaternion( -X * invNorm, -Y * invNorm, -Z * invNorm, W * invNorm); } return TQuaternion::Zero(); } template void TQuaternion::SetAxisAngleD(const TVector& Axis, RealType AngleDeg) { SetAxisAngleR(Axis, TMathUtil::DegToRad * AngleDeg); } template void TQuaternion::SetAxisAngleR(const TVector& 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 void TQuaternion::SetFromTo(const TVector& From, const TVector& 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 from = UE::Geometry::Normalized(From), to = UE::Geometry::Normalized(To); TVector bisector = UE::Geometry::Normalized(from + to, TMathUtil::ZeroTolerance); W = from.Dot(bisector); if (W != 0) { TVector 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 void TQuaternion::SetToSlerp( TQuaternion From, TQuaternion 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::ACos(cs); if (TMathUtil::Abs(angle) >= TMathUtil::ZeroTolerance) { RealType sn = TMathUtil::Sin(angle); RealType invSn = (RealType)1 / sn; RealType tAngle = InterpT * angle; RealType coeff0 = TMathUtil::Sin(angle - tAngle) * invSn; RealType coeff1 = TMathUtil::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 void TQuaternion::SetFromRotationMatrix(const TMatrix3& 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 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 TMatrix3 TQuaternion::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 m = TMatrix3::Zero(); m.Row0 = TVector(1 - (twoYY + twoZZ), twoXY - twoWZ, twoXZ + twoWY); m.Row1 = TVector(twoXY + twoWZ, 1 - (twoXX + twoZZ), twoYZ - twoWX); m.Row2 = TVector(twoXZ - twoWY, twoYZ + twoWX, 1 - (twoXX + twoYY)); return m; } template bool TQuaternion::EpsilonEqual(const TQuaternion& 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