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 Pong;
using Syntriax.Engine.Core;
using Syntriax.Engine.Core.Abstract;
using Syntriax.Engine.Graphics.TwoDimensional;
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 CollisionInformation collisionInformation)
    {
        collisionInformation = new CollisionInformation(Vector2.Zero, Vector2.Zero);

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

            OnCollision?.Invoke(this, otherCollider);

            Edge main = new() { A = otherCollider.Transform.Position, B = point };

            foreach (var edge in GetEdges(triangle))
            {
                if (!DoIntersect(main, edge))
                    continue;

                Vector2 contactPoint = ClosestPointOnEdge(point, edge);

                Vector2 normal = contactPoint - point;
                normal.Normalize();

                collisionInformation = new CollisionInformation(normal, contactPoint);

                GameObject gameObject = Game1.gameManager.InstantiateGameObject<GameObject>();
                gameObject.BehaviourController.AddBehaviour<DisplayableSpriteBehaviour>().Assign(Game1.spriteBox);
                gameObject.Transform.Position = point;
                gameObject.Transform.Scale = new Vector2(1f, .01f) * 100f;
                gameObject.Transform.Rotation = (float)Math.Atan2(normal.Y, normal.X);
                break;
            }
            return true;
        }

        return false;
    }

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

        _vertices.Clear();
        foreach (var vertex in verticesOriginal)
        {
            Vector2 scaledPosition = new Vector2(vertex.X * Transform.Scale.X * OffsetScale.X, vertex.Y * Transform.Scale.Y * OffsetScale.Y);
            _vertices.Add(scaledPosition + 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 Vector2 ClosestPointOnEdge(Vector2 point, Edge edge)
    {
        // Convert edge points to vectors
        var edgeVector = new Vector2(edge.B.X - edge.A.X, edge.B.Y - edge.A.Y);
        var pointVector = new Vector2(point.X - edge.A.X, point.Y - edge.A.Y);

        // Calculate the projection of pointVector onto edgeVector
        float t = (pointVector.X * edgeVector.X + pointVector.Y * edgeVector.Y) / (edgeVector.X * edgeVector.X + edgeVector.Y * edgeVector.Y);

        // Clamp t to the range [0, 1] to ensure the closest point is on the edge
        t = Math.Max(0, Math.Min(1, t));

        // Calculate the closest point on the edge
        float closestX = edge.A.X + t * edgeVector.X;
        float closestY = edge.A.Y + t * edgeVector.Y;

        return new Vector2(closestX, closestY);
    }

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

    // Given three collinear points p, q, r, the function checks if 
    // point q lies on line segment 'pr' 
    private bool OnSegment(Vector2 p, Vector2 q, Vector2 r)
    {
        if (q.X <= Math.Max(p.X, r.X) && q.X >= Math.Min(p.X, r.X) &&
            q.Y <= Math.Max(p.Y, r.Y) && q.Y >= Math.Min(p.Y, r.Y))
            return true;

        return false;
    }

    // To find orientation of ordered triplet (p, q, r). 
    // The function returns following values 
    // 0 --> p, q and r are collinear 
    // 1 --> Clockwise 
    // 2 --> Counterclockwise 
    private int Orientation(Vector2 p, Vector2 q, Vector2 r)
    {
        // See https://www.geeksforgeeks.org/orientation-3-ordered-points/ 
        // for details of below formula. 
        float val = (q.Y - p.Y) * (r.X - q.X) -
                (q.X - p.X) * (r.Y - q.Y);

        if (val == 0) return 0; // collinear 

        return (val > 0) ? 1 : 2; // clock or counterclock wise 
    }

    // The main function that returns true if line segment 'edge1.Aedge1.B' 
    // and 'edge2.Aedge2.B' intersect. 
    private bool DoIntersect(Edge edge1, Edge edge2)
    {
        // Find the four orientations needed for general and 
        // special cases 
        int o1 = Orientation(edge1.A, edge1.B, edge2.A);
        int o2 = Orientation(edge1.A, edge1.B, edge2.B);
        int o3 = Orientation(edge2.A, edge2.B, edge1.A);
        int o4 = Orientation(edge2.A, edge2.B, edge1.B);

        // General case 
        if (o1 != o2 && o3 != o4)
            return true;

        // Special Cases 
        // edge1.A, edge1.B and edge2.A are collinear and edge2.A lies on segment edge1.Aedge1.B 
        if (o1 == 0 && OnSegment(edge1.A, edge2.A, edge1.B)) return true;

        // edge1.A, edge1.B and edge2.B are collinear and edge2.B lies on segment edge1.Aedge1.B 
        if (o2 == 0 && OnSegment(edge1.A, edge2.B, edge1.B)) return true;

        // edge2.A, edge2.B and edge1.A are collinear and edge1.A lies on segment edge2.Aedge2.B 
        if (o3 == 0 && OnSegment(edge2.A, edge1.A, edge2.B)) return true;

        // edge2.A, edge2.B and edge1.B are collinear and edge1.B lies on segment edge2.Aedge2.B 
        if (o4 == 0 && OnSegment(edge2.A, edge1.B, edge2.B)) return true;

        return false; // Doesn't fall in any of the above cases 
    }
}