/// <summary>
/// Resolve the collision by calculating the resulting velocity, using the
/// given normal and penetration, stored in the intersection.
/// </summary>
/// <param name="collision">Intersection instance containing all the collision data.</param>
public void ApplyImpulse()
{
Vector3F dv = bodyB.Velocity - bodyA.Velocity;
Fix32 vn = Vector3F.Dot(dv, normal);
Fix32 imp = ((-((Fix32)1 + restitution) * vn + bias)) / totalInvMass;
imp = Fix32.Max(imp, (Fix32)0);
Vector3F impulse = normal * imp;
bodyA.Velocity -= impulse * bodyA.InvMass;
bodyB.Velocity += impulse * bodyB.InvMass;
}