public static void ApplyImpulse(ref PenetrationLimitOneBodyProjection projection, ref BodyInertias inertiaA, ref Vector3Wide normal,
ref Vector <float> correctiveImpulse,
ref BodyVelocities wsvA)
{
var linearVelocityChangeA = correctiveImpulse * inertiaA.InverseMass;
Vector3Wide.Scale(ref normal, ref linearVelocityChangeA, out var correctiveVelocityALinearVelocity);
Vector3Wide.Scale(ref projection.AngularA, ref correctiveImpulse, out var correctiveAngularImpulseA);
Triangular3x3Wide.TransformBySymmetricWithoutOverlap(ref correctiveAngularImpulseA, ref inertiaA.InverseInertiaTensor, out var correctiveVelocityAAngularVelocity);
Vector3Wide.Add(ref wsvA.LinearVelocity, ref correctiveVelocityALinearVelocity, out wsvA.LinearVelocity);
Vector3Wide.Add(ref wsvA.AngularVelocity, ref correctiveVelocityAAngularVelocity, out wsvA.AngularVelocity);
}
public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA,
ref PenetrationLimitOneBodyProjection projection,
ref Vector3Wide normal, ref Vector <float> softnessImpulseScale,
ref Vector <float> accumulatedImpulse, out Vector <float> correctiveCSI)
{
//Note that we do NOT use pretransformed jacobians here; the linear jacobian sharing (normal) meant that we had the effective mass anyway.
Vector3Wide.Dot(ref wsvA.LinearVelocity, ref normal, out var csvaLinear);
Vector3Wide.Dot(ref wsvA.AngularVelocity, ref projection.AngularA, out var csvaAngular);
//Compute negated version to avoid the need for an explicit negate.
var negatedCSI = accumulatedImpulse * softnessImpulseScale + (csvaLinear + csvaAngular - projection.BiasVelocity) * projection.EffectiveMass;
var previousAccumulated = accumulatedImpulse;
accumulatedImpulse = Vector.Max(Vector <float> .Zero, accumulatedImpulse - negatedCSI);
correctiveCSI = accumulatedImpulse - previousAccumulated;
}
