public static void Prestep(ref BodyInertias inertiaA, ref Vector3Wide normal, ref Contact3OneBodyPrestepData prestep, float dt, float inverseDt,
out Projection projection)
{
Vector3Wide.CrossWithoutOverlap(prestep.OffsetA0, normal, out projection.Penetration0.AngularA);
Vector3Wide.CrossWithoutOverlap(prestep.OffsetA1, normal, out projection.Penetration1.AngularA);
Vector3Wide.CrossWithoutOverlap(prestep.OffsetA2, normal, out projection.Penetration2.AngularA);
//effective mass
Symmetric3x3Wide.VectorSandwich(projection.Penetration0.AngularA, inertiaA.InverseInertiaTensor, out var angularA0);
Symmetric3x3Wide.VectorSandwich(projection.Penetration1.AngularA, inertiaA.InverseInertiaTensor, out var angularA1);
Symmetric3x3Wide.VectorSandwich(projection.Penetration2.AngularA, inertiaA.InverseInertiaTensor, out var angularA2);
//Linear effective mass contribution notes:
//1) The J * M^-1 * JT can be reordered to J * JT * M^-1 for the linear components, since M^-1 is a scalar and dot(n * scalar, n) = dot(n, n) * scalar.
//2) dot(normal, normal) == 1, so the contribution from each body is just its inverse mass.
SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale);
//Note that we don't precompute the JT * effectiveMass term. Since the jacobians are shared, we have to do that multiply anyway.
projection.Penetration0.EffectiveMass = effectiveMassCFMScale / (inertiaA.InverseMass + angularA0);
projection.Penetration1.EffectiveMass = effectiveMassCFMScale / (inertiaA.InverseMass + angularA1);
projection.Penetration2.EffectiveMass = effectiveMassCFMScale / (inertiaA.InverseMass + angularA2);
//If depth is negative, the bias velocity will permit motion up until the depth hits zero. This works because positionErrorToVelocity * dt will always be <=1.
var inverseDtVector = new Vector <float>(inverseDt);
projection.Penetration0.BiasVelocity = Vector.Min(prestep.PenetrationDepth0 * inverseDtVector, Vector.Min(prestep.PenetrationDepth0 * positionErrorToVelocity, prestep.MaximumRecoveryVelocity));
projection.Penetration1.BiasVelocity = Vector.Min(prestep.PenetrationDepth1 * inverseDtVector, Vector.Min(prestep.PenetrationDepth1 * positionErrorToVelocity, prestep.MaximumRecoveryVelocity));
projection.Penetration2.BiasVelocity = Vector.Min(prestep.PenetrationDepth2 * inverseDtVector, Vector.Min(prestep.PenetrationDepth2 * positionErrorToVelocity, prestep.MaximumRecoveryVelocity));
}
public static void Prestep(ref BodyInertias inertiaA, ref BodyInertias inertiaB,
ref Vector3Wide contactOffsetA, ref Vector3Wide contactOffsetB, ref Vector3Wide normal, ref Vector <float> depth, ref SpringSettingsWide springSettings, ref Vector <float> maximumRecoveryVelocity,
float dt, float inverseDt,
out Projection projection)
{
//We directly take the prestep data here since the jacobians and error don't undergo any processing.
//The contact penetration constraint takes the form:
//dot(positionA + offsetA, N) >= dot(positionB + offsetB, N)
//Or:
//dot(positionA + offsetA, N) - dot(positionB + offsetB, N) >= 0
//dot(positionA + offsetA - positionB - offsetB, N) >= 0
//where positionA and positionB are the center of mass positions of the bodies offsetA and offsetB are world space offsets from the center of mass to the contact,
//and N is a unit length vector calibrated to point from B to A. (The normal pointing direction is important; it changes the sign.)
//In practice, we'll use the collision detection system's penetration depth instead of trying to recompute the error here.
//So, treating the normal as constant, the velocity constraint is:
//dot(d/dt(positionA + offsetA - positionB - offsetB), N) >= 0
//dot(linearVelocityA + d/dt(offsetA) - linearVelocityB - d/dt(offsetB)), N) >= 0
//The velocity of the offsets are defined by the angular velocity.
//dot(linearVelocityA + angularVelocityA x offsetA - linearVelocityB - angularVelocityB x offsetB), N) >= 0
//dot(linearVelocityA, N) + dot(angularVelocityA x offsetA, N) - dot(linearVelocityB, N) - dot(angularVelocityB x offsetB), N) >= 0
//Use the properties of the scalar triple product:
//dot(linearVelocityA, N) + dot(offsetA x N, angularVelocityA) - dot(linearVelocityB, N) - dot(offsetB x N, angularVelocityB) >= 0
//Bake in the negations:
//dot(linearVelocityA, N) + dot(offsetA x N, angularVelocityA) + dot(linearVelocityB, -N) + dot(-offsetB x N, angularVelocityB) >= 0
//A x B = -B x A:
//dot(linearVelocityA, N) + dot(offsetA x N, angularVelocityA) + dot(linearVelocityB, -N) + dot(N x offsetB, angularVelocityB) >= 0
//And there you go, the jacobians!
//linearA: N
//angularA: offsetA x N
//linearB: -N
//angularB: N x offsetB
//Note that we leave the penetration depth as is, even when it's negative. Speculative contacts!
Vector3Wide.CrossWithoutOverlap(contactOffsetA, normal, out projection.Penetration0.AngularA);
Vector3Wide.CrossWithoutOverlap(normal, contactOffsetB, out projection.Penetration0.AngularB);
//effective mass
Symmetric3x3Wide.VectorSandwich(projection.Penetration0.AngularA, inertiaA.InverseInertiaTensor, out var angularA0);
Symmetric3x3Wide.VectorSandwich(projection.Penetration0.AngularB, inertiaB.InverseInertiaTensor, out var angularB0);
//Linear effective mass contribution notes:
//1) The J * M^-1 * JT can be reordered to J * JT * M^-1 for the linear components, since M^-1 is a scalar and dot(n * scalar, n) = dot(n, n) * scalar.
//2) dot(normal, normal) == 1, so the contribution from each body is just its inverse mass.
SpringSettingsWide.ComputeSpringiness(springSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale);
var linear = inertiaA.InverseMass + inertiaB.InverseMass;
//Note that we don't precompute the JT * effectiveMass term. Since the jacobians are shared, we have to do that multiply anyway.
projection.Penetration0.EffectiveMass = effectiveMassCFMScale / (linear + angularA0 + angularB0);
//If depth is negative, the bias velocity will permit motion up until the depth hits zero. This works because positionErrorToVelocity * dt will always be <=1.
projection.Penetration0.BiasVelocity = Vector.Min(
depth * new Vector <float>(inverseDt),
Vector.Min(depth * positionErrorToVelocity, maximumRecoveryVelocity));
}