fixing sweep test

Bonk/BroadPhase/SpatialHash.cs

@@ -73,7 +73,38 @@ namespace MoonTools.Core.Bonk
                         foreach (var t in hashDictionary[key])
                         {
                             var (otherShape, otherTransform) = IDLookup[t];
-                            if (!id.Equals(t) && AABB.TestOverlap(shape.TransformedAABB(transform2D), otherShape.TransformedAABB(otherTransform)))
+                            if (!id.Equals(t) && AABB.TestOverlap(box, otherShape.TransformedAABB(otherTransform)))
+                            {
+                                yield return (t, otherShape, otherTransform);
+                            }
+                        }
+                    }
+                }
+            }
+        }
+
+
+        /// <summary>
+        /// Retrieves objects based on a pre-transformed AABB.
+        /// </summary>
+        /// <param name="aabb">A transformed AABB.</param>
+        /// <returns></returns>
+        public IEnumerable<(T, IHasAABB2D, Transform2D)> 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, otherTransform) = IDLookup[t];
+                            if (AABB.TestOverlap(aabb, otherShape.TransformedAABB(otherTransform)))
                             {
                                 yield return (t, otherShape, otherTransform);
                             }

Bonk/MultiShape.cs

@@ -30,6 +30,16 @@ namespace MoonTools.Core.Bonk
             }
         }
 
+        public bool IsSingleShape<T>() where T : struct, IShape2D
+        {
+            return ShapeTransformPairs.Length == 1 && ShapeTransformPairs[0].Item1 is T;
+        }
+
+        public (T, Transform2D) ShapeTransformPair<T>() where T : struct, IShape2D
+        {
+            return ((T, Transform2D))ShapeTransformPairs[0];
+        }
+
         private static AABB AABBFromShapes(IEnumerable<(IShape2D, Transform2D)> shapeTransforms)
         {
             var minX = float.MaxValue;

Bonk/SweepTest/SweepTest.cs

@@ -7,7 +7,7 @@ namespace MoonTools.Core.Bonk
     public static class SweepTest
     {
         /// <summary>
-        /// Performs a sweep test on rectangles.
+        /// Performs a sweep test on rectangles. Returns the position 1 pixel before overlap occurs.
         /// </summary>
         /// <typeparam name="T"></typeparam>
         /// <param name="spatialHash">A spatial hash.</param>
@@ -27,67 +27,88 @@ namespace MoonTools.Core.Bonk
 
             foreach (var (id, shape, shapeTransform) in spatialHash.Retrieve(sweepBox))
             {
-                if (shape is Rectangle otherRectangle)
+                Rectangle otherRectangle;
+                Transform2D otherTransform;
+                AABB otherTransformedAABB;
+                if (shape is Rectangle)
                 {
-                    var otherTransformedAABB = otherRectangle.TransformedAABB(shapeTransform);
+                    otherRectangle = (Rectangle)shape;
-                    float xInvEntry, yInvEntry;
+                    otherTransformedAABB = shape.TransformedAABB(shapeTransform);
+                    otherTransform = shapeTransform;
+                }
+                else if (shape is MultiShape multiShape && multiShape.IsSingleShape<Rectangle>())
+                {
+                    Transform2D rectangleOffset;
+                    (otherRectangle, rectangleOffset) = multiShape.ShapeTransformPair<Rectangle>();
+                    otherTransform = shapeTransform.Compose(rectangleOffset);
+                    otherTransformedAABB = shape.TransformedAABB(otherTransform);
+                }
+                else
+                {
+                    continue;
+                }
 
-                    if (ray.X > 0)
+                float xInvEntry, yInvEntry;
-                    {
-                        xInvEntry = shapeTransform.Position.X - (transform.Position.X + transformedAABB.Width);
-                    }
-                    else
-                    {
-                        xInvEntry = (shapeTransform.Position.X + otherTransformedAABB.Width) - transform.Position.X;
-                    }
 
-                    if (ray.Y > 0)
+                if (ray.X > 0)
-                    {
+                {
-                        yInvEntry = shapeTransform.Position.Y - (transform.Position.Y + transformedAABB.Height);
+                    xInvEntry = otherTransformedAABB.Left - (transformedAABB.Right);
-                    }
+                }
-                    else
+                else
-                    {
+                {
-                        yInvEntry = (shapeTransform.Position.Y + otherTransformedAABB.Height) - shapeTransform.Position.Y;
+                    xInvEntry = (otherTransformedAABB.Right) - transformedAABB.Left;
-                    }
+                }
 
-                    float xEntry, yEntry;
+                if (ray.Y > 0)
+                {
+                    yInvEntry = otherTransformedAABB.Top - (transformedAABB.Bottom);
+                }
+                else
+                {
+                    yInvEntry = (otherTransformedAABB.Bottom) - transformedAABB.Top;
+                }
 
-                    if (ray.X == 0)
+                float xEntry, yEntry;
-                    {
-                        xEntry = float.MinValue;
-                    }
-                    else
-                    {
-                        xEntry = xInvEntry / ray.X;
-                    }
 
-                    if (ray.Y == 0)
+                if (ray.X == 0)
-                    {
+                {
-                        yEntry = float.MinValue;
+                    xEntry = float.MinValue;
-                    }
+                }
-                    else
+                else
-                    {
+                {
-                        yEntry = yInvEntry / ray.Y;
+                    xEntry = xInvEntry / ray.X;
-                    }
+                }
 
-                    var entryTime = Math.Max(xEntry, yEntry);
+                if (ray.Y == 0)
+                {
+                    yEntry = float.MinValue;
+                }
+                else
+                {
+                    yEntry = yInvEntry / ray.Y;
+                }
 
-                    if (entryTime > 0 && entryTime < 1)
+                var entryTime = Math.Max(xEntry, yEntry);
+
+                if (entryTime >= 0 && entryTime <= 1)
+                {
+                    if (entryTime < shortestDistance)
                     {
-                        if (entryTime < shortestDistance)
+                        shortestDistance = entryTime;
-                        {
+                        nearestID = id;
-                            shortestDistance = entryTime;
+                        nearestRectangle = otherRectangle;
-                            nearestID = id;
+                        nearestTransform = shapeTransform;
-                            nearestRectangle = rectangle;
-                            nearestTransform = shapeTransform;
-                        }
                     }
                 }
+                
             }
 
             if (nearestRectangle.HasValue)
             {
-                return new SweepResult<T, Rectangle>(true, ray * shortestDistance, nearestID, nearestRectangle.Value, nearestTransform.Value);
+                var overlapPosition = ray * shortestDistance;
+                var correctionX = ray.X > 0 ? -1 : 1;
+                var correctionY = ray.Y > 0 ? -1 : 1;
+                return new SweepResult<T, Rectangle>(true, new Position2D((int)overlapPosition.X + correctionX, (int)overlapPosition.Y + correctionY), nearestID, nearestRectangle.Value, nearestTransform.Value);
             }
             else
             {

Test/SweepTestTest.cs

@@ -20,15 +20,23 @@ namespace Tests
             var farthestRectangle = new Rectangle(4, 4);
             var farthestTransform = new Transform2D(new Position2D(12, 0));
 
+            var downRectangle = new Rectangle(12, 4);
+            var downTransform = new Transform2D(new Position2D(-6, 20));
+
             var spatialHash = new SpatialHash<int>(16);
             spatialHash.Insert(1, otherRectangle, otherTransform);
             spatialHash.Insert(2, farthestRectangle, farthestTransform);
+            spatialHash.Insert(3, downRectangle, downTransform);
 
             SweepTest.Rectangle(spatialHash, rectangle, transform, new Vector2(12, 0)).Should().Be(
                 new SweepResult<int, Rectangle>(true, new Vector2(8, 0), 1, otherRectangle, otherTransform)
             );
 
             SweepTest.Rectangle(spatialHash, rectangle, transform, new Vector2(-12, 0)).Hit.Should().BeFalse();
+
+            SweepTest.Rectangle(spatialHash, rectangle, transform, new Vector2(0, 20)).Should().Be(
+                new SweepResult<int, Rectangle>(true, new Vector2(0, 16), 3, downRectangle, downTransform)
+            );
         }
     }
 }