public void ApplyImpulse()
{
// Early out and positional correct if both objects have infinite mass
if (A.invMass + B.invMass == 0)
{
InfiniteMassCorrection();
return;
}
for (int i = 0; i < contactCount; ++i)
{
// Calculate radii from COM to contact
FixVec2 ra = contacts[i] - A.Position;
FixVec2 rb = contacts[i] - B.Position;
//Relative velocity
FixVec2 rv = B.info.velocity + FixVec2.Cross(B.info.angularVelocity, rb) - A.info.velocity - FixVec2.Cross(A.info.angularVelocity, ra);
//Relative velocity along the normal
Fix contactVel = rv.Dot(normal);
if (contactVel > 0)
{
return;
}
Fix raCrossN = FixVec2.Cross(ra, normal);
Fix rbCrossN = FixVec2.Cross(rb, normal);
Fix invMassSum = A.invMass + B.invMass
+ (raCrossN * raCrossN) * A.invInertia
+ (rbCrossN * rbCrossN) * B.invInertia;
// Calculate impulse scalar
Fix j = -(Fix.one + e) * contactVel;
j /= invMassSum;
j /= contactCount;
// Apply impulse
FixVec2 impulse = normal * j;
A.ApplyImpulse(-impulse, ra);
B.ApplyImpulse(impulse, rb);
// Friction Impulse
rv = B.info.velocity + FixVec2.Cross(B.info.angularVelocity, rb)
- A.info.velocity - FixVec2.Cross(A.info.angularVelocity, ra);
FixVec2 t = rv - (normal * FixVec2.Dot(rv, normal));
t = t.Normalized();
// j tangent magnitude
Fix jt = -FixVec2.Dot(rv, t);
jt /= invMassSum;
jt /= contactCount;
//Don't apply tiny friction impulses
if (FixMath.Abs(jt) <= Fix.zero)
{
return;
}
// Coulumb's law
FixVec2 tangentImpulse;
if (FixMath.Abs(jt) < j * sf)
{
tangentImpulse = t * jt;
}
else
{
tangentImpulse = t * -j * df;
}
// Apply friction impulse
A.ApplyImpulse(-tangentImpulse, ra);
B.ApplyImpulse(tangentImpulse, rb);
}
}
