// Generic solve method that dispatches to specific ones
public void Solve(
ref JacobianHeader jacHeader, ref MotionVelocity velocityA, ref MotionVelocity velocityB,
Solver.StepInput stepInput, ref BlockStream.Writer collisionEventsWriter)
{
bool bothBodiesWithInfInertiaAndMass = math.all(velocityA.InverseInertiaAndMass == float4.zero) && math.all(velocityB.InverseInertiaAndMass == float4.zero);
if (bothBodiesWithInfInertiaAndMass)
{
SolveInfMassPair(ref jacHeader, velocityA, velocityB, stepInput, ref collisionEventsWriter);
}
else
{
SolveContact(ref jacHeader, ref velocityA, ref velocityB, stepInput, ref collisionEventsWriter);
}
}
// 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;
}
// Generic solve method that dispatches to specific ones
public void Solve(
ref JacobianHeader jacHeader, ref MotionVelocity velocityA, ref MotionVelocity velocityB,
Solver.StepInput stepInput, ref NativeStream.Writer collisionEventsWriter, bool enableFrictionVelocitiesHeuristic,
Solver.MotionStabilizationInput motionStabilizationSolverInputA, Solver.MotionStabilizationInput motionStabilizationSolverInputB)
{
bool bothBodiesWithInfInertiaAndMass = velocityA.HasInfiniteInertiaAndMass && velocityB.HasInfiniteInertiaAndMass;
if (bothBodiesWithInfInertiaAndMass)
{
SolveInfMassPair(ref jacHeader, velocityA, velocityB, stepInput, ref collisionEventsWriter);
}
else
{
SolveContact(ref jacHeader, ref velocityA, ref velocityB, stepInput, ref collisionEventsWriter,
enableFrictionVelocitiesHeuristic, motionStabilizationSolverInputA, motionStabilizationSolverInputB);
}
}
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;
}
