fix: Self Referencing Call

Engine.Core/Math.cs

@@ -28,5 +28,6 @@ public static class Math
     public static float MinMagnitude(float x, float y) => MathF.MinMagnitude(x, y);
     public static float Pow(float x, float y) => MathF.Pow(x, y);
     public static float Round(float x, int digits, MidpointRounding mode) => MathF.Round(x, digits, mode);
+    public static float Sqrt(float x) => MathF.Sqrt(x);
     public static float Truncate(float x) => MathF.Truncate(x);
 }

Engine.Physics2D/Collider2DCircleBehaviour.cs

@@ -12,17 +12,15 @@ public class Collider2DCircleBehaviour : BehaviourOverride, ICircleCollider2D
     public Action<ICollider2D, ICollider2D>? OnCollisionPreResolve { get; set; } = null;
     public Action<ICollider2D, ICollider2D>? OnCollisionResolved { get; set; } = null;
 
-    public Action<IAssignableTransform>? OnTransformAssigned { get; set; } = null;
-
 
     public Circle CircleWorld { get; protected set; } = new(Vector2D.Zero, 1f);
     public Circle CircleLocal { get; set; } = new(Vector2D.Zero, 1f);
     public IRigidBody2D? RigidBody2D { get; set; } = null;
 
 
-    ITransform IAssignableTransform.Transform => GameObject.Transform;
-
-    public bool Assign(ITransform transform) => GameObject.Assign(transform);
+    ITransform IAssignableTransform.Transform => Transform;
+    Action<IAssignableTransform>? IAssignableTransform.OnTransformAssigned { get => GameObject.OnTransformAssigned; set => GameObject.OnTransformAssigned = value; }
+    bool IAssignableTransform.Assign(ITransform transform) => GameObject.Assign(transform);
 
     public virtual void Recalculate() => CircleWorld = Transform.TransformCircle(CircleLocal).Displace(Transform.Position);
 }

Engine.Physics2D/Collider2DShapeBehaviour.cs

@@ -12,8 +12,6 @@ public class Collider2DShapeBehaviour : BehaviourOverride, IShapeCollider2D
     public Action<ICollider2D, ICollider2D>? OnCollisionPreResolve { get; set; } = null;
     public Action<ICollider2D, ICollider2D>? OnCollisionResolved { get; set; } = null;
 
-    public Action<IAssignableTransform>? OnTransformAssigned { get; set; } = null;
-
 
     public Shape ShapeWorld => _shapeWorld;
     public Shape ShapeLocal { get; set; } = new([new(1f, 1f), new(-1f, 1f), new(-1f, -1f), new(1f, -1f)]);
@@ -21,9 +19,9 @@ public class Collider2DShapeBehaviour : BehaviourOverride, IShapeCollider2D
 
     protected Shape _shapeWorld = new([new(1f, 1f), new(-1f, 1f), new(-1f, -1f), new(1f, -1f)]);
 
-    ITransform IAssignableTransform.Transform => GameObject.Transform;
-
-    public bool Assign(ITransform transform) => GameObject.Assign(transform);
+    ITransform IAssignableTransform.Transform => Transform;
+    Action<IAssignableTransform>? IAssignableTransform.OnTransformAssigned { get => GameObject.OnTransformAssigned; set => GameObject.OnTransformAssigned = value; }
+    bool IAssignableTransform.Assign(ITransform transform) => GameObject.Assign(transform);
 
     public virtual void Recalculate() => Transform.TransformShape(ShapeLocal, ref _shapeWorld);
 }

Engine.Physics2D/CollisionDetectionInformation.cs

@@ -0,0 +1,12 @@
+using Syntriax.Engine.Core;
+using Syntriax.Engine.Physics2D.Abstract;
+
+namespace Syntriax.Engine.Physics2D;
+
+public record CollisionDetectionInformation
+(
+    ICollider2D Left,
+    ICollider2D Right,
+    Vector2D Normal,
+    float Penetration
+);

Engine.Physics2D/CollisionDetector.cs

@@ -0,0 +1,65 @@
+using System.Diagnostics.CodeAnalysis;
+
+using Syntriax.Engine.Core;
+using Syntriax.Engine.Physics2D.Abstract;
+
+namespace Syntriax.Engine.Physics2D;
+
+public class CollisionDetector : ICollisionDetector
+{
+    public bool TryDetect<T1, T2>(T1 left, T2 right, [NotNullWhen(true)] out CollisionDetectionInformation? collisionInformation)
+        where T1 : ICollider2D
+        where T2 : ICollider2D
+    {
+        collisionInformation = default;
+        if (left is IShapeCollider2D shapeColliderLeft)
+        {
+            if (right is IShapeCollider2D shapeColliderRight)
+                return DetectShapeShape(shapeColliderLeft, shapeColliderRight, out collisionInformation);
+            else if (right is ICircleCollider2D circleColliderRight)
+                return DetectShapeCircle(shapeColliderLeft, circleColliderRight, out collisionInformation);
+        }
+        else if (left is ICircleCollider2D circleColliderLeft)
+        {
+            if (right is IShapeCollider2D shapeColliderRight)
+                return DetectCircleShape(circleColliderLeft, shapeColliderRight, out collisionInformation);
+            else if (right is ICircleCollider2D circleColliderRight)
+                return DetectCircleCircle(circleColliderLeft, circleColliderRight, out collisionInformation);
+        }
+
+        return false;
+    }
+
+    private bool DetectCircleShape(ICircleCollider2D circleCollider, IShapeCollider2D shapeCollider, out CollisionDetectionInformation? collisionInformation)
+    {
+        throw new System.NotImplementedException();
+    }
+
+    private static bool DetectShapeShape(IShapeCollider2D left, IShapeCollider2D right, out CollisionDetectionInformation? collisionInformation)
+    {
+        throw new System.NotImplementedException();
+    }
+
+    private static bool DetectShapeCircle(IShapeCollider2D shapeCollider, ICircleCollider2D circleCollider, out CollisionDetectionInformation? collisionInformation)
+    {
+        throw new System.NotImplementedException();
+    }
+
+    private static bool DetectCircleCircle(ICircleCollider2D left, ICircleCollider2D right, out CollisionDetectionInformation? collisionInformation)
+    {
+        collisionInformation = default;
+
+        Vector2D leftToRightCenter = left.CircleWorld.Center.FromTo(right.CircleWorld.Center);
+        float distanceCircleCenterSquared = leftToRightCenter.MagnitudeSquared;
+        float radiusSumSquared = left.CircleWorld.RadiusSquared + right.CircleWorld.RadiusSquared;
+
+        float circleSurfaceDistanceSquared = distanceCircleCenterSquared - radiusSumSquared;
+
+        bool collision = circleSurfaceDistanceSquared <= 0f;
+
+        if (collision)
+            collisionInformation = new(left, right, leftToRightCenter.Normalized, Math.Sqrt(circleSurfaceDistanceSquared));
+
+        return collision;
+    }
+}

Engine.Physics2D/CollisionInformation.cs

@@ -1,10 +0,0 @@
-
-using Syntriax.Engine.Core;
-
-namespace Syntriax.Engine.Physics2D;
-
-public record CollisionInformation
-(
-    Vector2D Normal,
-    Vector2D ContactPosition
-);

Engine.Physics2D/ICollisionDetector.cs

@@ -0,0 +1,10 @@
+using System.Diagnostics.CodeAnalysis;
+
+using Syntriax.Engine.Physics2D.Abstract;
+
+namespace Syntriax.Engine.Physics2D;
+
+public interface ICollisionDetector
+{
+    bool TryDetect<T1, T2>(T1 left, T2 right, [NotNullWhen(returnValue: true)] out CollisionDetectionInformation? collisionInformation) where T1 : ICollider2D where T2 : ICollider2D;
+}

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)

Engine.Physics2D/Primitives/Circle.cs

@@ -27,7 +27,7 @@ public static class CircleExtensions
 
     public static Circle Displace(this Circle circle, Vector2D displaceVector) => Circle.Displace(circle, displaceVector);
 
-    public static Circle TransformCircle(this ITransform transform, Circle circle) => TransformCircle(transform, circle);
+    public static Circle TransformCircle(this ITransform transform, Circle circle) => Circle.TransformCircle(transform, circle);
 
     public static bool ApproximatelyEquals(this Circle left, Circle right) => Circle.ApproximatelyEquals(left, right);
 }