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