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