Syntriax.Engine/Engine.Physics2D/PhysicsEngine2D.cs

using System.Collections.Generic;

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

namespace Syntriax.Engine.Physics2D;

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

    private int _iterationCount = 1;
    private ICollisionDetector2D collisionDetector = new CollisionDetector2D();

    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 iterationIndex = 0; iterationIndex < IterationCount; iterationIndex++)
        {
            // Can Parallel
            for (int i = 0; i < rigidBodies.Count; i++)
                StepRigidBody(rigidBodies[i], intervalDeltaTime);

            // Can Parallel
            foreach (var collider in colliders)
                collider.Recalculate();

            // Can Parallel
            for (int x = 0; x < colliders.Count; x++)
            {
                ICollider2D? colliderX = colliders[x];
                for (int y = x + 1; y < colliders.Count; y++)
                {
                    ICollider2D? colliderY = colliders[y];

                    if (colliderX.RigidBody2D == colliderY.RigidBody2D)
                        continue;

                    if (collisionDetector.TryDetect(colliderX, colliderY, out CollisionDetectionInformation information))
                    {
                        colliderX.OnCollisionDetected?.Invoke(colliderX, information);
                        colliderY.OnCollisionDetected?.Invoke(colliderY, information);

                        Vector2D displacementVector = information.Normal * information.Penetration;


                        bool isStaticLeft = information.Left.RigidBody2D?.IsStatic ?? true;
                        bool isStaticRight = information.Right.RigidBody2D?.IsStatic ?? true;

                        if (isStaticLeft && isStaticRight)
                            continue;
                        else if (isStaticLeft)
                            information.Right.Transform.Position += displacementVector;
                        else if (isStaticRight)
                            information.Left.Transform.Position -= displacementVector;
                        else
                        {
                            float leftMass = information.Left.RigidBody2D?.Mass ?? float.Epsilon;
                            float rightMass = information.Right.RigidBody2D?.Mass ?? float.Epsilon;
                            float massSum = leftMass + rightMass;

                            float leftMomentumPercentage = leftMass / massSum;
                            float rightMomentumPercentage = rightMass / massSum;

                            information.Right.Transform.Position += leftMomentumPercentage * displacementVector;
                            information.Left.Transform.Position -= rightMomentumPercentage * displacementVector;
                        }

                        information.Left.Recalculate();
                        information.Right.Recalculate();

                        colliderX.OnCollisionResolved?.Invoke(colliderX, information);
                        colliderY.OnCollisionResolved?.Invoke(colliderY, information);
                    }
                }
            }
        }
    }

    private static void StepRigidBody(IRigidBody2D rigidBody, float intervalDeltaTime)
    {
        if (rigidBody.IsStatic)
            return;

        rigidBody.Transform.Position += rigidBody.Velocity * intervalDeltaTime;
        rigidBody.Transform.Rotation += rigidBody.AngularVelocity * intervalDeltaTime;
    }

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