SolverBody.h

Go to the documentation of this file.

// Copyright Epic Games, Inc. All Rights Reserved.
#pragma once
 
#include "Chaos/Core.h"
#include "Math/Quat.h"
 
 
// Set to 1 to enable extra NaN tests in the constraint solver
// NOTE: These will cause slowness!
#ifndef CHAOS_CONSTRAINTSOLVER_NAN_DIAGNOSTIC
#define CHAOS_CONSTRAINTSOLVER_NAN_DIAGNOSTIC ((DO_CHECK && UE_BUILD_DEBUG) || ENABLE_NAN_DIAGNOSTIC)
#endif
 
// Set to 1 to force single precision in the constraint solver, even if the default numeric type is double
#define CHAOS_CONSTRAINTSOLVER_LOWPRECISION 1
 
// Add some NaN checking when CHAOS_CONSTRAINTSOLVER_NAN_DIAGNOSTIC is defined
#if CHAOS_CONSTRAINTSOLVER_NAN_DIAGNOSTIC
inline void ChaosSolverCheckNaN(const Chaos::FRealSingle& V) { ensure(!FMath::IsNaN(V)); }
inline void ChaosSolverCheckNaN(const Chaos::TVec3<Chaos::FRealSingle>& V) { ensure(!V.ContainsNaN()); }
inline void ChaosSolverCheckNaN(const Chaos::TRotation3<Chaos::FRealSingle>& V) { ensure(!V.ContainsNaN()); }
 
inline void ChaosSolverCheckNaN(const Chaos::FRealDouble& V) { ensure(!FMath::IsNaN(V)); }
inline void ChaosSolverCheckNaN(const Chaos::TVec3<Chaos::FRealDouble>& V) { ensure(!V.ContainsNaN()); }
inline void ChaosSolverCheckNaN(const Chaos::TRotation3<Chaos::FRealDouble>& V) { ensure(!V.ContainsNaN()); }
#else
#define ChaosSolverCheckNaN(...)
#endif
 
 
namespace Chaos
{
    // Set the math types used by the constraint solvers when single precision is acceptable.
    // NOTE: public APIs should still use FReal, FVec3 etc. whereas FSolverReal and associated
    // types are for use internally where double precision is unnecessary and causes performance
    // issues and/or memory bloat.
#if CHAOS_CONSTRAINTSOLVER_LOWPRECISION
    using FSolverReal = FRealSingle;
    using SolverVectorRegister = VectorRegister4Float;
#else
    using FSolverReal = FReal;
    using SolverVectorRegister = VectorRegister4;
#endif
    using FSolverVec3 = TVec3<FSolverReal>;
    using FSolverRotation3 = TRotation3<FSolverReal>;
    using FSolverMatrix33 = TMatrix33<FSolverReal>;
 
    class FSolverBody;
    class FSolverBodyContainer;
 
 
    // A pair of pointers to solver bodies
    // @note Pointers are only valid for the Constraint Solving phase of the tick
    using FSolverBodyPtrPair = TVector<FSolverBody*, 2>;
 
    CHAOS_API void SolverQuaternionNormalizeApprox(FRotation3& InOutQ);
 
    CHAOS_API FRotation3 SolverQuaternionApplyAngularDeltaApprox(const FRotation3& InQ0, const FVec3& InDR);
 
 
    class FSolverBody
    {
    public:
        static constexpr FSolverReal ZeroMassThreshold() { return std::numeric_limits<FSolverReal>::min(); }
 
 
        static FSolverBody MakeInitialized()
        {
            FSolverBody SolverBody;
            SolverBody.State.Init();
            return SolverBody;
        }
 
        static FSolverBody MakeUninitialized()
        {
            return FSolverBody();
        }
 
        FSolverBody() {}
 
        void Reset()
        {
            State.DP = FSolverVec3(0);
            State.DQ = FSolverVec3(0);
            State.CP = FSolverVec3(0);
            State.CQ = FSolverVec3(0);
        }
 
        inline void SetImplicitVelocity(FReal Dt)
        {
            if (IsDynamic() && (Dt != FReal(0)))
            {
                const FSolverReal InvDt = FSolverReal(1) / FSolverReal(Dt);
                SetV(State.V + FVec3((State.DP - State.CP) * InvDt));
                SetW(State.W + FVec3((State.DQ - State.CQ) * InvDt));
            }
        }
 
        //inline FReal InvM() const { return State.InvM; }
        inline FSolverReal InvM() const { return State.InvM; }
 
        inline void SetInvM(FReal InInvM) { State.InvM = FSolverReal(InInvM); }
 
        inline const FSolverMatrix33& InvI() const { return State.InvI; }
 
        inline void SetInvI(const FMatrix33& InInvI) { State.InvI = FSolverMatrix33(InInvI); }
 
        inline const FSolverVec3& InvILocal() const { return State.InvILocal; }
 
        inline void SetInvILocal(const FVec3& InInvILocal)
        { 
            State.InvILocal = FSolverVec3(InInvILocal); 
            UpdateRotationDependentState();
        }
 
        inline FRigidTransform3 CoMTransform() const { return FRigidTransform3(P(), Q()); }
 
        inline const FVec3& X() const { return State.X; }
        inline void SetX(const FVec3& InX) { State.X = InX; }
 
        inline const FRotation3& R() const { return State.R; }
        inline void SetR(const FRotation3& InR)
        {
            ChaosSolverCheckNaN(InR);
            State.R = InR;
        }
 
        inline const FVec3& P() const { return State.P; }
        inline void SetP(const FVec3& InP) { State.P = InP; }
 
        inline const FRotation3& Q() const { return State.Q; }
        inline void SetQ(const FRotation3& InQ)
        { 
            ChaosSolverCheckNaN(InQ);
            State.Q = InQ;
        }
 
        inline const FSolverVec3& V() const { return State.V; }
        inline void SetV(const FVec3& InV)
        { 
            ChaosSolverCheckNaN(InV);
            State.V = FSolverVec3(InV);
        }
 
        inline const FSolverVec3& W() const { return State.W; }
        inline void SetW(const FVec3& InW)
        {
            ChaosSolverCheckNaN(InW);
            State.W = FSolverVec3(InW);
        }
 
        inline const FVec3& CoM() const { return State.CoM; }
        inline void SetCoM(const FVec3& InCoM) { State.CoM = InCoM; }
 
        inline const FRotation3& RoM() const { return State.RoM; }
        inline void SetRoM(const FRotation3& InRoM) { State.RoM = InRoM; }
 
        inline const FSolverVec3& DP() const { return State.DP; }
        inline void SetDP(const FSolverVec3& InDP) { State.DP = InDP; }
 
        inline const FSolverVec3& DQ() const { return State.DQ; }
        inline void SetDQ(const FSolverVec3& InDQ) { State.DQ = InDQ; }
 
        inline const FSolverVec3& CP() const { return State.CP; }
 
        inline const FSolverVec3& CQ() const { return State.CQ; }
 
        inline FVec3 CorrectedP() const { return State.P + FVec3(State.DP); }
 
        inline FRotation3 CorrectedQ() const { return (IsDynamic() && !State.DQ.IsZero()) ? FRotation3::IntegrateRotationWithAngularVelocity(State.Q, FVec3(State.DQ), FReal(1)) : State.Q; }
 
        void ApplyCorrections()
        {
            State.P = CorrectedP();
            State.Q = CorrectedQ();
            State.DP = FSolverVec3(0);
            State.DQ = FSolverVec3(0);
            State.CP = FSolverVec3(0);
            State.CQ = FSolverVec3(0);
        }
 
        inline FVec3 ActorP() const { return P() - FVec3(ActorQ().RotateVector(CoM())); }
 
        inline FRotation3 ActorQ() const { return Q() * RoM().Inverse(); }
 
        inline FVec3 CorrectedActorP() const { return CorrectedP() - CorrectedActorQ().RotateVector(CoM()); }
 
        inline FRotation3 CorrectedActorQ() const { return CorrectedQ() * RoM().Inverse(); }
 
        inline int32 Level() const { return State.Level; }
        inline void SetLevel(int32 InLevel) { State.Level = InLevel; }
 
        inline bool IsDynamic() const { return (State.InvM > FSolverBody::ZeroMassThreshold()); }
 
        inline void ApplyTransformDelta(const FSolverVec3& DP, const FSolverVec3& DR)
        {
            ApplyPositionDelta(DP);
            ApplyRotationDelta(DR);
        }
 
        inline void ApplyPositionDelta(const FSolverVec3& DP)
        {
            ChaosSolverCheckNaN(DP);
            State.DP += DP;
        }
 
        inline void ApplyRotationDelta(const FSolverVec3& DR)
        {
            ChaosSolverCheckNaN(DR);
            State.DQ += DR;
        }
 
        inline void ApplyPositionCorrectionDelta(const FSolverVec3& CP)
        {
            ChaosSolverCheckNaN(CP);
            State.CP += CP;
            State.DP += CP;
        }
 
        inline void ApplyRotationCorrectionDelta(const FSolverVec3& CR)
        {
            ChaosSolverCheckNaN(CR);
            State.CQ += CR;
            State.DQ += CR;
        }
 
        inline void ApplyVelocityDelta(const FSolverVec3& DV, const FSolverVec3& DW)
        {
            ApplyLinearVelocityDelta(DV);
            ApplyAngularVelocityDelta(DW);
        }
 
        inline void ApplyLinearVelocityDelta(const FSolverVec3& DV)
        {
            ChaosSolverCheckNaN(DV);
            State.V += DV;
        }
 
        inline void ApplyAngularVelocityDelta(const FSolverVec3& DW)
        {
            ChaosSolverCheckNaN(DW);
            SetW(State.W + DW);
        }
 
        inline void EnforceShortestRotationTo(const FRotation3& InQ)
        {
            State.Q.EnforceShortestArcWith(InQ);
        }
 
        void UpdateRotationDependentState();
 
 
 
        void PrefetchPositionSolverData() const
        {
            // The position solver only uses DP,DQ
            FPlatformMisc::PrefetchBlock(&State.DP, 8 * sizeof(float));
        }
 
        void PrefetchVelocitySolverData() const
        {
            // The velocity solver only uses V,W
            FPlatformMisc::PrefetchBlock(&State.V, 8 * sizeof(float));
        }
 
    private:
 
        // The struct exists only so that we can use the variable names
        // as accessor names without violation the variable naming convention
        struct FState
        {
            FState()
            {}
 
            void Init()
            {
                DP = FSolverVec3(0);
                DQ = FSolverVec3(0);
                V = FSolverVec3(0);
                W = FSolverVec3(0);
                InvI = FSolverMatrix33(0);
                RoM = FRotation3::FromIdentity();
                R = FRotation3::FromIdentity();
                Q = FRotation3::FromIdentity();
                CoM = FVec3(0);
                X = FVec3(0);
                P = FVec3(0);
                InvILocal = FSolverVec3(0);
                InvM = 0;
                CP = FSolverVec3(0);
                CQ = FSolverVec3(0);
                Level = 0;
            }
 
            // Net position delta applied by all constraints (constantly changing as we iterate over constraints)
            alignas(16) FSolverVec3 DP;
 
            // Net rotation delta applied by all constraints (constantly changing as we iterate over constraints)
            alignas(16) FSolverVec3 DQ;
 
            // World-space center of mass velocity
            alignas(16) FSolverVec3 V;
 
            // World-space center of mass angular velocity
            alignas(16) FSolverVec3 W;
 
            // World-space inverse inertia
            // NOTE: Matrix and Rotation are alignas(16) so beware of padding
            // @todo(chaos): do we need this, or should we force all systems to use the FConstraintSolverBody decorator?
            FSolverMatrix33 InvI;
 
            // Actor-space center of mass rotation
            FRotation3 RoM;
 
            // World-space rotation of mass at start of sub step
            FRotation3 R;
 
            // Predicted world-space center of mass rotation (post-integration, pre-constraint-solve)
            FRotation3 Q;
 
            // Actor-space center of mass location
            FVec3 CoM;
 
            // World-space center of mass state at start of sub step
            FVec3 X;
 
            // Predicted world-space center of mass position (post-integration, pre-constraint-solve)
            FVec3 P;
 
 
 
            // Local-space inverse inertia (diagonal, so only 3 elements)
            FSolverVec3 InvILocal;
 
            // Inverse mass
            FSolverReal InvM;
 
            // Net position correction delta applied by all constraints (constantly changing as we iterate over constraints)
            // Will translate the body without introducing linear velocity
            FSolverVec3 CP;
 
            // Net rotation correction delta applied by all constraints (constantly changing as we iterate over constraints)
            // Will rotate the body without introducing angular velocity
            FSolverVec3 CQ;
 
            // Distance to a kinematic body (through the contact graph). Used by collision shock propagation
            int32 Level;
        };
 
        FState State;
    };
 
 
    class FConstraintSolverBody
    {
    public:
        FConstraintSolverBody()
            : Body(nullptr)
        {
        }
 
        FConstraintSolverBody(FSolverBody& InBody)
            : Body(&InBody)
        {
        }
 
        inline bool IsValid() const { return Body != nullptr; }
 
        inline void Reset() { Body = nullptr; }
 
        void Init() { State.Init(); }
 
        void SetSolverBody(FSolverBody& InSolverBody)
        {
            Body = &InSolverBody;
        }
 
        inline FSolverBody& SolverBody() { check(IsValid()); return *Body; }
        inline const FSolverBody& SolverBody() const { check(IsValid()); return *Body; }
 
        inline FSolverReal InvMScale() const { return State.InvMScale; }
        inline void SetInvMScale(FReal InValue) { State.InvMScale = FSolverReal(InValue); }
 
        inline FSolverReal InvIScale() const { return State.InvIScale; }
        inline void SetInvIScale(FReal InValue) { State.InvIScale = FSolverReal(InValue); }
 
 
        inline FSolverReal ShockPropagationScale() const { return State.ShockPropagationScale; }
        inline void SetShockPropagationScale(FReal InValue) { State.ShockPropagationScale = FSolverReal(InValue); }
 
        FSolverReal InvM() const { return (State.ShockPropagationScale * State.InvMScale) * Body->InvM(); }
 
        FSolverMatrix33 InvI() const { return (State.ShockPropagationScale * State.InvIScale) * Body->InvI(); }
 
        FSolverVec3 InvILocal() const { return (State.ShockPropagationScale * State.InvIScale) * Body->InvILocal(); }
 
        inline bool IsDynamic() const { return (InvM() > FSolverBody::ZeroMassThreshold()); }
 
        //
        // From here all methods just forward to the FSolverBody
        //
 
        inline void SetImplicitVelocity(FReal Dt) { Body->SetImplicitVelocity(Dt); }
        inline FRigidTransform3 CoMTransform() const { return Body->CoMTransform(); }
        inline const FVec3& X() const { return Body->X(); }
        inline const FRotation3& R() const { return Body->R(); }
        inline const FVec3& P() const { return Body->P(); }
        inline const FRotation3& Q() const { return Body->Q(); }
        inline const FVec3 ActorP() const { return Body->ActorP(); }
        inline const FRotation3 ActorQ() const { return Body->ActorQ(); }
        inline const FVec3 CorrectedActorP() const { return Body->CorrectedActorP(); }
        inline const FRotation3 CorrectedActorQ() const { return Body->CorrectedActorQ(); }
        inline const FSolverVec3& V() const { return Body->V(); }
        inline const FSolverVec3& W() const { return Body->W(); }
        inline int32 Level() const { return Body->Level(); }
        inline const FSolverVec3& DP() const { return Body->DP(); }
        inline const FSolverVec3& DQ() const { return Body->DQ(); }
        inline const FSolverVec3& CP() const { return Body->CP(); }
        inline const FSolverVec3& CQ() const { return Body->CQ(); }
        inline FVec3 CorrectedP() const { return Body->CorrectedP(); }
        inline FRotation3 CorrectedQ() const { return Body->CorrectedQ(); }
 
        inline void ApplyTransformDelta(const FSolverVec3& DP, const FSolverVec3& DR) { Body->ApplyTransformDelta(DP, DR); }
        inline void ApplyPositionDelta(const FSolverVec3& DP) { Body->ApplyPositionDelta(DP); }
        inline void ApplyRotationDelta(const FSolverVec3& DR) { Body->ApplyRotationDelta(DR); }
        inline void ApplyPositionCorrectionDelta(const FSolverVec3& DP) { Body->ApplyPositionCorrectionDelta(DP); }
        inline void ApplyRotationCorrectionDelta(const FSolverVec3& DR) { Body->ApplyRotationCorrectionDelta(DR); }
        inline void ApplyVelocityDelta(const FSolverVec3& DV, const FSolverVec3& DW) { Body->ApplyVelocityDelta(DV, DW); }
        inline void ApplyLinearVelocityDelta(const FSolverVec3& DV) { Body->ApplyLinearVelocityDelta(DV); }
        inline void ApplyAngularVelocityDelta(const FSolverVec3& DW) { Body->ApplyAngularVelocityDelta(DW); }
        inline void EnforceShortestRotationTo(const FRotation3& InQ) { Body->EnforceShortestRotationTo(InQ); }
        inline void UpdateRotationDependentState() { Body->UpdateRotationDependentState(); }
 
    private:
        // Struct is only so that we can use the same var names as function names
        struct FState
        {
            FState() 
            {
            }
 
            void Init()
            {
                InvMScale = FSolverReal(1);
                InvIScale = FSolverReal(1);
                ShockPropagationScale = FSolverReal(1);
            }
 
            FSolverReal InvMScale;
            FSolverReal InvIScale;
            FSolverReal ShockPropagationScale;
        };
 
        // The body we decorate
        FSolverBody* Body;
 
        // The body modifiers
        FState State;
    };
}