UE5欧拉角转四元数

薛文旺

于 2024-05-18 15:58:57 发布

阅读量144

点赞数 1

分类专栏：图形学文章标签： ue5

本文链接：https://blog.csdn.net/wenwang88/article/details/139026926

版权

图形学专栏收录该内容

3 篇文章 0 订阅

订阅专栏

TQuat

@/p/sx/UnrealEngine/UnrealEngine/Engine/Source/Runtime/Core/Public/Math/Quat.h
FORCEINLINE TQuat<T>::TQuat(const TRotator<T>& R)
    *this = R.Quaternion();
@/p/sx/UnrealEngine/UnrealEngine/Engine/Source/Runtime/Core/Public/Math/Rotator.h
@/p/sx/UnrealEngine/UnrealEngine/Engine/Source/Runtime/Core/Private/Math/UnrealMath.cpp
        FQuat4d FRotator3d::Quaternion() const
        DiagnosticCheckNaN();
    
    #if PLATFORM_ENABLE_VECTORINTRINSICS
        const VectorRegister4Double Angles = MakeVectorRegisterDouble(Pitch, Yaw, Roll, 0.0);
        const VectorRegister4Double AnglesNoWinding = VectorMod360(Angles);
        const VectorRegister4Double HalfAngles = VectorMultiply(AnglesNoWinding, GlobalVectorConstants::DOUBLE_DEG_TO_RAD_HALF);
    
        VectorRegister4Double 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 VectorRegister4Double SR = VectorReplicate(SinAngles, 2);
        const VectorRegister4Double CR = VectorReplicate(CosAngles, 2);
    
        const VectorRegister4Double SY_SY_CY_CY_Temp = VectorShuffle(SinAngles, CosAngles, 1, 1, 1, 1);
    
        const VectorRegister4Double SP_SP_CP_CP = VectorShuffle(SinAngles, CosAngles, 0, 0, 0, 0);
        const VectorRegister4Double SY_CY_SY_CY = VectorShuffle(SY_SY_CY_CY_Temp, SY_SY_CY_CY_Temp, 0, 2, 0, 2);
    
        const VectorRegister4Double CP_CP_SP_SP = VectorShuffle(CosAngles, SinAngles, 0, 0, 0, 0);
        const VectorRegister4Double CY_SY_CY_SY = VectorShuffle(SY_SY_CY_CY_Temp, SY_SY_CY_CY_Temp, 2, 0, 2, 0);
    
        const uint64 Neg = uint64(1) << 63;
        const uint64 Pos = uint64(0);
        const VectorRegister4Double SignBitsLeft = MakeVectorRegisterDoubleMask(Pos, Neg, Pos, Pos);
        const VectorRegister4Double SignBitsRight = MakeVectorRegisterDoubleMask(Neg, Neg, Neg, Pos);
        const VectorRegister4Double LeftTerm = VectorBitwiseXor(SignBitsLeft, VectorMultiply(CR, VectorMultiply(SP_SP_CP_CP, SY_CY_SY_CY)));
        const VectorRegister4Double RightTerm = VectorBitwiseXor(SignBitsRight, VectorMultiply(SR, VectorMultiply(CP_CP_SP_SP, CY_SY_CY_SY)));
    
        const VectorRegister4Double Result = VectorAdd(LeftTerm, RightTerm);
        FQuat4d RotationQuat = FQuat4d::MakeFromVectorRegister(Result);
    #else
        const double DEG_TO_RAD = UE_DOUBLE_PI / (180.0);
        const double RADS_DIVIDED_BY_2 = DEG_TO_RAD / 2.0;
        double SP, SY, SR;
        double CP, CY, CR;
    
        const double PitchNoWinding = FMath::Fmod(Pitch, 360.0);
        const double YawNoWinding = FMath::Fmod(Yaw, 360.0);
        const double RollNoWinding = FMath::Fmod(Roll, 360.0);
    
        FMath::SinCos(&SP, &CP, PitchNoWinding * RADS_DIVIDED_BY_2);
        FMath::SinCos(&SY, &CY, YawNoWinding * RADS_DIVIDED_BY_2);
        FMath::SinCos(&SR, &CR, RollNoWinding * RADS_DIVIDED_BY_2);
    
        FQuat4d 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
        if (RotationQuat.ContainsNaN())
        {
            logOrEnsureNanError(TEXT("Invalid input %s to FRotator::Quaternion - generated NaN output: %s"), *ToString(), *RotationQuat.ToString());
            RotationQuat = FQuat4d::Identity;
            FDebug::DumpStackTraceToLog(ELogVerbosity::Warning);
        }
    #endif
    
        return RotationQuat;