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>
#include <Jolt/Physics/DeterminismLog.h>
 
JPH_NAMESPACE_BEGIN
 
class AxisConstraintPart
{
    template <EMotionType Type1, EMotionType Type2>
    JPH_INLINE bool             ApplyVelocityStep(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, 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 * inInvMass1) * inWorldSpaceAxis);
                ioMotionProperties1->SubAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis));
            }
            if constexpr (Type2 == EMotionType::Dynamic)
            {
                ioMotionProperties2->AddLinearVelocityStep((inLambda * inInvMass2) * inWorldSpaceAxis);
                ioMotionProperties2->AddAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis));
            }
            return true;
        }
 
        return false;
    }
 
    template <EMotionType Type1, EMotionType Type2>
    JPH_INLINE float            TemplatedCalculateInverseEffectiveMass(float inInvMass1, Mat44Arg inInvI1, Vec3Arg inR1PlusU, float inInvMass2, Mat44Arg inInvI2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis)
    {
        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);
        }
        else
        {
        #ifdef JPH_DEBUG
            Vec3::sNaN().StoreFloat3(&mR1PlusUxAxis);
        #endif
        }
 
        Vec3 r2_x_axis;
        if constexpr (Type2 != EMotionType::Static)
        {
            r2_x_axis = inR2.Cross(inWorldSpaceAxis);
            r2_x_axis.StoreFloat3(&mR2xAxis);
        }
        else
        {
        #ifdef JPH_DEBUG
            Vec3::sNaN().StoreFloat3(&mR2xAxis);
        #endif
        }
 
        // 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 = inInvMass1 + 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 += inInvMass2 + 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);)
        }
 
        return inv_effective_mass;
    }
 
    JPH_INLINE float            CalculateInverseEffectiveMass(const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis)
    {
        // Dispatch to the correct templated form
        switch (inBody1.GetMotionType())
        {
        case EMotionType::Dynamic:
            {
                const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked();
                float inv_m1 = mp1->GetInverseMass();
                Mat44 inv_i1 = inBody1.GetInverseInertia();
                switch (inBody2.GetMotionType())
                {
                case EMotionType::Dynamic:
                    return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Dynamic>(inv_m1, inv_i1, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
 
                case EMotionType::Kinematic:
                    return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Kinematic>(inv_m1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
 
                case EMotionType::Static:
                    return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Static>(inv_m1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
 
                default:
                    break;
                }
                break;
            }
 
        case EMotionType::Kinematic:
            JPH_ASSERT(inBody2.IsDynamic());
            return TemplatedCalculateInverseEffectiveMass<EMotionType::Kinematic, EMotionType::Dynamic>(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
 
        case EMotionType::Static:
            JPH_ASSERT(inBody2.IsDynamic());
            return TemplatedCalculateInverseEffectiveMass<EMotionType::Static, EMotionType::Dynamic>(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
 
        default:
            break;
        }
 
        JPH_ASSERT(false);
        return 0.0f;
    }
 
    JPH_INLINE float            CalculateInverseEffectiveMassWithMassOverride(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, Vec3Arg inR1PlusU, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis)
    {
        // Dispatch to the correct templated form
        switch (inBody1.GetMotionType())
        {
        case EMotionType::Dynamic:
            {
                Mat44 inv_i1 = inInvInertiaScale1 * inBody1.GetInverseInertia();
                switch (inBody2.GetMotionType())
                {
                case EMotionType::Dynamic:
                    return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Dynamic>(inInvMass1, inv_i1, inR1PlusU, inInvMass2, inInvInertiaScale2 * inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
 
                case EMotionType::Kinematic:
                    return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Kinematic>(inInvMass1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
 
                case EMotionType::Static:
                    return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Static>(inInvMass1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
 
                default:
                    break;
                }
                break;
            }
 
        case EMotionType::Kinematic:
            JPH_ASSERT(inBody2.IsDynamic());
            return TemplatedCalculateInverseEffectiveMass<EMotionType::Kinematic, EMotionType::Dynamic>(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inInvMass2, inInvInertiaScale2 * inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
 
        case EMotionType::Static:
            JPH_ASSERT(inBody2.IsDynamic());
            return TemplatedCalculateInverseEffectiveMass<EMotionType::Static, EMotionType::Dynamic>(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inInvMass2, inInvInertiaScale2 * inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
 
        default:
            break;
        }
 
        JPH_ASSERT(false);
        return 0.0f;
    }
 
public:
    template <EMotionType Type1, EMotionType Type2>
    JPH_INLINE void             TemplatedCalculateConstraintProperties(float inInvMass1, Mat44Arg inInvI1, Vec3Arg inR1PlusU, float inInvMass2, Mat44Arg inInvI2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f)
    {
        float inv_effective_mass = TemplatedCalculateInverseEffectiveMass<Type1, Type2>(inInvMass1, inInvI1, inR1PlusU, inInvMass2, inInvI2, inR2, inWorldSpaceAxis);
 
        if (inv_effective_mass == 0.0f)
            Deactivate();
        else
        {
            mEffectiveMass = 1.0f / inv_effective_mass;
            mSpringPart.CalculateSpringPropertiesWithBias(inBias);
        }
 
        JPH_DET_LOG("TemplatedCalculateConstraintProperties: invM1: " << inInvMass1 << " invI1: " << inInvI1 << " r1PlusU: " << inR1PlusU << " invM2: " << inInvMass2 << " invI2: " << inInvI2 << " r2: " << inR2 << " bias: " << inBias << " r1PlusUxAxis: " << mR1PlusUxAxis << " r2xAxis: " << mR2xAxis << " invI1_R1PlusUxAxis: " << mInvI1_R1PlusUxAxis << " invI2_R2xAxis: " << mInvI2_R2xAxis << " effectiveMass: " << mEffectiveMass << " totalLambda: " << mTotalLambda);
    }
 
    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                 CalculateConstraintPropertiesWithMassOverride(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, Vec3Arg inR1PlusU, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f)
    {
        float inv_effective_mass = CalculateInverseEffectiveMassWithMassOverride(inBody1, inInvMass1, inInvInertiaScale1, inR1PlusU, inBody2, inInvMass2, inInvInertiaScale2, 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
            mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inFrequency, inDamping, mEffectiveMass);
    }
 
    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
            mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inStiffness, inDamping, mEffectiveMass);
    }
 
    inline void                 CalculateConstraintPropertiesWithSettings(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.mMode == ESpringMode::FrequencyAndDamping)
            mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mFrequency, inSpringSettings.mDamping, mEffectiveMass);
        else
            mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mStiffness, inSpringSettings.mDamping, mEffectiveMass);
    }
 
    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, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inWarmStartImpulseRatio)
    {
        mTotalLambda *= inWarmStartImpulseRatio;
 
        ApplyVelocityStep<Type1, Type2>(ioMotionProperties1, inInvMass1, ioMotionProperties2, inInvMass2, 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_properties1->GetInverseMass(), motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inWarmStartImpulseRatio);
            else
                TemplatedWarmStart<EMotionType::Dynamic, EMotionType::Static>(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inWarmStartImpulseRatio);
        }
        else
        {
            JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
            TemplatedWarmStart<EMotionType::Static, EMotionType::Dynamic>(motion_properties1, 0.0f /* Unused */, motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inWarmStartImpulseRatio);
        }
    }
 
    template <EMotionType Type1, EMotionType Type2>
    JPH_INLINE float            TemplatedSolveVelocityConstraintGetTotalLambda(const MotionProperties *ioMotionProperties1, const MotionProperties *ioMotionProperties2, Vec3Arg inWorldSpaceAxis) const
    {
        // 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));
 
        // Return the total accumulated lambda
        return mTotalLambda + lambda;
    }
 
    template <EMotionType Type1, EMotionType Type2>
    JPH_INLINE bool             TemplatedSolveVelocityConstraintApplyLambda(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inTotalLambda)
    {
        float delta_lambda = inTotalLambda - mTotalLambda; // Calculate change in lambda
        mTotalLambda = inTotalLambda; // Store accumulated impulse
 
        return ApplyVelocityStep<Type1, Type2>(ioMotionProperties1, inInvMass1, ioMotionProperties2, inInvMass2, inWorldSpaceAxis, delta_lambda);
    }
 
    template <EMotionType Type1, EMotionType Type2>
    inline bool                 TemplatedSolveVelocityConstraint(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda)
    {
        float total_lambda = TemplatedSolveVelocityConstraintGetTotalLambda<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, inWorldSpaceAxis);
 
        // Clamp impulse to specified range
        total_lambda = Clamp(total_lambda, inMinLambda, inMaxLambda);
 
        return TemplatedSolveVelocityConstraintApplyLambda<Type1, Type2>(ioMotionProperties1, inInvMass1, ioMotionProperties2, inInvMass2, inWorldSpaceAxis, total_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_properties1->GetInverseMass(), motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inMinLambda, inMaxLambda);
 
            case EMotionType::Kinematic:
                return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Kinematic>(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda);
 
            case EMotionType::Static:
                return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Static>(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, 0.0f /* Unused */, 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, 0.0f /* Unused */, motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inMinLambda, inMaxLambda);
 
        case EMotionType::Static:
            JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
            return TemplatedSolveVelocityConstraint<EMotionType::Static, EMotionType::Dynamic>(motion_properties1, 0.0f /* Unused */, motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inMinLambda, inMaxLambda);
 
        default:
            JPH_ASSERT(false);
            break;
        }
 
        return false;
    }
 
    inline bool                 SolveVelocityConstraintWithMassOverride(Body &ioBody1, float inInvMass1, Body &ioBody2, float inInvMass2, 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, inInvMass1, motion_properties2, inInvMass2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
 
            case EMotionType::Kinematic:
                return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Kinematic>(motion_properties1, inInvMass1, motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda);
 
            case EMotionType::Static:
                return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Static>(motion_properties1, inInvMass1, motion_properties2, 0.0f /* Unused */, 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, 0.0f /* Unused */, motion_properties2, inInvMass2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
 
        case EMotionType::Static:
            JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
            return TemplatedSolveVelocityConstraint<EMotionType::Static, EMotionType::Dynamic>(motion_properties1, 0.0f /* Unused */, motion_properties2, inInvMass2, 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.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;
    }
 
    inline bool                 SolvePositionConstraintWithMassOverride(Body &ioBody1, float inInvMass1, Body &ioBody2, float inInvMass2, 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 * inInvMass1) * inWorldSpaceAxis);
                ioBody1.SubRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis));
            }
            if (ioBody2.IsDynamic())
            {
                ioBody2.AddPositionStep((lambda * inInvMass2) * 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