Syntriax.Engine/Engine.Physics2D/PhysicsEngine2D.cs

using System.Collections.Generic;

using Engine.Core;

namespace Engine.Physics2D;

public class PhysicsEngine2D : Behaviour, IEnterUniverse, IExitUniverse, IPreUpdate, IPhysicsEngine2D
{
    public Event<IPhysicsEngine2D, float> OnPhysicsIteration { get; } = new();
    public Event<IPhysicsEngine2D, float> OnPhysicsStep { get; } = new();

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

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

    private static Comparer<int> SortByPriority() => Comparer<int>.Create((x, y) => y.CompareTo(x));
    private static System.Func<IBehaviour, int> GetPriority() => (b) => b.Priority;
    protected ActiveBehaviourCollectorOrdered<int, IPrePhysicsUpdate> physicsPreUpdateCollector = new(GetPriority(), SortByPriority());
    protected ActiveBehaviourCollectorOrdered<int, IPhysicsUpdate> physicsUpdateCollector = new(GetPriority(), SortByPriority());
    protected ActiveBehaviourCollectorOrdered<int, IPhysicsIteration> physicsIterationCollector = new(GetPriority(), SortByPriority());
    protected ActiveBehaviourCollectorOrdered<int, IPostPhysicsUpdate> physicsPostUpdateCollector = new(GetPriority(), SortByPriority());
    protected BehaviourCollector<IRigidBody2D> rigidBodyCollector = new();
    protected BehaviourCollector<ICollider2D> colliderCollector = new();

    private readonly ListPool<ICollider2D> colliderPool = new();
    private readonly ListPool<IPrePhysicsUpdate> prePhysicsUpdatePool = new();
    private readonly ListPool<IPhysicsUpdate> physicsUpdatePool = new();
    private readonly ListPool<IPhysicsIteration> physicsIterationPool = new();
    private readonly ListPool<IPostPhysicsUpdate> postPhysicsUpdatePool = new();

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

    public RaycastResult? Raycast(Ray2D ray, float length = float.MaxValue)
    {
        RaycastResult? closestResult = null;
        float closestDistance = float.MaxValue;

        for (int i = 0; i < colliderCollector.Count; i++)
            if (raycastResolver.RaycastAgainst(colliderCollector[i], ray, length) is RaycastResult raycastResult)
            {
                float magnitudeSquared = raycastResult.Position.FromTo(ray.Origin).MagnitudeSquared;

                if (magnitudeSquared > closestDistance)
                    continue;

                closestDistance = magnitudeSquared;
                closestResult = raycastResult;
            }

        return closestResult;
    }

    public void Raycast(Ray2D ray, IList<RaycastResult> results, float length = float.MaxValue)
    {
        results.Clear();

        for (int i = 0; i < colliderCollector.Count; i++)
            if (raycastResolver.RaycastAgainst(colliderCollector[i], ray, length) is RaycastResult raycastResult)
                results.Add(raycastResult);
    }

    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++)
        {
            for (int i = physicsIterationCollector.Count - 1; i >= 0; i--)
                physicsIterationCollector[i].PhysicsIterate(intervalDeltaTime);

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

        List<ICollider2D> childColliders = colliderPool.Get();
        List<IPrePhysicsUpdate> physicsPreUpdates = prePhysicsUpdatePool.Get();
        List<IPhysicsUpdate> physicsUpdates = physicsUpdatePool.Get();
        List<IPhysicsIteration> physicsIterations = physicsIterationPool.Get();
        List<IPostPhysicsUpdate> physicsPostUpdates = postPhysicsUpdatePool.Get();

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

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

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

        for (int iterationIndex = 0; iterationIndex < IterationPerStep; iterationIndex++)
        {
            for (int i = physicsIterations.Count - 1; i >= 0; i--)
                physicsIterations[i].PhysicsIterate(intervalDeltaTime);

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

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

        colliderPool.Return(childColliders);
        prePhysicsUpdatePool.Return(physicsPreUpdates);
        physicsUpdatePool.Return(physicsUpdates);
        physicsIterationPool.Return(physicsIterations);
        postPhysicsUpdatePool.Return(physicsPostUpdates);
    }

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

    public void EnterUniverse(IUniverse universe)
    {
        physicsPreUpdateCollector.Assign(universe);
        physicsUpdateCollector.Assign(universe);
        physicsIterationCollector.Assign(universe);
        physicsPostUpdateCollector.Assign(universe);
        colliderCollector.Assign(universe);
        rigidBodyCollector.Assign(universe);
    }

    public void ExitUniverse(IUniverse universe)
    {
        physicsPreUpdateCollector.Unassign();
        physicsUpdateCollector.Unassign();
        physicsIterationCollector.Unassign();
        physicsPostUpdateCollector.Unassign();
        colliderCollector.Unassign();
        rigidBodyCollector.Unassign();
    }

    void IPreUpdate.PreUpdate()
    {
        physicsTicker += Universe.Time.DeltaTime;

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

    public PhysicsEngine2D()
    {
        collisionDetector = new CollisionDetector2D();
        collisionResolver = new CollisionResolver2D();
        raycastResolver = new RaycastResolver2D();
        Priority = int.MaxValue;
    }

    public PhysicsEngine2D(ICollisionDetector2D collisionDetector, ICollisionResolver2D collisionResolver, IRaycastResolver2D raycastResolver)
    {
        this.collisionDetector = collisionDetector;
        this.collisionResolver = collisionResolver;
        this.raycastResolver = raycastResolver;
        Priority = int.MaxValue;
    }
}