Engine-Pong/Game/Physics2D/Collider2DBehaviour.cs

using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Diagnostics.CodeAnalysis;
using Microsoft.Xna.Framework;

using Syntriax.Engine.Core;
using Syntriax.Engine.Core.Abstract;
using Syntriax.Engine.Physics2D.Abstract;

namespace Syntriax.Engine.Physics2D;

public class Collider2DBehaviour(IList<Vector2> vertices) : BehaviourOverride, ICollider2D
{
    private List<Triangle> triangles = new List<Triangle>(32);
    private readonly List<Vector2> _vertices = new List<Vector2>(32);
    private IRigidBody2D? _rigidBody2D = null;

    public Action<IAssignableTransform>? OnTransformAssigned { get => GameObject.OnTransformAssigned; set => GameObject.OnTransformAssigned = value; }
    public Action<ICollider2D, ICollider2D>? OnCollision { get; set; } = null;


    private IList<Vector2> verticesOriginal { get; } = vertices;

    public Vector2 OffsetPosition { get; set; } = Vector2.Zero;
    public Vector2 OffsetScale { get; set; } = Vector2.One;
    public float OffsetRotation { get; set; } = 0f;

    ITransform IAssignableTransform.Transform => Transform;
    public IReadOnlyList<Vector2> Vertices => _vertices;

    public IRigidBody2D? RigidBody2D
    {
        get
        {
            if (_rigidBody2D is null)
                BehaviourController.TryGetBehaviour(out _rigidBody2D);

            return _rigidBody2D;
        }
    }

    public bool Assign(ITransform transform) => GameObject.Assign(transform);

    public bool CheckCollision(Vector2 point, ICollider2D otherCollider, out Vector2 normal)
    {
        normal = Vector2.Zero;

        foreach (var triangle in triangles)
        {
            if (!isInside(point, triangle))
                continue;

            OnCollision?.Invoke(this, otherCollider);

            normal = Transform.Position - otherCollider.Transform.Position;
            return true;
        }

        return false;
    }

    public void RecalculateVertices()
    {
        triangles.Clear();

        _vertices.Clear();
        foreach (var vertex in verticesOriginal)
            _vertices.Add(vertex + Transform.Position);

        Triangle superTriangle = GetSuperTriangle(_vertices);
        triangles.Add(superTriangle);

        List<Triangle> badTriangles = new(32);
        List<Edge> polygon = new(32);

        foreach (var vertex in _vertices)
        {
            badTriangles.Clear();
            polygon.Clear();

            foreach (var triangle in triangles)
            {
                Circle circle = GetCircumCircle(triangle);
                if (Vector2.DistanceSquared(circle.Center, vertex) <= circle.Radius * circle.Radius)
                    badTriangles.Add(triangle);
            }

            foreach (var triangle in badTriangles)
                foreach (var edge in GetEdges(triangle))
                {
                    if (DoesEdgeExistInTriangles(edge, badTriangles))
                        polygon.Add(edge);
                }

            foreach (var triangle in badTriangles)
                triangles.Remove(triangle);

            foreach (var edge in polygon)
            {
                triangles.Add(new()
                {
                    A = edge.A,
                    B = edge.B,
                    C = vertex
                });
            }
        }

        for (int i = triangles.Count - 1; i >= 0; i--)
        {
            Triangle triangle = triangles[i];
            if (
                triangle.A == superTriangle.A || triangle.A == superTriangle.B || triangle.A == superTriangle.C ||
                triangle.B == superTriangle.A || triangle.B == superTriangle.B || triangle.B == superTriangle.C ||
                triangle.C == superTriangle.A || triangle.C == superTriangle.B || triangle.C == superTriangle.C
            )
                triangles.RemoveAt(i);
        }

        // for (int i = 0; i < triangles.Count; i++)
        // {
        //     Triangle triangle = triangles[i];

        //     triangle.A += Transform.Position;
        //     triangle.B += Transform.Position;
        //     triangle.C += Transform.Position;

        //     triangles[i] = triangle;
        // }
    }

    private bool DoesEdgeExistInTriangles(Edge edge, List<Triangle> triangles)
    {
        foreach (var triangle in triangles)
            foreach (var edgeOther in GetEdges(triangle))
                if (edge.A == edgeOther.A && edge.B == edgeOther.B)
                    return true;
                else if (edge.A == edgeOther.B && edge.B == edgeOther.A)
                    return true;
        return false;
    }

    private List<Edge> GetEdges(Triangle triangle)
        => [
            new() { A = triangle.A, B = triangle.B },
            new() { A = triangle.B, B = triangle.C },
            new() { A = triangle.C, B = triangle.A }
        ];

    private Triangle GetSuperTriangle(IReadOnlyList<Vector2> vertices)
    {
        float minX = float.MaxValue, minY = float.MaxValue;
        float maxX = float.MinValue, maxY = float.MinValue;

        foreach (Vector2 point in vertices)
        {
            minX = Math.Min(minX, point.X);
            minY = Math.Min(minY, point.Y);
            maxX = Math.Max(maxX, point.X);
            maxY = Math.Max(maxY, point.Y);
        }

        float dx = maxX - minX;
        float dy = maxY - minY;
        float deltaMax = Math.Max(dx, dy);
        float midX = (minX + maxX) / 2;
        float midY = (minY + maxY) / 2;

        Vector2 p1 = new Vector2(midX - 20 * deltaMax, midY - deltaMax);
        Vector2 p2 = new Vector2(midX, midY + 20 * deltaMax);
        Vector2 p3 = new Vector2(midX + 20 * deltaMax, midY - deltaMax);

        return new Triangle() { A = p1, B = p2, C = p3 };
    }

    private struct Triangle { public Vector2 A, B, C; }

    private double GetArea(Triangle triangle)
    {
        return Math.Abs((triangle.A.X * (triangle.B.Y - triangle.C.Y) +
                         triangle.B.X * (triangle.C.Y - triangle.A.Y) +
                         triangle.C.X * (triangle.A.Y - triangle.B.Y)) / 2.0);
    }

    /* A function to check whether point P(x, y) lies
    inside the triangle formed by A(x1, y1),
    B(x2, y2) and C(x3, y3) */
    private bool isInside(Vector2 point, Triangle triangle)
    {
        double A = GetArea(triangle);
        /* Calculate area of triangle ABC */
        // double A = area(x1, y1, x2, y2, x3, y3);

        double A1 = GetArea(new() { A = point, B = triangle.B, C = triangle.C });
        /* Calculate area of triangle PBC */
        // double A1 = area(x, y, x2, y2, x3, y3);

        /* Calculate area of triangle PAC */
        double A2 = GetArea(new() { A = triangle.A, B = point, C = triangle.C });
        // double A2 = area(x1, y1, x, y, x3, y3);

        /* Calculate area of triangle PAB */
        double A3 = GetArea(new() { A = triangle.A, B = triangle.B, C = point });
        // double A3 = area(x1, y1, x2, y2, x, y);

        /* Check if sum of A1, A2 and A3 is same as A */
        return A == A1 + A2 + A3;
    }

    private struct Edge
    {
        public Vector2 A;
        public Vector2 B;
    }

    private struct Circle
    {
        public double Radius;
        public Vector2 Center;
    }

    private Circle GetCircumCircle(Triangle triangle)
    {
        Circle result = new();

        Vector2 midAB = (triangle.A + triangle.B) / 2;
        Vector2 midBC = (triangle.B + triangle.C) / 2;

        float slopeAB = (triangle.B.Y - triangle.A.Y) / (triangle.B.X - triangle.A.X);
        float slopeBC = (triangle.C.Y - triangle.B.Y) / (triangle.C.X - triangle.B.X);

        // Check if the slopes are not parallel
        if (Math.Abs(slopeAB - slopeBC) > float.Epsilon)
        {
            float x = (slopeAB * slopeBC * (triangle.A.Y - triangle.C.Y) + slopeBC * (triangle.A.X + triangle.B.X) - slopeAB * (triangle.B.X + triangle.C.X)) / (2 * (slopeBC - slopeAB));
            float y = -(x - (triangle.A.X + triangle.B.X) / 2) / slopeAB + (triangle.A.Y + triangle.B.Y) / 2;

            result.Center = new Vector2(x, y);
            result.Radius = Vector2.Distance(result.Center, triangle.A);
        }
        else
        {
            // If slopes are parallel, use the midpoints of the sides as the circumcenter
            result.Center = (midAB + midBC) / 2;
            result.Radius = Vector2.Distance(result.Center, triangle.A);
        }

        return result;
    }
}