IndependentAxisConstraintPart.h

Go to the documentation of this file.

// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
// SPDX-License-Identifier: MIT
 
#pragma once
 
#include <Jolt/Physics/Body/Body.h>
#include <Jolt/Physics/StateRecorder.h>
 
JPH_NAMESPACE_BEGIN
 
class IndependentAxisConstraintPart
{
    JPH_INLINE bool             ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, 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.GetMotionProperties();
                mp1->AddLinearVelocityStep((mp1->GetInverseMass() * inLambda) * inN1);
                mp1->AddAngularVelocityStep(mInvI1_R1xN1 * inLambda);
            }
            if (ioBody2.IsDynamic())
            {
                MotionProperties *mp2 = ioBody2.GetMotionProperties();
                mp2->AddLinearVelocityStep((inRatio * mp2->GetInverseMass() * inLambda) * inN2);
                mp2->AddAngularVelocityStep(mInvI2_RatioR2xN2 * inLambda);
            }
            return true;
        }
 
        return false;
    }
 
public:
    inline void                 CalculateConstraintProperties(const Body &inBody1, const Body &inBody2, Vec3Arg inR1, Vec3Arg inN1, Vec3Arg inR2, Vec3Arg inN2, float inRatio)
    {
        JPH_ASSERT(inN1.IsNormalized(1.0e-4f) && inN2.IsNormalized(1.0e-4f));
 
        float inv_effective_mass = 0.0f;
 
        if (!inBody1.IsStatic())
        {
            const MotionProperties *mp1 = inBody1.GetMotionProperties();
            
            mR1xN1 = inR1.Cross(inN1);
            mInvI1_R1xN1 = mp1->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), mR1xN1);
            
            inv_effective_mass += mp1->GetInverseMass() + mInvI1_R1xN1.Dot(mR1xN1);
        }
 
        if (!inBody2.IsStatic())
        {
            const MotionProperties *mp2 = inBody2.GetMotionProperties();
 
            mRatioR2xN2 = inRatio * inR2.Cross(inN2);
            mInvI2_RatioR2xN2 = mp2->MultiplyWorldSpaceInverseInertiaByVector(inBody2.GetRotation(), mRatioR2xN2);
 
            inv_effective_mass += Square(inRatio) * mp2->GetInverseMass() + mInvI2_RatioR2xN2.Dot(mRatioR2xN2);
        }
 
        // Calculate inverse effective mass: K = J M^-1 J^T
        mEffectiveMass = 1.0f / inv_effective_mass;
    }
 
    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 inN1, Vec3Arg inN2, float inRatio, float inWarmStartImpulseRatio)
    {
        mTotalLambda *= inWarmStartImpulseRatio;
        ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, inRatio, mTotalLambda);
    }
 
    inline bool                 SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inMinLambda, float inMaxLambda)
    {
        // Lagrange multiplier is:
        //
        // lambda = -K^-1 (J v + b)
        float lambda = -mEffectiveMass * (inN1.Dot(ioBody1.GetLinearVelocity()) + mR1xN1.Dot(ioBody1.GetAngularVelocity()) + inRatio * inN2.Dot(ioBody2.GetLinearVelocity()) + mRatioR2xN2.Dot(ioBody2.GetAngularVelocity()));
        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, inN1, inN2, inRatio, lambda);
    }
 
    float                       GetTotalLambda() const
    {
        return mTotalLambda;
    }
 
    inline bool                 SolvePositionConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inC, float inBaumgarte) const
    {
        if (inC != 0.0f)
        {
            // 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.AddPositionStep((lambda * ioBody1.GetMotionPropertiesUnchecked()->GetInverseMass()) * inN1);
                ioBody1.AddRotationStep(lambda * mInvI1_R1xN1);
            }
            if (ioBody2.IsDynamic())
            {
                ioBody2.AddPositionStep((lambda * inRatio * ioBody2.GetMotionPropertiesUnchecked()->GetInverseMass()) * inN2);
                ioBody2.AddRotationStep(lambda * mInvI2_RatioR2xN2);
            }
            return true;
        }
 
        return false;
    }
 
    void                        SaveState(StateRecorder &inStream) const
    {
        inStream.Write(mTotalLambda);
    }
 
    void                        RestoreState(StateRecorder &inStream)
    {
        inStream.Read(mTotalLambda);
    }
 
private:
    Vec3                        mR1xN1;
    Vec3                        mInvI1_R1xN1;
    Vec3                        mRatioR2xN2;
    Vec3                        mInvI2_RatioR2xN2;
    float                       mEffectiveMass = 0.0f;
    float                       mTotalLambda = 0.0f;
};
 
JPH_NAMESPACE_END