public static Vector2 CalculatePenetrationVector(CircleF circle, CircleF otherCircle, Vector2 velocity,
Vector2 otherVelocity)
{
// Reset the circles to their old positions. Structs, so no external mutation.
circle.Center -= velocity;
otherCircle.Center -= otherVelocity;
var oldDistance = circle.Center - otherCircle.Center;
var sumRadius = circle.Radius + otherCircle.Radius;
if (oldDistance.Dot(oldDistance) <= sumRadius * sumRadius)
{
// Already overlapping
return(circle.CalculatePenetrationVector(otherCircle));
}
// make other circle static, expand its radius and do a ray cast
var relativeVelocity = velocity - otherVelocity;
// Find quadratic formula coefficients:
// u^2 coefficient
var a = relativeVelocity.Dot(relativeVelocity);
// u coefficient
var b = 2 * relativeVelocity.Dot(oldDistance);
// constant
var c = oldDistance.Dot(oldDistance) - sumRadius * sumRadius;
if (SolveQuadraticFormula(a, b, c, out var u0, out var u1) && u0.IsBetween(0, 1) && u1.IsBetween(0, 1))
{
var maxPenetrationAt = (u0 + u1) / 2;
circle.Position += velocity * maxPenetrationAt;
otherCircle.Position += otherVelocity * maxPenetrationAt;
return(circle.CalculatePenetrationVector(otherCircle));
}
return(Vector2.Zero);
}
