SimpleSuspension.h

Go to the documentation of this file.

// Copyright Epic Games, Inc. All Rights Reserved.
 
#pragma once
 
#include "CoreMinimal.h"
#include "Math/UnrealMath.h"
#include "Templates/Function.h"
#include "Containers/Array.h"
 
struct FSimpleSuspensionSpringParams
{
    FSimpleSuspensionSpringParams()
        : Stiffness(0.f)
        , Damping(0.f)
        , Length(0.f)
        , Mass(0.f)
    { }
 
    float Stiffness;
    float Damping;
    float Length;
    float Mass;
};
 
struct FSimpleSuspensionParams
{
    FSimpleSuspensionParams()
        : LocalCenterOfMass(FVector::ZeroVector)
        , LocalSuspensionNormal(FVector::UpVector)
        , LocalGroundDistance(0.f)
        , NaturalFrequency(10.f)
        , DampingRatio(.1f)
    { }
 
    TArray<FSimpleSuspensionSpringParams> SpringParams;
    TArray<FVector> LocalSpringOrigins;
    FVector LocalCenterOfMass;
    FVector LocalSuspensionNormal;
    float LocalGroundDistance;
    float NaturalFrequency;
    float DampingRatio;
};
 
struct FSimpleSuspensionState
{
    FSimpleSuspensionState()
        : TotalForce(FVector::ZeroVector)
        , TotalTorqueRad(FVector::ZeroVector)
        , WorldCenterOfMass(FVector::ZeroVector)
        , WorldSuspensionNormal(FVector::UpVector)
    { }
 
    TArray<float> SpringDisplacements;
    TArray<float> SpringForces;
    TArray<bool> SpringContacts;
    TArray<FVector> WorldSpringOrigins;
    FVector TotalForce;
    FVector TotalTorqueRad;
    FVector WorldCenterOfMass;
    FVector WorldSuspensionNormal;
};
 
using FSimpleSuspensionRaycast = TFunction<bool(const FVector& RayBegin, const FVector& RayEnd, const int32 SpringIndex, FVector& OutRayHitLocation, FVector& OutRayHitNormal)>;
 
const FSimpleSuspensionRaycast SimpleSuspensionRaycastGroundPlane = [](const FVector& RayBegin, const FVector& RayEnd, const int32 SpringIndex, FVector& OutRayHitLocation, FVector& OutRayHitNormal)
{
    // Do an intersection with the z-plane at 0,0,0.
    const FVector Norm = FVector::UpVector;
    const FVector Diff = RayEnd - RayBegin;
    const FVector::FReal ProjA = FVector::DotProduct(RayBegin, Norm);
    const FVector::FReal ProjDiff = FVector::DotProduct(Diff, Norm);
    if (FMath::Abs(ProjDiff) > UE_SMALL_NUMBER)
    {
        const FVector::FReal Lambda = -ProjA / ProjDiff;
        if (Lambda > 0.f && Lambda <= 1.f)
        {
            OutRayHitLocation = RayBegin + (Lambda * Diff);
            OutRayHitNormal = Norm;
            return true;
        }
    }
    return false;
};
 
struct FSimpleSuspension
{
    FSimpleSuspension()
    { }
 
    FSimpleSuspension(const FSimpleSuspensionParams& SuspensionParams)
    {
        Setup(SuspensionParams);
    }
 
    FSimpleSuspensionParams SuspensionParams;
    FSimpleSuspensionState SuspensionState;
 
    void Setup(const FSimpleSuspensionParams& InSuspensionParams);
 
    void Setup(const FSimpleSuspensionParams& InSuspensionParams, const float TotalMass, const float Gravity);
 
    void ComputeSpringParams(const float TotalMass, const float Gravity);
 
    void Update(
        const FTransform& LocalToWorld,
        const FVector& LinearVelocity,
        const FVector& AngularVelocityRad,
        const FSimpleSuspensionRaycast& RaycastFunction = SimpleSuspensionRaycastGroundPlane);
};
 
struct FSimpleSuspensionHelpers
{
    //
    // Setup functions
    //
 
    ENGINE_API static bool ComputeSprungMasses(const TArray<FVector>& MassSpringPositions, const float TotalMass, TArray<float>& OutSprungMasses, FString* ErrMsg = nullptr);
 
    ENGINE_API static bool ComputeSprungMasses(const TArray<FVector>& LocalSpringPositions, const FVector& LocalCenterOfMass, const float TotalMass, TArray<float>& OutSprungMasses, FString* ErrMsg = nullptr);
 
    static bool ComputeSingleAxisLambda(const FVector::FReal AxisDot, const FVector::FReal SumAxis, const uint32 Count, TArray<FVector::FReal, TFixedAllocator<2>>& Lambdas, FString* ErrMsg = nullptr);
 
    static void ComputeSpringNaturalFrequencyAndDampingRatio(const float SprungMass, const float SpringStiffness, const float SpringDamping, float& OutNaturalFrequency, float& OutDampingRatio);
 
    static float ComputeSpringStiffness(const float SprungMass, const float NaturalFrequency);
 
    static float ComputeSpringDamping(const float SprungMass, const float NaturalFrequency, const float DampingRatio);
 
    static float ComputeSpringCriticalDamping(const float SprungMass, const float NaturalFrequency);
 
    static float ComputeSpringRestLength(const float SpringStiffness, const float SprungMass, const float SuspendedLength, const float Gravity);
 
    static void ComputeSpringParams(const TArray<FVector>& LocalSuspensionOrigins, const FVector& LocalCenterOfMass, const float TotalMass, const FVector& LocalSuspensionNormal, const float LocalGroundDistance, const float NaturalFrequency, const float DampingRatio, const float Gravity, TArray<FSimpleSuspensionSpringParams>& OutSpringParams);
 
    static void ComputeSpringParams(const FSimpleSuspensionParams& SuspensionParams, const float TotalMass, const float Gravity, TArray<FSimpleSuspensionSpringParams>& OutSpringParams);
 
    //
    // Dynamics functions
    //
 
    static void ComputeWorldSuspensionCoordinates(const FVector& LocalCenterOfMass, const FVector& LocalSuspensionNormal, const TArray<FVector>& LocalSuspensionOrigins, const FTransform& LocalToWorld, FVector& OutWorldCenterOfMass, FVector& OutWorldSuspensionNormal, TArray<FVector>& OutWorldSuspensionOrigins);
 
    static void ComputeWorldSuspensionCoordinates(const FSimpleSuspensionParams& SuspensionParams, const FTransform& LocalToWorld, FSimpleSuspensionState& OutSuspensionState);
 
    static void ComputeSuspensionDisplacements(const TArray<FVector> WorldSuspensionOrigins, const TArray<FSimpleSuspensionSpringParams>& SpringParams, const FVector& WorldSuspensionNormal, const FSimpleSuspensionRaycast& RaycastFunction, TArray<float>& OutSpringDisplacements);
 
    static void ComputeSuspensionDisplacements(const FSimpleSuspensionParams& SuspensionParams, const FSimpleSuspensionRaycast& RaycastFunction, FSimpleSuspensionState InOutSuspensionState);
 
    static float ComputeSpringForce(const float SpringStiffness, const float SpringDamping, const float SpringDisplacement, const float SpringVelocity);
 
    static void ComputeSuspensionForces(const FVector& LinearVelocity, const FVector& AngularVelocityRad, const FVector& WorldCenterOfMass, const FVector& WorldSuspensionNormal, const TArray<FVector> WorldSuspensionOrigins, const TArray<FSimpleSuspensionSpringParams>& SpringParams, const TArray<float>& SpringDisplacements, FVector& OutTotalForce, FVector& OutTotalTorque, TArray<float>& OutSpringForces);
 
    static void ComputeSuspensionForces(const FVector& LinearVelocity, const FVector& AngularVelocityRad, const FSimpleSuspensionState& SuspensionState, const TArray<FSimpleSuspensionSpringParams>& SpringParams, FSimpleSuspensionState& OutSuspensionState);
 
    static void IntegrateSpring(const float DeltaTime, const float SpringDisplacement, const float SpringVelocity, const FSimpleSuspensionSpringParams& SpringParams, const float SprungMass, float& OutNewSpringDisplacement, float& OutNewSpringVelocity);
 
    static void IntegrateSprings(const float DeltaTime, const TArray<float>& SpringDisplacements, const TArray<float>& SpringVelocities, const TArray<FSimpleSuspensionSpringParams>& SuspensionParams, const TArray<float>& SprungMasses, TArray<float>& OutNewSpringDisplacements, TArray<float>& OutNewSpringVelocities);
 
private:
 
    static bool ErrCheck(const bool bCondition, const FString& Message, FString* ErrMsg);
};