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/Constraints/SpringSettings.h>
#include <Jolt/Physics/StateRecorder.h>
 
JPH_NAMESPACE_BEGIN
 
class AxisConstraintPart
{
    JPH_INLINE bool             ApplyVelocityStep(Body &ioBody1, Body &ioBody2, 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 (ioBody1.IsDynamic())
            {
                MotionProperties *mp1 = ioBody1.GetMotionPropertiesUnchecked();
                mp1->SubLinearVelocityStep((inLambda * mp1->GetInverseMass()) * inWorldSpaceAxis);
                mp1->SubAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis));
            }
            if (ioBody2.IsDynamic())
            {
                MotionProperties *mp2 = ioBody2.GetMotionPropertiesUnchecked();
                mp2->AddLinearVelocityStep((inLambda * mp2->GetInverseMass()) * inWorldSpaceAxis);
                mp2->AddAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis));
            }
            return true;
        }
 
        return false;
    }
 
    JPH_INLINE float            CalculateInverseEffectiveMass(const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis)
    {
        JPH_ASSERT(inWorldSpaceAxis.IsNormalized(1.0e-5f));
 
        // Calculate inverse effective mass: K = J M^-1 J^T
        float inv_effective_mass;
 
        if (!inBody1.IsStatic())
        {
            Vec3 r1_plus_u_x_axis = inR1PlusU.Cross(inWorldSpaceAxis);
            r1_plus_u_x_axis.StoreFloat3(&mR1PlusUxAxis);
 
            if (inBody1.IsDynamic())
            {
                const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked();
                Vec3 invi1_r1_plus_u_x_axis = mp1->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), r1_plus_u_x_axis);
                invi1_r1_plus_u_x_axis.StoreFloat3(&mInvI1_R1PlusUxAxis);
 
                inv_effective_mass = mp1->GetInverseMass() + invi1_r1_plus_u_x_axis.Dot(r1_plus_u_x_axis);
            }
            else
            {
                JPH_IF_DEBUG(Vec3::sNaN().StoreFloat3(&mInvI1_R1PlusUxAxis);)
 
                inv_effective_mass = 0.0f;
            }
        }
        else
        {
            JPH_IF_DEBUG(Vec3::sNaN().StoreFloat3(&mR1PlusUxAxis);)
            JPH_IF_DEBUG(Vec3::sNaN().StoreFloat3(&mInvI1_R1PlusUxAxis);)
 
            inv_effective_mass = 0.0f;
        }
 
        if (!inBody2.IsStatic())
        {
            Vec3 r2_x_axis = inR2.Cross(inWorldSpaceAxis);
            r2_x_axis.StoreFloat3(&mR2xAxis);
 
            if (inBody2.IsDynamic())
            {
                const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked();
                Vec3 invi2_r2_x_axis = mp2->MultiplyWorldSpaceInverseInertiaByVector(inBody2.GetRotation(), r2_x_axis);
                invi2_r2_x_axis.StoreFloat3(&mInvI2_R2xAxis);
 
                inv_effective_mass += mp2->GetInverseMass() + invi2_r2_x_axis.Dot(r2_x_axis);
            }
            else
            {
                JPH_IF_DEBUG(Vec3::sNaN().StoreFloat3(&mInvI2_R2xAxis);)
            }
        }
        else
        {
            JPH_IF_DEBUG(Vec3::sNaN().StoreFloat3(&mR2xAxis);)
            JPH_IF_DEBUG(Vec3::sNaN().StoreFloat3(&mInvI2_R2xAxis);)
        }
 
        return inv_effective_mass;
    }
 
public:
    inline void                 CalculateConstraintProperties(const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f)
    {
        float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis);
 
        if (inv_effective_mass == 0.0f)
            Deactivate();
        else
        {
            mEffectiveMass = 1.0f / inv_effective_mass;
            mSpringPart.CalculateSpringPropertiesWithBias(inBias);
        }
    }
 
    inline void                 CalculateConstraintPropertiesWithFrequencyAndDamping(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inFrequency, float inDamping)
    {
        float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis);
 
        if (inv_effective_mass == 0.0f)
            Deactivate();
        else if (inFrequency > 0.0f)
            mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inFrequency, inDamping, mEffectiveMass);
        else
        {
            mEffectiveMass = 1.0f / inv_effective_mass;
            mSpringPart.CalculateSpringPropertiesWithBias(inBias);
        }
    }
 
    inline void                 CalculateConstraintPropertiesWithStiffnessAndDamping(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inStiffness, float inDamping)
    {
        float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis);
 
        if (inv_effective_mass == 0.0f)
            Deactivate();
        else if (inStiffness > 0.0f || inDamping > 0.0f)
            mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inStiffness, inDamping, mEffectiveMass);
        else
        {
            mEffectiveMass = 1.0f / inv_effective_mass;
            mSpringPart.CalculateSpringPropertiesWithBias(inBias);
        }
    }
 
    inline void                 CalculateConstraintPropertiesWithSettingsForLimit(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, const SpringSettings &inSpringSettings)
    {
        float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis);
 
        if (inv_effective_mass == 0.0f)
            Deactivate();
        else if (!inSpringSettings.HasStiffness())
        {
            mEffectiveMass = 1.0f / inv_effective_mass;
            mSpringPart.CalculateSpringPropertiesWithBias(inBias);
        }
        else
            mSpringPart.CalculateSpringPropertiesWithSettings(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings, mEffectiveMass);
    }
 
    inline void                 CalculateConstraintPropertiesWithSettingsForMotor(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, const SpringSettings &inSpringSettings)
    {
        JPH_ASSERT(inSpringSettings.HasStiffnessOrDamping());
 
        float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis);
 
        if (inv_effective_mass == 0.0f)
            Deactivate();
        else
            mSpringPart.CalculateSpringPropertiesWithSettings(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings, mEffectiveMass);
    }
 
    inline void                 Deactivate()
    {
        mEffectiveMass = 0.0f;
        mTotalLambda = 0.0f;
    }
 
    inline bool                 IsActive() const
    {
        return mEffectiveMass != 0.0f;
    }
 
    inline void                 WarmStart(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inWarmStartImpulseRatio)
    {
        mTotalLambda *= inWarmStartImpulseRatio;
 
        ApplyVelocityStep(ioBody1, ioBody2, inWorldSpaceAxis, mTotalLambda);
    }
 
    inline bool                 SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda)
    {
        const MotionProperties *mp1 = ioBody1.GetMotionPropertiesUnchecked();
        const MotionProperties *mp2 = ioBody2.GetMotionPropertiesUnchecked();
 
        // Calculate jacobian multiplied by linear velocity
        float jv;
        if (!ioBody1.IsStatic())
        {
            if (!ioBody2.IsStatic())
                jv = inWorldSpaceAxis.Dot(mp1->GetLinearVelocity() - mp2->GetLinearVelocity());
            else
                jv = inWorldSpaceAxis.Dot(mp1->GetLinearVelocity());
        }
        else
        {
            JPH_ASSERT(!ioBody2.IsStatic());
            jv = inWorldSpaceAxis.Dot(-mp2->GetLinearVelocity());
        }
 
        // Calculate jacobian multiplied by angular velocity
        if (!ioBody1.IsStatic())
            jv += Vec3::sLoadFloat3Unsafe(mR1PlusUxAxis).Dot(mp1->GetAngularVelocity());
        if (!ioBody2.IsStatic())
            jv -= Vec3::sLoadFloat3Unsafe(mR2xAxis).Dot(mp2->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(ioBody1, ioBody2, inWorldSpaceAxis, lambda);
    }
 
    float                       GetTotalLambda() const
    {
        return mTotalLambda;
    }
 
    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.GetMotionProperties()->GetInverseMass()) * inWorldSpaceAxis);
                ioBody1.SubRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis));
            }
            if (ioBody2.IsDynamic())
            {
                ioBody2.AddPositionStep((lambda * ioBody2.GetMotionProperties()->GetInverseMass()) * inWorldSpaceAxis);
                ioBody2.AddRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis));
            }
            return true;
        }
 
        return false;
    }
 
    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