private static DispatchResult DispatchCircleShape(Body Body, Info Info, Vector2 LineEndPoint)
{
CircleShape shape = (CircleShape)Body.Shape;
Vector2 lineDiff = LineEndPoint - Info.Origin;
Number lineDiffSqr = lineDiff.SqrMagnitude;
Vector2 centerDiff = Body.Position - Info.Origin;
Number centerDiffSqr = centerDiff.SqrMagnitude;
Number radiusSqr = shape.Radius * shape.Radius;
if ((Info.Origin - Body.Position).SqrMagnitude <= radiusSqr)
{
return new DispatchResult()
{
Hit = false
}
}
;
if ((Info.Origin + (Info.Direction * Math.Min(Info.Distance, centerDiff.Magnitude)) - Body.Position).SqrMagnitude > radiusSqr)
{
return new DispatchResult()
{
Hit = false
}
}
;
centerDiff *= -1;
Number B = 2 * ((lineDiff.X * centerDiff.X) + (lineDiff.Y * centerDiff.Y));
Number C = centerDiffSqr - radiusSqr;
Number det = B * B - 4 * lineDiffSqr * C;
if ((lineDiffSqr <= Math.Epsilon) || (det < 0))
{
return new DispatchResult()
{
Hit = false
}
}
;
Number distance = 0;
if (det == 0) // One solution
{
distance = -B / (2 * lineDiffSqr);
}
else // Two solutions
{
// Farest point
//distance = (-B + Math.Sqrt(det)) / (2 * A);
// Nearest point
distance = (float)((-B - Math.Sqrt(det)) / (2 * lineDiffSqr));
}
return(new DispatchResult()
{
Hit = true, Distance = distance, Point = Info.Origin + (lineDiff * distance)
});
}
private static DispatchResult DispatchShape(Body Body, Info Info, Vector2 LineEndPoint)
{
return(Dispatchers[(int)Body.Shape.GetType()](Body, Info, LineEndPoint));
}