testing generic transformed shapes

Bonk/BroadPhase/SpatialHash.cs

@@ -14,7 +14,7 @@ namespace MoonTools.Core.Bonk
         private readonly int cellSize;
 
         private readonly Dictionary<long, HashSet<T>> hashDictionary = new Dictionary<long, HashSet<T>>();
-        private readonly Dictionary<T, (IHasAABB2D, Transform2D)> IDLookup = new Dictionary<T, (IHasAABB2D, Transform2D)>();
+        private readonly Dictionary<T, IHasAABB2D> IDLookup = new Dictionary<T, IHasAABB2D>();
 
         public SpatialHash(int cellSize)
         {
@@ -31,10 +31,9 @@ namespace MoonTools.Core.Bonk
         /// </summary>
         /// <param name="id">A unique ID for the shape-transform pair.</param>
         /// <param name="shape"></param>
-        /// <param name="transform2D"></param>
+        public void Insert(T id, IHasAABB2D shape)
-        public void Insert(T id, IHasAABB2D shape, Transform2D transform2D)
         {
-            var box = shape.TransformedAABB(transform2D);
+            var box = shape.AABB;
             var minHash = Hash(box.Min);
             var maxHash = Hash(box.Max);
 
@@ -57,9 +56,9 @@ namespace MoonTools.Core.Bonk
         /// <summary>
         /// Retrieves all the potential collisions of a shape-transform pair. Excludes any shape-transforms with the given ID.
         /// </summary>
-        public IEnumerable<(T, IHasAABB2D, Transform2D)> Retrieve(T id, IHasAABB2D shape, Transform2D transform2D)
+        public IEnumerable<(T, IHasAABB2D)> Retrieve(T id, IHasAABB2D shape)
         {
-            var box = shape.TransformedAABB(transform2D);
+            var box = shape.AABB;
             var minHash = Hash(box.Min);
             var maxHash = Hash(box.Max);
 
@@ -72,10 +71,40 @@ namespace MoonTools.Core.Bonk
                     {
                         foreach (var t in hashDictionary[key])
                         {
-                            var (otherShape, otherTransform) = IDLookup[t];
+                            var otherShape = IDLookup[t];
-                            if (!id.Equals(t) && AABB.TestOverlap(shape.TransformedAABB(transform2D), otherShape.TransformedAABB(otherTransform)))
+                            if (!id.Equals(t) && AABB.TestOverlap(shape.AABB, otherShape.AABB))
                             {
-                                yield return (t, otherShape, otherTransform);
+                                yield return (t, otherShape);
+                            }
+                        }
+                    }
+                }
+            }
+        }
+
+        /// <summary>
+        /// Retrieves objects based on a pre-transformed AABB.
+        /// </summary>
+        /// <param name="aabb">A transformed AABB.</param>
+        /// <returns></returns>
+        public IEnumerable<(T, IHasAABB2D)> Retrieve(AABB aabb)
+        {
+            var minHash = Hash(aabb.Min);
+            var maxHash = Hash(aabb.Max);
+
+            for (var i = minHash.Item1; i <= maxHash.Item1; i++)
+            {
+                for (var j = minHash.Item2; j <= maxHash.Item2; j++)
+                {
+                    var key = MakeLong(i, j);
+                    if (hashDictionary.ContainsKey(key))
+                    {
+                        foreach (var t in hashDictionary[key])
+                        {
+                            var otherShape = IDLookup[t];
+                            if (AABB.TestOverlap(aabb, otherShape.AABB))
+                            {
+                                yield return (t, otherShape);
                             }
                         }
                     }

Bonk/ICollisionTestable.cs

@@ -0,0 +1,15 @@
+using System.Collections.Generic;
+
+namespace MoonTools.Core.Bonk
+{
+    public interface ICollisionTestable
+    {
+        bool TestCollision<T>(ICollisionTestable collisionTestable) where T : struct, IShape2D;
+        IEnumerable<TransformedShape2D<T>> TransformedShapes<T>() where T : struct, IShape2D;
+    }
+
+    public interface ICollisionTestable<T> : ICollisionTestable where T : struct, IShape2D
+    {
+        IEnumerable<TransformedShape2D<T>> TransformedShapes { get; }
+    }
+}

Bonk/IHasAABB2D.cs

@@ -1,17 +1,7 @@
-using System;
+namespace MoonTools.Core.Bonk
-using MoonTools.Core.Structs;
-
-namespace MoonTools.Core.Bonk
 {
     public interface IHasAABB2D
     {
         AABB AABB { get; }
-
-        /// <summary>
-        /// Returns a bounding box based on the shape.
-        /// </summary>
-        /// <param name="transform">A Transform for transforming the shape vertices.</param>
-        /// <returns>Returns a bounding box based on the shape.</returns>
-        AABB TransformedAABB(Transform2D transform);
     }
 }

Bonk/IShape2D.cs

@@ -13,5 +13,7 @@ namespace MoonTools.Core.Bonk
         /// <param name="transform">A Transform for transforming the shape vertices.</param>
         /// <returns>The farthest point on the edge of the shape along the given direction.</returns>
         Vector2 Support(Vector2 direction, Transform2D transform);
+
+        AABB TransformedAABB(Transform2D transform);
     }
 }

Bonk/MinkowskiDifference.cs

@@ -1,57 +1,50 @@
 using System;
 using System.Numerics;
-using MoonTools.Core.Structs;
 
 namespace MoonTools.Core.Bonk
 {
     /// <summary>
     /// A Minkowski difference between two shapes.
     /// </summary>
-    public struct MinkowskiDifference : IEquatable<MinkowskiDifference>
+    public struct MinkowskiDifference<T, U> : IEquatable<MinkowskiDifference<T, U>> where T : struct, IShape2D where U : struct, IShape2D
     {
-        private IShape2D ShapeA { get; }
+        private TransformedShape2D<T> ShapeA { get; }
-        private Transform2D TransformA { get; }
+        private TransformedShape2D<U> ShapeB { get; }
-        private IShape2D ShapeB { get; }
-        private Transform2D TransformB { get; }
 
-        public MinkowskiDifference(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB)
+        public MinkowskiDifference(TransformedShape2D<T> shapeA, TransformedShape2D<U> shapeB)
         {
             ShapeA = shapeA;
-            TransformA = transformA;
             ShapeB = shapeB;
-            TransformB = transformB;
         }
 
         public Vector2 Support(Vector2 direction)
         {
-            return ShapeA.Support(direction, TransformA) - ShapeB.Support(-direction, TransformB);
+            return ShapeA.Support(direction) - ShapeB.Support(-direction);
         }
 
         public override bool Equals(object other)
         {
-            return other is MinkowskiDifference minkowskiDifference && Equals(minkowskiDifference);
+            return other is MinkowskiDifference<T, U> minkowskiDifference && Equals(minkowskiDifference);
         }
 
-        public bool Equals(MinkowskiDifference other)
+        public bool Equals(MinkowskiDifference<T, U> other)
         {
             return
                 ShapeA == other.ShapeA &&
-                TransformA == other.TransformA &&
+                ShapeB == other.ShapeB;
-                ShapeB == other.ShapeB &&
-                TransformB == other.TransformB;
         }
 
         public override int GetHashCode()
         {
-            return HashCode.Combine(ShapeA, TransformA, ShapeB, TransformB);
+            return HashCode.Combine(ShapeA, ShapeB);
         }
 
-        public static bool operator ==(MinkowskiDifference a, MinkowskiDifference b)
+        public static bool operator ==(MinkowskiDifference<T, U> a, MinkowskiDifference<T, U> b)
         {
             return a.Equals(b);
         }
 
-        public static bool operator !=(MinkowskiDifference a, MinkowskiDifference b)
+        public static bool operator !=(MinkowskiDifference<T, U> a, MinkowskiDifference<T, U> b)
         {
             return !(a == b);
         }

Bonk/MultiShape.cs

@@ -4,17 +4,30 @@ using MoonTools.Core.Structs;
 
 namespace MoonTools.Core.Bonk
 {
-    public struct MultiShape : IHasAABB2D
+    public struct MultiShape<TShape2D> : IHasAABB2D, ICollisionTestable<TShape2D> where TShape2D : struct, IShape2D
     {
-        public ImmutableArray<(IShape2D, Transform2D)> ShapeTransformPairs { get; }
+        private static ImmutableArray<TransformedShape2D<TShape2D>>.Builder _builder = ImmutableArray.CreateBuilder<TransformedShape2D<TShape2D>>();
+        private ImmutableArray<TransformedShape2D<TShape2D>> _transformedShapes;
+        public IEnumerable<TransformedShape2D<TShape2D>> TransformedShapes { get { return _transformedShapes; } }
 
         public AABB AABB { get; }
 
-        public MultiShape(ImmutableArray<(IShape2D, Transform2D)> shapeTransformPairs)
+        public MultiShape(ImmutableArray<(TShape2D, Transform2D)> shapeTransformPairs)
         {
-            ShapeTransformPairs = shapeTransformPairs;
+            _builder.Clear();
+            foreach (var (shape, transform) in shapeTransformPairs)
+            {
+                _builder.Add(new TransformedShape2D<TShape2D>(shape, transform));
+            }
 
-            AABB = AABBFromShapes(shapeTransformPairs);
+            _transformedShapes = _builder.ToImmutable();
+            AABB = AABBFromShapes(_transformedShapes);
+        }
+
+        public MultiShape(ImmutableArray<TransformedShape2D<TShape2D>> transformedShapes)
+        {
+            _transformedShapes = transformedShapes;
+            AABB = AABBFromShapes(transformedShapes);
         }
 
         public AABB TransformedAABB(Transform2D transform)
@@ -22,24 +35,24 @@ namespace MoonTools.Core.Bonk
             return AABB.Transformed(AABB, transform);
         }
 
-        public IEnumerable<(IShape2D, Transform2D)> TransformedShapeTransforms(Transform2D transform)
+        public IEnumerable<TransformedShape2D<TShape2D>> Compose(Transform2D transform)
         {
-            foreach (var (shape, shapeTransform) in ShapeTransformPairs)
+            foreach (var transformedShape in TransformedShapes)
             {
-                yield return (shape, transform.Compose(shapeTransform));
+                yield return transformedShape.Compose(transform);
             }
         }
 
-        private static AABB AABBFromShapes(IEnumerable<(IShape2D, Transform2D)> shapeTransforms)
+        private static AABB AABBFromShapes(IEnumerable<TransformedShape2D<TShape2D>> transformedShapes)
         {
             var minX = float.MaxValue;
             var minY = float.MaxValue;
             var maxX = float.MinValue;
             var maxY = float.MinValue;
 
-            foreach (var (shape, transform) in shapeTransforms)
+            foreach (var transformedShape in transformedShapes)
             {
-                var aabb = shape.TransformedAABB(transform);
+                var aabb = transformedShape.AABB;
 
                 if (aabb.Min.X < minX)
                 {

Bonk/NarrowPhase/NarrowPhase.cs

@@ -1,4 +1,6 @@
 using MoonTools.Core.Structs;
+using System;
+using System.Collections.Generic;
 using System.Numerics;
 
 namespace MoonTools.Core.Bonk
@@ -11,52 +13,37 @@ namespace MoonTools.Core.Bonk
             CounterClockwise
         }
 
-        /// <summary>
+        public static bool TestCollision<T, U>(ICollisionTestable<T> a, ICollisionTestable<U> b) where T : struct, IShape2D where U : struct, IShape2D
-        /// Tests if two shape-transform pairs are overlapping. Automatically detects fast-path optimizations.
-        /// </summary>
-        public static bool TestCollision(IHasAABB2D hasBoundingBoxA, Transform2D transformA, IHasAABB2D hasBoundingBoxB, Transform2D transformB)
         {
-            if (hasBoundingBoxA is MultiShape && hasBoundingBoxB is MultiShape)
+            foreach (var shape in a.TransformedShapes)
             {
-                return TestCollision((MultiShape)hasBoundingBoxA, transformA, (MultiShape)hasBoundingBoxB, transformB);
+                foreach (var shapeB in b.TransformedShapes)
+                {
+                    return TestCollision(shape, shapeB);
                 }
-            else if (hasBoundingBoxA is MultiShape && hasBoundingBoxB is IShape2D)
-            {
-                return TestCollision((MultiShape)hasBoundingBoxA, transformA, (IShape2D)hasBoundingBoxB, transformB);
-            }
-            else if (hasBoundingBoxA is IShape2D && hasBoundingBoxB is MultiShape)
-            {
-                return TestCollision((IShape2D)hasBoundingBoxA, transformA, (MultiShape)hasBoundingBoxB, transformB);
-            }
-            else if (hasBoundingBoxA is IShape2D && hasBoundingBoxB is IShape2D)
-            {
-                return TestCollision((IShape2D)hasBoundingBoxA, transformA, (IShape2D)hasBoundingBoxB, transformB);
-            }
-            else
-            {
-                throw new System.ArgumentException("Collision testing requires MultiShapes or IShape2Ds.");
             }
+            return false;
         }
 
-        public static bool TestCollision(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB)
+        /// <summary>
+        /// Tests if two shape-transform pairs are overlapping.
+        /// </summary>
+        public static bool TestCollision<T, U>(T shape, Transform2D transform, U shapeB, Transform2D transformB) where T : struct, IShape2D where U : struct, IShape2D
         {
-            if (shapeA is Rectangle rectangleA && shapeB is Rectangle rectangleB && transformA.Rotation == 0 && transformB.Rotation == 0)
+            return TestCollision(new TransformedShape2D<T>(shape, transform), new TransformedShape2D<U>(shapeB, transformB));
-            {
-                return TestRectangleOverlap(rectangleA, transformA, rectangleB, transformB);
         }
-            else if (shapeA is Point && shapeB is Rectangle && transformB.Rotation == 0)
+
+        /// <summary>
+        /// Tests if two TransformedShapes are overlapping.
+        /// </summary>
+        /// <typeparam name="T"></typeparam>
+        /// <typeparam name="U"></typeparam>
+        /// <param name="transformedShapeA"></param>
+        /// <param name="transformedShapeB"></param>
+        /// <returns></returns>
+        public static bool TestCollision<T, U>(TransformedShape2D<T> transformedShapeA, TransformedShape2D<U> transformedShapeB) where T : struct, IShape2D where U : struct, IShape2D
         {
-                return TestPointRectangleOverlap((Point)shapeA, transformA, (Rectangle)shapeB, transformB);
+            return FindCollisionSimplex(transformedShapeA, transformedShapeB).Item1;
-            }
-            else if (shapeA is Rectangle && shapeB is Point && transformA.Rotation == 0)
-            {
-                return TestPointRectangleOverlap((Point)shapeB, transformB, (Rectangle)shapeA, transformA);
-            }
-            else if (shapeA is Circle circleA && shapeB is Circle circleB && transformA.Scale.X == transformA.Scale.Y && transformB.Scale.X == transformB.Scale.Y)
-            {
-                return TestCircleOverlap(circleA, transformA, circleB, transformB);
-            }
-            return FindCollisionSimplex(shapeA, transformA, shapeB, transformB).Item1;
         }
 
         /// <summary>
@@ -66,13 +53,21 @@ namespace MoonTools.Core.Bonk
         /// <param name="multiShape"></param>
         /// <param name="multiShapeTransform"></param>
         /// <param name="shape"></param>
-        /// <param name="shapeTransform"></param>
         /// <returns></returns>
-        public static bool TestCollision(MultiShape multiShape, Transform2D multiShapeTransform, IShape2D shape, Transform2D shapeTransform)
+        public static bool TestCollision<T, U>(MultiShape<T> multiShape, Transform2D multiShapeTransform, TransformedShape2D<U> shape) where T : struct, IShape2D where U : struct, IShape2D
         {
-            foreach (var (otherShape, otherTransform) in multiShape.ShapeTransformPairs)
+            foreach (var transformedShape in multiShape.Compose(multiShapeTransform))
             {
-                if (TestCollision(shape, shapeTransform, otherShape, multiShapeTransform.Compose(otherTransform))) { return true; }
+                if (TestCollision(shape, transformedShape)) { return true; }
+            }
+            return false;
+        }
+
+        public static bool TestCollison<T>(IEnumerable<TransformedShape2D<T>> transformedShapes, Transform2D multiShapeTransform, TransformedShape2D<U> shape) where T : struct, IShape2D where U : struct, IShape2D
+        {
+            foreach (var transformedShape in transformedShapes)
+            {
+                if (TestCollision(transformedShape.Compose(multiShapeTransform), shape)) { return true; }
             }
             return false;
         }
@@ -84,15 +79,10 @@ namespace MoonTools.Core.Bonk
         /// <param name="multiShape"></param>
         /// <param name="multiShapeTransform"></param>
         /// <param name="shape"></param>
-        /// <param name="shapeTransform"></param>
         /// <returns></returns>
-        public static bool TestCollision(IShape2D shape, Transform2D shapeTransform, MultiShape multiShape, Transform2D multiShapeTransform)
+        public static bool TestCollision<T, U>(TransformedShape2D<T> shape, MultiShape<U> multiShape, Transform2D multiShapeTransform) where T : struct, IShape2D where U : struct, IShape2D
         {
-            foreach (var (otherShape, otherTransform) in multiShape.ShapeTransformPairs)
+            return TestCollision(multiShape, multiShapeTransform, shape);
-            {
-                if (TestCollision(shape, shapeTransform, otherShape, multiShapeTransform.Compose(otherTransform))) { return true; }
-            }
-            return false;
         }
 
         /// <summary>
@@ -104,13 +94,13 @@ namespace MoonTools.Core.Bonk
         /// <param name="multiShapeB"></param>
         /// <param name="transformB"></param>
         /// <returns></returns>
-        public static bool TestCollision(MultiShape multiShapeA, Transform2D transformA, MultiShape multiShapeB, Transform2D transformB)
+        public static bool TestCollision<T, U>(MultiShape<T> multiShapeA, Transform2D transformA, MultiShape<U> multiShapeB, Transform2D transformB) where T : struct, IShape2D where U : struct, IShape2D
         {
-            foreach (var (shapeA, shapeTransformA) in multiShapeA.ShapeTransformPairs)
+            foreach (var transformedShapeA in multiShapeA.Compose(transformA))
             {
-                foreach (var (shapeB, shapeTransformB) in multiShapeB.ShapeTransformPairs)
+                foreach (var transformedShapeB in multiShapeB.Compose(transformB))
                 {
-                    if (TestCollision(shapeA, transformA.Compose(shapeTransformA), shapeB, transformB.Compose(shapeTransformB))) { return true; }
+                    if (TestCollision(transformedShapeA, transformedShapeB)) { return true; }
                 }
             }
             return false;
@@ -120,14 +110,12 @@ namespace MoonTools.Core.Bonk
         /// Fast path for axis-aligned 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)
+        public static bool TestCollision(TransformedShape2D<Rectangle> rectangleA, TransformedShape2D<Rectangle> rectangleB)
         {
-            var firstAABB = rectangleA.TransformedAABB(transformA);
+            var firstAABB = rectangleA.AABB;
-            var secondAABB = rectangleB.TransformedAABB(transformB);
+            var secondAABB = rectangleB.AABB;
 
             return firstAABB.Left <= secondAABB.Right && firstAABB.Right >= secondAABB.Left && firstAABB.Top <= secondAABB.Bottom && firstAABB.Bottom >= secondAABB.Top;
         }
@@ -136,33 +124,34 @@ namespace MoonTools.Core.Bonk
         /// Fast path for overlapping point and axis-aligned rectangle. The rectangle transform must have non-zero rotation.
         /// </summary>
         /// <param name="point"></param>
-        /// <param name="pointTransform"></param>
         /// <param name="rectangle"></param>
-        /// <param name="rectangleTransform"></param>
         /// <returns></returns>
-        public static bool TestPointRectangleOverlap(Point point, Transform2D pointTransform, Rectangle rectangle, Transform2D rectangleTransform)
+        public static bool TestCollision(TransformedShape2D<Point> point, TransformedShape2D<Rectangle> rectangle)
         {
-            var transformedPoint = pointTransform.Position;
+            var transformedPoint = point.Transform.Position;
-            var AABB = rectangle.TransformedAABB(rectangleTransform);
+            var AABB = rectangle.AABB;
 
             return transformedPoint.X >= AABB.Left && transformedPoint.X <= AABB.Right && transformedPoint.Y <= AABB.Bottom && transformedPoint.Y >= AABB.Top;
         }
 
+        public static bool TestCollision(TransformedShape2D<Rectangle> rectangle, TransformedShape2D<Point> point)
+        {
+            return TestCollision(point, rectangle);
+        }
+
         /// <summary>
         /// Fast path for overlapping circles. The circles must have uniform scaling.
         /// </summary>
         /// <param name="circleA"></param>
-        /// <param name="transformA"></param>
         /// <param name="circleB"></param>
-        /// <param name="transformB"></param>
         /// <returns></returns>
-        public static bool TestCircleOverlap(Circle circleA, Transform2D transformA, Circle circleB, Transform2D transformB)
+        public static bool TestCollision(TransformedShape2D<Circle> circleA, TransformedShape2D<Circle> circleB)
         {
-            var radiusA = circleA.Radius * transformA.Scale.X;
+            var radiusA = circleA.Shape.Radius * circleA.Transform.Scale.X;
-            var radiusB = circleB.Radius * transformB.Scale.Y;
+            var radiusB = circleB.Shape.Radius * circleB.Transform.Scale.Y;
 
-            var centerA = transformA.Position;
+            var centerA = circleA.Transform.Position;
-            var centerB = transformB.Position;
+            var centerB = circleB.Transform.Position;
 
             var distanceSquared = (centerA - centerB).LengthSquared();
             var radiusSumSquared = (radiusA + radiusB) * (radiusA + radiusB);
@@ -173,9 +162,9 @@ namespace MoonTools.Core.Bonk
         /// <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)
+        public static (bool, Simplex2D) FindCollisionSimplex<T, U>(TransformedShape2D<T> shapeA, TransformedShape2D<U> shapeB) where T : struct, IShape2D where U : struct, IShape2D
         {
-            var minkowskiDifference = new MinkowskiDifference(shapeA, transformA, shapeB, transformB);
+            var minkowskiDifference = new MinkowskiDifference<T, U>(shapeA, shapeB);
             var c = minkowskiDifference.Support(Vector2.UnitX);
             var b = minkowskiDifference.Support(-Vector2.UnitX);
             return Check(minkowskiDifference, c, b);
@@ -185,7 +174,7 @@ namespace MoonTools.Core.Bonk
         /// 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)
+        public unsafe static Vector2 Intersect<T, U>(TransformedShape2D<T> shapeA, TransformedShape2D<U> shapeB, Simplex2D simplex) where T : struct, IShape2D where U : struct, IShape2D
         {
             if (shapeA == null) { throw new System.ArgumentNullException(nameof(shapeA)); }
             if (shapeB == null) { throw new System.ArgumentNullException(nameof(shapeB)); }
@@ -207,7 +196,7 @@ namespace MoonTools.Core.Bonk
             for (var i = 0; i < 32; i++)
             {
                 var edge = FindClosestEdge(winding, simplexVertices);
-                var support = CalculateSupport(shapeA, Transform2DA, shapeB, Transform2DB, edge.normal);
+                var support = CalculateSupport(shapeA, shapeB, edge.normal);
                 var distance = Vector2.Dot(support, edge.normal);
 
                 intersection = edge.normal;
@@ -261,12 +250,12 @@ namespace MoonTools.Core.Bonk
             return new Edge(closestDistance, closestNormal, closestIndex);
         }
 
-        private static Vector2 CalculateSupport(IShape2D shapeA, Transform2D Transform2DA, IShape2D shapeB, Transform2D Transform2DB, Vector2 direction)
+        private static Vector2 CalculateSupport<T, U>(TransformedShape2D<T> shapeA, TransformedShape2D<U> shapeB, Vector2 direction) where T : struct, IShape2D where U : struct, IShape2D
         {
-            return shapeA.Support(direction, Transform2DA) - shapeB.Support(-direction, Transform2DB);
+            return shapeA.Support(direction) - shapeB.Support(-direction);
         }
 
-        private static (bool, Simplex2D) Check(MinkowskiDifference minkowskiDifference, Vector2 c, Vector2 b)
+        private static (bool, Simplex2D) Check<T, U>(MinkowskiDifference<T, U> minkowskiDifference, Vector2 c, Vector2 b) where T : struct, IShape2D where U : struct, IShape2D
         {
             var cb = c - b;
             var c0 = -c;
@@ -274,7 +263,7 @@ namespace MoonTools.Core.Bonk
             return DoSimplex(minkowskiDifference, new Simplex2D(b, c), d);
         }
 
-        private static (bool, Simplex2D) DoSimplex(MinkowskiDifference minkowskiDifference, Simplex2D simplex, Vector2 direction)
+        private static (bool, Simplex2D) DoSimplex<T, U>(MinkowskiDifference<T, U> minkowskiDifference, Simplex2D simplex, Vector2 direction) where T : struct, IShape2D where U : struct, IShape2D
         {
             var a = minkowskiDifference.Support(direction);
             var notPastOrigin = Vector2.Dot(a, direction) < 0;

Bonk/TransformedShape2D.cs

@@ -0,0 +1,78 @@
+using System;
+using System.Collections.Generic;
+using System.Numerics;
+using MoonTools.Core.Structs;
+
+namespace MoonTools.Core.Bonk
+{
+    public struct TransformedShape2D<TShape2D> : IEquatable<TransformedShape2D<TShape2D>>, ICollisionTestable<TShape2D>, IHasAABB2D where TShape2D : struct, IShape2D
+    {
+        public TShape2D Shape { get; }
+        public Transform2D Transform { get; }
+        public AABB AABB { get; }
+        public IEnumerable<TransformedShape2D<TShape2D>> TransformedShapes { get { yield return this; } }
+
+        public TransformedShape2D(TShape2D shape, Transform2D transform)
+        {
+            Shape = shape;
+            Transform = transform;
+            AABB = shape.TransformedAABB(transform);
+        }
+
+        public TransformedShape2D<TShape2D> Compose(Transform2D transform)
+        {
+            return new TransformedShape2D<TShape2D>(Shape, Transform.Compose(transform));
+        }
+
+        public Vector2 Support(Vector2 direction)
+        {
+            return Shape.Support(direction, Transform);
+        }
+
+        public void Deconstruct(out TShape2D shape, out Transform2D transform)
+        {
+            shape = Shape;
+            transform = Transform;
+        }
+
+        public bool TestCollision<U>(ICollisionTestable collisionTestable) where U : struct, IShape2D
+        {
+            return NarrowPhase.TestCollision(TransformedShapes, collisionTestable.TransformedShapes);
+            if (collisionTestable is MultiShape<U> multiShape)
+            {
+                return NarrowPhase.TestCollision(this, multiShape);
+            }
+            else if (collisionTestable is TransformedShape2D<U> shape)
+            {
+                return NarrowPhase.TestCollision(this, shape);
+            }
+            return false;
+        }
+
+        public override bool Equals(object obj)
+        {
+            return obj is TransformedShape2D<TShape2D> d && Equals(d);
+        }
+
+        public bool Equals(TransformedShape2D<TShape2D> other)
+        {
+            return Shape.Equals(other.Shape) &&
+                   Transform.Equals(other.Transform);
+        }
+
+        public override int GetHashCode()
+        {
+            return HashCode.Combine(Shape, Transform);
+        }
+
+        public static bool operator ==(TransformedShape2D<TShape2D> left, TransformedShape2D<TShape2D> right)
+        {
+            return left.Equals(right);
+        }
+
+        public static bool operator !=(TransformedShape2D<TShape2D> left, TransformedShape2D<TShape2D> right)
+        {
+            return !(left == right);
+        }
+    }
+}

Test/EPA2DTest.cs

@@ -13,67 +13,61 @@ namespace Tests
         [Test]
         public void RectangleOverlap()
         {
-            var squareA = new Rectangle(2, 2);
+            var squareA = new TransformedShape2D<Rectangle>(new Rectangle(2, 2), Transform2D.DefaultTransform);
-            var transformA = Transform2D.DefaultTransform;
+            var squareB = new TransformedShape2D<Rectangle>(new Rectangle(2, 2), new Transform2D(new Vector2(1.5f, 0)));
-            var squareB = new Rectangle(2, 2);
-            var transformB = new Transform2D(new Vector2(1.5f, 0));
 
-            var (result, simplex) = NarrowPhase.FindCollisionSimplex(squareA, transformA, squareB, transformB);
+            var (result, simplex) = NarrowPhase.FindCollisionSimplex(squareA, squareB);
 
             result.Should().BeTrue();
 
-            var intersection = NarrowPhase.Intersect(squareA, transformA, squareB, transformB, simplex);
+            var intersection = NarrowPhase.Intersect(squareA, squareB, 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
+            var movedTransform = new Transform2D(-(intersection * 1.01f)); // move a tiny bit past
 
-            NarrowPhase.TestCollision(squareA, movedTransform, squareB, transformB).Should().BeFalse();
+            NarrowPhase.TestCollision(squareA.Compose(movedTransform), squareB).Should().BeFalse();
         }
 
         [Test]
         public void CircleOverlap()
         {
-            var circleA = new Circle(2);
+            var circleA = new TransformedShape2D<Circle>(new Circle(2), Transform2D.DefaultTransform);
-            var transformA = Transform2D.DefaultTransform;
+            var circleB = new TransformedShape2D<Circle>(new Circle(1), new Transform2D(new Vector2(1, 1)));
-            var circleB = new Circle(1);
-            var transformB = new Transform2D(new Vector2(1, 1));
 
-            var (result, simplex) = NarrowPhase.FindCollisionSimplex(circleA, transformA, circleB, transformB);
+            var (result, simplex) = NarrowPhase.FindCollisionSimplex(circleA, circleB);
 
             result.Should().BeTrue();
 
-            var intersection = NarrowPhase.Intersect(circleA, transformA, circleB, transformB, simplex);
+            var intersection = NarrowPhase.Intersect(circleA, circleB, simplex);
 
-            var ix = (circleA.Radius * (float)Math.Cos(Math.PI / 4)) - ((circleB.Radius * (float)Math.Cos(5 * Math.PI / 4)) + transformB.Position.X);
+            var ix = (2 * (float)Math.Cos(Math.PI / 4)) - ((1 * (float)Math.Cos(5 * Math.PI / 4)) + 1);
-            var iy = (circleA.Radius * (float)Math.Sin(Math.PI / 4)) - ((circleB.Radius * (float)Math.Sin(5 * Math.PI / 4)) + transformB.Position.Y);
+            var iy = (2 * (float)Math.Sin(Math.PI / 4)) - ((1 * (float)Math.Sin(5 * Math.PI / 4)) + 1);
 
             intersection.X.Should().BeApproximately(ix, 0.01f);
             intersection.Y.Should().BeApproximately(iy, 0.01f);
 
-            var movedTransform = new Transform2D(transformA.Position - (intersection * 1.01f)); // move a tiny bit past
+            var movedTransform = new Transform2D(-(intersection * 1.01f)); // move a tiny bit past
 
-            NarrowPhase.TestCollision(circleA, movedTransform, circleB, transformB).Should().BeFalse();
+            NarrowPhase.TestCollision(circleA.Compose(movedTransform), circleB).Should().BeFalse();
         }
 
         [Test]
         public void LineRectangleOverlap()
         {
-            var line = new Line(new Position2D(-4, -4), new Position2D(4, 4));
+            var line = new TransformedShape2D<Line>(new Line(new Position2D(-4, -4), new Position2D(4, 4)), Transform2D.DefaultTransform);
-            var transformA = Transform2D.DefaultTransform;
+            var square = new TransformedShape2D<Rectangle>(new Rectangle(2, 2), Transform2D.DefaultTransform);
-            var square = new Rectangle(2, 2);
-            var transformB = Transform2D.DefaultTransform;
 
-            var (result, simplex) = NarrowPhase.FindCollisionSimplex(line, transformA, square, transformB);
+            var (result, simplex) = NarrowPhase.FindCollisionSimplex(line, square);
 
             result.Should().BeTrue();
 
-            var intersection = NarrowPhase.Intersect(line, transformA, square, transformB, simplex);
+            var intersection = NarrowPhase.Intersect(line, square, simplex);
 
-            var movedTransform = new Transform2D(transformA.Position - (intersection * 1.01f)); // move a tiny bit past
+            var movedTransform = new Transform2D(-(intersection * 1.01f)); // move a tiny bit past
 
-            NarrowPhase.TestCollision(line, movedTransform, square, transformB).Should().BeFalse();
+            NarrowPhase.TestCollision(line.Compose(movedTransform), square).Should().BeFalse();
         }
     }
 }

Test/NarrowPhaseTest.cs

@@ -408,7 +408,7 @@ namespace Tests
             var rectangleB = new Rectangle(2, 2);
             var transformB = new Transform2D(new Vector2(1, 0));
 
-            NarrowPhase.TestRectangleOverlap(rectangleA, transformA, rectangleB, transformB).Should().BeTrue();
+            NarrowPhase.TestCollision(new TransformedShape2D<Rectangle>(rectangleA, transformA), new TransformedShape2D<Rectangle>(rectangleB, transformB)).Should().BeTrue();
         }
 
         [Test]
@@ -420,14 +420,14 @@ namespace Tests
             var rectangleB = new Rectangle(2, 2);
             var transformB = new Transform2D(new Vector2(1, 0));
 
-            NarrowPhase.TestRectangleOverlap(rectangleA, transformA, rectangleB, transformB).Should().BeTrue();
+            NarrowPhase.TestCollision(new TransformedShape2D<Rectangle>(rectangleA, transformA), new TransformedShape2D<Rectangle>(rectangleB, transformB)).Should().BeTrue();
         }
 
         [Test]
         public void MultiRectanglesOverlapping()
         {
-            var multiRectangleA = new MultiShape(
+            var multiRectangleA = new MultiShape<Rectangle>(
-                ImmutableArray.Create<(IShape2D, Transform2D)>(
+                ImmutableArray.Create(
                     (new Rectangle(4, 1), new Transform2D(new Position2D(-5, 0))),
                     (new Rectangle(4, 1), new Transform2D(new Position2D(-5, 1))),
                     (new Rectangle(4, 1), new Transform2D(new Position2D(-5, 2)))
@@ -435,8 +435,8 @@ namespace Tests
             );
             var transformA = new Transform2D(new Position2D(5, 0));
 
-            var multiRectangleB = new MultiShape(
+            var multiRectangleB = new MultiShape<Rectangle>(
-                ImmutableArray.Create<(IShape2D, Transform2D)>(
+                ImmutableArray.Create(
                     (new Rectangle(4, 1), new Transform2D(new Position2D(4, -1))),
                     (new Rectangle(4, 1), new Transform2D(new Position2D(4, 0))),
                     (new Rectangle(4, 1), new Transform2D(new Position2D(4, 1)))
@@ -450,8 +450,8 @@ namespace Tests
         [Test]
         public void MultiRectanglesNotOverlapping()
         {
-            var multiRectangleA = new MultiShape(
+            var multiRectangleA = new MultiShape<Rectangle>(
-                ImmutableArray.Create<(IShape2D, Transform2D)>(
+                ImmutableArray.Create(
                     (new Rectangle(4, 1), new Transform2D(new Position2D(-5, 0))),
                     (new Rectangle(4, 1), new Transform2D(new Position2D(-5, 1))),
                     (new Rectangle(4, 1), new Transform2D(new Position2D(-5, 2)))
@@ -459,8 +459,8 @@ namespace Tests
             );
             var transformA = new Transform2D(new Position2D(5, 0));
 
-            var multiRectangleB = new MultiShape(
+            var multiRectangleB = new MultiShape<Rectangle>(
-                ImmutableArray.Create<(IShape2D, Transform2D)>(
+                ImmutableArray.Create(
                     (new Rectangle(4, 1), new Transform2D(new Position2D(4, -1))),
                     (new Rectangle(4, 1), new Transform2D(new Position2D(4, 0))),
                     (new Rectangle(4, 1), new Transform2D(new Position2D(4, 1)))
@@ -470,5 +470,10 @@ namespace Tests
 
             NarrowPhase.TestCollision(multiRectangleA, transformA, multiRectangleB, transformB).Should().BeFalse();
         }
+
+        public class SweepTest
+        {
+
+        }
     }
 }

Test/SpatialHashTest.cs

@@ -38,8 +38,8 @@ namespace Tests
             var point = new Point();
             var pointTransform = new Transform2D(new Position2D(8, 8));
 
-            var multiRectangle = new MultiShape(
+            var multiRectangle = new MultiShape<Rectangle>(
-                ImmutableArray.Create<(IShape2D, Transform2D)>(
+                ImmutableArray.Create(
                     (new Rectangle(4, 1), new Transform2D(new Position2D(-2, -2))),
                     (new Rectangle(4, 1), new Transform2D(new Position2D(-2, -1))),
                     (new Rectangle(4, 1), new Transform2D(new Position2D(-2, 0)))