AxisConstraintPart.h

Go to the documentation of this file.

// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
 
#pragma once
 
#include <Jolt/Physics/Body/Body.h>
#include <Jolt/Physics/Constraints/ConstraintPart/SpringPart.h>
#include <Jolt/Physics/StateRecorder.h>
#include <Jolt/Physics/DeterminismLog.h>
 
JPH_NAMESPACE_BEGIN
 
class AxisConstraintPart
{
    template <EMotionType Type1, EMotionType Type2>
    JPH_INLINE bool             ApplyVelocityStep(MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, Vec3Arg inWorldSpaceAxis, float inLambda) const
    {
        // Apply impulse if delta is not zero
        if (inLambda != 0.0f)
        {
            // Calculate velocity change due to constraint
            //
            // Impulse:
            // P = J^T lambda
            //
            // Euler velocity integration: 
            // v' = v + M^-1 P
            if constexpr (Type1 == EMotionType::Dynamic)
            {
                ioMotionProperties1->SubLinearVelocityStep((inLambda * ioMotionProperties1->GetInverseMass()) * inWorldSpaceAxis);
                ioMotionProperties1->SubAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis));
            }
            if constexpr (Type2 == EMotionType::Dynamic)
            {
                ioMotionProperties2->AddLinearVelocityStep((inLambda * ioMotionProperties2->GetInverseMass()) * inWorldSpaceAxis);
                ioMotionProperties2->AddAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis));
            }
            return true;
        }
 
        return false;
    }
 
public:
    template <EMotionType Type1, EMotionType Type2>
    JPH_INLINE void             TemplatedCalculateConstraintProperties(float inDeltaTime, const MotionProperties *inMotionProperties1, Mat44Arg inInvI1, Vec3Arg inR1PlusU, const MotionProperties *inMotionProperties2, Mat44Arg inInvI2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f, float inC = 0.0f, float inFrequency = 0.0f, float inDamping = 0.0f)
    {
        JPH_ASSERT(inWorldSpaceAxis.IsNormalized(1.0e-5f));
 
        // Calculate properties used below
        Vec3 r1_plus_u_x_axis;
        if constexpr (Type1 != EMotionType::Static)
        {
            r1_plus_u_x_axis = inR1PlusU.Cross(inWorldSpaceAxis);
            r1_plus_u_x_axis.StoreFloat3(&mR1PlusUxAxis);
        }
            JPH_IF_DEBUG(else Vec3::sNaN().StoreFloat3(&mR1PlusUxAxis);)
 
        Vec3 r2_x_axis;
        if constexpr (Type2 != EMotionType::Static)
        {
            r2_x_axis = inR2.Cross(inWorldSpaceAxis);
            r2_x_axis.StoreFloat3(&mR2xAxis);
        }       
            JPH_IF_DEBUG(else Vec3::sNaN().StoreFloat3(&mR2xAxis);)
 
        // Calculate inverse effective mass: K = J M^-1 J^T
        float inv_effective_mass;
 
        if constexpr (Type1 == EMotionType::Dynamic)
        {
            Vec3 invi1_r1_plus_u_x_axis = inInvI1.Multiply3x3(r1_plus_u_x_axis);
            invi1_r1_plus_u_x_axis.StoreFloat3(&mInvI1_R1PlusUxAxis);
            inv_effective_mass = inMotionProperties1->GetInverseMass() + invi1_r1_plus_u_x_axis.Dot(r1_plus_u_x_axis);
        }
        else
        {
            (void)r1_plus_u_x_axis; // Fix compiler warning: Not using this (it's not calculated either)
            JPH_IF_DEBUG(Vec3::sNaN().StoreFloat3(&mInvI1_R1PlusUxAxis);)
            inv_effective_mass = 0.0f;
        }
 
        if constexpr (Type2 == EMotionType::Dynamic)
        {
            Vec3 invi2_r2_x_axis = inInvI2.Multiply3x3(r2_x_axis);
            invi2_r2_x_axis.StoreFloat3(&mInvI2_R2xAxis);
            inv_effective_mass += inMotionProperties2->GetInverseMass() + invi2_r2_x_axis.Dot(r2_x_axis);
        }
        else
        {
            (void)r2_x_axis; // Fix compiler warning: Not using this (it's not calculated either)
            JPH_IF_DEBUG(Vec3::sNaN().StoreFloat3(&mInvI2_R2xAxis);)
        }
 
        // Calculate effective mass and spring properties
        mSpringPart.CalculateSpringProperties(inDeltaTime, inv_effective_mass, inBias, inC, inFrequency, inDamping, mEffectiveMass);
 
        JPH_DET_LOG("TemplatedCalculateConstraintProperties: dt: " << inDeltaTime << " invI1: " << inInvI1 << " r1PlusU: " << inR1PlusU << " invI2: " << inInvI2 << " r2: " << inR2 << " bias: " << inBias << ", c: " << inC << ", frequency: " << inFrequency << ", damping: " << inDamping << " r1PlusUxAxis: " << mR1PlusUxAxis << " r2xAxis: " << mR2xAxis << " invI1_R1PlusUxAxis: " << mInvI1_R1PlusUxAxis << " invI2_R2xAxis: " << mInvI2_R2xAxis << " effectiveMass: " << mEffectiveMass << " totalLambda: " << mTotalLambda);
    }
 
    inline void                 CalculateConstraintProperties(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f, float inC = 0.0f, float inFrequency = 0.0f, float inDamping = 0.0f)
    {
        // Dispatch to the correct templated form
        switch (inBody1.GetMotionType())
        {
        case EMotionType::Dynamic:
            {
                const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked();
                Mat44 invi1 = inBody1.GetInverseInertia();
                switch (inBody2.GetMotionType())
                {
                case EMotionType::Dynamic:
                    TemplatedCalculateConstraintProperties<EMotionType::Dynamic, EMotionType::Dynamic>(inDeltaTime, mp1, invi1, inR1PlusU, inBody2.GetMotionPropertiesUnchecked(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis, inBias, inC, inFrequency, inDamping);
                    break;
 
                case EMotionType::Kinematic:
                    TemplatedCalculateConstraintProperties<EMotionType::Dynamic, EMotionType::Kinematic>(inDeltaTime, mp1, invi1, inR1PlusU, nullptr, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis, inBias, inC, inFrequency, inDamping);
                    break;
 
                case EMotionType::Static:
                    TemplatedCalculateConstraintProperties<EMotionType::Dynamic, EMotionType::Static>(inDeltaTime, mp1, invi1, inR1PlusU, nullptr, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis, inBias, inC, inFrequency, inDamping);
                    break;
 
                default:
                    JPH_ASSERT(false);
                    break;
                }
                break;
            }
 
        case EMotionType::Kinematic:
            JPH_ASSERT(inBody2.IsDynamic());
            TemplatedCalculateConstraintProperties<EMotionType::Kinematic, EMotionType::Dynamic>(inDeltaTime, nullptr, Mat44() /* Will not be used */, inR1PlusU, inBody2.GetMotionPropertiesUnchecked(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis, inBias, inC, inFrequency, inDamping);
            break;
 
        case EMotionType::Static:
            JPH_ASSERT(inBody2.IsDynamic());
            TemplatedCalculateConstraintProperties<EMotionType::Static, EMotionType::Dynamic>(inDeltaTime, nullptr, Mat44() /* Will not be used */, inR1PlusU, inBody2.GetMotionPropertiesUnchecked(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis, inBias, inC, inFrequency, inDamping);
            break;
 
        default:
            JPH_ASSERT(false);
            break;
        }
    }
 
    inline void                 Deactivate()
    {
        mEffectiveMass = 0.0f;
        mTotalLambda = 0.0f;
    }
 
    inline bool                 IsActive() const
    {
        return mEffectiveMass != 0.0f;
    }
 
    template <EMotionType Type1, EMotionType Type2>
    inline void                 TemplatedWarmStart(MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, Vec3Arg inWorldSpaceAxis, float inWarmStartImpulseRatio)
    {
        mTotalLambda *= inWarmStartImpulseRatio;
 
        ApplyVelocityStep<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, inWorldSpaceAxis, mTotalLambda);
    }
 
    inline void                 WarmStart(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inWarmStartImpulseRatio)
    {
        EMotionType motion_type1 = ioBody1.GetMotionType();
        MotionProperties *motion_properties1 = ioBody1.GetMotionPropertiesUnchecked();
 
        EMotionType motion_type2 = ioBody2.GetMotionType();
        MotionProperties *motion_properties2 = ioBody2.GetMotionPropertiesUnchecked();
 
        // Dispatch to the correct templated form
        // Note: Warm starting doesn't differentiate between kinematic/static bodies so we handle both as static bodies
        if (motion_type1 == EMotionType::Dynamic)
        {
            if (motion_type2 == EMotionType::Dynamic)
                TemplatedWarmStart<EMotionType::Dynamic, EMotionType::Dynamic>(motion_properties1, motion_properties2, inWorldSpaceAxis, inWarmStartImpulseRatio);
            else
                TemplatedWarmStart<EMotionType::Dynamic, EMotionType::Static>(motion_properties1, motion_properties2, inWorldSpaceAxis, inWarmStartImpulseRatio);
        }
        else
        {
            JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
            TemplatedWarmStart<EMotionType::Static, EMotionType::Dynamic>(motion_properties1, motion_properties2, inWorldSpaceAxis, inWarmStartImpulseRatio);
        }
    }
 
    template <EMotionType Type1, EMotionType Type2>
    inline bool                 TemplatedSolveVelocityConstraint(MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda)
    {
        // Calculate jacobian multiplied by linear velocity
        float jv;
        if constexpr (Type1 != EMotionType::Static && Type2 != EMotionType::Static)
            jv = inWorldSpaceAxis.Dot(ioMotionProperties1->GetLinearVelocity() - ioMotionProperties2->GetLinearVelocity());
        else if constexpr (Type1 != EMotionType::Static)
            jv = inWorldSpaceAxis.Dot(ioMotionProperties1->GetLinearVelocity());
        else if constexpr (Type2 != EMotionType::Static)
            jv = inWorldSpaceAxis.Dot(-ioMotionProperties2->GetLinearVelocity());
        else
            JPH_ASSERT(false); // Static vs static is nonsensical!
 
        // Calculate jacobian multiplied by angular velocity
        if constexpr (Type1 != EMotionType::Static)
            jv += Vec3::sLoadFloat3Unsafe(mR1PlusUxAxis).Dot(ioMotionProperties1->GetAngularVelocity());
        if constexpr (Type2 != EMotionType::Static)
            jv -= Vec3::sLoadFloat3Unsafe(mR2xAxis).Dot(ioMotionProperties2->GetAngularVelocity());
 
        // Lagrange multiplier is:
        //
        // lambda = -K^-1 (J v + b)
        float lambda = mEffectiveMass * (jv - mSpringPart.GetBias(mTotalLambda));
        float new_lambda = Clamp(mTotalLambda + lambda, inMinLambda, inMaxLambda); // Clamp impulse
        lambda = new_lambda - mTotalLambda; // Lambda potentially got clamped, calculate the new impulse to apply
        mTotalLambda = new_lambda; // Store accumulated impulse
 
        return ApplyVelocityStep<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, inWorldSpaceAxis, lambda);
    }
 
    inline bool                 SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda)
    {
        EMotionType motion_type1 = ioBody1.GetMotionType();
        MotionProperties *motion_properties1 = ioBody1.GetMotionPropertiesUnchecked();
 
        EMotionType motion_type2 = ioBody2.GetMotionType();
        MotionProperties *motion_properties2 = ioBody2.GetMotionPropertiesUnchecked();
 
        // Dispatch to the correct templated form
        switch (motion_type1)
        {
        case EMotionType::Dynamic:
            switch (motion_type2)
            {
            case EMotionType::Dynamic:
                return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Dynamic>(motion_properties1, motion_properties2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
 
            case EMotionType::Kinematic:
                return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Kinematic>(motion_properties1, motion_properties2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
 
            case EMotionType::Static:
                return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Static>(motion_properties1, motion_properties2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
 
            default:
                JPH_ASSERT(false);
                break;
            }
            break;
 
        case EMotionType::Kinematic:
            JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
            return TemplatedSolveVelocityConstraint<EMotionType::Kinematic, EMotionType::Dynamic>(motion_properties1, motion_properties2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
 
        case EMotionType::Static:
            JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
            return TemplatedSolveVelocityConstraint<EMotionType::Static, EMotionType::Dynamic>(motion_properties1, motion_properties2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
 
        default:
            JPH_ASSERT(false);
            break;
        }
 
        return false;
    }
 
    inline bool                 SolvePositionConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inC, float inBaumgarte) const
    {
        // Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint
        if (inC != 0.0f && !mSpringPart.IsActive())
        {
            // Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
            //
            // lambda = -K^-1 * beta / dt * C
            //
            // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
            float lambda = -mEffectiveMass * inBaumgarte * inC; 
 
            // Directly integrate velocity change for one time step
            //
            // Euler velocity integration: 
            // dv = M^-1 P
            //
            // Impulse:
            // P = J^T lambda
            //
            // Euler position integration:
            // x' = x + dv * dt
            //
            // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and 
            // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte 
            // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity 
            // integrate + a position integrate and then discard the velocity change.
            if (ioBody1.IsDynamic())
            {
                ioBody1.SubPositionStep((lambda * ioBody1.GetMotionPropertiesUnchecked()->GetInverseMass()) * inWorldSpaceAxis);
                ioBody1.SubRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis));
            }
            if (ioBody2.IsDynamic())
            {
                ioBody2.AddPositionStep((lambda * ioBody2.GetMotionPropertiesUnchecked()->GetInverseMass()) * inWorldSpaceAxis);
                ioBody2.AddRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis));
            }
            return true;
        }
 
        return false;
    }
 
    inline void                 SetTotalLambda(float inLambda)
    {
        mTotalLambda = inLambda;
    }
 
    inline float                GetTotalLambda() const
    {
        return mTotalLambda;
    }
 
    void                        SaveState(StateRecorder &inStream) const
    {
        inStream.Write(mTotalLambda);
    }
 
    void                        RestoreState(StateRecorder &inStream)
    {
        inStream.Read(mTotalLambda);
    }
 
private:
    Float3                      mR1PlusUxAxis;
    Float3                      mR2xAxis;
    Float3                      mInvI1_R1PlusUxAxis;
    Float3                      mInvI2_R2xAxis;
    float                       mEffectiveMass = 0.0f;
    SpringPart                  mSpringPart;
    float                       mTotalLambda = 0.0f;
};
 
JPH_NAMESPACE_END