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); }