public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref BodyVelocities wsvB,
ref PenetrationLimitProjection 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(wsvA.Linear, normal, out var csvaLinear);
Vector3Wide.Dot(wsvA.Angular, projection.AngularA, out var csvaAngular);
Vector3Wide.Dot(wsvB.Linear, normal, out var negatedCSVBLinear);
Vector3Wide.Dot(wsvB.Angular, projection.AngularB, out var csvbAngular);
//Compute negated version to avoid the need for an explicit negate.
var negatedCSI = accumulatedImpulse * softnessImpulseScale + (csvaLinear - negatedCSVBLinear + csvaAngular + csvbAngular - projection.BiasVelocity) * projection.EffectiveMass;
var previousAccumulated = accumulatedImpulse;
accumulatedImpulse = Vector.Max(Vector <float> .Zero, accumulatedImpulse - negatedCSI);
correctiveCSI = accumulatedImpulse - previousAccumulated;
}
public static void ApplyImpulse(ref PenetrationLimitProjection projection, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide normal,
ref Vector <float> correctiveImpulse,
ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
var linearVelocityChangeA = correctiveImpulse * inertiaA.InverseMass;
Vector3Wide.Scale(normal, linearVelocityChangeA, out var correctiveVelocityALinearVelocity);
Vector3Wide.Scale(projection.AngularA, correctiveImpulse, out var correctiveAngularImpulseA);
Symmetric3x3Wide.TransformWithoutOverlap(correctiveAngularImpulseA, inertiaA.InverseInertiaTensor, out var correctiveVelocityAAngularVelocity);
var linearVelocityChangeB = correctiveImpulse * inertiaB.InverseMass;
Vector3Wide.Scale(normal, linearVelocityChangeB, out var correctiveVelocityBLinearVelocity);
Vector3Wide.Scale(projection.AngularB, correctiveImpulse, out var correctiveAngularImpulseB);
Symmetric3x3Wide.TransformWithoutOverlap(correctiveAngularImpulseB, inertiaB.InverseInertiaTensor, out var correctiveVelocityBAngularVelocity);
Vector3Wide.Add(wsvA.Linear, correctiveVelocityALinearVelocity, out wsvA.Linear);
Vector3Wide.Add(wsvA.Angular, correctiveVelocityAAngularVelocity, out wsvA.Angular);
Vector3Wide.Subtract(wsvB.Linear, correctiveVelocityBLinearVelocity, out wsvB.Linear); //Note subtract; normal = -jacobianLinearB
Vector3Wide.Add(wsvB.Angular, correctiveVelocityBAngularVelocity, out wsvB.Angular);
}
