PHY/Plugins/GMS/Source/GenericMovementSystem/Public/Utility/GMS_Rotation.h

// Copyright 2025 https://yuewu.dev/en  All Rights Reserved.

#pragma once

#include "GMS_Math.h"
#include "GMS_Rotation.generated.h"

UCLASS(Meta = (BlueprintThreadSafe))
class GENERICMOVEMENTSYSTEM_API UGMS_Rotation : public UBlueprintFunctionLibrary
{
	GENERATED_BODY()

public:
	static constexpr auto CounterClockwiseRotationAngleThreshold{5.0f};

public:
	template <typename ValueType> requires std::is_floating_point_v<ValueType>
	static constexpr ValueType RemapAngleForCounterClockwiseRotation(ValueType Angle);

	static VectorRegister4Double RemapRotationForCounterClockwiseRotation(const VectorRegister4Double& Rotation);

	// Remaps the angle from the [175, 180] range to [-185, -180]. Used to
	// make the character rotate counterclockwise during a 180 degree turn.
	UFUNCTION(BlueprintCallable, BlueprintPure, Category = "GMS|Rotation Utility", Meta = (ReturnDisplayName = "Angle"))
	static float RemapAngleForCounterClockwiseRotation(float Angle);

	UFUNCTION(BlueprintCallable, BlueprintPure, Category = "GMS|Rotation Utility", Meta = (ReturnDisplayName = "Angle"))
	static float LerpAngle(float From, float To, float Ratio);

	UFUNCTION(BlueprintCallable, BlueprintPure, Category = "GMS|Rotation Utility", Meta = (AutoCreateRefTerm = "From, To", ReturnDisplayName = "Rotation"))
	static FRotator LerpRotation(const FRotator& From, const FRotator& To, float Ratio);

	UFUNCTION(BlueprintCallable, BlueprintPure, Category = "GMS|Rotation Utility", Meta = (ReturnDisplayName = "Angle"))
	static float InterpolateAngleConstant(float Current, float Target, float DeltaTime, float Speed);

	/**
	 * Smoothly lerp current to target in fame-rate stable way.
	 * 以帧率稳定方式平滑地从Current过渡到Target。
	 * @param HalfLife How fast to reach target.
	 */
	UFUNCTION(BlueprintCallable, BlueprintPure, Category = "GMS|Rotation Utility", Meta = (ReturnDisplayName = "Angle"))
	static float DamperExactAngle(float Current, float Target, float DeltaTime, float HalfLife);

	// HalfLife is the time it takes for the distance to the target to be reduced by half.
	UFUNCTION(BlueprintCallable, BlueprintPure, Category = "GMS|Rotation Utility",
		Meta = (AutoCreateRefTerm = "Current, Target", ReturnDisplayName = "Rotation"))
	static FRotator DamperExactRotation(const FRotator& Current, const FRotator& Target, float DeltaTime, float HalfLife);

	// Same as FMath::QInterpTo(), but uses FQuat::FastLerp() instead of FQuat::Slerp().
	UFUNCTION(BlueprintCallable, BlueprintPure, Category = "GMS|Rotation Utility", Meta = (ReturnDisplayName = "Quaternion"))
	static FQuat InterpolateQuaternionFast(const FQuat& Current, const FQuat& Target, float DeltaTime, float Speed);

	UFUNCTION(BlueprintCallable, BlueprintPure, Category = "GMS|Rotation Utility", Meta = (AutoCreateRefTerm = "TwistAxis", ReturnDisplayName = "Twist"))
	static FQuat GetTwist(const FQuat& Quaternion, const FVector& TwistAxis = FVector::UpVector);
};

template <typename ValueType> requires std::is_floating_point_v<ValueType>
constexpr ValueType UGMS_Rotation::RemapAngleForCounterClockwiseRotation(const ValueType Angle)
{
	if (Angle > 180.0f - CounterClockwiseRotationAngleThreshold)
	{
		return Angle - 360.0f;
	}

	return Angle;
}

inline VectorRegister4Double UGMS_Rotation::RemapRotationForCounterClockwiseRotation(const VectorRegister4Double& Rotation)
{
	static constexpr auto RemapThreshold{
		MakeVectorRegisterDoubleConstant(180.0f - CounterClockwiseRotationAngleThreshold, 180.0f - CounterClockwiseRotationAngleThreshold,
		                                 180.0f - CounterClockwiseRotationAngleThreshold, 180.0f - CounterClockwiseRotationAngleThreshold)
	};

	static constexpr auto RemapAngles{MakeVectorRegisterDoubleConstant(360.0f, 360.0f, 360.0f, 0.0f)};

	const auto ReverseRotationMask{VectorCompareGE(Rotation, RemapThreshold)};

	const auto ReversedRotation{VectorSubtract(Rotation, RemapAngles)};

	return VectorSelect(ReverseRotationMask, ReversedRotation, Rotation);
}

inline float UGMS_Rotation::RemapAngleForCounterClockwiseRotation(const float Angle)
{
	return RemapAngleForCounterClockwiseRotation<float>(Angle);
}

inline float UGMS_Rotation::LerpAngle(const float From, const float To, const float Ratio)
{
	auto Delta{FMath::UnwindDegrees(To - From)};
	Delta = RemapAngleForCounterClockwiseRotation(Delta);

	return FMath::UnwindDegrees(From + Delta * Ratio);
}

inline FRotator UGMS_Rotation::LerpRotation(const FRotator& From, const FRotator& To, const float Ratio)
{
#if PLATFORM_ENABLE_VECTORINTRINSICS
	const auto FromRegister{VectorLoadFloat3_W0(&From)};
	const auto ToRegister{VectorLoadFloat3_W0(&To)};

	auto Delta{VectorSubtract(ToRegister, FromRegister)};
	Delta = VectorNormalizeRotator(Delta);

	if (!VectorAnyGreaterThan(VectorAbs(Delta), GlobalVectorConstants::DoubleKindaSmallNumber))
	{
		return To;
	}

	Delta = RemapRotationForCounterClockwiseRotation(Delta);

	auto ResultRegister{VectorMultiplyAdd(Delta, VectorLoadFloat1(&Ratio), FromRegister)};
	ResultRegister = VectorNormalizeRotator(ResultRegister);

	FRotator Result;
	VectorStoreFloat3(ResultRegister, &Result);

	return Result;
#else
	auto Result{To - From};
	Result.Normalize();

	Result.Pitch = RemapAngleForCounterClockwiseRotation(Result.Pitch);
	Result.Yaw = RemapAngleForCounterClockwiseRotation(Result.Yaw);
	Result.Roll = RemapAngleForCounterClockwiseRotation(Result.Roll);

	Result *= Ratio;
	Result += From;
	Result.Normalize();

	return Result;
#endif
}

inline float UGMS_Rotation::InterpolateAngleConstant(const float Current, const float Target, const float DeltaTime, const float Speed)
{
	auto Delta{FMath::UnwindDegrees(Target - Current)};
	const auto MaxDelta{Speed * DeltaTime};

	if (Speed <= 0.0f || FMath::Abs(Delta) <= MaxDelta)
	{
		return Target;
	}

	Delta = RemapAngleForCounterClockwiseRotation(Delta);
	return FMath::UnwindDegrees(Current + FMath::Sign(Delta) * MaxDelta);
}

inline float UGMS_Rotation::DamperExactAngle(const float Current, const float Target, const float DeltaTime, const float HalfLife)
{
	auto Delta{FMath::UnwindDegrees(Target - Current)};

	if (FMath::IsNearlyZero(Delta, UE_KINDA_SMALL_NUMBER))
	{
		return Target;
	}

	Delta = RemapAngleForCounterClockwiseRotation(Delta);

	const auto Alpha{UGMS_Math::DamperExactAlpha(DeltaTime, HalfLife)};
	return FMath::UnwindDegrees(Current + Delta * Alpha);
}

inline FRotator UGMS_Rotation::DamperExactRotation(const FRotator& Current, const FRotator& Target,
                                                   const float DeltaTime, const float HalfLife)
{
#if PLATFORM_ENABLE_VECTORINTRINSICS
	const auto CurrentRegister{VectorLoadFloat3_W0(&Current)};
	const auto TargetRegister{VectorLoadFloat3_W0(&Target)};

	auto Delta{VectorSubtract(TargetRegister, CurrentRegister)};
	Delta = VectorNormalizeRotator(Delta);

	if (!VectorAnyGreaterThan(VectorAbs(Delta), GlobalVectorConstants::DoubleKindaSmallNumber))
	{
		return Target;
	}

	Delta = RemapRotationForCounterClockwiseRotation(Delta);

	const double Alpha{UGMS_Math::DamperExactAlpha(DeltaTime, HalfLife)};

	auto ResultRegister{VectorMultiplyAdd(Delta, VectorLoadDouble1(&Alpha), CurrentRegister)};
	ResultRegister = VectorNormalizeRotator(ResultRegister);

	FRotator Result;
	VectorStoreFloat3(ResultRegister, &Result);

	return Result;
#else
	auto Result{Target - Current};
	Result.Normalize();

	if (FMath::IsNearlyZero(Result.Pitch, UE_KINDA_SMALL_NUMBER) &&
		FMath::IsNearlyZero(Result.Yaw, UE_KINDA_SMALL_NUMBER) &&
		FMath::IsNearlyZero(Result.Roll, UE_KINDA_SMALL_NUMBER))
	{
		return Target;
	}

	Result.Pitch = RemapAngleForCounterClockwiseRotation(Result.Pitch);
	Result.Yaw = RemapAngleForCounterClockwiseRotation(Result.Yaw);
	Result.Roll = RemapAngleForCounterClockwiseRotation(Result.Roll);

	const auto Alpha{UGMS_Math::DamperExactAlpha(DeltaTime, HalfLife)};

	Result *= Alpha;
	Result += Current;
	Result.Normalize();

	return Result;
#endif
}

inline FQuat UGMS_Rotation::InterpolateQuaternionFast(const FQuat& Current, const FQuat& Target, const float DeltaTime, const float Speed)
{
	if (Speed <= 0.0f || Current.Equals(Target))
	{
		return Target;
	}

	return FQuat::FastLerp(Current, Target, UGMS_Math::Clamp01(Speed * DeltaTime)).GetNormalized();
}

inline FQuat UGMS_Rotation::GetTwist(const FQuat& Quaternion, const FVector& TwistAxis)
{
	// Based on TQuat<T>::ToSwingTwist().

	const auto Projection{(TwistAxis | FVector{Quaternion.X, Quaternion.Y, Quaternion.Z}) * TwistAxis};

	return FQuat{Projection.X, Projection.Y, Projection.Z, Quaternion.W}.GetNormalized();
}