Syntriax.Engine/Engine.Physics2D/PhysicsEngine2DStandalone.cs

using System.Collections.Generic;

using Engine.Core;

namespace Engine.Physics2D;

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

    private readonly Event<IBehaviourController, IBehaviourController.BehaviourAddedArguments>.EventHandler delegateOnBehaviourAdded = null!;
    private readonly Event<IBehaviourController, IBehaviourController.BehaviourRemovedArguments>.EventHandler delegateOnBehaviourRemoved = null!;

    private readonly List<IRigidBody2D> rigidBodies = new(32);
    private readonly List<ICollider2D> colliders = new(64);

    private int _iterationCount = 1;

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

    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 => _iterationCount; set => _iterationCount = value < 1 ? 1 : value; }

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

        rigidBodies.Add(rigidBody);

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

        rigidBody.BehaviourController.OnBehaviourAdded.AddListener(delegateOnBehaviourAdded);
        rigidBody.BehaviourController.OnBehaviourRemoved.AddListener(delegateOnBehaviourRemoved);
    }

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

        rigidBody.BehaviourController.OnBehaviourAdded.RemoveListener(delegateOnBehaviourAdded);
        rigidBody.BehaviourController.OnBehaviourRemoved.RemoveListener(delegateOnBehaviourRemoved);
    }
    public RaycastResult? Raycast(Ray2D ray, float length = float.MaxValue)
    {
        RaycastResult? closestResult = null;
        float closestDistance = float.MaxValue;

        for (int i = 0; i < colliders.Count; i++)
            if (raycastResolver.RaycastAgainst(colliders[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 < colliders.Count; i++)
            if (raycastResolver.RaycastAgainst(colliders[i], ray, length) is RaycastResult raycastResult)
                results.Add(raycastResult);
    }

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

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

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

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

                for (int y = x + 1; y < colliders.Count; y++)
                {
                    ICollider2D? colliderY = colliders[y];
                    if (!colliderY.IsActive)
                        return;

                    ResolveColliders(colliderX, colliderY);
                }
            }
            OnPhysicsIteration?.Invoke(this, intervalDeltaTime);
        }
        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 < colliders.Count; y++)
                {
                    ICollider2D? colliderY = colliders[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;
    }

    private void OnBehaviourAdded(IBehaviourController sender, IBehaviourController.BehaviourAddedArguments args)
    {
        if (args.BehaviourAdded is not ICollider2D collider2D)
            return;

        colliders.Add(collider2D);
    }

    private void OnBehaviourRemoved(IBehaviourController sender, IBehaviourController.BehaviourRemovedArguments args)
    {
        if (args.BehaviourRemoved is not ICollider2D collider2D)
            return;

        colliders.Remove(collider2D);
    }

    public PhysicsEngine2DStandalone()
    {
        collisionDetector = new CollisionDetector2D();
        collisionResolver = new CollisionResolver2D();
        raycastResolver = new RaycastResolver2D();

        delegateOnBehaviourAdded = OnBehaviourAdded;
        delegateOnBehaviourRemoved = OnBehaviourRemoved;
    }

    public PhysicsEngine2DStandalone(ICollisionDetector2D collisionDetector, ICollisionResolver2D collisionResolver, IRaycastResolver2D raycastResolver)
    {
        this.collisionDetector = collisionDetector;
        this.collisionResolver = collisionResolver;
        this.raycastResolver = raycastResolver;

        delegateOnBehaviourAdded = OnBehaviourAdded;
        delegateOnBehaviourRemoved = OnBehaviourRemoved;
    }
}