protected void ResolveSplitPenetrationImpulseCacheFriendly(
RigidBody body1,
RigidBody body2,
ref SolverConstraint c)
{
if (c.m_rhsPenetration != 0f)
{
gNumSplitImpulseRecoveries++;
float deltaImpulse = c.m_rhsPenetration - (c.m_appliedPushImpulse * c.m_cfm);
float deltaVel1Dotn = IndexedVector3.Dot(c.m_contactNormal, body1.InternalGetPushVelocity()) + IndexedVector3.Dot(c.m_relpos1CrossNormal, body1.InternalGetTurnVelocity());
float deltaVel2Dotn = -IndexedVector3.Dot(c.m_contactNormal, body2.InternalGetPushVelocity()) + IndexedVector3.Dot(c.m_relpos2CrossNormal, body2.InternalGetTurnVelocity());
deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
float sum = c.m_appliedPushImpulse + deltaImpulse;
if (sum < c.m_lowerLimit)
{
deltaImpulse = c.m_lowerLimit - c.m_appliedPushImpulse;
c.m_appliedPushImpulse = c.m_lowerLimit;
}
else
{
c.m_appliedPushImpulse = sum;
}
body1.InternalApplyPushImpulse(c.m_contactNormal * body1.InternalGetInvMass(), c.m_angularComponentA, deltaImpulse);
body2.InternalApplyPushImpulse(-c.m_contactNormal * body2.InternalGetInvMass(), c.m_angularComponentB, deltaImpulse);
}
}