public static void ApplyImpulse(ref Jacobians jacobians, ref BodyInertias inertiaA, ref Vector2Wide correctiveImpulse, ref BodyVelocities wsvA) { Matrix2x3Wide.Transform(correctiveImpulse, jacobians.LinearA, out var linearImpulseA); Matrix2x3Wide.Transform(correctiveImpulse, jacobians.AngularA, out var angularImpulseA); BodyVelocities correctiveVelocityA; Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out correctiveVelocityA.Linear); Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out correctiveVelocityA.Angular); Vector3Wide.Add(wsvA.Linear, correctiveVelocityA.Linear, out wsvA.Linear); Vector3Wide.Add(wsvA.Angular, correctiveVelocityA.Angular, out wsvA.Angular); }
public static void ApplyImpulse(ref Jacobians jacobians, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector2Wide correctiveImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB) { Matrix2x3Wide.Transform(correctiveImpulse, jacobians.LinearA, out var linearImpulseA); Matrix2x3Wide.Transform(correctiveImpulse, jacobians.AngularA, out var angularImpulseA); Matrix2x3Wide.Transform(correctiveImpulse, jacobians.AngularB, out var angularImpulseB); BodyVelocities correctiveVelocityA, correctiveVelocityB; Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out correctiveVelocityA.Linear); Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out correctiveVelocityA.Angular); Vector3Wide.Scale(linearImpulseA, inertiaB.InverseMass, out correctiveVelocityB.Linear); Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseB, inertiaB.InverseInertiaTensor, out correctiveVelocityB.Angular); Vector3Wide.Add(wsvA.Linear, correctiveVelocityA.Linear, out wsvA.Linear); Vector3Wide.Add(wsvA.Angular, correctiveVelocityA.Angular, out wsvA.Angular); Vector3Wide.Subtract(wsvB.Linear, correctiveVelocityB.Linear, out wsvB.Linear); //note subtract- we based it on the LinearA jacobian. Vector3Wide.Add(wsvB.Angular, correctiveVelocityB.Angular, out wsvB.Angular); }