public static void Prestep(ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide normal, ref Contact1PrestepData prestep, 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(ref prestep.OffsetA0, ref normal, out projection.Penetration0.AngularA);
Vector3Wide.Subtract(ref prestep.OffsetA0, ref prestep.OffsetB, out var offsetB0);
Vector3Wide.CrossWithoutOverlap(ref normal, ref offsetB0, out projection.Penetration0.AngularB);
//effective mass
Triangular3x3Wide.VectorSandwich(ref projection.Penetration0.AngularA, ref inertiaA.InverseInertiaTensor, out var angularA0);
Triangular3x3Wide.VectorSandwich(ref projection.Penetration0.AngularB, ref 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.
Springiness.ComputeSpringiness(ref prestep.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(
prestep.PenetrationDepth0 * new Vector <float>(inverseDt),
Vector.Min(prestep.PenetrationDepth0 * positionErrorToVelocity, prestep.MaximumRecoveryVelocity));
}
public static void Prestep(ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Contact1PrestepData prestep, float dt, float inverseDt,
out Projection projection)
{
Vector3Wide.Subtract(prestep.OffsetA0, prestep.OffsetB, out var contactOffsetB);
Prestep(ref inertiaA, ref inertiaB,
ref prestep.OffsetA0, ref contactOffsetB, ref prestep.Normal, ref prestep.PenetrationDepth0, ref prestep.SpringSettings, ref prestep.MaximumRecoveryVelocity,
dt, inverseDt, out projection);
}