// Solve the Jacobian
public void SolveContact(
ref JacobianHeader jacHeader, ref MotionVelocity velocityA, ref MotionVelocity velocityB,
Solver.StepInput stepInput, ref NativeStream.Writer collisionEventsWriter, bool enableFrictionVelocitiesHeuristic,
Solver.MotionStabilizationInput motionStabilizationSolverInputA, Solver.MotionStabilizationInput motionStabilizationSolverInputB)
{
// Copy velocity data
MotionVelocity tempVelocityA = velocityA;
MotionVelocity tempVelocityB = velocityB;
if (jacHeader.HasMassFactors)
{
MassFactors jacMod = jacHeader.AccessMassFactors();
tempVelocityA.InverseInertia *= jacMod.InverseInertiaFactorA;
tempVelocityA.InverseMass *= jacMod.InverseMassFactorA;
tempVelocityB.InverseInertia *= jacMod.InverseInertiaFactorB;
tempVelocityB.InverseMass *= jacMod.InverseMassFactorB;
}
// Solve normal impulses
float sumImpulses = 0.0f;
float totalAccumulatedImpulse = 0.0f;
bool forceCollisionEvent = false;
for (int j = 0; j < BaseJacobian.NumContacts; j++)
{
ref ContactJacAngAndVelToReachCp jacAngular = ref jacHeader.AccessAngularJacobian(j);
// Solve velocity so that predicted contact distance is greater than or equal to zero
float relativeVelocity = BaseContactJacobian.GetJacVelocity(BaseJacobian.Normal, jacAngular.Jac,
tempVelocityA.LinearVelocity, tempVelocityA.AngularVelocity, tempVelocityB.LinearVelocity, tempVelocityB.AngularVelocity);
float dv = jacAngular.VelToReachCp - relativeVelocity;
float impulse = dv * jacAngular.Jac.EffectiveMass;
float accumulatedImpulse = math.max(jacAngular.Jac.Impulse + impulse, 0.0f);
if (accumulatedImpulse != jacAngular.Jac.Impulse)
{
float deltaImpulse = accumulatedImpulse - jacAngular.Jac.Impulse;
ApplyImpulse(deltaImpulse, BaseJacobian.Normal, jacAngular.Jac, ref tempVelocityA, ref tempVelocityB,
motionStabilizationSolverInputA.InverseInertiaScale, motionStabilizationSolverInputB.InverseInertiaScale);
}
jacAngular.Jac.Impulse = accumulatedImpulse;
sumImpulses += accumulatedImpulse;
totalAccumulatedImpulse += jacAngular.Jac.Impulse;
// Force contact event even when no impulse is applied, but there is penetration.
forceCollisionEvent |= jacAngular.VelToReachCp > 0.0f;
}
public float3 FrictionEffectiveMassOffDiag; // Effective mass matrix (0, 1), (0, 2), (1, 2) == (1, 0), (2, 0), (2, 1)
// Solve the Jacobian
public void Solve(
ref JacobianHeader jacHeader, ref MotionVelocity velocityA, ref MotionVelocity velocityB,
Solver.StepInput stepInput, ref BlockStream.Writer collisionEventsWriter)
{
// Copy velocity data
MotionVelocity tempVelocityA = velocityA;
MotionVelocity tempVelocityB = velocityB;
if (jacHeader.HasMassFactors)
{
MassFactors jacMod = jacHeader.AccessMassFactors();
tempVelocityA.InverseInertiaAndMass *= jacMod.InvInertiaAndMassFactorA;
tempVelocityB.InverseInertiaAndMass *= jacMod.InvInertiaAndMassFactorB;
}
// Calculate maximum impulse per sub step
float maxImpulseToApply;
if (jacHeader.HasMaxImpulse)
{
maxImpulseToApply = jacHeader.AccessMaxImpulse() * stepInput.Timestep * stepInput.InvNumSolverIterations;
}
else
{
maxImpulseToApply = float.MaxValue;
}
// Solve normal impulses
float sumImpulses = 0.0f;
float frictionFactor = 1.0f;
float4 totalAccumulatedImpulses = float4.zero;
bool forceCollisionEvent = false;
for (int j = 0; j < BaseJacobian.NumContacts; j++)
{
ref ContactJacAngAndVelToReachCp jacAngular = ref jacHeader.AccessAngularJacobian(j);
// Solve velocity so that predicted contact distance is greater than or equal to zero
float relativeVelocity = BaseContactJacobian.GetJacVelocity(BaseJacobian.Normal, jacAngular.Jac, tempVelocityA, tempVelocityB);
float dv = jacAngular.VelToReachCp - relativeVelocity;
// Restitution (typically set to zero)
if (dv > 0.0f && CoefficientOfRestitution > 0.0f)
{
float negContactRestingVelocity = -stepInput.GravityLength * stepInput.Timestep * 1.5f;
if (relativeVelocity < negContactRestingVelocity)
{
float invMassA = tempVelocityA.InverseInertiaAndMass.w;
float invMassB = tempVelocityB.InverseInertiaAndMass.w;
float effInvMassAtCenter = invMassA + invMassB;
jacAngular.VelToReachCp = -(CoefficientOfRestitution * (relativeVelocity - negContactRestingVelocity)) *
(jacAngular.Jac.EffectiveMass * effInvMassAtCenter);
dv = jacAngular.VelToReachCp - relativeVelocity;
// reduce friction to 1/4 of the impulse
frictionFactor = 0.25f;
}
}
float impulse = dv * jacAngular.Jac.EffectiveMass;
bool clipped = impulse > maxImpulseToApply;
impulse = math.min(impulse, maxImpulseToApply);
float accumulatedImpulse = math.max(jacAngular.Jac.Impulse + impulse, 0.0f);
if (accumulatedImpulse != jacAngular.Jac.Impulse)
{
float deltaImpulse = accumulatedImpulse - jacAngular.Jac.Impulse;
ApplyImpulse(deltaImpulse, BaseJacobian.Normal, jacAngular.Jac, ref tempVelocityA, ref tempVelocityB);
}
jacAngular.Jac.Impulse = accumulatedImpulse;
sumImpulses += accumulatedImpulse;
// If there are more than 4 contacts, accumulate their impulses to the last contact point
totalAccumulatedImpulses[math.min(j, 3)] += jacAngular.Jac.Impulse;
// Force contact event even when no impulse is applied, but there is penetration.
// Also force when impulse was clipped, even if clipped to 0.
forceCollisionEvent |= jacAngular.VelToReachCp > 0.0f || clipped;
}
