From 0af1b11396e61dfa4c7c2d080a10d760a7786db2 Mon Sep 17 00:00:00 2001
From: Syntriax <syntriax@gmail.com>
Date: Thu, 25 Jan 2024 18:35:24 +0300
Subject: [PATCH] feat: Test Collision Detection & Move

---
 Engine.Physics2D/PhysicsEngine2D.cs | 46 +++++++++++++++++++++++++----
 1 file changed, 40 insertions(+), 6 deletions(-)

diff --git a/Engine.Physics2D/PhysicsEngine2D.cs b/Engine.Physics2D/PhysicsEngine2D.cs
index 684bdeb..0d4c476 100644
--- a/Engine.Physics2D/PhysicsEngine2D.cs
+++ b/Engine.Physics2D/PhysicsEngine2D.cs
@@ -4,17 +4,16 @@ using System.Collections.Generic;
 using Syntriax.Engine.Core;
 using Syntriax.Engine.Core.Abstract;
 using Syntriax.Engine.Physics2D.Abstract;
-using Syntriax.Engine.Physics2D.Primitives;
 
 namespace Syntriax.Engine.Physics2D;
 
 public class PhysicsEngine2D : IPhysicsEngine2D
 {
-    private List<IRigidBody2D> rigidBodies = new List<IRigidBody2D>(32);
-    private List<ICollider2D> colliders = new List<ICollider2D>(64);
+    private readonly List<IRigidBody2D> rigidBodies = new(32);
+    private readonly List<ICollider2D> colliders = new(64);
 
     private int _iterationCount = 1;
-
+    private ICollisionDetector collisionDetector = new CollisionDetector();
 
     public int IterationCount { get => _iterationCount; set => _iterationCount = value < 1 ? 1 : value; }
 
@@ -43,11 +42,32 @@ public class PhysicsEngine2D : IPhysicsEngine2D
 
         for (int iterationIndex = 0; iterationIndex < IterationCount; iterationIndex++)
         {
-            foreach (var rigidBody in rigidBodies)
-                StepRigidBody(rigidBody, intervalDeltaTime);
+            // 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 = 0; y < colliders.Count; y++)
+                {
+                    ICollider2D? colliderY = colliders[y];
+
+                    if (colliderX.RigidBody2D == colliderY.RigidBody2D && colliderY.RigidBody2D is null)
+                        continue;
+
+                    if (collisionDetector.TryDetect(colliderX, colliderY, out CollisionDetectionInformation? information))
+                    {
+                        information.Left.Transform.Position -= .5f * information.Normal * information.Penetration;
+                        information.Right.Transform.Position += .5f * information.Normal * information.Penetration;
+                    }
+                }
+            }
         }
     }
 
@@ -57,6 +77,20 @@ public class PhysicsEngine2D : IPhysicsEngine2D
         rigidBody.Transform.Rotation += rigidBody.AngularVelocity * intervalDeltaTime;
     }
 
+    private static void CacheRotations(List<IRigidBody2D> rigidBodies, List<float> rotations)
+    {
+        rotations.Clear();
+        for (int i = 0; i < rigidBodies.Count; i++)
+            rotations.Add(rigidBodies[i].Transform.Rotation);
+    }
+
+    private static void CachePositions(List<IRigidBody2D> rigidBodies, List<Vector2D> positions)
+    {
+        positions.Clear();
+        for (int i = 0; i < rigidBodies.Count; i++)
+            positions.Add(rigidBodies[i].Transform.Position);
+    }
+
     private void OnBehaviourAdded(IBehaviourController controller, IBehaviour behaviour)
     {
         if (behaviour is not ICollider2D collider2D)