public static void Prestep(ref BodyInertias inertiaA, ref Vector3Wide angularJacobianA,
out TwistFrictionProjection projection)
{
//Compute effective mass matrix contributions. No linear contributions for the twist constraint.
Triangular3x3Wide.VectorSandwich(ref angularJacobianA, ref inertiaA.InverseInertiaTensor, out var inverseEffectiveMass);
//No softening; this constraint is rigid by design. (It does support a maximum force, but that is distinct from a proper damping ratio/natural frequency.)
//Note that we have to guard against two bodies with infinite inertias. This is a valid state!
//(We do not have to do such guarding on constraints with linear jacobians; dynamic bodies cannot have zero *mass*.)
//(Also note that there's no need for epsilons here... users shouldn't be setting their inertias to the absurd values it would take to cause a problem.
//Invalid conditions can't arise dynamically.)
var inverseIsZero = Vector.Equals(Vector <float> .Zero, inverseEffectiveMass);
projection.EffectiveMass = Vector.ConditionalSelect(inverseIsZero, Vector <float> .Zero, Vector <float> .One / inverseEffectiveMass);
//Note that friction constraints have no bias velocity. They target zero velocity.
}
public static void Solve(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref TwistFrictionProjection projection,
ref Vector <float> maximumImpulse, ref Vector <float> accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
ComputeCorrectiveImpulse(ref angularJacobianA, ref projection, ref wsvA, ref wsvB, ref maximumImpulse, ref accumulatedImpulse, out var correctiveCSI);
ApplyImpulse(ref angularJacobianA, ref inertiaA, ref inertiaB, ref correctiveCSI, ref wsvA, ref wsvB);
}
public static void ComputeCorrectiveImpulse(ref Vector3Wide angularJacobianA, ref TwistFrictionProjection projection,
ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref Vector <float> maximumImpulse,
ref Vector <float> accumulatedImpulse, out Vector <float> correctiveCSI)
{
Vector3Wide.Dot(ref wsvA.AngularVelocity, ref angularJacobianA, out var csvA);
Vector3Wide.Dot(ref wsvB.AngularVelocity, ref angularJacobianA, out var negatedCSVB);
var negativeCSI = (csvA + negatedCSVB) * projection.EffectiveMass; //Since there is no bias or softness to give us the negative, we just do it when we apply to the accumulated impulse.
var previousAccumulated = accumulatedImpulse;
//The maximum force of friction depends upon the normal impulse.
accumulatedImpulse = Vector.Min(maximumImpulse, Vector.Max(-maximumImpulse, accumulatedImpulse - negativeCSI));
correctiveCSI = accumulatedImpulse - previousAccumulated;
}