Quat.h

Go to the documentation of this file.

// Copyright Epic Games, Inc. All Rights Reserved.
 
#pragma once
 
#include "CoreTypes.h"
#include "Misc/AssertionMacros.h"
#include "Math/UnrealMathUtility.h"
#include "Containers/UnrealString.h"
#include "Logging/LogMacros.h"
#include "Math/MathFwd.h" // IWYU pragma: export
#include "Math/Vector.h"
#include "Math/VectorRegister.h"
#include "Math/Rotator.h"
#include "Math/Matrix.h"
#include "Misc/LargeWorldCoordinatesSerializer.h"
#include "UObject/ObjectVersion.h"
 
class Error;
 
namespace UE
{
namespace Math
{
 
template<typename T>
struct alignas(16) TQuat
{
    // Can't have a UE_REQUIRES in the declaration because of the forward declarations, so check for allowed types here.
    static_assert(std::is_floating_point_v<T>, "TQuat only supports float and double types.");
 
public:
    using FReal = T;
    using QuatVectorRegister = TVectorRegisterType<T>;
 
    T X;
 
    T Y;
 
    T Z;
 
    T W;
 
public:
 
    CORE_API static const TQuat<T> Identity;
 
public:
 
    [[nodiscard]] TQuat() = default;
 
    [[nodiscard]] explicit UE_FORCEINLINE_HINT TQuat(EForceInit);
 
    [[nodiscard]] inline TQuat(T InX, T InY, T InZ, T InW);
 
    template<typename DummyT = T UE_REQUIRES(std::is_arithmetic_v<T>)>
    [[nodiscard]] explicit inline TQuat(T V)
        : X(V)
        , Y(V)
        , Z(V)
        , W(V)
    {
        DiagnosticCheckNaN();
    }
 
protected:
    [[nodiscard]] explicit TQuat(QuatVectorRegister V);
 
public:
 
    [[nodiscard]] UE_FORCEINLINE_HINT static TQuat<T> MakeFromVectorRegister(QuatVectorRegister V)
    {
        return TQuat<T>(V);
    }
 
    explicit TQuat(const TRotator<T>& R);
 
    [[nodiscard]] UE_FORCEINLINE_HINT static TQuat<T> MakeFromRotator(const TRotator<T>& R)
    {
        return TQuat<T>(R);
    }
 
    explicit TQuat(const TMatrix<T>& M);
 
    TQuat(TVector<T> Axis, T AngleRad);
 
public:
 
    [[nodiscard]] inline TQuat<T> operator+(const TQuat<T>& Q) const;
 
    inline TQuat<T> operator+=(const TQuat<T>& Q);
 
    [[nodiscard]] inline TQuat<T> operator-(const TQuat<T>& Q) const;
 
    [[nodiscard]] inline TQuat<T> operator-() const;
 
    [[nodiscard]] inline bool Equals(const TQuat<T>& Q, T Tolerance=UE_KINDA_SMALL_NUMBER) const;
 
    [[nodiscard]] UE_FORCEINLINE_HINT bool IsIdentity(T Tolerance=UE_SMALL_NUMBER) const;
 
    inline TQuat<T> operator-=(const TQuat<T>& Q);
 
    [[nodiscard]] inline TQuat<T> operator*(const TQuat<T>& Q) const;
 
    inline TQuat<T> operator*=(const TQuat<T>& Q);
 
    [[nodiscard]] TVector<T> operator*(const TVector<T>& V) const;
 
    [[nodiscard]] CORE_API TMatrix<T> operator*(const TMatrix<T>& M) const;
    
    template<typename FArg UE_REQUIRES(std::is_arithmetic_v<FArg>)>
    inline TQuat<T> operator*=(const FArg Scale)
    {
#if PLATFORM_ENABLE_VECTORINTRINSICS
        QuatVectorRegister A = VectorLoadAligned(this);
        QuatVectorRegister B = VectorSetFloat1(Scale);
        VectorStoreAligned(VectorMultiply(A, B), this);
#else
        X *= Scale;
        Y *= Scale;
        Z *= Scale;
        W *= Scale;
#endif // PLATFORM_ENABLE_VECTORINTRINSICS
 
        DiagnosticCheckNaN();
        return *this;
    }
 
    template<typename FArg UE_REQUIRES(std::is_arithmetic_v<FArg>)>
    [[nodiscard]] inline TQuat<T> operator*(const FArg Scale) const
    {
#if PLATFORM_ENABLE_VECTORINTRINSICS
        QuatVectorRegister A = VectorLoadAligned(this);
        QuatVectorRegister B = VectorSetFloat1((T)Scale);
        return TQuat(VectorMultiply(A, B));
#else
        return TQuat(Scale * X, Scale * Y, Scale * Z, Scale * W);
#endif // PLATFORM_ENABLE_VECTORINTRINSICS
    }
    
    template<typename FArg UE_REQUIRES(std::is_arithmetic_v<FArg>)>
    inline TQuat<T> operator/=(const FArg Scale)
    {
#if PLATFORM_ENABLE_VECTORINTRINSICS
        QuatVectorRegister A = VectorLoadAligned(this);
        QuatVectorRegister B = VectorSetFloat1((T)Scale);
        VectorStoreAligned(VectorDivide(A, B), this);
#else
        const T Recip = T(1.0f) / Scale;
        X *= Recip;
        Y *= Recip;
        Z *= Recip;
        W *= Recip;
#endif // PLATFORM_ENABLE_VECTORINTRINSICS
 
        DiagnosticCheckNaN();
        return *this;
    }
 
    template<typename FArg UE_REQUIRES(std::is_arithmetic_v<FArg>)>
    [[nodiscard]] inline TQuat<T> operator/(const FArg Scale) const
    {
#if PLATFORM_ENABLE_VECTORINTRINSICS
        QuatVectorRegister A = VectorLoadAligned(this);
        QuatVectorRegister B = VectorSetFloat1(Scale);
        return TQuat(VectorDivide(A, B));
#else
        const T Recip = 1.0f / Scale;
        return TQuat(X * Recip, Y * Recip, Z * Recip, W * Recip);
#endif // PLATFORM_ENABLE_VECTORINTRINSICS
    }
 
    [[nodiscard]] bool Identical(const TQuat* Q, const uint32 PortFlags) const;
 
    [[nodiscard]] bool operator==(const TQuat<T>& Q) const;
 
    [[nodiscard]] bool operator!=(const TQuat<T>& Q) const;
 
    [[nodiscard]] T operator|(const TQuat<T>& Q) const;
 
public:
 
    static CORE_API TQuat<T> MakeFromEuler(const TVector<T>& Euler);
 
    [[nodiscard]] CORE_API TVector<T> Euler() const;
 
    inline void Normalize(T Tolerance = UE_SMALL_NUMBER);
 
    [[nodiscard]] inline TQuat<T> GetNormalized(T Tolerance = UE_SMALL_NUMBER) const;
 
    // Return true if this quaternion is normalized
    [[nodiscard]] bool IsNormalized() const;
 
    [[nodiscard]] UE_FORCEINLINE_HINT T Size() const;
 
    [[nodiscard]] UE_FORCEINLINE_HINT T SizeSquared() const;
 
    [[nodiscard]] UE_FORCEINLINE_HINT T GetAngle() const;
 
    void ToAxisAndAngle(TVector<T>& Axis, float& Angle) const;
    void ToAxisAndAngle(TVector<T>& Axis, double& Angle) const;
 
    [[nodiscard]] TVector<T> ToRotationVector() const;
 
    [[nodiscard]] static TQuat<T> MakeFromRotationVector(const TVector<T>& RotationVector);
 
    CORE_API void ToSwingTwist(const TVector<T>& InTwistAxis, TQuat<T>& OutSwing, TQuat<T>& OutTwist) const;
 
    [[nodiscard]] CORE_API T GetTwistAngle(const TVector<T>& TwistAxis) const;
 
    [[nodiscard]] TVector<T> RotateVector(TVector<T> V) const;
    
    [[nodiscard]] TVector<T> UnrotateVector(TVector<T> V) const;
 
    [[nodiscard]] CORE_API TQuat<T> Log() const;
 
    [[nodiscard]] CORE_API TQuat<T> Exp() const;
 
    [[nodiscard]] TQuat<T> Inverse() const;
 
    void EnforceShortestArcWith(const TQuat<T>& OtherQuat);
 
    [[nodiscard]] TQuat<T> GetShortestArcWith(const TQuat<T>& OtherQuat) const;
 
    [[nodiscard]] UE_FORCEINLINE_HINT TVector<T> GetAxisX() const;
 
    [[nodiscard]] UE_FORCEINLINE_HINT TVector<T> GetAxisY() const;
 
    [[nodiscard]] UE_FORCEINLINE_HINT TVector<T> GetAxisZ() const;
 
    [[nodiscard]] UE_FORCEINLINE_HINT TVector<T> GetForwardVector() const;
 
    [[nodiscard]] UE_FORCEINLINE_HINT TVector<T> GetRightVector() const;
 
    [[nodiscard]] UE_FORCEINLINE_HINT TVector<T> GetUpVector() const;
 
    [[nodiscard]] UE_FORCEINLINE_HINT TVector<T> Vector() const;
 
    [[nodiscard]] CORE_API TRotator<T> Rotator() const;
 
    [[nodiscard]] UE_FORCEINLINE_HINT TMatrix<T> ToMatrix() const;
 
    CORE_API void ToMatrix(TMatrix<T>& Mat) const;
 
    [[nodiscard]] inline TVector<T> GetRotationAxis() const;
 
    [[nodiscard]] T AngularDistance(const TQuat<T>& Q) const;
 
    CORE_API bool NetSerialize(FArchive& Ar, class UPackageMap* Map, bool& bOutSuccess);
 
    [[nodiscard]] bool ContainsNaN() const;
 
    [[nodiscard]] FString ToString() const;
 
    bool InitFromString(const FString& InSourceString);
 
public:
 
#if ENABLE_NAN_DIAGNOSTIC
    inline void DiagnosticCheckNaN() const
    {
        if (ContainsNaN())
        {
            logOrEnsureNanError(TEXT("Quat contains NaN: %s"), *ToString());
            *const_cast<TQuat*>(this) = TQuat<T>(0.f, 0.f, 0.f, 1.f);
        }
    }
 
    inline void DiagnosticCheckNaN(const TCHAR* Message) const
    {
        if (ContainsNaN())
        {
            logOrEnsureNanError(TEXT("%s: Quat contains NaN: %s"), Message, *ToString());
            *const_cast<TQuat*>(this) = TQuat<T>(0.f, 0.f, 0.f, 1.f);
        }
    }
#else
    UE_FORCEINLINE_HINT void DiagnosticCheckNaN() const {}
    UE_FORCEINLINE_HINT void DiagnosticCheckNaN(const TCHAR* Message) const {}
#endif
 
public:
 
    [[nodiscard]] static UE_FORCEINLINE_HINT TQuat<T> FindBetween(const TVector<T>& Vector1, const TVector<T>& Vector2)
    {
        return FindBetweenVectors(Vector1, Vector2);
    }
 
    [[nodiscard]] static CORE_API TQuat<T> FindBetweenNormals(const TVector<T>& Normal1, const TVector<T>& Normal2);
 
    [[nodiscard]] static CORE_API TQuat<T> FindBetweenVectors(const TVector<T>& Vector1, const TVector<T>& Vector2);
 
    [[nodiscard]] static inline T Error(const TQuat<T>& Q1, const TQuat<T>& Q2);
 
    [[nodiscard]] static inline T ErrorAutoNormalize(const TQuat<T>& A, const TQuat<T>& B);
 
    [[nodiscard]] static inline TQuat<T> FastLerp(const TQuat<T>& A, const TQuat<T>& B, const T Alpha);
 
    [[nodiscard]] static inline TQuat<T> FastBilerp(const TQuat<T>& P00, const TQuat<T>& P10, const TQuat<T>& P01, const TQuat<T>& P11, T FracX, T FracY);
 
 
    [[nodiscard]] static CORE_API TQuat<T> Slerp_NotNormalized(const TQuat<T>& Quat1, const TQuat<T>& Quat2, T Slerp);
 
    [[nodiscard]] static UE_FORCEINLINE_HINT TQuat<T> Slerp(const TQuat<T>& Quat1, const TQuat<T>& Quat2, T Slerp)
    {
        return Slerp_NotNormalized(Quat1, Quat2, Slerp).GetNormalized();
    }
 
    [[nodiscard]] static CORE_API TQuat<T> SlerpFullPath_NotNormalized(const TQuat<T>& quat1, const TQuat<T>& quat2, T Alpha);
 
    [[nodiscard]] static UE_FORCEINLINE_HINT TQuat<T> SlerpFullPath(const TQuat<T>& quat1, const TQuat<T>& quat2, T Alpha)
    {
        return SlerpFullPath_NotNormalized(quat1, quat2, Alpha).GetNormalized();
    }
    
    [[nodiscard]] static CORE_API TQuat<T> Squad(const TQuat<T>& quat1, const TQuat<T>& tang1, const TQuat<T>& quat2, const TQuat<T>& tang2, T Alpha);
 
    [[nodiscard]] static CORE_API TQuat<T> SquadFullPath(const TQuat<T>& quat1, const TQuat<T>& tang1, const TQuat<T>& quat2, const TQuat<T>& tang2, T Alpha);
 
    static CORE_API void CalcTangents(const TQuat<T>& PrevP, const TQuat<T>& P, const TQuat<T>& NextP, T Tension, TQuat<T>& OutTan);
 
    [[nodiscard]] TTuple<T, T, T> ToLUFEuler() const;
 
    // Converts to euler angles (in rads) in verse conventions
    [[nodiscard]] static TQuat MakeFromLUFEuler(const TTuple<T, T, T>& InLUFEuler);
 
public:
 
    bool Serialize(FArchive& Ar)
    {
        Ar << (TQuat<T>&)*this;
        return true;
    }
 
    bool SerializeFromMismatchedTag(FName StructTag, FArchive& Ar)
    {
        if constexpr (std::is_same_v<T, float>)
        {
            return UE_SERIALIZE_VARIANT_FROM_MISMATCHED_TAG(Ar, Quat, Quat4f, Quat4d);
        }
        else
        {
            return UE_SERIALIZE_VARIANT_FROM_MISMATCHED_TAG(Ar, Quat, Quat4d, Quat4f);
        }
    }
 
    // Conversion to other type.
    template<typename FArg UE_REQUIRES(!std::is_same_v<T, FArg>)>
    explicit TQuat(const TQuat<FArg>& From)
        : TQuat<T>((T)From.X, (T)From.Y, (T)From.Z, (T)From.W)
    {
    }
};
 
inline FArchive& operator<<(FArchive& Ar, TQuat<float>& F)
{
    return Ar << F.X << F.Y << F.Z << F.W;
}
 
inline FArchive& operator<<(FArchive& Ar, TQuat<double>& F)
{
    if (Ar.UEVer() >= EUnrealEngineObjectUE5Version::LARGE_WORLD_COORDINATES)
    {
        return Ar << F.X << F.Y << F.Z << F.W;
    }
    else
    {
        checkf(Ar.IsLoading(), TEXT("float -> double conversion applied outside of load!"));
        // Stored as floats, so serialize float and copy.
        float X, Y, Z, W;
        Ar << X << Y << Z << W;
        if(Ar.IsLoading())
        {
            F = TQuat<double>(X, Y, Z, W);
        }
    }
    return Ar;
}
 
#if !defined(_MSC_VER) || defined(__clang__)  // MSVC can't forward declare explicit specializations
template<> CORE_API const FQuat4f FQuat4f::Identity;
template<> CORE_API const FQuat4d FQuat4d::Identity;
#endif
/* TQuat inline functions
 *****************************************************************************/
template<typename T>
inline TQuat<T>::TQuat(const UE::Math::TMatrix<T>& M)
{
    // If Matrix is NULL, return Identity quaternion. If any of them is 0, you won't be able to construct rotation
    // if you have two plane at least, we can reconstruct the frame using cross product, but that's a bit expensive op to do here
    // for now, if you convert to matrix from 0 scale and convert back, you'll lose rotation. Don't do that. 
    if (M.GetScaledAxis(EAxis::X).IsNearlyZero() || M.GetScaledAxis(EAxis::Y).IsNearlyZero() || M.GetScaledAxis(EAxis::Z).IsNearlyZero())
    {
        *this = TQuat<T>::Identity;
        return;
    }
 
#if !(UE_BUILD_SHIPPING || UE_BUILD_TEST)
    // Make sure the Rotation part of the Matrix is unit length.
    // Changed to this (same as RemoveScaling) from RotDeterminant as using two different ways of checking unit length matrix caused inconsistency. 
    if (!ensure((FMath::Abs(1.f - M.GetScaledAxis(EAxis::X).SizeSquared()) <= UE_KINDA_SMALL_NUMBER) && (FMath::Abs(1.f - M.GetScaledAxis(EAxis::Y).SizeSquared()) <= UE_KINDA_SMALL_NUMBER) && (FMath::Abs(1.f - M.GetScaledAxis(EAxis::Z).SizeSquared()) <= UE_KINDA_SMALL_NUMBER)))
    {
        *this = TQuat<T>::Identity;
        return;
    }
#endif
 
    //const MeReal *const t = (MeReal *) tm;
    T s;
 
    // Check diagonal (trace)
    const T tr = M.M[0][0] + M.M[1][1] + M.M[2][2];
 
    if (tr > 0.0f) 
    {
        T InvS = FMath::InvSqrt(tr + T(1.f));
        this->W = T(T(0.5f) * (T(1.f) / InvS));
        s = T(0.5f) * InvS;
 
        this->X = ((M.M[1][2] - M.M[2][1]) * s);
        this->Y = ((M.M[2][0] - M.M[0][2]) * s);
        this->Z = ((M.M[0][1] - M.M[1][0]) * s);
    } 
    else 
    {
        // diagonal is negative
        int32 i = 0;
 
        if (M.M[1][1] > M.M[0][0])
            i = 1;
 
        if (M.M[2][2] > M.M[i][i])
            i = 2;
 
        static constexpr int32 nxt[3] = { 1, 2, 0 };
        const int32 j = nxt[i];
        const int32 k = nxt[j];
 
        s = M.M[i][i] - M.M[j][j] - M.M[k][k] + T(1.0f);
 
        T InvS = FMath::InvSqrt(s);
 
        T qt[4];
        qt[i] = T(0.5f) * (T(1.f) / InvS);
 
        s = T(0.5f) * InvS;
 
        qt[3] = (M.M[j][k] - M.M[k][j]) * s;
        qt[j] = (M.M[i][j] + M.M[j][i]) * s;
        qt[k] = (M.M[i][k] + M.M[k][i]) * s;
 
        this->X = qt[0];
        this->Y = qt[1];
        this->Z = qt[2];
        this->W = qt[3];
 
        DiagnosticCheckNaN();
    }
}
 
 
template<typename T>
inline TQuat<T>::TQuat(const TRotator<T>& R)
{
    *this = R.Quaternion();
    DiagnosticCheckNaN();
}
 
template<typename T>
UE_FORCEINLINE_HINT TVector<T> TQuat<T>::operator*(const TVector<T>& V) const
{
    return RotateVector(V);
}
 
/* TQuat inline functions
 *****************************************************************************/
 
template<typename T>
UE_FORCEINLINE_HINT TQuat<T>::TQuat(EForceInit ZeroOrNot)
    :   X(0), Y(0), Z(0), W(ZeroOrNot == ForceInitToZero ? 0.0f : 1.0f)
{ }
 
template<typename T>
inline TQuat<T>::TQuat(T InX, T InY, T InZ, T InW)
    : X(InX)
    , Y(InY)
    , Z(InZ)
    , W(InW)
{
    static_assert(TIsFloatingPoint<T>::Value);
    DiagnosticCheckNaN();
}
 
template<typename T>
inline TQuat<T>::TQuat(QuatVectorRegister V)
{
    VectorStoreAligned(V, this);
    DiagnosticCheckNaN();
}
 
 
template<typename T>
UE_FORCEINLINE_HINT FString TQuat<T>::ToString() const
{
    return FString::Printf(TEXT("X=%.9f Y=%.9f Z=%.9f W=%.9f"), X, Y, Z, W);
}
 
template<typename T>
inline bool TQuat<T>::InitFromString(const FString& InSourceString)
{
    X = Y = Z = 0.f;
    W = 1.f;
 
    const bool bSuccess
        =  FParse::Value(*InSourceString, TEXT("X="), X)
        && FParse::Value(*InSourceString, TEXT("Y="), Y)
        && FParse::Value(*InSourceString, TEXT("Z="), Z)
        && FParse::Value(*InSourceString, TEXT("W="), W);
    DiagnosticCheckNaN();
    return bSuccess;
}
 
template<typename T>
inline TQuat<T>::TQuat(TVector<T> Axis, T AngleRad)
{
    const T half_a = 0.5f * AngleRad;
    T s, c;
    FMath::SinCos(&s, &c, half_a);
 
    X = s * Axis.X;
    Y = s * Axis.Y;
    Z = s * Axis.Z;
    W = c;
 
    DiagnosticCheckNaN();
}
 
 
template<typename T>
inline TQuat<T> TQuat<T>::operator+(const TQuat<T>& Q) const
{
#if PLATFORM_ENABLE_VECTORINTRINSICS
    QuatVectorRegister A = VectorLoadAligned(this);
    QuatVectorRegister B = VectorLoadAligned(&Q);
    return TQuat(VectorAdd(A, B));
#else
    return TQuat(X + Q.X, Y + Q.Y, Z + Q.Z, W + Q.W);
#endif // PLATFORM_ENABLE_VECTORINTRINSICS
}
 
 
template<typename T>
inline TQuat<T> TQuat<T>::operator+=(const TQuat<T>& Q)
{
#if PLATFORM_ENABLE_VECTORINTRINSICS
    QuatVectorRegister A = VectorLoadAligned(this);
    QuatVectorRegister B = VectorLoadAligned(&Q);
    VectorStoreAligned(VectorAdd(A, B), this);
#else
    this->X += Q.X;
    this->Y += Q.Y;
    this->Z += Q.Z;
    this->W += Q.W;
#endif // PLATFORM_ENABLE_VECTORINTRINSICS
 
    DiagnosticCheckNaN();
 
    return *this;
}
 
 
template<typename T>
inline TQuat<T> TQuat<T>::operator-(const TQuat<T>& Q) const
{
#if PLATFORM_ENABLE_VECTORINTRINSICS
    QuatVectorRegister A = VectorLoadAligned(this);
    QuatVectorRegister B = VectorLoadAligned(&Q);
    return TQuat(VectorSubtract(A, B));
#else
    return TQuat(X - Q.X, Y - Q.Y, Z - Q.Z, W - Q.W);
#endif // PLATFORM_ENABLE_VECTORINTRINSICS
}
 
template<typename T>
inline TQuat<T> TQuat<T>::operator-() const
{
#if PLATFORM_ENABLE_VECTORINTRINSICS
    return TQuat(VectorNegate(VectorLoadAligned(this)));
#else
    return TQuat(-X, -Y, -Z, -W);
#endif
}
 
template<typename T>
inline bool TQuat<T>::Equals(const TQuat<T>& Q, T Tolerance) const
{
#if PLATFORM_ENABLE_VECTORINTRINSICS
    const QuatVectorRegister ToleranceV = VectorLoadFloat1(&Tolerance);
    const QuatVectorRegister A = VectorLoadAligned(this);
    const QuatVectorRegister B = VectorLoadAligned(&Q);
 
    const QuatVectorRegister RotationSub = VectorAbs(VectorSubtract(A, B));
    const QuatVectorRegister RotationAdd = VectorAbs(VectorAdd(A, B));
    return !VectorAnyGreaterThan(RotationSub, ToleranceV) || !VectorAnyGreaterThan(RotationAdd, ToleranceV);
#else
    return (FMath::Abs(X - Q.X) <= Tolerance && FMath::Abs(Y - Q.Y) <= Tolerance && FMath::Abs(Z - Q.Z) <= Tolerance && FMath::Abs(W - Q.W) <= Tolerance)
        || (FMath::Abs(X + Q.X) <= Tolerance && FMath::Abs(Y + Q.Y) <= Tolerance && FMath::Abs(Z + Q.Z) <= Tolerance && FMath::Abs(W + Q.W) <= Tolerance);
#endif // PLATFORM_ENABLE_VECTORINTRINSICS
}
 
template<typename T>
UE_FORCEINLINE_HINT bool TQuat<T>::IsIdentity(T Tolerance) const
{
    return Equals(TQuat<T>::Identity, Tolerance);
}
 
template<typename T>
inline TQuat<T> TQuat<T>::operator-=(const TQuat<T>& Q)
{
#if PLATFORM_ENABLE_VECTORINTRINSICS
    QuatVectorRegister A = VectorLoadAligned(this);
    QuatVectorRegister B = VectorLoadAligned(&Q);
    VectorStoreAligned(VectorSubtract(A, B), this);
#else
    this->X -= Q.X;
    this->Y -= Q.Y;
    this->Z -= Q.Z;
    this->W -= Q.W;
#endif // PLATFORM_ENABLE_VECTORINTRINSICS
 
    DiagnosticCheckNaN();
 
    return *this;
}
 
 
template<typename T>
inline TQuat<T> TQuat<T>::operator*(const TQuat<T>& Q) const
{
    TQuat<T> Result;
    VectorQuaternionMultiply(&Result, this, &Q);
    
    Result.DiagnosticCheckNaN();
    
    return Result;
}
 
 
template<typename T>
inline TQuat<T> TQuat<T>::operator*=(const TQuat<T>& Q)
{
    QuatVectorRegister A = VectorLoadAligned(this);
    QuatVectorRegister B = VectorLoadAligned(&Q);
    QuatVectorRegister Result = VectorQuaternionMultiply2(A, B);
    VectorStoreAligned(Result, this);
 
    DiagnosticCheckNaN();
 
    return *this; 
}
 
 
// Global operator for (float * Quat)
template<typename T>
UE_FORCEINLINE_HINT TQuat<T> operator*(const float Scale, const TQuat<T>& Q)
{
    return Q.operator*(Scale);
}
 
// Global operator for (double * Quat)
template<typename T>
UE_FORCEINLINE_HINT TQuat<T> operator*(const double Scale, const TQuat<T>& Q)
{
    return Q.operator*(Scale);
}
 
template<typename T>
UE_FORCEINLINE_HINT bool TQuat<T>::Identical(const TQuat* Q, const uint32 PortFlags) const
{
    return X == Q->X && Y == Q->Y && Z == Q->Z && W == Q->W;
}
 
template<typename T>
inline bool TQuat<T>::operator==(const TQuat<T>& Q) const
{
#if PLATFORM_ENABLE_VECTORINTRINSICS
    const QuatVectorRegister A = VectorLoadAligned(this);
    const QuatVectorRegister B = VectorLoadAligned(&Q);
    return VectorMaskBits(VectorCompareEQ(A, B)) == 0x0F;
#else
    return X == Q.X && Y == Q.Y && Z == Q.Z && W == Q.W;
#endif // PLATFORM_ENABLE_VECTORINTRINSICS
}
 
 
template<typename T>
inline bool TQuat<T>::operator!=(const TQuat<T>& Q) const
{
#if PLATFORM_ENABLE_VECTORINTRINSICS
    const QuatVectorRegister A = VectorLoadAligned(this);
    const QuatVectorRegister B = VectorLoadAligned(&Q);
    return VectorMaskBits(VectorCompareNE(A, B)) != 0x00;
#else
    return X != Q.X || Y != Q.Y || Z != Q.Z || W != Q.W;
#endif // PLATFORM_ENABLE_VECTORINTRINSICS
}
 
 
template<typename T>
UE_FORCEINLINE_HINT T TQuat<T>::operator|(const TQuat<T>& Q) const
{
    return X * Q.X + Y * Q.Y + Z * Q.Z + W * Q.W;
}
 
 
template<typename T>
inline void TQuat<T>::Normalize(T Tolerance)
{
#if PLATFORM_ENABLE_VECTORINTRINSICS
    const QuatVectorRegister Vector = VectorLoadAligned(this);
 
    const QuatVectorRegister SquareSum = VectorDot4(Vector, Vector);
    const QuatVectorRegister NonZeroMask = VectorCompareGE(SquareSum, VectorLoadFloat1(&Tolerance));
    const QuatVectorRegister InvLength = VectorReciprocalSqrtAccurate(SquareSum);
    const QuatVectorRegister NormalizedVector = VectorMultiply(InvLength, Vector);
    QuatVectorRegister Result = VectorSelect(NonZeroMask, NormalizedVector, GlobalVectorConstants::Float0001);
 
    VectorStoreAligned(Result, this);
#else
    const T SquareSum = X * X + Y * Y + Z * Z + W * W;
 
    if (SquareSum >= Tolerance)
    {
        const T Scale = FMath::InvSqrt(SquareSum);
 
        X *= Scale; 
        Y *= Scale; 
        Z *= Scale;
        W *= Scale;
    }
    else
    {
        *this = TQuat<T>::Identity;
    }
#endif // PLATFORM_ENABLE_VECTORINTRINSICS
}
 
 
template<typename T>
inline TQuat<T> TQuat<T>::GetNormalized(T Tolerance) const
{
    TQuat<T> Result(*this);
    Result.Normalize(Tolerance);
    return Result;
}
 
 
template<typename T>
inline bool TQuat<T>::IsNormalized() const
{
#if PLATFORM_ENABLE_VECTORINTRINSICS
    QuatVectorRegister A = VectorLoadAligned(this);
    QuatVectorRegister TestValue = VectorAbs(VectorSubtract(VectorOne(), VectorDot4(A, A)));
    return !VectorAnyGreaterThan(TestValue, GlobalVectorConstants::ThreshQuatNormalized);
#else
    return (FMath::Abs(1.f - SizeSquared()) < UE_THRESH_QUAT_NORMALIZED);
#endif // PLATFORM_ENABLE_VECTORINTRINSICS
}
 
 
template<typename T>
UE_FORCEINLINE_HINT T TQuat<T>::Size() const
{
    return FMath::Sqrt(X * X + Y * Y + Z * Z + W * W);
}
 
template<typename T>
UE_FORCEINLINE_HINT T TQuat<T>::SizeSquared() const
{
    return (X * X + Y * Y + Z * Z + W * W);
}
 
template<typename T>
UE_FORCEINLINE_HINT T TQuat<T>::GetAngle() const
{
    return T(2.0) * FMath::Acos(W);
}
 
 
template<typename T>
inline void TQuat<T>::ToAxisAndAngle(TVector<T>& Axis, float& Angle) const
{
    Angle = (float)GetAngle();
    Axis = GetRotationAxis();
}
 
template<typename T>
inline void TQuat<T>::ToAxisAndAngle(TVector<T>& Axis, double& Angle) const
{
    Angle = (double)GetAngle();
    Axis = GetRotationAxis();
}
 
template<typename T>
inline TVector<T> TQuat<T>::ToRotationVector() const
{
    checkSlow(IsNormalized());
    TQuat<T> RotQ = Log();
    return TVector<T>(RotQ.X * 2.0f, RotQ.Y * 2.0f, RotQ.Z * 2.0f);
}
 
template<typename T>
inline TQuat<T> TQuat<T>::MakeFromRotationVector(const TVector<T>& RotationVector)
{
    TQuat<T> RotQ(RotationVector.X * 0.5f, RotationVector.Y * 0.5f, RotationVector.Z * 0.5f, 0.0f);
    return RotQ.Exp();
}
 
template<typename T>
inline TVector<T> TQuat<T>::GetRotationAxis() const
{
#if PLATFORM_ENABLE_VECTORINTRINSICS
    TVector<T> V;
    QuatVectorRegister A = VectorLoadAligned(this);
    QuatVectorRegister R = VectorNormalizeSafe(VectorSet_W0(A), GlobalVectorConstants::Float1000);
    VectorStoreFloat3(R, &V);
    return V;
#else
    const T SquareSum = X * X + Y * Y + Z * Z;
    if (SquareSum < UE_SMALL_NUMBER)
    {
        return TVector<T>::XAxisVector;
    }
    const T Scale = FMath::InvSqrt(SquareSum);
    return TVector<T>(X * Scale, Y * Scale, Z * Scale);
#endif // PLATFORM_ENABLE_VECTORINTRINSICS
}
 
template<typename T>
T TQuat<T>::AngularDistance(const TQuat<T>& Q) const
{
    T InnerProd = X*Q.X + Y*Q.Y + Z*Q.Z + W*Q.W;
    return FMath::Acos((2 * InnerProd * InnerProd) - 1.f);
}
 
 
template<typename T>
inline TVector<T> TQuat<T>::RotateVector(TVector<T> V) const
{   
    // http://people.csail.mit.edu/bkph/articles/Quaternions.pdf
    // V' = V + 2w(Q x V) + (2Q x (Q x V))
    // refactor:
    // V' = V + w(2(Q x V)) + (Q x (2(Q x V)))
    // T = 2(Q x V);
    // V' = V + w*(T) + (Q x T)
 
    const TVector<T> Q(X, Y, Z);
    const TVector<T> TT = 2.f * TVector<T>::CrossProduct(Q, V);
    const TVector<T> Result = V + (W * TT) + TVector<T>::CrossProduct(Q, TT);
    return Result;
}
 
template<typename T>
inline TVector<T> TQuat<T>::UnrotateVector(TVector<T> V) const
{   
    const TVector<T> Q(-X, -Y, -Z); // Inverse
    const TVector<T> TT = 2.f * TVector<T>::CrossProduct(Q, V);
    const TVector<T> Result = V + (W * TT) + TVector<T>::CrossProduct(Q, TT);
    return Result;
}
 
 
template<typename T>
inline TQuat<T> TQuat<T>::Inverse() const
{
    checkSlow(IsNormalized());
 
#if PLATFORM_ENABLE_VECTORINTRINSICS
    return TQuat(VectorQuaternionInverse(VectorLoadAligned(this)));
#else
    return TQuat(-X, -Y, -Z, W);
#endif // PLATFORM_ENABLE_VECTORINTRINSICS
}
 
template<typename T>
inline void TQuat<T>::EnforceShortestArcWith(const TQuat<T>& OtherQuat)
{
    const T DotResult = (OtherQuat | *this);
    const T Bias = FMath::FloatSelect(DotResult, T(1.0f), T(-1.0f));
 
    X *= Bias;
    Y *= Bias;
    Z *= Bias;
    W *= Bias;
}
 
template<typename T>
inline TQuat<T> TQuat<T>::GetShortestArcWith(const TQuat<T>& OtherQuat) const
{
    TQuat<T> Result(*this);
    Result.EnforceShortestArcWith(OtherQuat);
    return Result;
}
 
template<typename T>
UE_FORCEINLINE_HINT TVector<T> TQuat<T>::GetAxisX() const
{
    return RotateVector(TVector<T>(1.f, 0.f, 0.f));
}
 
 
template<typename T>
UE_FORCEINLINE_HINT TVector<T> TQuat<T>::GetAxisY() const
{
    return RotateVector(TVector<T>(0.f, 1.f, 0.f));
}
 
 
template<typename T>
UE_FORCEINLINE_HINT TVector<T> TQuat<T>::GetAxisZ() const
{
    return RotateVector(TVector<T>(0.f, 0.f, 1.f));
}
 
 
template<typename T>
UE_FORCEINLINE_HINT TVector<T> TQuat<T>::GetForwardVector() const
{
    return GetAxisX();
}
 
template<typename T>
UE_FORCEINLINE_HINT TVector<T> TQuat<T>::GetRightVector() const
{
    return GetAxisY();
}
 
template<typename T>
UE_FORCEINLINE_HINT TVector<T> TQuat<T>::GetUpVector() const
{
    return GetAxisZ();
}
 
template<typename T>
UE_FORCEINLINE_HINT TVector<T> TQuat<T>::Vector() const
{
    return GetAxisX();
}
 
template<typename T>
inline TMatrix<T> TQuat<T>::ToMatrix() const
{
    TMatrix<T> R;
    ToMatrix(R);
    return R;
}
 
template<typename T>
inline T TQuat<T>::Error(const TQuat<T>& Q1, const TQuat<T>& Q2)
{
    const T cosom = FMath::Abs(Q1.X * Q2.X + Q1.Y * Q2.Y + Q1.Z * Q2.Z + Q1.W * Q2.W);
    return (FMath::Abs(cosom) < 0.9999999f) ? FMath::Acos(cosom)*(1.f / UE_PI) : 0.0f;
}
 
 
template<typename T>
inline T TQuat<T>::ErrorAutoNormalize(const TQuat<T>& A, const TQuat<T>& B)
{
    TQuat<T> Q1 = A;
    Q1.Normalize();
 
    TQuat<T> Q2 = B;
    Q2.Normalize();
 
    return TQuat<T>::Error(Q1, Q2);
}
 
template<typename T>
inline TQuat<T> TQuat<T>::FastLerp(const TQuat<T>& A, const TQuat<T>& B, const T Alpha)
{
    // To ensure the 'shortest route', we make sure the dot product between the both rotations is positive.
    const T DotResult = (A | B);
    const T Bias = FMath::FloatSelect(DotResult, T(1.0f), T(-1.0f));
    return (B * Alpha) + (A * (Bias * (1.f - Alpha)));
}
 
 
template<typename T>
inline TQuat<T> TQuat<T>::FastBilerp(const TQuat<T>& P00, const TQuat<T>& P10, const TQuat<T>& P01, const TQuat<T>& P11, T FracX, T FracY)
{
    return TQuat<T>::FastLerp(
        TQuat<T>::FastLerp(P00,P10,FracX),
        TQuat<T>::FastLerp(P01,P11,FracX),
        FracY
    );
}
 
 
template<typename T>
inline bool TQuat<T>::ContainsNaN() const
{
    return (!FMath::IsFinite(X) ||
            !FMath::IsFinite(Y) ||
            !FMath::IsFinite(Z) ||
            !FMath::IsFinite(W)
    );
}
    
template<typename T>
inline uint32 GetTypeHash(const TQuat<T>& Quat)
{
    // Note: this assumes there's no padding in Quat that could contain uncompared data.
    static_assert(sizeof(TQuat<T>) == sizeof(T[4]), "Unexpected padding in TQuat");
    return FCrc::MemCrc_DEPRECATED(&Quat, sizeof(Quat));
}
 
template<typename T>
[[nodiscard]] TTuple<T, T, T> TQuat<T>::ToLUFEuler() const
{
    auto ThreeAxisRot = [](T R11, T R12, T R21, T R31, T R32, T Euler[])
    {
        Euler[0] = FMath::Atan2(R31, R32);
        Euler[1] = FMath::Asin(R21);
        Euler[2] = FMath::Atan2(R11, R12);
    };
    
    T Euler[3];
    ThreeAxisRot(2*(Y*Z + W*X),
                 W*W - X*X + Y*Y - Z*Z,
                 -2*(X*Y - W*Z),
                 2*(X*Z + W*Y),
                 W*W + X*X - Y*Y - Z*Z,
                 Euler);
    // Going from left-handed to right-handed
    return {Euler[0], -Euler[1], -Euler[2]};
}
 
template<typename T>
[[nodiscard]] TQuat<T> TQuat<T>::MakeFromLUFEuler(const TTuple<T, T, T>& InLUFEuler)
{
    const T LeftAxisAngle = InLUFEuler.template Get<0>();
    const T UpAxisAngle = InLUFEuler.template Get<1>();
    const T ForwardAxisAngle = InLUFEuler.template Get<2>();
        
    // Going from left-handed to right-handed
    TQuat LeftRotation(TVector<T>::LeftVector, -LeftAxisAngle);
    TQuat UpRotation(TVector<T>::UpVector, -UpAxisAngle);
    TQuat ForwardRotation(TVector<T>::ForwardVector, -ForwardAxisAngle);
    return ForwardRotation * UpRotation * LeftRotation;
}
 
} // namespace UE::Math
} // namespace UE
 
UE_DECLARE_LWC_TYPE(Quat, 4);
 
template<> struct TIsPODType<FQuat4f> { enum { Value = true }; };
template<> struct TIsPODType<FQuat4d> { enum { Value = true }; };
template<> struct TIsUECoreVariant<FQuat4f> { enum { Value = true }; };
template<> struct TIsUECoreVariant<FQuat4d> { enum { Value = true }; };
template<> struct TCanBulkSerialize<FQuat4f> { enum { Value = true }; };
template<> struct TCanBulkSerialize<FQuat4d> { enum { Value = true }; };
DECLARE_INTRINSIC_TYPE_LAYOUT(FQuat4f);
DECLARE_INTRINSIC_TYPE_LAYOUT(FQuat4d);
 
// Forward declare all explicit specializations (in UnrealMath.cpp)
template<> CORE_API FRotator3f FQuat4f::Rotator() const;
template<> CORE_API FRotator3d FQuat4d::Rotator() const;
 
 
/* FMath inline functions
 *****************************************************************************/
 
 // TCustomLerp for FMath::Lerp()
template<typename T>
struct TCustomLerp< UE::Math::TQuat<T> >
{
    constexpr static bool Value = true;
    using QuatType = UE::Math::TQuat<T>;
 
    template<class U>
    static UE_FORCEINLINE_HINT QuatType Lerp(const QuatType& A, const QuatType& B, const U& Alpha)
    {
        return QuatType::Slerp(A, B, (T)Alpha);
    }
 
    template<class U>
    static inline QuatType BiLerp(const QuatType& P00, const QuatType& P10, const QuatType& P01, const QuatType& P11, const U& FracX, const U& FracY)
    {
        QuatType Result;
 
        Result = Lerp(
            QuatType::Slerp_NotNormalized(P00, P10, (T)FracX),
            QuatType::Slerp_NotNormalized(P01, P11, (T)FracX),
            (T)FracY
        );
 
        return Result;
    }
 
    template<class U>
    static UE_FORCEINLINE_HINT QuatType CubicInterp(const QuatType& P0, const QuatType& T0, const QuatType& P1, const QuatType& T1, const U& A)
    {
        return QuatType::Squad(P0, T0, P1, T1, (T)A);
    }
 
};