merge GJK2D and EPA2D into narrow phase class + optimize EPA2D with unsafe

Bonk/AABB.cs

@@ -16,6 +16,11 @@ namespace MoonTools.Core.Bonk
         public float Width { get { return Max.X - Min.X; } }
         public float Height { get { return Max.Y - Min.Y; } }
 
+        public float Right { get { return Max.X; } }
+        public float Left { get { return Min.X; } }
+        public float Top {  get { return Min.Y; } }
+        public float Bottom { get { return Max.Y; } }
+
         public AABB(float minX, float minY, float maxX, float maxY)
         {
             Min = new Vector2(minX, minY);

Bonk/Bonk.csproj

@@ -13,6 +13,7 @@
     <AssemblyName>MoonTools.Core.Bonk</AssemblyName>
     <PackageLicenseExpression>LGPL-3.0-only</PackageLicenseExpression>
     <PackageProjectUrl>https://github.com/MoonsideGames/MoonTools.Core.Bonk</PackageProjectUrl>
+    <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
   </PropertyGroup>
   <ItemGroup>
     <PackageReference Include="Microsoft.CodeAnalysis.FxCopAnalyzers" Version="2.9.8">

Bonk/NarrowPhase/EPA2D.cs

@@ -1,103 +0,0 @@
-/*
- * Implementation of the Expanding Polytope Algorithm
- * as based on the following blog post:
- * https://blog.hamaluik.ca/posts/building-a-collision-engine-part-2-2d-penetration-vectors/
- */
-
-using MoonTools.Core.Structs;
-using System;
-using System.Collections.Immutable;
-using System.Linq;
-using System.Numerics;
-
-namespace MoonTools.Core.Bonk
-{
-    internal enum PolygonWinding
-    {
-        Clockwise,
-        CounterClockwise
-    }
-
-    public static class EPA2D
-    {
-        /// <summary>
-        /// Returns a minimum separating vector in the direction from A to B.
-        /// </summary>
-        /// <param name="simplex">A simplex returned by the GJK algorithm.</param>
-        public static Vector2 Intersect(IShape2D shapeA, Transform2D Transform2DA, IShape2D shapeB, Transform2D Transform2DB, Simplex2D simplex)
-        {
-            if (shapeA == null) { throw new ArgumentNullException(nameof(shapeA)); }
-            if (shapeB == null) { throw new ArgumentNullException(nameof(shapeB)); }
-
-            var simplexVertices = simplex.Vertices.Select(vertex => vertex.ToVector2()).ToImmutableArray();
-
-            var e0 = (simplexVertices[1].X - simplexVertices[0].X) * (simplexVertices[1].Y + simplexVertices[0].Y);
-            var e1 = (simplexVertices[2].X - simplexVertices[1].X) * (simplexVertices[2].Y + simplexVertices[1].Y);
-            var e2 = (simplexVertices[0].X - simplexVertices[2].X) * (simplexVertices[0].Y + simplexVertices[2].Y);
-            var winding = e0 + e1 + e2 >= 0 ? PolygonWinding.Clockwise : PolygonWinding.CounterClockwise;
-
-            Vector2 intersection = default;
-
-            for (int i = 0; i < 32; i++)
-            {
-                var edge = FindClosestEdge(winding, simplexVertices);
-                var support = CalculateSupport(shapeA, Transform2DA, shapeB, Transform2DB, edge.normal);
-                var distance = Vector2.Dot(support, edge.normal);
-
-                intersection = edge.normal;
-                intersection *= distance;
-
-                if (Math.Abs(distance - edge.distance) <= float.Epsilon)
-                {
-                    return intersection;
-                }
-                else
-                {
-                    simplexVertices = simplexVertices.Insert(edge.index, support);
-                }
-            }
-
-            return intersection;
-        }
-
-        private static Edge FindClosestEdge(PolygonWinding winding, ImmutableArray<Vector2> simplexVertices)
-        {
-            var closestDistance = float.PositiveInfinity;
-            var closestNormal = Vector2.Zero;
-            var closestIndex = 0;
-
-            for (int i = 0; i < simplexVertices.Length; i++)
-            {
-                var j = i + 1;
-                if (j >= simplexVertices.Length) { j = 0; }
-                Vector2 edge = simplexVertices[j] - simplexVertices[i];
-
-                Vector2 norm;
-                if (winding == PolygonWinding.Clockwise)
-                {
-                    norm = Vector2.Normalize(new Vector2(edge.Y, -edge.X));
-                }
-                else
-                {
-                    norm = Vector2.Normalize(new Vector2(-edge.Y, edge.X));
-                }
-
-                var dist = Vector2.Dot(norm, simplexVertices[i]);
-
-                if (dist < closestDistance)
-                {
-                    closestDistance = dist;
-                    closestNormal = norm;
-                    closestIndex = j;
-                }
-            }
-
-            return new Edge(closestDistance, closestNormal, closestIndex);
-        }
-
-        private static Vector2 CalculateSupport(IShape2D shapeA, Transform2D Transform2DA, IShape2D shapeB, Transform2D Transform2DB, Vector2 direction)
-        {
-            return shapeA.Support(direction, Transform2DA) - shapeB.Support(-direction, Transform2DB);
-        }
-    }
-}

Bonk/NarrowPhase/GJK2D.cs

@@ -1,145 +0,0 @@
-using MoonTools.Core.Structs;
-using MoonTools.Core.Bonk.Extensions;
-using System.Numerics;
-
-namespace MoonTools.Core.Bonk
-{
-    public static class GJK2D
-    {
-        /// <summary>
-        /// Tests if the two shape-transform pairs are overlapping.
-        /// </summary>
-        public static bool TestCollision(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB)
-        {
-            return FindCollisionSimplex(shapeA, transformA, shapeB, transformB).Item1;
-        }
-
-        /// <summary>
-        /// Tests if the two shape-transform pairs are overlapping, and returns a simplex that can be used by the EPA algorithm to determine a miminum separating vector.
-        /// </summary>
-        public static (bool, Simplex2D) FindCollisionSimplex(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB)
-        {
-            var minkowskiDifference = new MinkowskiDifference(shapeA, transformA, shapeB, transformB);
-            var c = minkowskiDifference.Support(Vector2.UnitX);
-            var b = minkowskiDifference.Support(-Vector2.UnitX);
-            return Check(minkowskiDifference, c, b);
-        }
-
-        private static (bool, Simplex2D) Check(MinkowskiDifference minkowskiDifference, Vector2 c, Vector2 b)
-        {
-            var cb = c - b;
-            var c0 = -c;
-            var d = Direction(cb, c0);
-            return DoSimplex(minkowskiDifference, new Simplex2D(b, c), d);
-        }
-
-        private static (bool, Simplex2D) DoSimplex(MinkowskiDifference minkowskiDifference, Simplex2D simplex, Vector2 direction)
-        {
-            var a = minkowskiDifference.Support(direction);
-            var notPastOrigin = Vector2.Dot(a, direction) < 0;
-            var (intersects, newSimplex, newDirection) = EnclosesOrigin(a, simplex);
-
-            if (notPastOrigin)
-            {
-                return (false, default(Simplex2D));
-            }
-            else if (intersects)
-            {
-                return (true, new Simplex2D(simplex.A, simplex.B.Value, a));
-            }
-            else
-            {
-                return DoSimplex(minkowskiDifference, newSimplex, newDirection);
-            }
-        }
-
-        private static (bool, Simplex2D, Vector2) EnclosesOrigin(Vector2 a, Simplex2D simplex)
-        {
-            if (simplex.ZeroSimplex)
-            {
-                return HandleZeroSimplex(a, simplex.A);
-            }
-            else if (simplex.OneSimplex)
-            {
-                return HandleOneSimplex(a, simplex.A, simplex.B.Value);
-            }
-            else
-            {
-                return (false, simplex, Vector2.Zero);
-            }
-        }
-
-        private static (bool, Simplex2D, Vector2) HandleZeroSimplex(Vector2 a, Vector2 b)
-        {
-            var ab = b - a;
-            var a0 = -a;
-            var (newSimplex, newDirection) = SameDirection(ab, a0) ? (new Simplex2D(a, b), Perpendicular(ab, a0)) : (new Simplex2D(a), a0);
-            return (false, newSimplex, newDirection);
-        }
-
-        private static (bool, Simplex2D, Vector2) HandleOneSimplex(Vector2 a, Vector2 b, Vector2 c)
-        {
-            var a0 = -a;
-            var ab = b - a;
-            var ac = c - a;
-            var abp = Perpendicular(ab, -ac);
-            var acp = Perpendicular(ac, -ab);
-
-            if (SameDirection(abp, a0))
-            {
-                if (SameDirection(ab, a0))
-                {
-                    return (false, new Simplex2D(a, b), abp);
-                }
-                else
-                {
-                    return (false, new Simplex2D(a), a0);
-                }
-            }
-            else if (SameDirection(acp, a0))
-            {
-                if (SameDirection(ac, a0))
-                {
-                    return (false, new Simplex2D(a, c), acp);
-                }
-                else
-                {
-                    return (false, new Simplex2D(a), a0);
-                }
-            }
-            else
-            {
-                return (true, new Simplex2D(b, c), a0);
-            }
-        }
-
-        private static Vector2 TripleProduct(Vector2 a, Vector2 b, Vector2 c)
-        {
-            var A = new Vector3(a.X, a.Y, 0);
-            var B = new Vector3(b.X, b.Y, 0);
-            var C = new Vector3(c.X, c.Y, 0);
-
-            var first = Vector3.Cross(A, B);
-            var second = Vector3.Cross(first, C);
-
-            return new Vector2(second.X, second.Y);
-        }
-
-        private static Vector2 Direction(Vector2 a, Vector2 b)
-        {
-            var d = TripleProduct(a, b, a);
-            var collinear = d == Vector2.Zero;
-            return collinear ? new Vector2(a.Y, -a.X) : d;
-        }
-
-        private static bool SameDirection(Vector2 a, Vector2 b)
-        {
-            return Vector2.Dot(a, b) > 0;
-        }
-
-        private static Vector2 Perpendicular(Vector2 a, Vector2 b)
-        {
-            return TripleProduct(a, b, a);
-        }
-    }
-}

Bonk/NarrowPhase/NarrowPhase.cs

@@ -0,0 +1,300 @@
+using MoonTools.Core.Structs;
+using System.Numerics;
+using System.Collections.Generic;
+
+namespace MoonTools.Core.Bonk
+{
+    internal unsafe struct SimplexVertexBuffer
+    {
+        private const int Size = 35;
+        
+        public int Length { get; private set; }
+
+        public SimplexVertexBuffer(IEnumerable<Position2D> positions)
+        {
+            var i = 0;
+            foreach (var position in positions)
+            {
+                if (i == Size) { break; }
+                var vertex = position.ToVector2();
+                _simplexXBuffer[i] = vertex.X;
+                _simplexYBuffer[i] = vertex.Y;
+                i++;
+            }
+            Length = i;
+        }
+
+        public Vector2 this[int key]
+        {
+            get => new Vector2(_simplexXBuffer[key], _simplexYBuffer[key]);
+            private set 
+            {
+                _simplexXBuffer[key] = value.X;
+                _simplexYBuffer[key] = value.Y;
+            }
+        }
+
+        public void Insert(int index, Vector2 value)
+        {
+            for (var i = Length; i > index; i--)
+            {
+                this[i] = this[i - 1];
+            }
+            this[index] = value;
+            Length++;
+        }
+
+        private fixed float _simplexXBuffer[Size];
+        private fixed float _simplexYBuffer[Size];
+    }
+
+    public static class NarrowPhase
+    {
+        private enum PolygonWinding
+        {
+            Clockwise,
+            CounterClockwise
+        }
+
+        /// <summary>
+        /// Tests if the two shape-transform pairs are overlapping.
+        /// </summary>
+        public static bool TestCollision(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB)
+        {
+            if (shapeA is Rectangle rectangleA && shapeB is Rectangle rectangleB && transformA.Rotation == 0 && transformB.Rotation == 0)
+            {
+                return TestRectangleOverlap(rectangleA, transformA, rectangleB, transformB);
+            }
+            return FindCollisionSimplex(shapeA, transformA, shapeB, transformB).Item1;
+        }
+
+        /// <summary>
+        /// Fast path for overlapping rectangles. If the transforms have non-zero rotation this will be inaccurate.
+        /// </summary>
+        /// <param name="rectangleA"></param>
+        /// <param name="transformA"></param>
+        /// <param name="rectangleB"></param>
+        /// <param name="transformB"></param>
+        /// <returns></returns>
+        public static bool TestRectangleOverlap(Rectangle rectangleA, Transform2D transformA, Rectangle rectangleB, Transform2D transformB)
+        {
+            var firstAABB = rectangleA.TransformedAABB(transformA);
+            var secondAABB = rectangleB.TransformedAABB(transformB);
+
+            return firstAABB.Left <= secondAABB.Right && firstAABB.Right >= secondAABB.Left && firstAABB.Top <= secondAABB.Bottom && firstAABB.Bottom >= secondAABB.Top;
+        }
+
+        /// <summary>
+        /// Tests if the two shape-transform pairs are overlapping, and returns a simplex that can be used by the EPA algorithm to determine a miminum separating vector.
+        /// </summary>
+        public static (bool, Simplex2D) FindCollisionSimplex(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB)
+        {
+            var minkowskiDifference = new MinkowskiDifference(shapeA, transformA, shapeB, transformB);
+            var c = minkowskiDifference.Support(Vector2.UnitX);
+            var b = minkowskiDifference.Support(-Vector2.UnitX);
+            return Check(minkowskiDifference, c, b);
+        }
+
+        /// <summary>
+        /// Returns a minimum separating vector in the direction from A to B.
+        /// </summary>
+        /// <param name="simplex">A simplex returned by the GJK algorithm.</param>
+        public unsafe static Vector2 Intersect(IShape2D shapeA, Transform2D Transform2DA, IShape2D shapeB, Transform2D Transform2DB, Simplex2D simplex)
+        {
+            if (shapeA == null) { throw new System.ArgumentNullException(nameof(shapeA)); }
+            if (shapeB == null) { throw new System.ArgumentNullException(nameof(shapeB)); }
+            if (!simplex.TwoSimplex) { throw new System.ArgumentException("Simplex must be a 2-Simplex.", nameof(simplex)); }
+
+            var a = simplex.A;
+            var b = simplex.B.Value;
+            var c = simplex.C.Value;
+
+            var e0 = (b.X - a.X) * (b.Y + a.Y);
+            var e1 = (c.X - b.X) * (c.Y + b.Y);
+            var e2 = (a.X - c.X) * (a.Y + c.Y);
+            var winding = e0 + e1 + e2 >= 0 ? PolygonWinding.Clockwise : PolygonWinding.CounterClockwise;
+
+            var simplexVertices = new SimplexVertexBuffer(simplex.Vertices);
+
+            Vector2 intersection = default;
+
+            for (var i = 0; i < 32; i++)
+            {
+                var edge = FindClosestEdge(winding, simplexVertices);
+                var support = CalculateSupport(shapeA, Transform2DA, shapeB, Transform2DB, edge.normal);
+                var distance = Vector2.Dot(support, edge.normal);
+
+                intersection = edge.normal;
+                intersection *= distance;
+
+                if (System.Math.Abs(distance - edge.distance) <= float.Epsilon)
+                {
+                    return intersection;
+                }
+                else
+                {
+                    simplexVertices.Insert(edge.index, support);
+                }
+            }
+
+            return intersection;
+        }
+
+        private static Edge FindClosestEdge(PolygonWinding winding, SimplexVertexBuffer simplexVertices)
+        {
+            var closestDistance = float.PositiveInfinity;
+            var closestNormal = Vector2.Zero;
+            var closestIndex = 0;
+
+            for (var i = 0; i < simplexVertices.Length; i++)
+            {
+                var j = i + 1;
+                if (j >= simplexVertices.Length) { j = 0; }
+                var edge = simplexVertices[j] - simplexVertices[i];
+
+                Vector2 norm;
+                if (winding == PolygonWinding.Clockwise)
+                {
+                    norm = Vector2.Normalize(new Vector2(edge.Y, -edge.X));
+                }
+                else
+                {
+                    norm = Vector2.Normalize(new Vector2(-edge.Y, edge.X));
+                }
+
+                var dist = Vector2.Dot(norm, simplexVertices[i]);
+
+                if (dist < closestDistance)
+                {
+                    closestDistance = dist;
+                    closestNormal = norm;
+                    closestIndex = j;
+                }
+            }
+
+            return new Edge(closestDistance, closestNormal, closestIndex);
+        }
+
+        private static Vector2 CalculateSupport(IShape2D shapeA, Transform2D Transform2DA, IShape2D shapeB, Transform2D Transform2DB, Vector2 direction)
+        {
+            return shapeA.Support(direction, Transform2DA) - shapeB.Support(-direction, Transform2DB);
+        }
+
+        private static (bool, Simplex2D) Check(MinkowskiDifference minkowskiDifference, Vector2 c, Vector2 b)
+        {
+            var cb = c - b;
+            var c0 = -c;
+            var d = Direction(cb, c0);
+            return DoSimplex(minkowskiDifference, new Simplex2D(b, c), d);
+        }
+
+        private static (bool, Simplex2D) DoSimplex(MinkowskiDifference minkowskiDifference, Simplex2D simplex, Vector2 direction)
+        {
+            var a = minkowskiDifference.Support(direction);
+            var notPastOrigin = Vector2.Dot(a, direction) < 0;
+            var (intersects, newSimplex, newDirection) = EnclosesOrigin(a, simplex);
+
+            if (notPastOrigin)
+            {
+                return (false, default(Simplex2D));
+            }
+            else if (intersects)
+            {
+                return (true, new Simplex2D(simplex.A, simplex.B.Value, a));
+            }
+            else
+            {
+                return DoSimplex(minkowskiDifference, newSimplex, newDirection);
+            }
+        }
+
+        private static (bool, Simplex2D, Vector2) EnclosesOrigin(Vector2 a, Simplex2D simplex)
+        {
+            if (simplex.ZeroSimplex)
+            {
+                return HandleZeroSimplex(a, simplex.A);
+            }
+            else if (simplex.OneSimplex)
+            {
+                return HandleOneSimplex(a, simplex.A, simplex.B.Value);
+            }
+            else
+            {
+                return (false, simplex, Vector2.Zero);
+            }
+        }
+
+        private static (bool, Simplex2D, Vector2) HandleZeroSimplex(Vector2 a, Vector2 b)
+        {
+            var ab = b - a;
+            var a0 = -a;
+            var (newSimplex, newDirection) = SameDirection(ab, a0) ? (new Simplex2D(a, b), Perpendicular(ab, a0)) : (new Simplex2D(a), a0);
+            return (false, newSimplex, newDirection);
+        }
+
+        private static (bool, Simplex2D, Vector2) HandleOneSimplex(Vector2 a, Vector2 b, Vector2 c)
+        {
+            var a0 = -a;
+            var ab = b - a;
+            var ac = c - a;
+            var abp = Perpendicular(ab, -ac);
+            var acp = Perpendicular(ac, -ab);
+
+            if (SameDirection(abp, a0))
+            {
+                if (SameDirection(ab, a0))
+                {
+                    return (false, new Simplex2D(a, b), abp);
+                }
+                else
+                {
+                    return (false, new Simplex2D(a), a0);
+                }
+            }
+            else if (SameDirection(acp, a0))
+            {
+                if (SameDirection(ac, a0))
+                {
+                    return (false, new Simplex2D(a, c), acp);
+                }
+                else
+                {
+                    return (false, new Simplex2D(a), a0);
+                }
+            }
+            else
+            {
+                return (true, new Simplex2D(b, c), a0);
+            }
+        }
+
+        private static Vector2 TripleProduct(Vector2 a, Vector2 b, Vector2 c)
+        {
+            var A = new Vector3(a.X, a.Y, 0);
+            var B = new Vector3(b.X, b.Y, 0);
+            var C = new Vector3(c.X, c.Y, 0);
+
+            var first = Vector3.Cross(A, B);
+            var second = Vector3.Cross(first, C);
+
+            return new Vector2(second.X, second.Y);
+        }
+
+        private static Vector2 Direction(Vector2 a, Vector2 b)
+        {
+            var d = TripleProduct(a, b, a);
+            var collinear = d == Vector2.Zero;
+            return collinear ? new Vector2(a.Y, -a.X) : d;
+        }
+
+        private static bool SameDirection(Vector2 a, Vector2 b)
+        {
+            return Vector2.Dot(a, b) > 0;
+        }
+
+        private static Vector2 Perpendicular(Vector2 a, Vector2 b)
+        {
+            return TripleProduct(a, b, a);
+        }
+    }
+}

Test/EPA2DTest.cs

@@ -18,18 +18,18 @@ namespace Tests
             var squareB = new MoonTools.Core.Bonk.Rectangle(-1, -1, 1, 1);
             var transformB = new Transform2D(new Vector2(1.5f, 0));
 
-            var (result, simplex) = GJK2D.FindCollisionSimplex(squareA, transformA, squareB, transformB);
+            var (result, simplex) = NarrowPhase.FindCollisionSimplex(squareA, transformA, squareB, transformB);
 
             result.Should().BeTrue();
 
-            var intersection = EPA2D.Intersect(squareA, transformA, squareB, transformB, simplex);
+            var intersection = NarrowPhase.Intersect(squareA, transformA, squareB, transformB, simplex);
 
             intersection.X.Should().Be(1f);
             intersection.Y.Should().Be(0);
 
             var movedTransform = new Transform2D(transformA.Position - (intersection * 1.01f)); // move a tiny bit past
 
-            GJK2D.TestCollision(squareA, movedTransform, squareB, transformB).Should().BeFalse();
+            NarrowPhase.TestCollision(squareA, movedTransform, squareB, transformB).Should().BeFalse();
         }
 
         [Test]
@@ -40,11 +40,11 @@ namespace Tests
             var circleB = new Circle(1);
             var transformB = new Transform2D(new Vector2(1, 1));
 
-            var (result, simplex) = GJK2D.FindCollisionSimplex(circleA, transformA, circleB, transformB);
+            var (result, simplex) = NarrowPhase.FindCollisionSimplex(circleA, transformA, circleB, transformB);
 
             result.Should().BeTrue();
 
-            var intersection = EPA2D.Intersect(circleA, transformA, circleB, transformB, simplex);
+            var intersection = NarrowPhase.Intersect(circleA, transformA, circleB, transformB, simplex);
 
             var ix = (circleA.Radius * (float)Math.Cos(Math.PI / 4)) - ((circleB.Radius * (float)Math.Cos(5 * Math.PI / 4)) + transformB.Position.X);
             var iy = (circleA.Radius * (float)Math.Sin(Math.PI / 4)) - ((circleB.Radius * (float)Math.Sin(5 * Math.PI / 4)) + transformB.Position.Y);
@@ -54,7 +54,7 @@ namespace Tests
 
             var movedTransform = new Transform2D(transformA.Position - (intersection * 1.01f)); // move a tiny bit past
 
-            GJK2D.TestCollision(circleA, movedTransform, circleB, transformB).Should().BeFalse();
+            NarrowPhase.TestCollision(circleA, movedTransform, circleB, transformB).Should().BeFalse();
         }
 
         [Test]
@@ -65,15 +65,15 @@ namespace Tests
             var square = new MoonTools.Core.Bonk.Rectangle(-1, -1, 1, 1);
             var transformB = Transform2D.DefaultTransform;
 
-            var (result, simplex) = GJK2D.FindCollisionSimplex(line, transformA, square, transformB);
+            var (result, simplex) = NarrowPhase.FindCollisionSimplex(line, transformA, square, transformB);
 
             result.Should().BeTrue();
 
-            var intersection = EPA2D.Intersect(line, transformA, square, transformB, simplex);
+            var intersection = NarrowPhase.Intersect(line, transformA, square, transformB, simplex);
 
             var movedTransform = new Transform2D(transformA.Position - (intersection * 1.01f)); // move a tiny bit past
 
-            GJK2D.TestCollision(line, movedTransform, square, transformB).Should().BeFalse();
+            NarrowPhase.TestCollision(line, movedTransform, square, transformB).Should().BeFalse();
         }
     }
 }

Test/GJK2DTest.cs

@@ -16,7 +16,7 @@ namespace Tests
             var pointTransform = new Transform2D(new Position2D(4, 4));
             var line = new Line(new Position2D(-2, -2), new Position2D(2, 2));
 
-            GJK2D.TestCollision(point, pointTransform, line, Transform2D.DefaultTransform).Should().BeTrue();
+            NarrowPhase.TestCollision(point, pointTransform, line, Transform2D.DefaultTransform).Should().BeTrue();
         }
 
         [Test]
@@ -25,7 +25,7 @@ namespace Tests
             var point = new Point(1, 1);
             var line = new Line(new Position2D(-3, -2), new Position2D(-9, -5));
 
-            GJK2D.TestCollision(point, Transform2D.DefaultTransform, line, Transform2D.DefaultTransform).Should().BeFalse();
+            NarrowPhase.TestCollision(point, Transform2D.DefaultTransform, line, Transform2D.DefaultTransform).Should().BeFalse();
         }
 
         [Test]
@@ -37,7 +37,7 @@ namespace Tests
             var pointTransform = new Transform2D(new Position2D(1, 1));
             var circleTransform = new Transform2D(new Position2D(-1, 0));
 
-            GJK2D.TestCollision(point, pointTransform, circle, circleTransform).Should().BeTrue();
+            NarrowPhase.TestCollision(point, pointTransform, circle, circleTransform).Should().BeTrue();
         }
 
         [Test]
@@ -47,7 +47,7 @@ namespace Tests
             var pointTransform = new Transform2D(new Position2D(3, 0));
             var circle = new Circle(1);
 
-            GJK2D.TestCollision(point, pointTransform, circle, Transform2D.DefaultTransform).Should().BeFalse();
+            NarrowPhase.TestCollision(point, pointTransform, circle, Transform2D.DefaultTransform).Should().BeFalse();
         }
 
         [Test]
@@ -56,7 +56,7 @@ namespace Tests
             var point = new Point(1, 1);
             var rectangle = new Rectangle(-2, -2, 2, 2);
 
-            GJK2D.TestCollision(point, Transform2D.DefaultTransform, rectangle, Transform2D.DefaultTransform).Should().BeTrue();
+            NarrowPhase.TestCollision(point, Transform2D.DefaultTransform, rectangle, Transform2D.DefaultTransform).Should().BeTrue();
         }
 
         [Test]
@@ -65,7 +65,7 @@ namespace Tests
             var point = new Point(5, 5);
             var rectangle = new Rectangle(-2, -2, 2, 2);
 
-            GJK2D.TestCollision(point, Transform2D.DefaultTransform, rectangle, Transform2D.DefaultTransform).Should().BeFalse();
+            NarrowPhase.TestCollision(point, Transform2D.DefaultTransform, rectangle, Transform2D.DefaultTransform).Should().BeFalse();
         }
 
         [Test]
@@ -79,7 +79,7 @@ namespace Tests
                 new Position2D(3, -2)
             ));
 
-            GJK2D.TestCollision(point, Transform2D.DefaultTransform, polygon, Transform2D.DefaultTransform).Should().BeTrue();
+            NarrowPhase.TestCollision(point, Transform2D.DefaultTransform, polygon, Transform2D.DefaultTransform).Should().BeTrue();
         }
 
         [Test]
@@ -93,7 +93,7 @@ namespace Tests
                 new Position2D(3, -2)
             ));
 
-            GJK2D.TestCollision(point, Transform2D.DefaultTransform, polygon, Transform2D.DefaultTransform).Should().BeFalse();
+            NarrowPhase.TestCollision(point, Transform2D.DefaultTransform, polygon, Transform2D.DefaultTransform).Should().BeFalse();
         }
 
         [Test]
@@ -102,7 +102,7 @@ namespace Tests
             var lineA = new Line(new Position2D(-1, -1), new Position2D(1, 1));
             var lineB = new Line(new Position2D(-1, 1), new Position2D(1, -1));
 
-            GJK2D.TestCollision(lineA, Transform2D.DefaultTransform, lineB, Transform2D.DefaultTransform).Should().BeTrue();
+            NarrowPhase.TestCollision(lineA, Transform2D.DefaultTransform, lineB, Transform2D.DefaultTransform).Should().BeTrue();
         }
 
         [Test]
@@ -113,7 +113,7 @@ namespace Tests
 
             var transform = new Transform2D(new Position2D(0, 0), 0f, new Vector2(2, 2));
 
-            GJK2D.TestCollision(lineA, transform, lineB, transform).Should().BeTrue();
+            NarrowPhase.TestCollision(lineA, transform, lineB, transform).Should().BeTrue();
         }
 
         [Test]
@@ -122,7 +122,7 @@ namespace Tests
             var lineA = new Line(new Position2D(0, 1), new Position2D(1, 0));
             var lineB = new Line(new Position2D(-1, -1), new Position2D(-2, -2));
 
-            GJK2D.TestCollision(lineA, Transform2D.DefaultTransform, lineB, Transform2D.DefaultTransform).Should().BeFalse();
+            NarrowPhase.TestCollision(lineA, Transform2D.DefaultTransform, lineB, Transform2D.DefaultTransform).Should().BeFalse();
         }
 
         [Test]
@@ -133,7 +133,7 @@ namespace Tests
 
             var transform = new Transform2D(new Position2D(0, 0), 0f, new Vector2(2, 2));
 
-            GJK2D.TestCollision(lineA, transform, lineB, transform).Should().BeFalse();
+            NarrowPhase.TestCollision(lineA, transform, lineB, transform).Should().BeFalse();
         }
 
         [Test]
@@ -144,7 +144,7 @@ namespace Tests
             var circleB = new Circle(2);
             var transformB = new Transform2D(new Vector2(1, 1));
 
-            GJK2D.TestCollision(circleA, transformA, circleB, transformB).Should().BeTrue();
+            NarrowPhase.TestCollision(circleA, transformA, circleB, transformB).Should().BeTrue();
         }
 
         [Test]
@@ -155,7 +155,7 @@ namespace Tests
             var circleB = new Circle(2);
             var transformB = new Transform2D(new Vector2(3, 0), 0f, new Vector2(2, 2));
 
-            GJK2D.TestCollision(circleA, transformA, circleB, transformB).Should().BeTrue();
+            NarrowPhase.TestCollision(circleA, transformA, circleB, transformB).Should().BeTrue();
         }
 
         [Test]
@@ -166,7 +166,7 @@ namespace Tests
             var circleB = new Circle(2);
             var transformB = new Transform2D(new Vector2(5, 5));
 
-            GJK2D.TestCollision(circleA, transformA, circleB, transformB).Should().BeFalse();
+            NarrowPhase.TestCollision(circleA, transformA, circleB, transformB).Should().BeFalse();
         }
 
         [Test]
@@ -177,7 +177,7 @@ namespace Tests
             var circleB = new Circle(2);
             var transformB = new Transform2D(new Vector2(5, 5), 0, new Vector2(0.2f, 0.2f));
 
-            GJK2D.TestCollision(circleA, transformA, circleB, transformB).Should().BeFalse();
+            NarrowPhase.TestCollision(circleA, transformA, circleB, transformB).Should().BeFalse();
         }
 
         [Test]
@@ -197,7 +197,7 @@ namespace Tests
 
             var transformB = new Transform2D(new Vector2(0.5f, 0.5f));
 
-            GJK2D.TestCollision(shapeA, transformA, shapeB, transformB).Should().BeTrue();
+            NarrowPhase.TestCollision(shapeA, transformA, shapeB, transformB).Should().BeTrue();
         }
 
         [Test]
@@ -217,7 +217,7 @@ namespace Tests
 
             var transformB = new Transform2D(new Vector2(3f, 0f), 0f, new Vector2(3f, 3f));
 
-            GJK2D.TestCollision(shapeA, transformA, shapeB, transformB).Should().BeTrue();
+            NarrowPhase.TestCollision(shapeA, transformA, shapeB, transformB).Should().BeTrue();
         }
 
         [Test]
@@ -237,7 +237,7 @@ namespace Tests
 
             var transformB = new Transform2D(new Vector2(5, 0));
 
-            GJK2D.TestCollision(shapeA, transformA, shapeB, transformB).Should().BeFalse();
+            NarrowPhase.TestCollision(shapeA, transformA, shapeB, transformB).Should().BeFalse();
         }
 
         [Test]
@@ -257,7 +257,7 @@ namespace Tests
 
             var transformB = new Transform2D(new Vector2(3f, 0), 0f, new Vector2(0.5f, 0.5f));
 
-            GJK2D.TestCollision(shapeA, transformA, shapeB, transformB).Should().BeFalse();
+            NarrowPhase.TestCollision(shapeA, transformA, shapeB, transformB).Should().BeFalse();
         }
 
         [Test]
@@ -274,7 +274,7 @@ namespace Tests
 
             var transformB = Transform2D.DefaultTransform;
 
-            GJK2D.TestCollision(line, transformA, polygon, transformB).Should().BeTrue();
+            NarrowPhase.TestCollision(line, transformA, polygon, transformB).Should().BeTrue();
         }
 
         [Test]
@@ -291,7 +291,7 @@ namespace Tests
 
             var transformB = Transform2D.DefaultTransform;
 
-            GJK2D.TestCollision(line, transformA, polygon, transformB).Should().BeFalse();
+            NarrowPhase.TestCollision(line, transformA, polygon, transformB).Should().BeFalse();
         }
 
         [Test]
@@ -302,7 +302,7 @@ namespace Tests
             var circle = new Circle(1);
             var transformB = Transform2D.DefaultTransform;
 
-            GJK2D.TestCollision(line, transformA, circle, transformB).Should().BeTrue();
+            NarrowPhase.TestCollision(line, transformA, circle, transformB).Should().BeTrue();
         }
 
         [Test]
@@ -313,7 +313,7 @@ namespace Tests
             var circle = new Circle(1);
             var transformB = Transform2D.DefaultTransform;
 
-            GJK2D.TestCollision(line, transformA, circle, transformB).Should().BeFalse();
+            NarrowPhase.TestCollision(line, transformA, circle, transformB).Should().BeFalse();
         }
 
         [Test]
@@ -329,7 +329,7 @@ namespace Tests
 
             var transformB = Transform2D.DefaultTransform;
 
-            GJK2D.TestCollision(circle, transformA, square, transformB).Should().BeTrue();
+            NarrowPhase.TestCollision(circle, transformA, square, transformB).Should().BeTrue();
         }
 
         [Test]
@@ -344,7 +344,7 @@ namespace Tests
             ));
             var squareTransform = Transform2D.DefaultTransform;
 
-            GJK2D.TestCollision(circle, circleTransform, square, squareTransform).Should().BeFalse();
+            NarrowPhase.TestCollision(circle, circleTransform, square, squareTransform).Should().BeFalse();
         }
 
         [Test]
@@ -356,7 +356,7 @@ namespace Tests
             var rectangleB = new MoonTools.Core.Bonk.Rectangle(0, 0, 16, 16);
             var transformB = new Transform2D(new Position2D(16, 240));
 
-            GJK2D.TestCollision(rectangleA, transformA, rectangleB, transformB).Should().BeFalse();
+            NarrowPhase.TestCollision(rectangleA, transformA, rectangleB, transformB).Should().BeFalse();
         }
 
         [Test]
@@ -368,11 +368,11 @@ namespace Tests
             var rectangleB = new MoonTools.Core.Bonk.Rectangle(-1, -1, 1, 1);
             var transformB = new Transform2D(new Vector2(1, 0));
 
-            GJK2D.TestCollision(rectangleA, transformA, rectangleB, transformB).Should().BeTrue();
+            NarrowPhase.TestCollision(rectangleA, transformA, rectangleB, transformB).Should().BeTrue();
         }
 
         [Test]
-        public void RectanglesTouching()
+        public void RectanglesTouchingGJK2D()
         {
             var rectangleA = new MoonTools.Core.Bonk.Rectangle(-1, -1, 1, 1);
             var transformA = new Transform2D(new Position2D(-1, 0));
@@ -380,7 +380,43 @@ namespace Tests
             var rectangleB = new MoonTools.Core.Bonk.Rectangle(-1, -1, 1, 1);
             var transformB = new Transform2D(new Vector2(1, 0));
 
-            GJK2D.TestCollision(rectangleA, transformA, rectangleB, transformB).Should().BeTrue();
+            NarrowPhase.TestCollision(rectangleA, transformA, rectangleB, transformB).Should().BeTrue();
+        }
+
+        [Test]
+        public void RectanglesOverlappingGJK2D()
+        {
+            var rectangleA = new MoonTools.Core.Bonk.Rectangle(-1, -1, 1, 1);
+            var transformA = new Transform2D(new Position2D(0, 0));
+
+            var rectangleB = new MoonTools.Core.Bonk.Rectangle(-1, -1, 1, 1);
+            var transformB = new Transform2D(new Vector2(1, 0));
+
+            NarrowPhase.TestCollision(rectangleA, transformA, rectangleB, transformB).Should().BeTrue();
+        }
+
+        [Test]
+        public void RectanglesTouchingOverlap()
+        {
+            var rectangleA = new MoonTools.Core.Bonk.Rectangle(-1, -1, 1, 1);
+            var transformA = new Transform2D(new Position2D(-1, 0));
+
+            var rectangleB = new MoonTools.Core.Bonk.Rectangle(-1, -1, 1, 1);
+            var transformB = new Transform2D(new Vector2(1, 0));
+
+            NarrowPhase.TestRectangleOverlap(rectangleA, transformA, rectangleB, transformB).Should().BeTrue();
+        }
+
+        [Test]
+        public void RectanglesOverlappingOverlap()
+        {
+            var rectangleA = new MoonTools.Core.Bonk.Rectangle(-1, -1, 1, 1);
+            var transformA = new Transform2D(new Position2D(0, 0));
+
+            var rectangleB = new MoonTools.Core.Bonk.Rectangle(-1, -1, 1, 1);
+            var transformB = new Transform2D(new Vector2(1, 0));
+
+            NarrowPhase.TestRectangleOverlap(rectangleA, transformA, rectangleB, transformB).Should().BeTrue();
         }
     }
 }