RotationEulerConstraintPart.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 RotationEulerConstraintPart
{
private:
    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())
                ioBody1.GetMotionProperties()->SubAngularVelocityStep(mInvI1.Multiply3x3(inLambda));
            if (ioBody2.IsDynamic())
                ioBody2.GetMotionProperties()->AddAngularVelocityStep(mInvI2.Multiply3x3(inLambda));
            return true;
        }
 
        return false;
    }
 
public:
    static Quat                 sGetInvInitialOrientation(const Body &inBody1, const Body &inBody2)
    {
        // q20 = q10 r0
        // <=> r0 = q10^-1 q20
        // <=> r0^-1 = q20^-1 q10
        //
        // where:
        //
        // q20 = initial orientation of body 2
        // q10 = initial orientation of body 1
        // r0 = initial rotation from body 1 to body 2
        return inBody2.GetRotation().Conjugated() * inBody1.GetRotation();
    }
 
    static Quat                 sGetInvInitialOrientationXY(Vec3Arg inAxisX1, Vec3Arg inAxisY1, Vec3Arg inAxisX2, Vec3Arg inAxisY2)
    {
        // Store inverse of initial rotation from body 1 to body 2 in body 1 space:
        //
        // q20 = q10 r0
        // <=> r0 = q10^-1 q20
        // <=> r0^-1 = q20^-1 q10
        //
        // where:
        //
        // q10, q20 = world space initial orientation of body 1 and 2
        // r0 = initial rotation from body 1 to body 2 in local space of body 1
        //
        // We can also write this in terms of the constraint matrices:
        //
        // q20 c2 = q10 c1
        // <=> q20 = q10 c1 c2^-1
        // => r0 = c1 c2^-1
        // <=> r0^-1 = c2 c1^-1
        //
        // where:
        //
        // c1, c2 = matrix that takes us from body 1 and 2 COM to constraint space 1 and 2
        if (inAxisX1 == inAxisX2 && inAxisY1 == inAxisY2)
        {
            // Axis are the same -> identity transform
            return Quat::sIdentity();
        }
        else
        {
            Mat44 constraint1(Vec4(inAxisX1, 0), Vec4(inAxisY1, 0), Vec4(inAxisX1.Cross(inAxisY1), 0), Vec4(0, 0, 0, 1));
            Mat44 constraint2(Vec4(inAxisX2, 0), Vec4(inAxisY2, 0), Vec4(inAxisX2.Cross(inAxisY2), 0), Vec4(0, 0, 0, 1));
            return constraint2.GetQuaternion() * constraint1.GetQuaternion().Conjugated();
        }
    }
 
    static Quat                 sGetInvInitialOrientationXZ(Vec3Arg inAxisX1, Vec3Arg inAxisZ1, Vec3Arg inAxisX2, Vec3Arg inAxisZ2)
    {
        // See comment at sGetInvInitialOrientationXY
        if (inAxisX1 == inAxisX2 && inAxisZ1 == inAxisZ2)
        {
            return Quat::sIdentity();
        }
        else
        {
            Mat44 constraint1(Vec4(inAxisX1, 0), Vec4(inAxisZ1.Cross(inAxisX1), 0), Vec4(inAxisZ1, 0), Vec4(0, 0, 0, 1));
            Mat44 constraint2(Vec4(inAxisX2, 0), Vec4(inAxisZ2.Cross(inAxisX2), 0), Vec4(inAxisZ2, 0), Vec4(0, 0, 0, 1));
            return constraint2.GetQuaternion() * constraint1.GetQuaternion().Conjugated();
        }
    }
 
    inline void                 CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, const Body &inBody2, Mat44Arg inRotation2)
    {
        // Calculate properties used during constraint solving
        mInvI1 = inBody1.IsDynamic()? inBody1.GetMotionProperties()->GetInverseInertiaForRotation(inRotation1) : Mat44::sZero();
        mInvI2 = inBody2.IsDynamic()? inBody2.GetMotionProperties()->GetInverseInertiaForRotation(inRotation2) : Mat44::sZero();
 
        // Calculate effective mass: K^-1 = (J M^-1 J^T)^-1
        Mat44 inertia_sum = mInvI1 + mInvI2;
        if (!mEffectiveMass.SetInversed3x3(inertia_sum))
        {
            // If a column is zero, the axis is locked and we set the column to identity.
            // This does not matter because any impulse will always be multiplied with mInvI1 or mInvI2 which will result in zero for the locked coordinate.
            Vec4 zero = Vec4::sZero();
            if (inertia_sum.GetColumn4(0) == zero)
                inertia_sum.SetColumn4(0, Vec4(1, 0, 0, 0));
            if (inertia_sum.GetColumn4(1) == zero)
                inertia_sum.SetColumn4(1, Vec4(0, 1, 0, 0));
            if (inertia_sum.GetColumn4(2) == zero)
                inertia_sum.SetColumn4(2, Vec4(0, 0, 1, 0));
            if (!mEffectiveMass.SetInversed3x3(inertia_sum))
                Deactivate();
        }
    }
 
    inline void                 Deactivate()
    {
        mEffectiveMass = Mat44::sZero();
        mTotalLambda = Vec3::sZero();
    }
 
    inline bool                 IsActive() const
    {
        return mEffectiveMass(3, 3) != 0.0f;
    }
 
    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.Multiply3x3(ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity());
        mTotalLambda += lambda;
        return ApplyVelocityStep(ioBody1, ioBody2, lambda);
    }
 
    inline bool                 SolvePositionConstraint(Body &ioBody1, Body &ioBody2, QuatArg inInvInitialOrientation, float inBaumgarte) const
    {
        // Calculate difference in rotation
        //
        // The rotation should be:
        //
        // q2 = q1 r0
        //
        // But because of drift the actual rotation is
        //
        // q2 = diff q1 r0
        // <=> diff = q2 r0^-1 q1^-1
        //
        // Where:
        // q1 = current rotation of body 1
        // q2 = current rotation of body 2
        // diff = error that needs to be reduced to zero
        Quat diff = ioBody2.GetRotation() * inInvInitialOrientation * ioBody1.GetRotation().Conjugated();
 
        // A quaternion can be seen as:
        //
        // q = [sin(theta / 2) * v, cos(theta/2)]
        //
        // Where:
        // v = rotation vector
        // theta = rotation angle
        //
        // If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is:
        Vec3 error = 2.0f * diff.EnsureWPositive().GetXYZ();
        if (error != 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 = -inBaumgarte * mEffectiveMass * error;
 
            // 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.SubRotationStep(mInvI1.Multiply3x3(lambda));
            if (ioBody2.IsDynamic())
                ioBody2.AddRotationStep(mInvI2.Multiply3x3(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:
    Mat44                       mInvI1;
    Mat44                       mInvI2;
    Mat44                       mEffectiveMass;
    Vec3                        mTotalLambda { Vec3::sZero() };
};
 
JPH_NAMESPACE_END