static void HandleCirclesOutside(BodyCircle a, BodyCircle b)
{
float tr = a.r + b.r;
V2 dp = b.center - a.center;
float d2 = dp.Len2();
bool collided = d2 <= tr * tr;
if (collided)
{
V2 dv = b.velLin - a.velLin;
float totInvMass = a.invMass + b.invMass;
float invMassRatioA = a.invMass / totInvMass;
float invMassRatioB = b.invMass / totInvMass;
// eliminate overlapping (separation)
float dist = Math.Sqrt(d2);
float totOverlap = tr - dist;
V2 normal = dp / dist;
b.center += normal * (totOverlap * invMassRatioB);
a.center -= normal * (totOverlap * invMassRatioA);
bool movingTowardsEachOther = dp * dv < 0;
if (movingTowardsEachOther) // bounce
{
const float BOUNCINESS = 0.99f; // [0, 1]
float desiredDv = -BOUNCINESS * (dv * normal);
float desiredDvChange = desiredDv - (dv * normal);
float dva = 1 * a.invMass;
float dvb = -1 * b.invMass;
float dvChange = dvb - dva;
float impLen = desiredDvChange / dvChange;
a.ApplyImpulse(impLen * normal);
b.ApplyImpulse(-impLen * normal);
}
}
}
static void HandleCirclesInside(BodyCircle inner, BodyCircle outer)
{
float dr = outer.r - inner.r;
V2 dp = inner.center - outer.center;
float d2 = dp.Len2();
bool areOverlapping = d2 >= dr * dr;
if (areOverlapping)
{
float totInvMass = outer.invMass + inner.invMass;
float invMassRatioA = outer.invMass / totInvMass;
float invMassRatioB = inner.invMass / totInvMass;
// eliminate overlapping (separation)
float dist = Math.Sqrt(d2);
float totOverlap = dist - dr;
V2 normal = dp / -dist;
inner.center += normal * (totOverlap * invMassRatioB);
outer.center -= normal * (totOverlap * invMassRatioA);
V2 dv = inner.velLin - outer.velLin;
bool movingTowardsEachOther = dp * dv > 0;
if (movingTowardsEachOther) // bounce
{
const float BOUNCINESS = 0.99f; // [0, 1]
float desiredDv = -BOUNCINESS * (dv * normal);
float desiredDvChange = desiredDv - (dv * normal);
float dva = 1 * outer.invMass;
float dvb = -1 * inner.invMass;
float dvChange = dvb - dva;
float impLen = desiredDvChange / dvChange;
outer.ApplyImpulse(impLen * normal);
inner.ApplyImpulse(-impLen * normal);
}
}
}