Syntriax.Engine/Engine.Physics2D/RaycastResolver2D.cs

using System.Collections.Generic;

using Syntriax.Engine.Core;

namespace Syntriax.Engine.Physics2D;

public class RaycastResolver2D : IRaycastResolver2D
{
    private readonly ListPool<Line2D> lineCacheQueue = new(() => new(4));

    RaycastResult? IRaycastResolver2D.RaycastAgainst<T>(T shape, Ray2D ray, float length)
    {
        if (shape is IShapeCollider2D shapeCollider)
            return RaycastAgainstShape(shapeCollider, ray, length);
        else if (shape is ICircleCollider2D circleCollider)
            return RaycastAgainstCircle(circleCollider, ray, length);

        throw new System.NotSupportedException($"{shape.GetType().FullName} is not supported by {GetType().FullName}. Please implement a {nameof(IRaycastResolver2D)} and use it as the raycast resolver.");
    }

    public RaycastResult? RaycastAgainstShape(IShapeCollider2D shapeCollider, Ray2D ray, float length)
    {
        List<Line2D> line2Ds = lineCacheQueue.Get();

        RaycastResult? raycastResult = null;
        float closestRaycastResultSquared = float.MaxValue;

        shapeCollider.ShapeWorld.ToLines(line2Ds);

        Line2D rayLine = ray.ToLine(length);

        foreach (Line2D line in line2Ds)
        {
            if (line.Intersects(rayLine))
            {
                Vector2D hitPosition = line.IntersectionPoint(rayLine);

                float hitDistanceSquared = ray.Origin.FromTo(hitPosition).MagnitudeSquared;

                if (closestRaycastResultSquared < hitDistanceSquared)
                    continue;

                closestRaycastResultSquared = hitDistanceSquared;

                Vector2D lineDirection = line.Direction;

                Vector2D normal1 = lineDirection.Perpendicular();
                Vector2D normal2 = -lineDirection.Perpendicular();

                float normalDot1 = rayLine.Direction.Dot(normal1);
                float normalDot2 = rayLine.Direction.Dot(normal2);

                Vector2D hitNormal = normalDot1 < normalDot2 ? normal1 : normal2;

                if (shapeCollider.ShapeWorld.Overlaps(ray.Origin))
                    hitNormal = hitNormal.Reversed;

                raycastResult = new(ray, shapeCollider, hitPosition, hitNormal);
            }
        }

        lineCacheQueue.Return(line2Ds);

        return raycastResult;
    }

    public RaycastResult? RaycastAgainstCircle(ICircleCollider2D circleCollider, Ray2D ray, float length)
    {
        Circle circle = circleCollider.CircleWorld;

        if (circle.Overlaps(ray.Origin, out _, out float depth))
        {
            Vector2D insideNormal = circle.Center.FromTo(ray.Origin).Normalized;
            Vector2D insidePosition = circle.Center + insideNormal * circle.Radius;

            return new(ray, circleCollider, insidePosition, insideNormal);
        }

        Vector2D originToCircle = ray.Origin.FromTo(circle.Center);
        float distanceToClosest = ray.Direction.Dot(originToCircle);

        Vector2D closestPoint = ray.Origin + ray.Direction * distanceToClosest;
        Vector2D closestPointToCircle = closestPoint.FromTo(circle.Center);

        float closestToCircleDistanceSquared = closestPointToCircle.MagnitudeSquared;

        if (closestToCircleDistanceSquared > circle.RadiusSquared)
            return null;

        float distanceToHit = distanceToClosest - (circle.Radius - closestToCircleDistanceSquared.Sqrt());

        if (distanceToHit > length || distanceToHit < 0f)
            return null;

        Vector2D hitPosition = ray.Evaluate(distanceToHit);
        Vector2D hitNormal = circle.Center.FromTo(hitPosition).Normalized;

        return new(ray, circleCollider, hitPosition, hitNormal);
    }
}