private static Vector2 PenetrationVector(CircleF circ1, CircleF circ2)
{
if (!circ1.Intersects(circ2))
{
return(Vector2.Zero);
}
var displacement = Point2.Displacement(circ1.Center, circ2.Center);
Vector2 desiredDisplacement;
if (displacement != Vector2.Zero)
{
desiredDisplacement = displacement.NormalizedCopy() * (circ1.Radius + circ2.Radius);
}
else
{
desiredDisplacement = -Vector2.UnitY * (circ1.Radius + circ2.Radius);
}
var penetration = displacement - desiredDisplacement;
return(penetration);
}
private static Vector2 PenetrationVector(CircleF circ, RectangleF rect)
{
var collisionPoint = rect.ClosestPointTo(circ.Center);
var cToCollPoint = collisionPoint - circ.Center;
if (rect.Contains(circ.Center) || cToCollPoint.Equals(Vector2.Zero))
{
var displacement = Point2.Displacement(circ.Center, rect.Center);
Vector2 desiredDisplacement;
if (displacement != Vector2.Zero)
{
// Calculate penetration as only in X or Y direction.
// Whichever is lower.
var dispx = new Vector2(displacement.X, 0);
var dispy = new Vector2(0, displacement.Y);
dispx.Normalize();
dispy.Normalize();
dispx *= (circ.Radius + rect.Width / 2);
dispy *= (circ.Radius + rect.Height / 2);
if (dispx.LengthSquared() < dispy.LengthSquared())
{
desiredDisplacement = dispx;
displacement.Y = 0;
}
else
{
desiredDisplacement = dispy;
displacement.X = 0;
}
}
else
{
desiredDisplacement = -Vector2.UnitY * (circ.Radius + rect.Height / 2);
}
var penetration = displacement - desiredDisplacement;
return(penetration);
}
else
{
var penetration = circ.Radius * cToCollPoint.NormalizedCopy() - cToCollPoint;
return(penetration);
}
}
public void Displacement(Point2 point1, Point2 point2, Vector2 expectedVector)
{
Assert.AreEqual(expectedVector, point2 - point1);
Assert.AreEqual(expectedVector, Point2.Displacement(point2, point1));
}
