PointConstraintPart.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/StateRecorder.h>
 
JPH_NAMESPACE_BEGIN
 
class PointConstraintPart
{
    JPH_INLINE bool             ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inLambda) const
    {
        // Apply impulse if delta is not zero
        if (inLambda != Vec3::sZero())
        {
            // 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->SubLinearVelocityStep(mp1->GetInverseMass() * inLambda);
                mp1->SubAngularVelocityStep(mInvI1_R1X * inLambda);
            }
            if (ioBody2.IsDynamic())
            {
                MotionProperties *mp2 = ioBody2.GetMotionProperties();
                mp2->AddLinearVelocityStep(mp2->GetInverseMass() * inLambda);
                mp2->AddAngularVelocityStep(mInvI2_R2X * inLambda);
            }
            return true;
        }
 
        return false;
    }
 
public:
    inline void                 CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, Vec3Arg inR1, const Body &inBody2, Mat44Arg inRotation2, Vec3Arg inR2)
    {
        // Positions where the point constraint acts on (middle point between center of masses) in world space
        mR1 = inRotation1.Multiply3x3(inR1);
        mR2 = inRotation2.Multiply3x3(inR2);
 
        // Calculate effective mass: K^-1 = (J M^-1 J^T)^-1
        // Using: I^-1 = R * Ibody^-1 * R^T
        float summed_inv_mass;
        Mat44 inv_effective_mass;
        if (inBody1.IsDynamic())
        {
            const MotionProperties *mp1 = inBody1.GetMotionProperties();
            Mat44 invi1 = mp1->GetInverseInertiaForRotation(inRotation1);
            summed_inv_mass = mp1->GetInverseMass();
 
            Mat44 r1x = Mat44::sCrossProduct(mR1);
            mInvI1_R1X = invi1.Multiply3x3(r1x);
            inv_effective_mass = r1x.Multiply3x3(invi1).Multiply3x3RightTransposed(r1x);
        }
        else
        {
            JPH_IF_DEBUG(mInvI1_R1X = Mat44::sNaN();)
 
            summed_inv_mass = 0.0f;
            inv_effective_mass = Mat44::sZero();
        }
 
        if (inBody2.IsDynamic())
        {
            const MotionProperties *mp2 = inBody2.GetMotionProperties();
            Mat44 invi2 = mp2->GetInverseInertiaForRotation(inRotation2);
            summed_inv_mass += mp2->GetInverseMass();
 
            Mat44 r2x = Mat44::sCrossProduct(mR2);
            mInvI2_R2X = invi2.Multiply3x3(r2x);
            inv_effective_mass += r2x.Multiply3x3(invi2).Multiply3x3RightTransposed(r2x);
        }
        else
        {
            JPH_IF_DEBUG(mInvI2_R2X = Mat44::sNaN();)
        }
 
        inv_effective_mass += Mat44::sScale(summed_inv_mass);
        mEffectiveMass = inv_effective_mass.Inversed3x3();
    }
 
    inline void                 WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
    {
        mTotalLambda *= inWarmStartImpulseRatio;
        ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda);
    }
 
    inline bool                 SolveVelocityConstraint(Body &ioBody1, Body &ioBody2)
    {
        // Calculate lagrange multiplier:
        //
        // lambda = -K^-1 (J v + b)
        Vec3 lambda = mEffectiveMass * (ioBody1.GetLinearVelocity() - mR1.Cross(ioBody1.GetAngularVelocity()) - ioBody2.GetLinearVelocity() + mR2.Cross(ioBody2.GetAngularVelocity()));
        mTotalLambda += lambda; // Store accumulated lambda
        return ApplyVelocityStep(ioBody1, ioBody2, lambda);
    }
 
    inline bool                 SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inBaumgarte) const
    {
        Vec3 separation = (Vec3(ioBody2.GetCenterOfMassPosition() - ioBody1.GetCenterOfMassPosition()) + mR2 - mR1);
        if (separation != Vec3::sZero())
        {
            // 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
            Vec3 lambda = mEffectiveMass * -inBaumgarte * separation; 
 
            // 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(ioBody1.GetMotionProperties()->GetInverseMass() * lambda);
                ioBody1.SubRotationStep(mInvI1_R1X * lambda);
            }
            if (ioBody2.IsDynamic())
            {
                ioBody2.AddPositionStep(ioBody2.GetMotionProperties()->GetInverseMass() * lambda);
                ioBody2.AddRotationStep(mInvI2_R2X * lambda);
            }
 
            return true;
        }
 
        return false;
    }
    
    Vec3                        GetTotalLambda() const
    {
        return mTotalLambda;
    }
 
    void                        SaveState(StateRecorder &inStream) const
    {
        inStream.Write(mTotalLambda);
    }
 
    void                        RestoreState(StateRecorder &inStream)
    {
        inStream.Read(mTotalLambda);
    }
 
private:
    Vec3                        mR1;
    Vec3                        mR2;
    Mat44                       mInvI1_R1X;
    Mat44                       mInvI2_R2X;
    Mat44                       mEffectiveMass;
    Vec3                        mTotalLambda { Vec3::sZero() };
};
 
JPH_NAMESPACE_END