mirror of
https://github.com/Milxnor/Project-Reboot-3.0.git
synced 2026-01-13 10:52:22 +01:00
104 lines
3.5 KiB
C++
104 lines
3.5 KiB
C++
#include "Rotator.h"
|
|
|
|
#define INV_PI (0.31830988618f)
|
|
#define HALF_PI (1.57079632679f)
|
|
#define PI (3.1415926535897932f)
|
|
|
|
static FORCEINLINE void SinCos(float* ScalarSin, float* ScalarCos, float Value)
|
|
{
|
|
// Map Value to y in [-pi,pi], x = 2*pi*quotient + remainder.
|
|
float quotient = (INV_PI * 0.5f) * Value;
|
|
if (Value >= 0.0f)
|
|
{
|
|
quotient = (float)((int)(quotient + 0.5f));
|
|
}
|
|
else
|
|
{
|
|
quotient = (float)((int)(quotient - 0.5f));
|
|
}
|
|
float y = Value - (2.0f * PI) * quotient;
|
|
|
|
// Map y to [-pi/2,pi/2] with sin(y) = sin(Value).
|
|
float sign;
|
|
if (y > HALF_PI)
|
|
{
|
|
y = PI - y;
|
|
sign = -1.0f;
|
|
}
|
|
else if (y < -HALF_PI)
|
|
{
|
|
y = -PI - y;
|
|
sign = -1.0f;
|
|
}
|
|
else
|
|
{
|
|
sign = +1.0f;
|
|
}
|
|
|
|
float y2 = y * y;
|
|
|
|
// 11-degree minimax approximation
|
|
*ScalarSin = (((((-2.3889859e-08f * y2 + 2.7525562e-06f) * y2 - 0.00019840874f) * y2 + 0.0083333310f) * y2 - 0.16666667f) * y2 + 1.0f) * y;
|
|
|
|
// 10-degree minimax approximation
|
|
float p = ((((-2.6051615e-07f * y2 + 2.4760495e-05f) * y2 - 0.0013888378f) * y2 + 0.041666638f) * y2 - 0.5f) * y2 + 1.0f;
|
|
*ScalarCos = sign * p;
|
|
}
|
|
|
|
struct FQuat FRotator::Quaternion()
|
|
{
|
|
|
|
#if PLATFORM_ENABLE_VECTORINTRINSICS
|
|
const VectorRegister Angles = MakeVectorRegister(Rotator.Pitch, Rotator.Yaw, Rotator.Roll, 0.0f);
|
|
const VectorRegister HalfAngles = VectorMultiply(Angles, DEG_TO_RAD_HALF);
|
|
|
|
VectorRegister SinAngles, CosAngles;
|
|
VectorSinCos(&SinAngles, &CosAngles, &HalfAngles);
|
|
|
|
// Vectorized conversion, measured 20% faster than using scalar version after VectorSinCos.
|
|
// Indices within VectorRegister (for shuffles): P=0, Y=1, R=2
|
|
const VectorRegister SR = VectorReplicate(SinAngles, 2);
|
|
const VectorRegister CR = VectorReplicate(CosAngles, 2);
|
|
|
|
const VectorRegister SY_SY_CY_CY_Temp = VectorShuffle(SinAngles, CosAngles, 1, 1, 1, 1);
|
|
|
|
const VectorRegister SP_SP_CP_CP = VectorShuffle(SinAngles, CosAngles, 0, 0, 0, 0);
|
|
const VectorRegister SY_CY_SY_CY = VectorShuffle(SY_SY_CY_CY_Temp, SY_SY_CY_CY_Temp, 0, 2, 0, 2);
|
|
|
|
const VectorRegister CP_CP_SP_SP = VectorShuffle(CosAngles, SinAngles, 0, 0, 0, 0);
|
|
const VectorRegister CY_SY_CY_SY = VectorShuffle(SY_SY_CY_CY_Temp, SY_SY_CY_CY_Temp, 2, 0, 2, 0);
|
|
|
|
const uint32 Neg = uint32(1 << 31);
|
|
const uint32 Pos = uint32(0);
|
|
const VectorRegister SignBitsLeft = MakeVectorRegister(Pos, Neg, Pos, Pos);
|
|
const VectorRegister SignBitsRight = MakeVectorRegister(Neg, Neg, Neg, Pos);
|
|
const VectorRegister LeftTerm = VectorBitwiseXor(SignBitsLeft, VectorMultiply(CR, VectorMultiply(SP_SP_CP_CP, SY_CY_SY_CY)));
|
|
const VectorRegister RightTerm = VectorBitwiseXor(SignBitsRight, VectorMultiply(SR, VectorMultiply(CP_CP_SP_SP, CY_SY_CY_SY)));
|
|
|
|
FQuat RotationQuat;
|
|
const VectorRegister Result = VectorAdd(LeftTerm, RightTerm);
|
|
VectorStoreAligned(Result, &RotationQuat);
|
|
#else
|
|
const float DEG_TO_RAD = PI / (180.f);
|
|
const float DIVIDE_BY_2 = DEG_TO_RAD / 2.f;
|
|
float SP, SY, SR;
|
|
float CP, CY, CR;
|
|
|
|
SinCos(&SP, &CP, Pitch * DIVIDE_BY_2);
|
|
SinCos(&SY, &CY, Yaw * DIVIDE_BY_2);
|
|
SinCos(&SR, &CR, Roll * DIVIDE_BY_2);
|
|
|
|
FQuat RotationQuat{};
|
|
RotationQuat.X = CR * SP * SY - SR * CP * CY;
|
|
RotationQuat.Y = -CR * SP * CY - SR * CP * SY;
|
|
RotationQuat.Z = CR * CP * SY - SR * SP * CY;
|
|
RotationQuat.W = CR * CP * CY + SR * SP * SY;
|
|
#endif // PLATFORM_ENABLE_VECTORINTRINSICS
|
|
|
|
#if ENABLE_NAN_DIAGNOSTIC || DO_CHECK
|
|
// Very large inputs can cause NaN's. Want to catch this here
|
|
ensureMsgf(!RotationQuat.ContainsNaN(), TEXT("Invalid input to FRotator::Quaternion - generated NaN output: %s"), *RotationQuat.ToString());
|
|
#endif
|
|
|
|
return RotationQuat;
|
|
} |