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 = Vector3.Dot(c.m_contactNormal, body1.InternalGetPushVelocity()) + Vector3.Dot(c.m_relpos1CrossNormal, body1.InternalGetTurnVelocity());
float deltaVel2Dotn = -Vector3.Dot(c.m_contactNormal, body2.InternalGetPushVelocity()) + Vector3.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);
}
}
protected void ResolveSingleConstraintRowLowerLimit(RigidBody body1, RigidBody body2, ref SolverConstraint c)
{
//check magniture of applied impulse from SolverConstraint
float deltaImpulse = c.m_rhs - c.m_appliedImpulse * c.m_cfm;
float deltaVel1Dotn = Vector3.Dot(c.m_contactNormal, body1.InternalGetDeltaLinearVelocity()) + Vector3.Dot(c.m_relpos1CrossNormal, body1.InternalGetDeltaAngularVelocity());
float deltaVel2Dotn = -Vector3.Dot(c.m_contactNormal, body2.InternalGetDeltaLinearVelocity()) + Vector3.Dot(c.m_relpos2CrossNormal, body2.InternalGetDeltaAngularVelocity());
deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
float sum = c.m_appliedImpulse + deltaImpulse;
if (sum < c.m_lowerLimit)
{
deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
c.m_appliedImpulse = c.m_lowerLimit;
}
else
{
c.m_appliedImpulse = sum;
}
Vector3 temp = c.m_contactNormal * body1.InternalGetInvMass();
body1.InternalApplyImpulse(ref temp, ref c.m_angularComponentA, deltaImpulse);
temp = -c.m_contactNormal * body2.InternalGetInvMass();
body2.InternalApplyImpulse(ref temp, ref c.m_angularComponentB, deltaImpulse);
}