Engine-Pong/Game/Physics2D/PhysicsEngine2D.cs

using System;
using System.Collections.Generic;
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 PhysicsEngine2D : IPhysicsEngine2D
{
    private IList<IRigidBody2D> rigidBodies = new List<IRigidBody2D>(32);
    private IList<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);
    }

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

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


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

            for (int colliderIX = 0; colliderIX < colliders.Count; colliderIX++)
            {
                ICollider2D colliderX = colliders[colliderIX];
                for (int colliderIY = colliderIX + 1; colliderIY < colliders.Count; colliderIY++)
                    for (int verticesIndex = 0; verticesIndex < colliderX.Vertices.Count; verticesIndex++)
                    {
                        ICollider2D colliderY = colliders[colliderIY];
                        if (!colliderY.CheckCollision(colliderX.Vertices[verticesIndex], colliderX, out var collisionInformation))
                            continue;

                        if (colliderX.BehaviourController.TryGetBehaviour(out IRigidBody2D? rigidX))
                        {
                            Vector2 xVertex = colliderX.Vertices[verticesIndex];
                            Vector2 edgeDirection = new Vector2(collisionInformation.Normal.Y, collisionInformation.Normal.X);
                            Vector2 p1 = xVertex - rigidX.Velocity * intervalDeltaTime;
                            float t = IntersectionParameterT(collisionInformation.ContactPosition - edgeDirection * 100f, collisionInformation.ContactPosition + edgeDirection * 100f, xVertex, p1);
                            Vector2 vertexNewPosition = Vector2.Lerp(xVertex, p1, t);
                            rigidX.Velocity = Vector2.Reflect(rigidX.Velocity, collisionInformation.Normal);
                            rigidX.Transform.Position -= xVertex - vertexNewPosition;

                            // {
                            //     GameObject gameObject = Game1.gameManager.InstantiateGameObject<GameObject>();
                            //     DisplayableSpriteBehaviour displayableSpriteBehaviour = gameObject.BehaviourController.AddBehaviour<DisplayableSpriteBehaviour>();
                            //     displayableSpriteBehaviour.Assign(Game1.spriteBox);
                            //     gameObject.Transform.Position = collisionInformation.ContactPosition;
                            //     gameObject.Transform.Scale = new Vector2(1f, .01f) * 100f;
                            //     gameObject.Transform.Rotation = (float)Math.Atan2(collisionInformation.Normal.Y, collisionInformation.Normal.X);
                            // }
                            // {
                            //     GameObject gameObject = Game1.gameManager.InstantiateGameObject<GameObject>();
                            //     DisplayableSpriteBehaviour displayableSpriteBehaviour = gameObject.BehaviourController.AddBehaviour<DisplayableSpriteBehaviour>();
                            //     displayableSpriteBehaviour.Color = Color.Aqua;
                            //     displayableSpriteBehaviour.Assign(Game1.spriteBox);
                            //     gameObject.Transform.Position = collisionInformation.ContactPosition;
                            //     gameObject.Transform.Scale = new Vector2(1f, .01f) * 100f;
                            //     gameObject.Transform.Rotation = (float)Math.Atan2(rigidX.Velocity.Y, rigidX.Velocity.X);
                            // }
                            // {
                            //     GameObject gameObject = Game1.gameManager.InstantiateGameObject<GameObject>();
                            //     DisplayableSpriteBehaviour displayableSpriteBehaviour = gameObject.BehaviourController.AddBehaviour<DisplayableSpriteBehaviour>();
                            //     displayableSpriteBehaviour.Color = Color.Pink;
                            //     displayableSpriteBehaviour.Assign(Game1.spriteBox);
                            //     gameObject.Transform.Position = collisionInformation.ContactPosition;
                            //     gameObject.Transform.Scale = new Vector2(1f, .01f) * 100f;
                            //     gameObject.Transform.Rotation = (float)Math.Atan2(edgeDirection.Y, edgeDirection.X);
                            // }
                            // {
                            //     GameObject gameObject = Game1.gameManager.InstantiateGameObject<GameObject>();
                            //     DisplayableSpriteBehaviour displayableSpriteBehaviour = gameObject.BehaviourController.AddBehaviour<DisplayableSpriteBehaviour>();
                            //     displayableSpriteBehaviour.Color = Color.DarkGoldenrod;
                            //     displayableSpriteBehaviour.Assign(Game1.spriteBox);
                            //     gameObject.Transform.Position = collisionInformation.ContactPosition;
                            //     gameObject.Transform.Scale = new Vector2(1f, .01f) * 100f;
                            //     gameObject.Transform.Rotation = (float)Math.Atan2((xVertex - p1).Y, (xVertex - p1).X);
                            // }

                            StepRigidBody(rigidX, intervalDeltaTime * (1f - t));
                            colliders[colliderIX].RecalculateVertices();
                        }
                        Console.WriteLine($"/////////////////////////////////////////////");
                        // Console.WriteLine($"Collision");
                    }
            }
        }
    }

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

        rigidBody.Transform.Position = nextPosition;
    }

    private float IntersectionParameterT(Vector2 p0, Vector2 p1, Vector2 q0, Vector2 q1)
    {
        // Solve the linear interpolation equation for 't'
        float t = ((q0.X - p0.X) * (p1.Y - p0.Y) - (q0.Y - p0.Y) * (p1.X - p0.X)) /
                  ((q1.Y - q0.Y) * (p1.X - p0.X) - (q1.X - q0.X) * (p1.Y - p0.Y));

        return t;
    }

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