SpringMath.h

Go to the documentation of this file.

// Copyright Epic Games, Inc. All Rights Reserved.
 
#pragma once
 
#include "Math/MathFwd.h"
#include "Math/Vector.h"
#include "Math/Quat.h"
 
// Collection of useful spring methods which can be used for damping, simulating characters etc.
// Reference https://theorangeduck.com/page/spring-roll-call
 
struct SpringMath
{
private:
    static constexpr float SmoothingTimeToDamping(float SmoothingTime)
    {
        return 4.0f / FMath::Max(SmoothingTime, UE_KINDA_SMALL_NUMBER);
    }
    
public:
    UE_EXPERIMENTAL(5.7, "SpringMath is experimental")
    static constexpr float SmoothingTimeToHalfLife(float SmoothingTime)
    {
        return SmoothingTime * UE_LN2;
    }
 
    UE_EXPERIMENTAL(5.7, "SpringMath is experimental")
    static constexpr float HalfLifeToSmoothingTime(float HalfLife)
    {
        return HalfLife / UE_LN2;
    }
 
    UE_EXPERIMENTAL(5.7, "SpringMath is experimental")
    static constexpr float SmoothingTimeToStrength(float SmoothingTime)
    {
        return 2.0f / FMath::Max(SmoothingTime, UE_KINDA_SMALL_NUMBER);
    }
 
    UE_EXPERIMENTAL(5.7, "SpringMath is experimental")
    static constexpr float StrengthToSmoothingTime(float Strength)
    {
        return 2.0f / FMath::Max(Strength, UE_KINDA_SMALL_NUMBER);
    }
 
    template <typename T>
    UE_EXPERIMENTAL(5.7, "SpringMath is experimental")
    static void CriticalSpringDamper(
        T& InOutX,
        T& InOutV,
        T TargetX,
        float SmoothingTime,
        float DeltaTime)
    {
        float Y = SmoothingTimeToDamping(SmoothingTime) / 2.0f;
        T J0 = InOutX - TargetX;
        T J1 = InOutV + J0 * Y;
        float EyDt = FMath::InvExpApprox(Y * DeltaTime);
 
        InOutX = EyDt * (J0 + J1 * DeltaTime) + TargetX;
        InOutV = EyDt * (InOutV - J1 * Y * DeltaTime);
    }
 
    UE_EXPERIMENTAL(5.7, "SpringMath is experimental")
    static void CriticalSpringDamperAngle(
        float& InOutAngleRadians,
        float& InOutAngularVelocityRadians,
        float TargetAngleRadians,
        float SmoothingTime,
        float DeltaTime)
    {
        PRAGMA_DISABLE_EXPERIMENTAL_WARNINGS
        float Y = SmoothingTimeToDamping(SmoothingTime) / 2.0f;
        PRAGMA_ENABLE_EXPERIMENTAL_WARNINGS
 
        float J0 = FMath::FindDeltaAngleRadians(TargetAngleRadians, InOutAngleRadians);
        float J1 = InOutAngularVelocityRadians + J0 * Y;
        float EyDt = FMath::InvExpApprox(Y * DeltaTime);
 
        InOutAngleRadians = EyDt * (J0 + J1 * DeltaTime) + TargetAngleRadians;
        InOutAngularVelocityRadians = EyDt * (InOutAngularVelocityRadians - J1 * Y * DeltaTime);
    }
 
    UE_EXPERIMENTAL(5.7, "SpringMath is experimental")
    static void CriticalSpringDamperQuat(
        FQuat& InOutRotation,
        FVector& InOutAngularVelocityRadians,
        const FQuat& TargetRotation,
        float SmoothingTime,
        float DeltaTime)
    {
        PRAGMA_DISABLE_EXPERIMENTAL_WARNINGS
        float Y = SmoothingTimeToDamping(SmoothingTime) / 2.0f;
        PRAGMA_ENABLE_EXPERIMENTAL_WARNINGS
 
        FQuat Diff = InOutRotation * TargetRotation.Inverse();
        Diff.EnforceShortestArcWith(FQuat::Identity);
        FVector J0 = Diff.ToRotationVector();
        FVector J1 = InOutAngularVelocityRadians + J0 * Y;
 
        float EyDt = FMath::InvExpApprox(Y * DeltaTime);
 
        
        InOutRotation = FQuat::MakeFromRotationVector(EyDt * (J0 + J1 * DeltaTime)) * TargetRotation;
        InOutAngularVelocityRadians = EyDt * (InOutAngularVelocityRadians - J1 * Y * DeltaTime);
    }
 
    template <typename TFloat>
    UE_EXPERIMENTAL(5.7, "SpringMath is experimental")
    static void VelocitySpringDamperF(
        TFloat& InOutX,
        TFloat& InOutV,
        TFloat& InOutXi,
        TFloat TargetX,
        TFloat MaxSpeed,
        float SmoothingTime,
        float DeltaTime)
    {
        static_assert(std::is_floating_point_v<TFloat>, "TFloat must be floating point");
 
        MaxSpeed = FMath::Max(MaxSpeed, 0.0f); // MaxSpeed can't be negative
 
        TFloat XDiff = ((TargetX - InOutXi) > 0.0f
            ? 1.0f
            : -1.0f) * MaxSpeed;
 
        float TGoalFuture = SmoothingTime;
        TFloat XGoalFuture = fabs(TargetX - InOutXi) > TGoalFuture * MaxSpeed
            ? InOutXi + XDiff * TGoalFuture
            : TargetX;
 
        CriticalSpringDamper(InOutX, InOutV, XGoalFuture, SmoothingTime, DeltaTime);
 
        InOutXi = FMath::Abs(TargetX - InOutXi) > DeltaTime * MaxSpeed
            ? InOutXi + XDiff * DeltaTime
            : TargetX;
    }
 
    template <typename TVector>
    UE_EXPERIMENTAL(5.7, "SpringMath is experimental")
    static void VelocitySpringDamper(
        TVector& InOutX,
        TVector& InOutV,
        TVector& InOutXi,
        TVector TargetX,
        float MaxSpeed,
        float SmoothingTime,
        float DeltaTime)
    {
        TVector XDiff = TargetX - InOutXi;
        float XDiffLength = XDiff.Length();
        TVector XDiffDir = XDiffLength > FLT_EPSILON
            ? XDiff / XDiffLength
            : TVector::ZeroVector;
 
        float TGoalFuture = SmoothingTime;
        TVector XGoalFuture = XDiffLength > TGoalFuture * MaxSpeed
            ? InOutXi + (XDiffDir * MaxSpeed) * TGoalFuture
            : TargetX;
 
        CriticalSpringDamper(InOutX, InOutV, XGoalFuture, SmoothingTime, DeltaTime);
 
        InOutXi = XDiffLength > DeltaTime * MaxSpeed
            ? InOutXi + XDiffDir * MaxSpeed * DeltaTime
            : TargetX;
    }
 
    template <typename TVector>
    UE_EXPERIMENTAL(5.7, "SpringMath is experimental")
    static void SpringCharacterUpdate(
        TVector& InOutPosition,
        TVector& InOutVelocity,
        TVector& InOutAcceleration,
        const TVector& TargetVelocity,
        float SmoothingTime,
        float DeltaTime,
        float VDeadzone = 1e-2f,
        float ADeadzone = 1e-4f)
    {
        float Y = SmoothingTimeToDamping(SmoothingTime) / 2.0f;
        TVector J0 = InOutVelocity - TargetVelocity;
        TVector J1 = InOutAcceleration + J0 * Y;
        float EyDt = FMath::InvExpApprox(Y * DeltaTime);
 
        InOutPosition = EyDt * (((-J1) / (Y * Y)) + ((-J0 - J1 * DeltaTime) / Y)) +
            (J1 / (Y * Y)) + J0 / Y + TargetVelocity * DeltaTime + InOutPosition;
        InOutVelocity = EyDt * (J0 + J1 * DeltaTime) + TargetVelocity;
        InOutAcceleration = EyDt * (InOutAcceleration - J1 * Y * DeltaTime);
 
        if ((TargetVelocity - InOutVelocity).SquaredLength() < FMath::Square(VDeadzone))
        {
            // We reached our target
            InOutVelocity = TargetVelocity;
 
            if (InOutAcceleration.SquaredLength() < FMath::Square(ADeadzone))
            {
                InOutAcceleration = TVector::ZeroVector;
            }
        }
    }
 
    template <typename TVector>
    UE_EXPERIMENTAL(5.7, "SpringMath is experimental")
    static void SpringCharacterPredict(
        TArrayView<TVector> OutPredictedPositions,
        TArrayView<TVector> OutPredictedVelocities,
        TArrayView<TVector> OutPredictedAccelerations,
        const TVector& CurrentPosition,
        const TVector& CurrentVelocity,
        const TVector& CurrentAcceleration,
        const TVector& TargetVelocity,
        float SmoothingTime,
        float SecondsPerPredictionStep,
        float VDeadzone = 1e-2f,
        float ADeadzone = 1e-4f)
    {
        int32 PredictCount = OutPredictedPositions.Num();
        check(PredictCount > 0);
        check(OutPredictedVelocities.Num() == PredictCount);
        check(OutPredictedAccelerations.Num() == PredictCount);
        
        for (int32 i = 0; i < PredictCount; i++)
        {
            OutPredictedPositions[i] = CurrentPosition;
            OutPredictedVelocities[i] = CurrentVelocity;
            OutPredictedAccelerations[i] = CurrentAcceleration;
        }
 
        for (int32 i = 0; i < PredictCount; i++)
        {
            const float PredictTime = (float)(i + 1) * SecondsPerPredictionStep; // Note i+1 since we want index 0 to be the first prediction step
            PRAGMA_DISABLE_EXPERIMENTAL_WARNINGS
            SpringCharacterUpdate(OutPredictedPositions[i], OutPredictedVelocities[i], OutPredictedAccelerations[i], TargetVelocity, SmoothingTime,
                PredictTime, VDeadzone, ADeadzone);
            PRAGMA_ENABLE_EXPERIMENTAL_WARNINGS
        }
    }
 
    template <typename TVector>
    UE_EXPERIMENTAL(5.7, "SpringMath is experimental")
    static void VelocitySpringCharacterUpdate(
        TVector& InOutPosition,
        TVector& InOutVelocity,
        TVector& InOutVelocityIntermediate,
        TVector& InOutAcceleration,
        TVector TargetVelocity,
        float SmoothingTime,
        float MaxAcceleration,
        float DeltaTime,
        float VDeadzone = 1e-2f,
        float ADeadzone = 1e-4f)
    {
        TVector VDiff = TargetVelocity - InOutVelocityIntermediate;
        float VDiffLength = VDiff.Length();
        TVector VDiffDir = VDiffLength > 0.0001f
            ? VDiff / VDiffLength
            : TVector::ZeroVector;
 
        float TGoalFuture = SmoothingTime;
        TVector MaxVFuture = VDiffLength > TGoalFuture * MaxAcceleration
            ? InOutVelocityIntermediate + (VDiffDir * MaxAcceleration) * TGoalFuture
            : TargetVelocity;
 
        float Y = SmoothingTimeToDamping(SmoothingTime) / 2.0f;
        TVector J0 = InOutVelocity - MaxVFuture;
        TVector J1 = InOutAcceleration + J0 * Y;
        float EyDt = FMath::InvExpApprox(Y * DeltaTime);
 
        InOutPosition = EyDt * (((-J1) / (Y * Y)) + ((-J0 - J1 * DeltaTime) / Y)) +
            (J1 / (Y * Y)) + J0 / Y + MaxVFuture * DeltaTime + InOutPosition;
        InOutVelocity = EyDt * (J0 + J1 * DeltaTime) + MaxVFuture;
        InOutAcceleration = EyDt * (InOutAcceleration - J1 * Y * DeltaTime);
        InOutVelocityIntermediate = VDiffLength > DeltaTime * MaxAcceleration
            ? InOutVelocityIntermediate + VDiffDir * MaxAcceleration * DeltaTime
            : TargetVelocity;
 
        if ((TargetVelocity - InOutVelocity).SquaredLength() < FMath::Square(VDeadzone))
        {
            // We reached our target
            InOutVelocity = TargetVelocity;
 
            if (InOutAcceleration.SquaredLength() < FMath::Square(ADeadzone))
            {
                InOutAcceleration = TVector::ZeroVector;
            }
        }
    }
 
    template <typename TVector>
    UE_EXPERIMENTAL(5.7, "SpringMath is experimental")
    static void VelocitySpringCharacterPredict(
        TArrayView<TVector> OutPredictedPositions,
        TArrayView<TVector> OutPredictedVelocities,
        TArrayView<TVector> OutPredictedIntermediateVelocities,
        TArrayView<TVector> OutPredictedAccelerations,
        const TVector& CurrentPosition,
        const TVector& CurrentVelocity,
        const TVector& CurrentIntermediateVelocity,
        const TVector& CurrentAcceleration,
        const TVector& TargetVelocity,
        float SmoothingTime,
        float MaxAcceleration,
        float SecondsPerPredictionStep,
        float VDeadzone = 1e-2f,
        float ADeadzone = 1e-4f)
    {
        int32 PredictCount = OutPredictedPositions.Num();
        check(PredictCount > 0);
        check(OutPredictedVelocities.Num() == PredictCount);
        check(OutPredictedAccelerations.Num() == PredictCount);
        check(OutPredictedIntermediateVelocities.Num() == PredictCount);
        
        for (int32 i = 0; i < PredictCount; i++)
        {
            OutPredictedPositions[i] = CurrentPosition;
            OutPredictedVelocities[i] = CurrentVelocity;
            OutPredictedIntermediateVelocities[i] = CurrentIntermediateVelocity;
            OutPredictedAccelerations[i] = CurrentAcceleration;
        }
 
        for (int32 i = 0; i < PredictCount; i++)
        {
            const float PredictTime = (float)(i + 1) * SecondsPerPredictionStep; // Note i+1 since we want index 0 to be the first prediction step
            VelocitySpringCharacterUpdate(OutPredictedPositions[i],
                OutPredictedVelocities[i],
                OutPredictedIntermediateVelocities[i],
                OutPredictedAccelerations[i],
                TargetVelocity,
                SmoothingTime,
                MaxAcceleration,
                PredictTime,
                VDeadzone,
                ADeadzone);
        }
    }
 
    UE_EXPERIMENTAL(5.7, "SpringMath is experimental")
    static void CriticalSpringDamperQuatPredict(TArrayView<FQuat> OutPredictedRotations, TArrayView<FVector> OutPredictedAngularVelocities,
                                              int32 PredictCount, const FQuat& CurrentRotation, const FVector& CurrentAngularVelocity,
                                              const FQuat& TargetRotation, float SmoothingTime, float SecondsPerPredictionStep)
    {
        check(OutPredictedRotations.Num() == PredictCount);
        check(OutPredictedAngularVelocities.Num() == PredictCount);
        
        for (int32 i = 0; i < PredictCount; i++)
        {
            OutPredictedRotations[i] = CurrentRotation;
            OutPredictedAngularVelocities[i] = CurrentAngularVelocity;
        }
 
        for (int32 i = 0; i < PredictCount; i++)
        {
            const float PredictTime = (float)(i + 1) * SecondsPerPredictionStep; // Note i+1 since we want index 0 to be the first prediction step
            PRAGMA_DISABLE_EXPERIMENTAL_WARNINGS
            CriticalSpringDamperQuat(OutPredictedRotations[i], OutPredictedAngularVelocities[i], TargetRotation, SmoothingTime, PredictTime);
            PRAGMA_ENABLE_EXPERIMENTAL_WARNINGS
        }
    }
 
    UE_EXPERIMENTAL(5.7, "SpringMath is experimental")
    static constexpr void ExponentialSmoothingApproxQuat(FQuat& InOutRotation, const FQuat& InTargetRotation, const float InDeltaTime, const float InSmoothingTime)
    {
        if (InSmoothingTime > UE_KINDA_SMALL_NUMBER)
        {
            InOutRotation = FQuat::Slerp(InOutRotation, InTargetRotation, 1.0f - FMath::InvExpApprox(InDeltaTime / InSmoothingTime));
        }
        else
        {
            InOutRotation = InTargetRotation;
        }
    }
 
    UE_EXPERIMENTAL(5.7, "SpringMath is experimental")
    static constexpr void ExponentialSmoothingApproxAngle(float& InOutAngleRadians, const float& InTargetAngleRadians, const float InDeltaTime, const float InSmoothingTime)
    {
        if (InSmoothingTime > UE_KINDA_SMALL_NUMBER)
        {
            InOutAngleRadians += FMath::FindDeltaAngleRadians(InOutAngleRadians, InTargetAngleRadians) * (1.0f - FMath::InvExpApprox(InDeltaTime / InSmoothingTime));
        }
        else
        {
            InOutAngleRadians = InTargetAngleRadians;
        }
    }
};