Syntriax.Engine/Engine.Physics2D/CollisionDetector2D.cs

using System.Collections.Generic;

using Engine.Core;
using Engine.Physics2D;

namespace Engine.Physics2D;

public class CollisionDetector2D : ICollisionDetector2D
{
    public bool TryDetect<T1, T2>(T1 left, T2 right, out CollisionDetectionInformation collisionInformation)
        where T1 : ICollider2D
        where T2 : ICollider2D
    {
        collisionInformation = default;
        if (left is IShapeCollider2D shapeColliderLeft)
        {
            if (right is IShapeCollider2D shapeColliderRight)
                return DetectShapeShape(shapeColliderLeft, shapeColliderRight, out collisionInformation);
            else if (right is ICircleCollider2D circleColliderRight)
                return DetectShapeCircle(shapeColliderLeft, circleColliderRight, out collisionInformation);
        }
        else if (left is ICircleCollider2D circleColliderLeft)
        {
            if (right is IShapeCollider2D shapeColliderRight)
                return DetectCircleShape(circleColliderLeft, shapeColliderRight, out collisionInformation);
            else if (right is ICircleCollider2D circleColliderRight)
                return DetectCircleCircle(circleColliderLeft, circleColliderRight, out collisionInformation);
        }

        return false;
    }

    private static bool DetectCircleShape(ICircleCollider2D circleCollider, IShapeCollider2D shapeCollider, out CollisionDetectionInformation collisionInformation)
    {
        return DetectShapeCircle(shapeCollider, circleCollider, out collisionInformation);
    }

    private static bool DetectShapeShape(IShapeCollider2D left, IShapeCollider2D right, out CollisionDetectionInformation collisionInformation)
    {
        collisionInformation = default;
        return DetectShapeShapeOneWay(left, right, ref collisionInformation) && DetectShapeShapeOneWay(right, left, ref collisionInformation);
    }

    private static bool DetectShapeShapeOneWay(IShapeCollider2D left, IShapeCollider2D right, ref CollisionDetectionInformation collisionInformation)
    {
        IReadOnlyList<Vector2D> vertices = left.ShapeWorld.Vertices;
        int count = vertices.Count;

        for (int indexProjection = 0; indexProjection < count; indexProjection++)
        {
            Vector2D leftEdge = vertices[indexProjection].FromTo(vertices[(indexProjection + 1) % count]);
            Vector2D projectionVector = leftEdge.Perpendicular().Normalized;

            Projection1D leftProjection = left.ShapeWorld.ToProjection(projectionVector);
            Projection1D rightProjection = right.ShapeWorld.ToProjection(projectionVector);

            if (!leftProjection.Overlaps(rightProjection, out float depth))
                return false;

            if (collisionInformation.Detector is null || Math.Abs(collisionInformation.Penetration) > Math.Abs(depth))
            {
                Vector2D contactPoint = FindShapeToShapeContactPoint(left, right, projectionVector);
                collisionInformation = new(left, right, contactPoint, projectionVector, depth);
            }
        }

        return true;
    }

    private static Vector2D FindShapeToShapeContactPoint(IShapeCollider2D left, IShapeCollider2D right, Vector2D contactProjectionVector)
    {
        IReadOnlyList<Vector2D> leftVertices = left.ShapeWorld.Vertices;
        IReadOnlyList<Vector2D> rightVertices = right.ShapeWorld.Vertices;

        Line2D leftSupportLine = GetSupportLine(leftVertices, contactProjectionVector);
        Line2D rightSupportLine = GetSupportLine(rightVertices, -contactProjectionVector);

        if (leftSupportLine.Direction.Dot(rightSupportLine.Direction).Abs() > .99f)
            return (leftSupportLine.From + leftSupportLine.To + rightSupportLine.From + rightSupportLine.To) / 4f;

        return leftSupportLine.IntersectionPoint(rightSupportLine);
    }

    private static Line2D GetSupportLine(IReadOnlyList<Vector2D> vertices, Vector2D contactProjectionVector)
    {
        System.Span<Vector2D> points = stackalloc Vector2D[2];
        System.Span<float> distances = stackalloc float[2] { float.MaxValue, float.MaxValue };
        for (int i = 0; i < vertices.Count; i++)
        {
            Vector2D point = vertices[i];
            float distance = contactProjectionVector.Dot(point);

            if (distance < distances[0])
            {
                points[1] = points[0];
                distances[1] = distances[0];

                points[0] = point;
                distances[0] = distance;
            }
            else if (distance < distances[1])
            {
                points[1] = point;
                distances[1] = distance;
            }
        }

        return new(points[0], points[1]);
    }

    private static bool DetectShapeCircle(IShapeCollider2D shapeCollider, ICircleCollider2D circleCollider, out CollisionDetectionInformation collisionInformation)
    {
        collisionInformation = default;

        IReadOnlyList<Vector2D> vertices = shapeCollider.ShapeWorld.Vertices;
        int count = vertices.Count;

        for (int indexProjection = 0; indexProjection < count; indexProjection++)
        {
            Vector2D projectionVector = vertices[indexProjection].FromTo(vertices[(indexProjection + 1) % count]).Perpendicular().Normalized;

            Projection1D shapeProjection = shapeCollider.ShapeWorld.ToProjection(projectionVector);
            Projection1D circleProjection = circleCollider.CircleWorld.ProjectTo(projectionVector);

            if (!shapeProjection.Overlaps(circleProjection, out float depth))
                return false;

            Vector2D contactPoint = circleCollider.CircleWorld.Center + projectionVector * circleCollider.CircleWorld.Radius;

            if (collisionInformation.Detector is null || Math.Abs(collisionInformation.Penetration) > Math.Abs(depth))
                collisionInformation = new(shapeCollider, circleCollider, contactPoint, projectionVector, depth);
        }

        {
            Vector2D shapeToCircleProjectionVector = shapeCollider.Transform.Position.FromTo(circleCollider.CircleWorld.Center).Normalized;

            Projection1D shapeProjection = shapeCollider.ShapeWorld.ToProjection(shapeToCircleProjectionVector);
            Projection1D circleProjection = circleCollider.CircleWorld.ProjectTo(shapeToCircleProjectionVector);

            if (!shapeProjection.Overlaps(circleProjection, out float depth))
                return false;

            Vector2D contactPoint = circleCollider.CircleWorld.Center + shapeToCircleProjectionVector * circleCollider.CircleWorld.Radius;

            if (collisionInformation.Detector is null || Math.Abs(collisionInformation.Penetration) > Math.Abs(depth))
                collisionInformation = new(shapeCollider, circleCollider, contactPoint, shapeToCircleProjectionVector, depth);
        }

        return true;
    }

    private static bool DetectCircleCircle(ICircleCollider2D left, ICircleCollider2D right, out CollisionDetectionInformation collisionInformation)
    {
        collisionInformation = default;

        Vector2D leftToRightCenterProjectionVector = left.CircleWorld.Center.FromTo(right.CircleWorld.Center).Normalized;

        Projection1D leftProjection = left.CircleWorld.ProjectTo(leftToRightCenterProjectionVector);
        Projection1D rightProjection = right.CircleWorld.ProjectTo(leftToRightCenterProjectionVector);

        bool collision = leftProjection.Overlaps(rightProjection, out float depth);

        if (collision)
        {
            Vector2D contactPoint = left.CircleWorld.Center + leftToRightCenterProjectionVector * left.CircleWorld.Radius;
            collisionInformation = new(left, right, contactPoint, leftToRightCenterProjectionVector, depth);
        }

        return collision;
    }

    // private static bool DetectCircleCircle(ICircleCollider2D left, ICircleCollider2D right, out CollisionDetectionInformation collisionInformation)
    // {
    //     collisionInformation = default;

    //     Vector2D leftToRightCenter = left.CircleWorld.Center.FromTo(right.CircleWorld.Center);
    //     float distanceCircleCenter = leftToRightCenter.Magnitude;
    //     float radiusSum = left.CircleWorld.Radius + right.CircleWorld.Radius;

    //     float circleSurfaceDistance = distanceCircleCenter - radiusSum;

    //     bool collision = circleSurfaceDistance <= 0f;

    //     if (collision)
    //         collisionInformation = new(left, right, leftToRightCenter.Normalized, -circleSurfaceDistance);

    //     return collision;
    // }
}