Syntriax.Engine/Engine.Physics2D/PhysicsEngine2D.cs

using Syntriax.Engine.Core;

namespace Syntriax.Engine.Physics2D;

public class PhysicsEngine2D : UniverseObject, IPhysicsEngine2D
{
    public Event<IPhysicsEngine2D, float> OnPhysicsIteration { get; init; } = new();
    public Event<IPhysicsEngine2D, float> OnPhysicsStep { get; init; } = new();

    private readonly Event<IUniverse, IUniverse.UpdateArguments>.EventHandler cachedOnPreUpdate = null!;

    private float physicsTicker = 0f;
    private int _iterationPerStep = 1;
    private float _iterationPeriod = 1f / 60f;

    protected readonly ICollisionDetector2D collisionDetector = null!;
    protected readonly ICollisionResolver2D collisionResolver = null!;

    private static System.Comparison<IBehaviour> SortByPriority() => (x, y) => y.Priority.CompareTo(x.Priority);
    protected ActiveBehaviourCollectorSorted<IPrePhysicsUpdate> physicsPreUpdateCollector = new() { SortBy = SortByPriority() };
    protected ActiveBehaviourCollectorSorted<IPhysicsUpdate> physicsUpdateCollector = new() { SortBy = SortByPriority() };
    protected ActiveBehaviourCollectorSorted<IPostPhysicsUpdate> physicsPostUpdateCollector = new() { SortBy = SortByPriority() };
    protected BehaviourCollector<IRigidBody2D> rigidBodyCollector = new();
    protected BehaviourCollector<ICollider2D> colliderCollector = new();

    public int IterationPerStep { get => _iterationPerStep; set => _iterationPerStep = value < 1 ? 1 : value; }
    public float IterationPeriod { get => _iterationPeriod; set => _iterationPeriod = value.Max(0.0001f); }

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

        for (int i = physicsPreUpdateCollector.Count - 1; i >= 0; i--)
            physicsPreUpdateCollector[i].PrePhysicsUpdate(deltaTime);

        for (int i = physicsUpdateCollector.Count - 1; i >= 0; i--)
            physicsUpdateCollector[i].PhysicsUpdate(deltaTime);

        for (int iterationIndex = 0; iterationIndex < IterationPerStep; iterationIndex++)
        {
            // Can Parallel
            for (int i = rigidBodyCollector.Count - 1; i >= 0; i--)
                StepRigidBody(rigidBodyCollector[i], intervalDeltaTime);

            // Can Parallel
            for (int i = colliderCollector.Count - 1; i >= 0; i--)
                colliderCollector[i].Recalculate();

            // Can Parallel
            for (int x = 0; x < colliderCollector.Count; x++)
            {
                ICollider2D? colliderX = colliderCollector[x];
                if (!colliderX.IsActive)
                    continue;

                for (int y = x + 1; y < colliderCollector.Count; y++)
                {
                    ICollider2D? colliderY = colliderCollector[y];

                    if (!colliderY.IsActive)
                        continue;

                    ResolveColliders(colliderX, colliderY);
                }
            }

            OnPhysicsIteration?.Invoke(this, intervalDeltaTime);
        }

        for (int i = physicsPostUpdateCollector.Count - 1; i >= 0; i--)
            physicsPostUpdateCollector[i].PostPhysicsUpdate(deltaTime);

        OnPhysicsStep?.Invoke(this, deltaTime);
    }

    public void StepIndividual(IRigidBody2D rigidBody, float deltaTime)
    {
        float intervalDeltaTime = deltaTime / IterationPerStep;

        System.Collections.Generic.List<ICollider2D> childColliders = [];
        System.Collections.Generic.List<IPrePhysicsUpdate> physicsPreUpdates = [];
        System.Collections.Generic.List<IPhysicsUpdate> physicsUpdates = [];
        System.Collections.Generic.List<IPostPhysicsUpdate> physicsPostUpdates = [];

        rigidBody.BehaviourController.GetBehavioursInChildren(childColliders);
        rigidBody.BehaviourController.GetBehavioursInChildren(physicsPreUpdates);
        rigidBody.BehaviourController.GetBehavioursInChildren(physicsUpdates);
        rigidBody.BehaviourController.GetBehavioursInChildren(physicsPostUpdates);

        foreach (IPrePhysicsUpdate physicsPreUpdate in physicsPreUpdates)
            physicsPreUpdate.PrePhysicsUpdate(deltaTime);

        foreach (IPhysicsUpdate physicsUpdate in physicsUpdates)
            physicsUpdate.PhysicsUpdate(deltaTime);

        for (int iterationIndex = 0; iterationIndex < IterationPerStep; iterationIndex++)
        {
            StepRigidBody(rigidBody, intervalDeltaTime);

            for (int i = childColliders.Count - 1; i >= 0; i--)
                childColliders[i].Recalculate();

            for (int x = 0; x < childColliders.Count; x++)
            {
                ICollider2D? colliderX = childColliders[x];
                if (!colliderX.IsActive)
                    continue;

                for (int y = 0; y < colliderCollector.Count; y++)
                {
                    ICollider2D? colliderY = colliderCollector[y];

                    if (!colliderY.IsActive)
                        continue;

                    ResolveColliders(colliderX, colliderY);
                }
            }
        }

        foreach (IPostPhysicsUpdate physicsPostUpdate in physicsPostUpdates)
            physicsPostUpdate.PostPhysicsUpdate(deltaTime);
    }

    private void ResolveColliders(ICollider2D colliderX, ICollider2D colliderY)
    {
        if (colliderX.RigidBody2D == colliderY.RigidBody2D)
            return;

        bool bothCollidersAreTriggers = colliderX.IsTrigger && colliderX.IsTrigger == colliderY.IsTrigger;
        if (bothCollidersAreTriggers)
            return;

        bool bothCollidersAreStatic = (colliderX.RigidBody2D?.IsStatic ?? true) && (colliderY.RigidBody2D?.IsStatic ?? true);
        if (bothCollidersAreStatic)
            return;

        if (!collisionDetector.TryDetect(colliderX, colliderY, out CollisionDetectionInformation information))
            return;

        if (colliderX.IsTrigger)
        {
            colliderX.Trigger(colliderY);
            return;
        }
        else if (colliderY.IsTrigger)
        {
            colliderY.Trigger(colliderX);
            return;
        }

        if (information.Detector == colliderX)
        {
            colliderX.Detect(information);
            colliderY.Detect(information.Reverse());
        }
        else
        {
            colliderX.Detect(information.Reverse());
            colliderY.Detect(information);
        }

        collisionResolver?.Resolve(information);
    }

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

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

    protected override void OnEnteringUniverse(IUniverse universe)
    {
        physicsPreUpdateCollector.Assign(universe);
        physicsUpdateCollector.Assign(universe);
        physicsPostUpdateCollector.Assign(universe);
        colliderCollector.Assign(universe);
        rigidBodyCollector.Assign(universe);

        universe.OnPreUpdate.AddListener(OnEnginePreUpdate);
    }

    protected override void OnExitingUniverse(IUniverse universe)
    {
        physicsPreUpdateCollector.Unassign();
        physicsUpdateCollector.Unassign();
        physicsPostUpdateCollector.Unassign();
        colliderCollector.Unassign();
        rigidBodyCollector.Unassign();

        universe.OnPreUpdate.RemoveListener(OnEnginePreUpdate);
    }

    private void OnEnginePreUpdate(IUniverse sender, IUniverse.UpdateArguments arguments)
    {
        physicsTicker += arguments.EngineTime.DeltaTime;

        while (physicsTicker >= IterationPeriod)
        {
            physicsTicker -= IterationPeriod;
            Step(IterationPeriod);
        }
    }

    public PhysicsEngine2D()
    {
        collisionDetector = new CollisionDetector2D();
        collisionResolver = new CollisionResolver2D();

        cachedOnPreUpdate = OnEnginePreUpdate;
    }

    public PhysicsEngine2D(ICollisionDetector2D collisionDetector, ICollisionResolver2D collisionResolver)
    {
        this.collisionDetector = collisionDetector;
        this.collisionResolver = collisionResolver;

        cachedOnPreUpdate = OnEnginePreUpdate;
    }
}