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