Engine-Pong/Game/Physics2D/PhysicsEngine2D.cs

using System;
using System.Collections.Generic;

using Microsoft.Xna.Framework;

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

namespace Syntriax.Engine.Physics2D;

public class PhysicsEngine2D : IPhysicsEngine2D
{
    private List<IRigidBody2D> rigidBodies = new List<IRigidBody2D>(32);
    private List<ICollider2D> colliders = new List<ICollider2D>(64);

    private int _iterationCount = 1;


    public int IterationCount { get => _iterationCount; set => _iterationCount = value < 1 ? 1 : value; }

    public void AddRigidBody(IRigidBody2D rigidBody)
    {
        if (rigidBodies.Contains(rigidBody))
            return;

        rigidBodies.Add(rigidBody);

        foreach (var collider2D in rigidBody.BehaviourController.GetBehaviours<ICollider2D>())
            colliders.Add(collider2D);

        rigidBody.BehaviourController.OnBehaviourAdded += OnBehaviourAdded;
        rigidBody.BehaviourController.OnBehaviourRemoved += OnBehaviourRemoved;
    }

    public void RemoveRigidBody(IRigidBody2D rigidBody)
    {
        rigidBodies.Remove(rigidBody);
    }

    // List<IGameObject> gameObjects = new List<IGameObject>();
    private List<Line> lines = new List<Line>(32);

    public void Step(float deltaTime)
    {
        float intervalDeltaTime = deltaTime / IterationCount;

        for (int iterationIndex = 0; iterationIndex < IterationCount; iterationIndex++)
        {
            foreach (var rigidBody in rigidBodies)
                StepRigidBody(rigidBody, intervalDeltaTime);


            foreach (var collider in colliders)
                collider.Recalculate();

            // foreach (var gameObject in gameObjects)
            //     Game1.gameManager.RemoveGameObject(gameObject);
            // gameObjects.Clear();

            // foreach (var collider in colliders)
            //     foreach (var vertex in collider.Vertices)
            //     {
            //         GameObject gameObject = Game1.gameManager.InstantiateGameObject<GameObject>();
            //         gameObject.BehaviourController.AddBehaviour<DisplayableSpriteBehaviour>().Assign(Game1.spriteBox);
            //         gameObject.Transform.Position = vertex;
            //         gameObjects.Add(gameObject);
            //     }

            for (int i = colliders.Count - 1; i >= 0; i--)
                CheckCollisions(colliders[i], colliders, (c1, c2, v) =>
                {
                    NewMethod(c1, c2, v, intervalDeltaTime);
                    NewMethod(c2, c1, v, intervalDeltaTime);

                    // if (c2.RigidBody2D is IRigidBody2D)
                    // {
                    //     c2.RigidBody2D.Velocity = -c2.RigidBody2D.Velocity;
                    //     StepRigidBody(c2.RigidBody2D, intervalDeltaTime);
                    // }

                    c1.Recalculate();
                    c2.Recalculate();
                });
        }
    }

    private void NewMethod(ICollider2D c1, ICollider2D c2, Vector2 vertex, float intervalDeltaTime)
    {
        if (c1.RigidBody2D is not IRigidBody2D)
            return;

        Line vertexTrajectory = new Line(vertex, vertex - c1.RigidBody2D.Velocity);
        if (vertexTrajectory.LengthSquared <= 0.001f)
            return;

        c2.Vertices.ToLines(lines);

        Vector2 normal = Vector2.UnitY;
        float t = 0f;

        foreach (var line in lines)
        {
            if (!vertexTrajectory.Intersects(line, out Vector2? intersectionPoint) || intersectionPoint is not null)
                continue;

            t = vertexTrajectory.GetT(vertex);
            Vector2 lineDirection = line.Direction;
            normal = new(lineDirection.Y, lineDirection.X);
            break;
        }

        StepRigidBody(c1.RigidBody2D, -t);

        c1.RigidBody2D.Velocity = Vector2.Reflect(c1.RigidBody2D.Velocity, normal);
        StepRigidBody(c1.RigidBody2D, intervalDeltaTime - t);
    }

    private void CheckCollisions(ICollider2D collider2D, List<ICollider2D> collider2Ds, Action<ICollider2D, ICollider2D, Vector2> OnCollisionDetectedAction)
    {
        for (int i = 0; i < collider2Ds.Count; i++)
        {
            ICollider2D collider2DItem = collider2Ds[i];
            if (collider2DItem == collider2D)
                continue;

            for (int y = 0; y < collider2D.Vertices.Count; y++)
            {
                Vector2 vertex = collider2D.Vertices[y];
                if (collider2D.CheckCollision(vertex))
                {
                    OnCollisionDetectedAction?.Invoke(collider2D, collider2DItem, vertex);
                    y--; // Recheck current vertex
                }
            }
        }
    }

    private void StepRigidBody(IRigidBody2D rigidBody, float intervalDeltaTime)
    {
        Vector2 nextPosition = rigidBody.Transform.Position;
        nextPosition += rigidBody.Velocity * intervalDeltaTime;

        rigidBody.Transform.Position = nextPosition;
    }

    private void OnBehaviourAdded(IBehaviourController controller, IBehaviour behaviour)
    {
        if (behaviour is not ICollider2D collider2D)
            return;

        colliders.Add(collider2D);
    }

    private void OnBehaviourRemoved(IBehaviourController controller, IBehaviour behaviour)
    {
        if (behaviour is not ICollider2D collider2D)
            return;

        colliders.Remove(collider2D);
    }
}