Rotator.h

Go to the documentation of this file.

// Copyright Epic Games, Inc. All Rights Reserved.
 
#pragma once
 
#include "CoreTypes.h"
#include "Math/MathFwd.h" // IWYU pragma: export
#include "Math/UnrealMathUtility.h"
#include "Containers/UnrealString.h"
#include "Misc/Parse.h"
#include "Misc/LargeWorldCoordinatesSerializer.h"
#include "Logging/LogMacros.h"
#include "Math/Vector.h"
#include "Math/VectorRegister.h"
#include "UObject/ObjectVersion.h"
 
namespace UE
{
namespace Math
{
 
template<typename T>
struct TRotator
{
 
    // 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>, "TRotator only supports float and double types.");
 
public:
    using FReal = T;
 
    T Pitch;
 
    T Yaw;
 
    T Roll;
 
public:
 
    CORE_API static const TRotator<T> ZeroRotator;
 
public:
 
#if ENABLE_NAN_DIAGNOSTIC
    inline void DiagnosticCheckNaN() const
    {
        if (ContainsNaN())
        {
            logOrEnsureNanError(TEXT("TRotator contains NaN: %s"), *ToString());
            *const_cast<TRotator<T>*>(this) = ZeroRotator;
        }
    }
 
    inline void DiagnosticCheckNaN(const TCHAR* Message) const
    {
        if (ContainsNaN())
        {
            logOrEnsureNanError(TEXT("%s: TRotator contains NaN: %s"), Message, *ToString());
            *const_cast<TRotator<T>*>(this) = ZeroRotator;
        }
    }
#else
    UE_FORCEINLINE_HINT void DiagnosticCheckNaN() const {}
    UE_FORCEINLINE_HINT void DiagnosticCheckNaN(const TCHAR* Message) const {}
#endif
 
    [[nodiscard]] TRotator() = default;
 
    [[nodiscard]] explicit UE_FORCEINLINE_HINT TRotator(T InF);
 
    [[nodiscard]] UE_FORCEINLINE_HINT TRotator( T InPitch, T InYaw, T InRoll );
 
    [[nodiscard]] explicit UE_FORCEINLINE_HINT TRotator( EForceInit );
 
[[nodiscard]] explicit CORE_API TRotator( const TQuat<T>& Quat );
 
public:
 
    // Binary arithmetic operators.
 
    [[nodiscard]] TRotator operator+( const TRotator<T>& R ) const;
 
    [[nodiscard]] TRotator operator-( const TRotator<T>& R ) const;
 
    template<typename FArg UE_REQUIRES(std::is_arithmetic_v<FArg>)>
    [[nodiscard]] UE_FORCEINLINE_HINT TRotator operator*( FArg Scale ) const
    {
        return TRotator(Pitch * Scale, Yaw * Scale, Roll * Scale);
    }
 
    template<typename FArg UE_REQUIRES(std::is_arithmetic_v<FArg>)>
    inline TRotator operator*=( FArg Scale )
    {
        Pitch = Pitch * Scale; Yaw = Yaw * Scale; Roll = Roll * Scale;
        DiagnosticCheckNaN();
        return *this;
    }
 
    // Binary comparison operators.
 
    [[nodiscard]] bool operator==( const TRotator<T>& R ) const;
 
    [[nodiscard]] bool operator!=( const TRotator<T>& V ) const;
 
    // Assignment operators.
 
    TRotator operator+=( const TRotator<T>& R );
 
    TRotator operator-=( const TRotator<T>& R );
 
public:
 
    // Functions.
 
    [[nodiscard]] bool IsNearlyZero( T Tolerance = UE_KINDA_SMALL_NUMBER ) const;
 
    [[nodiscard]] bool IsZero() const;
 
    [[nodiscard]] bool Equals( const TRotator<T>& R, T Tolerance = UE_KINDA_SMALL_NUMBER ) const;
 
    [[nodiscard]] bool EqualsOrientation( const TRotator<T>& R, T Tolerance = UE_KINDA_SMALL_NUMBER ) const;
 
    TRotator Add( T DeltaPitch, T DeltaYaw, T DeltaRoll );
 
    [[nodiscard]] CORE_API TRotator GetInverse() const;
 
    [[nodiscard]] TRotator GridSnap( const TRotator<T>& RotGrid ) const;
 
    [[nodiscard]] CORE_API TVector<T> Vector() const;
 
    [[nodiscard]] CORE_API TQuat<T> Quaternion() const;
 
    [[nodiscard]] CORE_API TVector<T> Euler() const;
 
    [[nodiscard]] CORE_API TVector<T> RotateVector( const UE::Math::TVector<T>& V ) const;
 
    [[nodiscard]] CORE_API TVector<T> UnrotateVector( const UE::Math::TVector<T>& V ) const;
 
    [[nodiscard]] TRotator<T> Clamp() const;
 
    [[nodiscard]] TRotator<T> GetNormalized() const;
 
    [[nodiscard]] TRotator<T> GetDenormalized() const;
 
    [[nodiscard]] T GetComponentForAxis(EAxis::Type Axis) const;
 
    void SetComponentForAxis(EAxis::Type Axis, T Component);
 
    void Normalize();
 
    CORE_API void GetWindingAndRemainder( TRotator<T>& Winding, TRotator<T>& Remainder ) const;
 
    [[nodiscard]] T GetManhattanDistance(const TRotator<T> & Rotator) const;
 
    [[nodiscard]] TRotator GetEquivalentRotator() const;
 
    void SetClosestToMe(TRotator& MakeClosest) const;
 
    [[nodiscard]] FString ToString() const;
 
    [[nodiscard]] FString ToCompactString() const;
 
    bool InitFromString( const FString& InSourceString );
 
    [[nodiscard]] bool ContainsNaN() const;
 
    CORE_API void SerializeCompressed( FArchive& Ar );
 
    CORE_API void SerializeCompressedShort( FArchive& Ar );
 
    CORE_API bool NetSerialize( FArchive& Ar, class UPackageMap* Map, bool& bOutSuccess );
 
public:
    
    [[nodiscard]] static T ClampAxis( T Angle );
 
    [[nodiscard]] static T NormalizeAxis( T Angle );
 
    [[nodiscard]] static uint8 CompressAxisToByte( T Angle );
 
    [[nodiscard]] static T DecompressAxisFromByte( uint8 Angle );
 
    [[nodiscard]] static uint16 CompressAxisToShort( T Angle );
 
    [[nodiscard]] static T DecompressAxisFromShort( uint16 Angle );
 
    [[nodiscard]] static CORE_API TRotator MakeFromEuler( const TVector<T>& Euler );
 
 
public:
 
    bool Serialize( FArchive& Ar )
    {
        Ar << *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, Rotator, Rotator3f, Rotator3d);
        }
        else
        {
            return UE_SERIALIZE_VARIANT_FROM_MISMATCHED_TAG(Ar, Rotator, Rotator3d, Rotator3f);
        }
    }
 
    // Conversion from other type.
    template<typename FArg UE_REQUIRES(!std::is_same_v<T, FArg>)>
    explicit TRotator(const TRotator<FArg>& From)
        : TRotator<T>((T)From.Pitch, (T)From.Yaw, (T)From.Roll)
    {
    }
};
 
#if !defined(_MSC_VER) || defined(__clang__)  // MSVC can't forward declare explicit specializations
template<> CORE_API const FRotator3f FRotator3f::ZeroRotator;
template<> CORE_API const FRotator3d FRotator3d::ZeroRotator;
#endif
 
/* TRotator inline functions
 *****************************************************************************/
 
inline FArchive& operator<<(FArchive& Ar, TRotator<float>& R)
{
    Ar << R.Pitch << R.Yaw << R.Roll;
    return Ar;
}
 
inline FArchive& operator<<(FArchive& Ar, TRotator<double>& R)
{
    if (Ar.UEVer() >= EUnrealEngineObjectUE5Version::LARGE_WORLD_COORDINATES)
    {
        Ar << R.Pitch << R.Yaw << R.Roll;
    }
    else
    {
        checkf(Ar.IsLoading(), TEXT("float -> double conversion applied outside of load!"));
        // Stored as floats, so serialize float and copy.
        float Pitch, Yaw, Roll;
        Ar << Pitch << Yaw << Roll;
        R = TRotator<double>(Pitch, Yaw, Roll);
    }
    return Ar;
}
 
/* FRotator inline functions
 *****************************************************************************/
 
template<typename T, typename FArg UE_REQUIRES(std::is_arithmetic_v<FArg>)>
UE_FORCEINLINE_HINT TRotator<T> operator*(FArg Scale, const TRotator<T>& R )
{
    return R.operator*( Scale );
}
 
template<typename T>
inline TRotator<T>::TRotator( T InF ) 
    :   Pitch(InF), Yaw(InF), Roll(InF) 
{
    DiagnosticCheckNaN();
}
 
template<typename T>
inline TRotator<T>::TRotator( T InPitch, T InYaw, T InRoll )
    :   Pitch(InPitch), Yaw(InYaw), Roll(InRoll) 
{
    DiagnosticCheckNaN();
}
 
template<typename T>
UE_FORCEINLINE_HINT TRotator<T>::TRotator(EForceInit)
    : Pitch(0), Yaw(0), Roll(0)
{}
 
template<typename T>
UE_FORCEINLINE_HINT TRotator<T> TRotator<T>::operator+( const TRotator<T>& R ) const
{
    return TRotator( Pitch+R.Pitch, Yaw+R.Yaw, Roll+R.Roll );
}
 
template<typename T>
UE_FORCEINLINE_HINT TRotator<T> TRotator<T>::operator-( const TRotator<T>& R ) const
{
    return TRotator( Pitch-R.Pitch, Yaw-R.Yaw, Roll-R.Roll );
}
 
template<typename T>
UE_FORCEINLINE_HINT bool TRotator<T>::operator==( const TRotator<T>& R ) const
{
    return Pitch==R.Pitch && Yaw==R.Yaw && Roll==R.Roll;
}
 
template<typename T>
UE_FORCEINLINE_HINT bool TRotator<T>::operator!=( const TRotator<T>& V ) const
{
    return Pitch!=V.Pitch || Yaw!=V.Yaw || Roll!=V.Roll;
}
 
template<typename T>
inline TRotator<T> TRotator<T>::operator+=( const TRotator<T>& R )
{
    Pitch += R.Pitch; Yaw += R.Yaw; Roll += R.Roll;
    DiagnosticCheckNaN();
    return *this;
}
 
template<typename T>
inline TRotator<T> TRotator<T>::operator-=( const TRotator<T>& R )
{
    Pitch -= R.Pitch; Yaw -= R.Yaw; Roll -= R.Roll;
    DiagnosticCheckNaN();
    return *this;
}
 
template<typename T>
inline bool TRotator<T>::IsNearlyZero(T Tolerance) const
{
#if PLATFORM_ENABLE_VECTORINTRINSICS
    const TVectorRegisterType<T> RegA = VectorLoadFloat3_W0(this);
    const TVectorRegisterType<T> Norm = VectorNormalizeRotator(RegA);
    const TVectorRegisterType<T> AbsNorm = VectorAbs(Norm);
    return !VectorAnyGreaterThan(AbsNorm, VectorLoadFloat1(&Tolerance));
#else
    return
        FMath::Abs(NormalizeAxis(Pitch))<=Tolerance
        &&  FMath::Abs(NormalizeAxis(Yaw))<=Tolerance
        &&  FMath::Abs(NormalizeAxis(Roll))<=Tolerance;
#endif
}
 
template<typename T>
UE_FORCEINLINE_HINT bool TRotator<T>::IsZero() const
{
    return (ClampAxis(Pitch)==0.f) && (ClampAxis(Yaw)==0.f) && (ClampAxis(Roll)==0.f);
}
 
template<typename T>
inline bool TRotator<T>::Equals(const TRotator<T>& R, T Tolerance) const
{
#if PLATFORM_ENABLE_VECTORINTRINSICS
    const TVectorRegisterType<T> RegA = VectorLoadFloat3_W0(this);
    const TVectorRegisterType<T> RegB = VectorLoadFloat3_W0(&R);
    const TVectorRegisterType<T> NormDelta = VectorNormalizeRotator(VectorSubtract(RegA, RegB));
    const TVectorRegisterType<T> AbsNormDelta = VectorAbs(NormDelta);
    return !VectorAnyGreaterThan(AbsNormDelta, VectorLoadFloat1(&Tolerance));
#else
    return (FMath::Abs(NormalizeAxis(Pitch - R.Pitch)) <= Tolerance) 
        && (FMath::Abs(NormalizeAxis(Yaw - R.Yaw)) <= Tolerance) 
        && (FMath::Abs(NormalizeAxis(Roll - R.Roll)) <= Tolerance);
#endif
}
 
template <typename T>
bool TRotator<T>::EqualsOrientation(const TRotator<T>& R, T Tolerance) const
{
    return Quaternion().AngularDistance(R.Quaternion()) <= Tolerance;
}
 
template<typename T>
inline TRotator<T> TRotator<T>::Add( T DeltaPitch, T DeltaYaw, T DeltaRoll )
{
    Yaw   += DeltaYaw;
    Pitch += DeltaPitch;
    Roll  += DeltaRoll;
    DiagnosticCheckNaN();
    return *this;
}
 
template<typename T>
inline TRotator<T> TRotator<T>::GridSnap( const TRotator<T>& RotGrid ) const
{
    return TRotator
        (
        FMath::GridSnap(Pitch,RotGrid.Pitch),
        FMath::GridSnap(Yaw,  RotGrid.Yaw),
        FMath::GridSnap(Roll, RotGrid.Roll)
        );
}
 
template<typename T>
UE_FORCEINLINE_HINT TRotator<T> TRotator<T>::Clamp() const
{
    return TRotator(ClampAxis(Pitch), ClampAxis(Yaw), ClampAxis(Roll));
}
 
template<typename T>
inline T TRotator<T>::ClampAxis( T Angle )
{
    // returns Angle in the range (-360,360)
    Angle = FMath::Fmod(Angle, (T)360.0);
 
    if (Angle < (T)0.0)
    {
        // shift to [0,360) range
        Angle += (T)360.0;
    }
 
    return Angle;
}
 
template<typename T>
inline T TRotator<T>::NormalizeAxis( T Angle )
{
    // returns Angle in the range [0,360)
    Angle = ClampAxis(Angle);
 
    if (Angle > (T)180.0)
    {
        // shift to (-180,180]
        Angle -= (T)360.0;
    }
 
    return Angle;
}
 
template<typename T>
UE_FORCEINLINE_HINT uint8 TRotator<T>::CompressAxisToByte( T Angle )
{
    // map [0->360) to [0->256) and mask off any winding
    return FMath::RoundToInt(Angle * (T)256.f / (T)360.f) & 0xFF;
}
 
template<typename T>
UE_FORCEINLINE_HINT T TRotator<T>::DecompressAxisFromByte( uint8 Angle )
{
    // map [0->256) to [0->360)
    return (Angle * (T)360.f / (T)256.f);
}
 
template<typename T>
UE_FORCEINLINE_HINT uint16 TRotator<T>::CompressAxisToShort( T Angle )
{
    // map [0->360) to [0->65536) and mask off any winding
    return FMath::RoundToInt(Angle * (T)65536.f / (T)360.f) & 0xFFFF;
}
 
template<typename T>
UE_FORCEINLINE_HINT T TRotator<T>::DecompressAxisFromShort( uint16 Angle )
{
    // map [0->65536) to [0->360)
    return (Angle * (T)360.f / (T)65536.f);
}
 
template<typename T>
inline TRotator<T> TRotator<T>::GetNormalized() const
{
    TRotator Rot = *this;
    Rot.Normalize();
    return Rot;
}
 
template<typename T>
inline TRotator<T> TRotator<T>::GetDenormalized() const
{
    TRotator Rot = *this;
    Rot.Pitch   = ClampAxis(Rot.Pitch);
    Rot.Yaw     = ClampAxis(Rot.Yaw);
    Rot.Roll    = ClampAxis(Rot.Roll);
    return Rot;
}
 
template<typename T>
inline void TRotator<T>::Normalize()
{
#if PLATFORM_ENABLE_VECTORINTRINSICS
    TVectorRegisterType<T> VRotator = VectorLoadFloat3_W0(this);
    VRotator = VectorNormalizeRotator(VRotator);
    VectorStoreFloat3(VRotator, this);
#else
    Pitch = NormalizeAxis(Pitch);
    Yaw = NormalizeAxis(Yaw);
    Roll = NormalizeAxis(Roll);
#endif
    DiagnosticCheckNaN();
}
 
template<typename T>
inline T TRotator<T>::GetComponentForAxis(EAxis::Type Axis) const
{
    switch (Axis)
    {
    case EAxis::X:
        return Roll;
    case EAxis::Y:
        return Pitch;
    case EAxis::Z:
        return Yaw;
    case EAxis::None:
    default:
        return 0.f;
    }
}
 
template<typename T>
inline void TRotator<T>::SetComponentForAxis(EAxis::Type Axis, T Component)
{
    switch (Axis)
    {
    case EAxis::X:
        Roll = Component;
        break;
    case EAxis::Y:
        Pitch = Component;
        break;
    case EAxis::Z:
        Yaw = Component;
        break;
    case EAxis::None:
    default:
        break;
    }
}
 
template<typename T>
UE_FORCEINLINE_HINT FString TRotator<T>::ToString() const
{
    return FString::Printf(TEXT("P=%f Y=%f R=%f"), Pitch, Yaw, Roll );
}
 
template<typename T>
inline FString TRotator<T>::ToCompactString() const
{
    if( IsNearlyZero() )
    {
        return FString(TEXT("R(0)"));
    }
 
    FString ReturnString(TEXT("R("));
    bool bIsEmptyString = true;
    if( !FMath::IsNearlyZero(Pitch) )
    {
        ReturnString += FString::Printf(TEXT("P=%.2f"), Pitch);
        bIsEmptyString = false;
    }
    if( !FMath::IsNearlyZero(Yaw) )
    {
        if( !bIsEmptyString )
        {
            ReturnString += FString(TEXT(", "));
        }
        ReturnString += FString::Printf(TEXT("Y=%.2f"), Yaw);
        bIsEmptyString = false;
    }
    if( !FMath::IsNearlyZero(Roll) )
    {
        if( !bIsEmptyString )
        {
            ReturnString += FString(TEXT(", "));
        }
        ReturnString += FString::Printf(TEXT("R=%.2f"), Roll);
        bIsEmptyString = false;
    }
    ReturnString += FString(TEXT(")"));
    return ReturnString;
}
 
template<typename T>
inline bool TRotator<T>::InitFromString( const FString& InSourceString )
{
    Pitch = Yaw = Roll = 0;
 
    // The initialization is only successful if the X, Y, and Z values can all be parsed from the string
    const bool bSuccessful = FParse::Value( *InSourceString, TEXT("P=") , Pitch ) && FParse::Value( *InSourceString, TEXT("Y="), Yaw ) && FParse::Value( *InSourceString, TEXT("R="), Roll );
    DiagnosticCheckNaN();
    return bSuccessful;
}
 
template<typename T>
inline bool TRotator<T>::ContainsNaN() const
{
    return (!FMath::IsFinite(Pitch) ||
            !FMath::IsFinite(Yaw) ||
            !FMath::IsFinite(Roll));
}
 
template<typename T>
UE_FORCEINLINE_HINT T TRotator<T>::GetManhattanDistance(const TRotator<T> & Rotator) const
{
    return FMath::Abs<T>(Yaw - Rotator.Yaw) + FMath::Abs<T>(Pitch - Rotator.Pitch) + FMath::Abs<T>(Roll - Rotator.Roll);
}
 
template<typename T>
UE_FORCEINLINE_HINT TRotator<T> TRotator<T>::GetEquivalentRotator() const
{
    return TRotator(180.0f - Pitch,Yaw + 180.0f, Roll + 180.0f);
}
 
template<typename T>
inline void TRotator<T>::SetClosestToMe(TRotator& MakeClosest) const
{
    TRotator OtherChoice = MakeClosest.GetEquivalentRotator();
    T FirstDiff = GetManhattanDistance(MakeClosest);
    T SecondDiff = GetManhattanDistance(OtherChoice);
    if (SecondDiff < FirstDiff)
        MakeClosest = OtherChoice;
}
 
} // namespace UE::Math
} // namespace UE
 
UE_DECLARE_LWC_TYPE(Rotator, 3);
 
template<> struct TCanBulkSerialize<FRotator3f> { enum { Value = false }; };
template<> struct TIsPODType<FRotator3f> { enum { Value = true }; };
template<> struct TIsUECoreVariant<FRotator3f> { enum { Value = true }; };
DECLARE_INTRINSIC_TYPE_LAYOUT(FRotator3f);
 
template<> struct TCanBulkSerialize<FRotator3d> { enum { Value = false }; };
template<> struct TIsPODType<FRotator3d> { enum { Value = true }; };
template<> struct TIsUECoreVariant<FRotator3d> { enum { Value = true }; };
DECLARE_INTRINSIC_TYPE_LAYOUT(FRotator3d);
 
// Forward declare all explicit specializations (in UnrealMath.cpp)
template<> CORE_API FQuat4f FRotator3f::Quaternion() const;
template<> CORE_API FQuat4d FRotator3d::Quaternion() const;
 
 
/* FMath inline functions
 *****************************************************************************/
 
template<typename T>
struct TCustomLerp<UE::Math::TRotator<T>>
{
    // Required to make FMath::Lerp<TRotator>() call our custom Lerp() implementation below.
    constexpr static bool Value = true;
    using RotatorType = UE::Math::TRotator<T>;
 
    template<class U>
    static UE_FORCEINLINE_HINT RotatorType Lerp(const RotatorType& A, const RotatorType& B, const U& Alpha)
    {
        return A + (B - A).GetNormalized() * Alpha;
    }
};
 
template< typename T, typename U >
UE_FORCEINLINE_HINT UE::Math::TRotator<T> FMath::LerpRange(const UE::Math::TRotator<T>& A, const UE::Math::TRotator<T>& B, U Alpha)
{
    // Similar to Lerp, but does not take the shortest path. Allows interpolation over more than 180 degrees.
    return (A * ((T)1.0 - (T)Alpha) + B * Alpha).GetNormalized();
}
 
template<typename T>
inline T FMath::ClampAngle(T AngleDegrees, T MinAngleDegrees, T MaxAngleDegrees)
{
    const T MaxDelta = UE::Math::TRotator<T>::ClampAxis(MaxAngleDegrees - MinAngleDegrees) * 0.5f;          // 0..180
    const T RangeCenter = UE::Math::TRotator<T>::ClampAxis(MinAngleDegrees + MaxDelta);                     // 0..360
    const T DeltaFromCenter = UE::Math::TRotator<T>::NormalizeAxis(AngleDegrees - RangeCenter);             // -180..180
 
    // maybe clamp to nearest edge
    if (DeltaFromCenter > MaxDelta)
    {
        return UE::Math::TRotator<T>::NormalizeAxis(RangeCenter + MaxDelta);
    }
    else if (DeltaFromCenter < -MaxDelta)
    {
        return UE::Math::TRotator<T>::NormalizeAxis(RangeCenter - MaxDelta);
    }
 
    // already in range, just return it
    return UE::Math::TRotator<T>::NormalizeAxis(AngleDegrees);
}