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);
}
