AngularFrictionConstraintPart.h

Go to the documentation of this file.

// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2026 Jorrit Rouwe
// SPDX-License-Identifier: MIT
 
#pragma once
 
#include <Jolt/Physics/Constraints/ConstraintPart/ContactConstraintPart.h>
 
JPH_NAMESPACE_BEGIN
 
template <EMotionType Type1>
class AngularFrictionConstraintPart1 : public ContactConstraintPart1<EMotionType::Static>
{
};
 
template <>
class AngularFrictionConstraintPart1<EMotionType::Dynamic> : public AngularFrictionConstraintPart1<EMotionType::Static>
{
protected:
    // Note: Constructor will not be called
    Float3                      mInvI1_Axis;
};
 
template <EMotionType Type2>
class AngularFrictionConstraintPart2
{
};
 
template <>
class AngularFrictionConstraintPart2<EMotionType::Dynamic> : public AngularFrictionConstraintPart2<EMotionType::Static>
{
protected:
    // Note: Constructor will not be called
    Float3                      mInvI2_Axis;
};
 
template <EMotionType Type1, EMotionType Type2>
class AngularFrictionConstraintPart : public AngularFrictionConstraintPart1<Type1>, public AngularFrictionConstraintPart2<Type2>
{
    JPH_INLINE bool             ApplyVelocityStep(Vec3 &ioAngularVelocity1, Vec3 &ioAngularVelocity2, float inLambda) const
    {
        // Apply impulse if delta is not zero
        if (inLambda != 0.0f)
        {
            if constexpr (Type1 == EMotionType::Dynamic)
                ioAngularVelocity1 -= inLambda * Vec3::sLoadFloat3Unsafe(this->mInvI1_Axis);
            if constexpr (Type2 == EMotionType::Dynamic)
                ioAngularVelocity2 += inLambda * Vec3::sLoadFloat3Unsafe(this->mInvI2_Axis);
            return true;
        }
 
        return false;
    }
 
public:
    inline void                 CalculateConstraintProperties(Mat44Arg inInvI1, Mat44Arg inInvI2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f)
    {
        JPH_ASSERT(inWorldSpaceAxis.IsNormalized(1.0e-4f));
 
        // Store bias
        mBias = inBias;
 
        Vec3 invi1_axis, invi2_axis;
        if constexpr (Type1 == EMotionType::Dynamic)
        {
            invi1_axis = inInvI1.Multiply3x3(inWorldSpaceAxis);
            invi1_axis.StoreFloat3(&this->mInvI1_Axis);
        }
        if constexpr (Type2 == EMotionType::Dynamic)
        {
            invi2_axis = inInvI2.Multiply3x3(inWorldSpaceAxis);
            invi2_axis.StoreFloat3(&this->mInvI2_Axis);
        }
 
        float inv_effective_mass = 0.0f;
        if constexpr (Type1 == EMotionType::Dynamic && Type2 == EMotionType::Dynamic)
            inv_effective_mass = inWorldSpaceAxis.Dot(invi1_axis + invi2_axis);
        else if constexpr (Type1 == EMotionType::Dynamic)
            inv_effective_mass = inWorldSpaceAxis.Dot(invi1_axis);
        else if constexpr (Type2 == EMotionType::Dynamic)
            inv_effective_mass = inWorldSpaceAxis.Dot(invi2_axis);
        else
            JPH_ASSERT(false); // Static vs static is nonsensical!
 
        if (inv_effective_mass == 0.0f)
            this->Deactivate();
        else
            this->mEffectiveMass = 1.0f / inv_effective_mass;
    }
 
    inline bool                 WarmStart(Vec3 &ioAngularVelocity1, Vec3 &ioAngularVelocity2, float inWarmStartImpulseRatio)
    {
        this->mTotalLambda *= inWarmStartImpulseRatio;
        return ApplyVelocityStep(ioAngularVelocity1, ioAngularVelocity2, this->mTotalLambda);
    }
 
    inline bool                 SolveVelocityConstraint(Vec3 &ioAngularVelocity1, Vec3 &ioAngularVelocity2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda)
    {
        float jv;
        if constexpr (Type1 != EMotionType::Static && Type2 != EMotionType::Static)
            jv = inWorldSpaceAxis.Dot(ioAngularVelocity1 - ioAngularVelocity2);
        else if constexpr (Type1 != EMotionType::Static)
            jv = inWorldSpaceAxis.Dot(ioAngularVelocity1);
        else if constexpr (Type2 != EMotionType::Static)
            jv = -inWorldSpaceAxis.Dot(ioAngularVelocity2);
        else
            JPH_ASSERT(false); // Static vs static is nonsensical!
 
        float lambda = this->mEffectiveMass * (jv - mBias);
        float new_lambda = Clamp(this->mTotalLambda + lambda, inMinLambda, inMaxLambda); // Clamp impulse
        lambda = new_lambda - this->mTotalLambda; // Lambda potentially got clamped, calculate the new impulse to apply
        this->mTotalLambda = new_lambda; // Store accumulated impulse
 
        return ApplyVelocityStep(ioAngularVelocity1, ioAngularVelocity2, lambda);
    }
 
private:
    // Note: Constructor will not be called. This serves as 1 extra float so we can read the previous member using Vec3::sLoadFloat3Unsafe
    float                       mBias;
};
 
static_assert(sizeof(AngularFrictionConstraintPart<EMotionType::Dynamic, EMotionType::Dynamic>) == 3 * sizeof(float) + 2 * sizeof(Float3));
static_assert(sizeof(AngularFrictionConstraintPart<EMotionType::Dynamic, EMotionType::Kinematic>) == 3 * sizeof(float) + sizeof(Float3));
static_assert(sizeof(AngularFrictionConstraintPart<EMotionType::Dynamic, EMotionType::Static>) == 3 * sizeof(float) + sizeof(Float3));
static_assert(sizeof(AngularFrictionConstraintPart<EMotionType::Kinematic, EMotionType::Dynamic>) == 3 * sizeof(float) + sizeof(Float3));
static_assert(sizeof(AngularFrictionConstraintPart<EMotionType::Kinematic, EMotionType::Kinematic>) == 3 * sizeof(float));
static_assert(sizeof(AngularFrictionConstraintPart<EMotionType::Kinematic, EMotionType::Static>) == 3 * sizeof(float));
static_assert(sizeof(AngularFrictionConstraintPart<EMotionType::Static, EMotionType::Dynamic>) == 3 * sizeof(float) + sizeof(Float3));
static_assert(sizeof(AngularFrictionConstraintPart<EMotionType::Static, EMotionType::Kinematic>) == 3 * sizeof(float));
static_assert(sizeof(AngularFrictionConstraintPart<EMotionType::Static, EMotionType::Static>) == 3 * sizeof(float));
 
JPH_NAMESPACE_END