private void ResolveVelocity(float deltaT)
{
float seperatingVelocity = calculateSeperatingVelocity();
if (seperatingVelocity > 0)
{
return; //they are already seperating
}
float totalInverseMass = a.m_InverseMass + b.m_InverseMass;
if (totalInverseMass <= 0)
{
return; //dont do anything, they cannot move
}
float newSeperatingVelocity = -seperatingVelocity * restitution;
Vector3 accCausedVelocity = b.m_Acceleration - a.m_Acceleration;
float accCausedSepVelocity = Vector3.Dot(accCausedVelocity, contactNormal) * deltaT;
if (accCausedSepVelocity > 0)
{
newSeperatingVelocity += restitution * accCausedSepVelocity;
if (newSeperatingVelocity < 0)
{
newSeperatingVelocity = 0.0f;
}
}
float deltaVelocity = newSeperatingVelocity - seperatingVelocity;
float inpulse = deltaVelocity / totalInverseMass;
Vector3 inpulsePerMass = contactNormal * inpulse * newSeperatingVelocity;
a.AddForce(inpulsePerMass);
b.AddForce(-inpulsePerMass);
}