diff --git a/src/Collision/Fixed/Collider.cs b/src/Collision/Fixed/Collider.cs new file mode 100644 index 0000000..db27e38 --- /dev/null +++ b/src/Collision/Fixed/Collider.cs @@ -0,0 +1,33 @@ +using System; + +namespace MoonWorks.Collision.Fixed +{ + public class Collider : IHasAABB2D where T : struct, IShape2D + { + private readonly T[] Shapes; + + public ReadOnlySpan.Enumerator GetEnumerator() => new ReadOnlySpan(Shapes).GetEnumerator(); + + public AABB2D AABB { get; } + + public Collider(T shape) + { + Shapes = new T[1] { shape }; + AABB = shape.AABB; + } + + public Collider(T[] shapes) + { + Shapes = new T[shapes.Length]; + Array.Copy(shapes, Shapes, shapes.Length); + + var aabb = new AABB2D(); + foreach (var shape in Shapes) + { + aabb = aabb.Compose(shape.AABB); + } + + AABB = aabb; + } + } +} diff --git a/src/Collision/Fixed/ICollidable.cs b/src/Collision/Fixed/ICollidable.cs deleted file mode 100644 index f1aa27c..0000000 --- a/src/Collision/Fixed/ICollidable.cs +++ /dev/null @@ -1,12 +0,0 @@ -using System.Collections.Generic; -using MoonWorks.Math.Fixed; - -namespace MoonWorks.Collision.Fixed -{ - public interface ICollidable - { - IEnumerable Shapes { get; } - AABB2D AABB { get; } - AABB2D TransformedAABB(Transform2D transform); - } -} diff --git a/src/Collision/Fixed/IHasAABB2D.cs b/src/Collision/Fixed/IHasAABB2D.cs new file mode 100644 index 0000000..e8c17cd --- /dev/null +++ b/src/Collision/Fixed/IHasAABB2D.cs @@ -0,0 +1,7 @@ +namespace MoonWorks.Collision.Fixed +{ + public interface IHasAABB2D + { + AABB2D AABB { get; } + } +} diff --git a/src/Collision/Fixed/IShape2D.cs b/src/Collision/Fixed/IShape2D.cs index 819069c..05f0fc6 100644 --- a/src/Collision/Fixed/IShape2D.cs +++ b/src/Collision/Fixed/IShape2D.cs @@ -1,8 +1,8 @@ -using MoonWorks.Math.Fixed; +using MoonWorks.Math.Fixed; namespace MoonWorks.Collision.Fixed { - public interface IShape2D : ICollidable, System.IEquatable + public interface IShape2D : IHasAABB2D, System.IEquatable { /// /// A Minkowski support function. Gives the farthest point on the edge of a shape along the given direction. diff --git a/src/Collision/Fixed/MinkowskiDifference.cs b/src/Collision/Fixed/MinkowskiDifference.cs index a7b05e8..e9af4c6 100644 --- a/src/Collision/Fixed/MinkowskiDifference.cs +++ b/src/Collision/Fixed/MinkowskiDifference.cs @@ -1,18 +1,18 @@ -using MoonWorks.Math.Fixed; +using MoonWorks.Math.Fixed; namespace MoonWorks.Collision.Fixed { /// /// A Minkowski difference between two shapes. /// - public struct MinkowskiDifference : System.IEquatable + public struct MinkowskiDifference where T : IShape2D where U : IShape2D { - private IShape2D ShapeA { get; } + private T ShapeA { get; } private Transform2D TransformA { get; } - private IShape2D ShapeB { get; } + private U ShapeB { get; } private Transform2D TransformB { get; } - public MinkowskiDifference(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB) + public MinkowskiDifference(T shapeA, Transform2D transformA, U shapeB, Transform2D transformB) { ShapeA = shapeA; TransformA = transformA; @@ -24,34 +24,5 @@ namespace MoonWorks.Collision.Fixed { return ShapeA.Support(direction, TransformA) - ShapeB.Support(-direction, TransformB); } - - public override bool Equals(object other) - { - return other is MinkowskiDifference minkowskiDifference && Equals(minkowskiDifference); - } - - public bool Equals(MinkowskiDifference other) - { - return - ShapeA == other.ShapeA && - TransformA == other.TransformA && - ShapeB == other.ShapeB && - TransformB == other.TransformB; - } - - public override int GetHashCode() - { - return System.HashCode.Combine(ShapeA, TransformA, ShapeB, TransformB); - } - - public static bool operator ==(MinkowskiDifference a, MinkowskiDifference b) - { - return a.Equals(b); - } - - public static bool operator !=(MinkowskiDifference a, MinkowskiDifference b) - { - return !(a == b); - } } } diff --git a/src/Collision/Fixed/NarrowPhase.cs b/src/Collision/Fixed/NarrowPhase.cs index 731e969..aae4950 100644 --- a/src/Collision/Fixed/NarrowPhase.cs +++ b/src/Collision/Fixed/NarrowPhase.cs @@ -1,4 +1,5 @@ -using MoonWorks.Math.Fixed; +using MoonWorks.Math.Fixed; +using System.Runtime.CompilerServices; namespace MoonWorks.Collision.Fixed { @@ -11,63 +12,168 @@ namespace MoonWorks.Collision.Fixed public int Index; } - public static bool TestCollision(ICollidable collidableA, Transform2D transformA, ICollidable collidableB, Transform2D transformB) - { - foreach (var shapeA in collidableA.Shapes) - { - foreach (var shapeB in collidableB.Shapes) - { + public static bool TestCollision(Collider colliderA, in Transform2D transformA, Collider colliderB, in Transform2D transformB) where T : struct, IShape2D where U : struct, IShape2D + { + foreach (var shapeA in colliderA) + { + foreach (var shapeB in colliderB) + { if (TestCollision(shapeA, transformA, shapeB, transformB)) - { + { return true; } } - } + } return false; } - public static bool TestCollision(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB) + public static bool TestCollision(Collider collider, in Transform2D transformA, U shape, in Transform2D transformB) where T : struct, IShape2D where U : struct, IShape2D { - // If we can use a fast path check, let's do that! - if (shapeA is Rectangle rectangleA && shapeB is Rectangle rectangleB && transformA.IsAxisAligned && transformB.IsAxisAligned) + foreach (var colliderShape in collider) { - return TestRectangleOverlap(rectangleA, transformA, rectangleB, transformB); - } - else if (shapeA is Point && shapeB is Rectangle && transformB.IsAxisAligned) - { - return TestPointRectangleOverlap((Point) shapeA, transformA, (Rectangle) shapeB, transformB); - } - else if (shapeA is Rectangle && shapeB is Point && transformA.IsAxisAligned) - { - return TestPointRectangleOverlap((Point) shapeB, transformB, (Rectangle) shapeA, transformA); - } - else if (shapeA is Rectangle && shapeB is Circle && transformA.IsAxisAligned && transformB.IsUniformScale) - { - return TestCircleRectangleOverlap((Circle) shapeB, transformB, (Rectangle) shapeA, transformA); - } - else if (shapeA is Circle && shapeB is Rectangle && transformA.IsUniformScale && transformB.IsAxisAligned) - { - return TestCircleRectangleOverlap((Circle) shapeA, transformA, (Rectangle) shapeB, transformB); - } - else if (shapeA is Circle && shapeB is Point && transformA.IsUniformScale) - { - return TestCirclePointOverlap((Circle) shapeA, transformA, (Point) shapeB, transformB); - } - else if (shapeA is Point && shapeB is Circle && transformB.IsUniformScale) - { - return TestCirclePointOverlap((Circle) shapeB, transformB, (Point) shapeA, transformA); - } - else if (shapeA is Circle circleA && shapeB is Circle circleB && transformA.IsUniformScale && transformB.IsUniformScale) - { - return TestCircleOverlap(circleA, transformA, circleB, transformB); + if (TestCollision(colliderShape, transformA, shape, transformB)) + { + return true; + } + } + + return false; + } + + public static bool TestCollision(U shape, in Transform2D transformA, Collider collider, in Transform2D transformB) where T : struct, IShape2D where U : struct, IShape2D + { + return TestCollision(collider, transformB, shape, transformA); + } + + public static bool TestCollision(in T shapeA, in Transform2D transformA, in U shapeB, in Transform2D transformB) where T : struct, IShape2D where U : struct, IShape2D + { + if (shapeA is Circle circle) + { + if (shapeB is Circle circleB) + { + return TestCollision(circle, transformA, circleB, transformB); + } + else if (shapeB is Point pointB) + { + return TestCollision(circle, transformA, pointB, transformB); + } + else if (shapeB is Rectangle rectangleB) + { + return TestCollision(circle, transformA, rectangleB, transformB); + } + } + else if (shapeA is Point point) + { + if (shapeB is Circle circleB) + { + return TestCollision(point, transformA, circleB, transformB); + } + else if (shapeB is Point pointB) + { + return TestCollision(point, transformA, pointB, transformB); + } + else if (shapeB is Rectangle rectangleB) + { + return TestCollision(point, transformA, rectangleB, transformB); + } + } + else if (shapeA is Rectangle rectangle) + { + if (shapeB is Circle circleB) + { + return TestCollision(rectangle, transformA, circleB, transformB); + } + else if (shapeB is Point pointB) + { + return TestCollision(rectangle, transformA, pointB, transformB); + } + else if (shapeB is Rectangle rectangleB) + { + return TestCollision(rectangle, transformA, rectangleB, transformB); + } } - // Sad, we can't do a fast path optimization. Time for a simplex reduction. return FindCollisionSimplex(shapeA, transformA, shapeB, transformB).Item1; } - public static bool TestRectangleOverlap(Rectangle rectangleA, Transform2D transformA, Rectangle rectangleB, Transform2D transformB) + public static bool TestCollision(in Rectangle rectangleA, in Transform2D transformA, in Rectangle rectangleB, in Transform2D transformB) + { + if (transformA.IsAxisAligned && transformB.IsAxisAligned) + { + return TestRectangleOverlap(rectangleA, transformA, rectangleB, transformB); + } + else + { + return FindCollisionSimplex(rectangleA, transformA, rectangleB, transformB).Item1; + } + } + + public static bool TestCollision(in Point point, in Transform2D transformA, in Rectangle rectangle, in Transform2D transformB) + { + if (transformB.IsAxisAligned) + { + return TestPointRectangleOverlap(point, transformA, rectangle, transformB); + } + else + { + return FindCollisionSimplex(point, transformA, rectangle, transformB).Item1; + } + } + + public static bool TestCollision(in Rectangle rectangle, in Transform2D transformA, in Point point, in Transform2D transformB) + { + return TestCollision(point, transformB, rectangle, transformA); + } + + public static bool TestCollision(in Rectangle rectangle, in Transform2D transformA, in Circle circle, in Transform2D transformB) + { + if (transformA.IsAxisAligned && transformB.IsUniformScale) + { + return TestCircleRectangleOverlap(circle, transformB, rectangle, transformA); + } + else + { + return FindCollisionSimplex(rectangle, transformA, circle, transformB).Item1; + } + } + + public static bool TestCollision(in Circle circle, in Transform2D transformA, in Rectangle rectangle, in Transform2D transformB) + { + return TestCollision(rectangle, transformB, circle, transformA); + } + + public static bool TestCollision(in Circle circle, in Transform2D transformA, in Point point, in Transform2D transformB) + { + if (transformA.IsUniformScale) + { + return TestCirclePointOverlap(circle, transformA, point, transformB); + } + else + { + return FindCollisionSimplex(circle, transformA, point, transformB).Item1; + } + } + + public static bool TestCollision(in Point point, in Transform2D transformA, in Circle circle, in Transform2D transformB) + { + return TestCollision(circle, transformB, point, transformA); + } + + public static bool TestCollision(in Circle circleA, in Transform2D transformA, in Circle circleB, in Transform2D transformB) + { + if (transformA.IsUniformScale && transformB.IsUniformScale) + { + return TestCircleOverlap(circleA, transformA, circleB, transformB); + } + else + { + return FindCollisionSimplex(circleA, transformA, circleB, transformB).Item1; + } + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool TestRectangleOverlap(in Rectangle rectangleA, in Transform2D transformA, in Rectangle rectangleB, in Transform2D transformB) { var firstAABB = rectangleA.TransformedAABB(transformA); var secondAABB = rectangleB.TransformedAABB(transformB); @@ -75,7 +181,8 @@ namespace MoonWorks.Collision.Fixed return firstAABB.Left < secondAABB.Right && firstAABB.Right > secondAABB.Left && firstAABB.Top < secondAABB.Bottom && firstAABB.Bottom > secondAABB.Top; } - public static bool TestPointRectangleOverlap(Point point, Transform2D pointTransform, Rectangle rectangle, Transform2D rectangleTransform) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool TestPointRectangleOverlap(in Point point, in Transform2D pointTransform, in Rectangle rectangle, in Transform2D rectangleTransform) { var transformedPoint = pointTransform.Position; var AABB = rectangle.TransformedAABB(rectangleTransform); @@ -83,7 +190,8 @@ namespace MoonWorks.Collision.Fixed return transformedPoint.X > AABB.Left && transformedPoint.X < AABB.Right && transformedPoint.Y < AABB.Bottom && transformedPoint.Y > AABB.Top; } - public static bool TestCirclePointOverlap(Circle circle, Transform2D circleTransform, Point point, Transform2D pointTransform) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool TestCirclePointOverlap(in Circle circle, in Transform2D circleTransform, in Point point, in Transform2D pointTransform) { var circleCenter = circleTransform.Position; var circleRadius = circle.Radius * circleTransform.Scale.X; @@ -97,7 +205,8 @@ namespace MoonWorks.Collision.Fixed /// /// NOTE: The rectangle must be axis aligned, and the scaling of the circle must be uniform. /// - public static bool TestCircleRectangleOverlap(Circle circle, Transform2D circleTransform, Rectangle rectangle, Transform2D rectangleTransform) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool TestCircleRectangleOverlap(in Circle circle, in Transform2D circleTransform, in Rectangle rectangle, in Transform2D rectangleTransform) { var circleCenter = circleTransform.Position; var circleRadius = circle.Radius * circleTransform.Scale.X; @@ -113,7 +222,8 @@ namespace MoonWorks.Collision.Fixed return distanceSquared < (circleRadius * circleRadius); } - public static bool TestCircleOverlap(Circle circleA, Transform2D transformA, Circle circleB, Transform2D transformB) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool TestCircleOverlap(in Circle circleA, in Transform2D transformA, in Circle circleB, in Transform2D transformB) { var radiusA = circleA.Radius * transformA.Scale.X; var radiusB = circleB.Radius * transformB.Scale.Y; @@ -127,9 +237,14 @@ namespace MoonWorks.Collision.Fixed return distanceSquared < radiusSumSquared; } - public static (bool, Simplex2D) FindCollisionSimplex(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB) + public static bool TestPointOverlap(in Point pointA, in Transform2D transformA, in Point pointB, in Transform2D transformB) { - var minkowskiDifference = new MinkowskiDifference(shapeA, transformA, shapeB, transformB); + return transformA.Position == transformB.Position; + } + + public static (bool, Simplex2D) FindCollisionSimplex(T shapeA, Transform2D transformA, U shapeB, Transform2D transformB) where T : IShape2D where U : IShape2D + { + 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); @@ -208,12 +323,13 @@ namespace MoonWorks.Collision.Fixed }; } - private static Vector2 CalculateSupport(IShape2D shapeA, Transform2D Transform2DA, IShape2D shapeB, Transform2D Transform2DB, Vector2 direction) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static Vector2 CalculateSupport(T shapeA, Transform2D Transform2DA, U shapeB, Transform2D Transform2DB, Vector2 direction) where T : IShape2D where U : IShape2D { return shapeA.Support(direction, Transform2DA) - shapeB.Support(-direction, Transform2DB); } - private static (bool, Simplex2D) Check(MinkowskiDifference minkowskiDifference, Vector2 c, Vector2 b) + private static (bool, Simplex2D) Check(MinkowskiDifference minkowskiDifference, Vector2 c, Vector2 b) where T : IShape2D where U : IShape2D { var cb = c - b; var c0 = -c; @@ -221,7 +337,7 @@ namespace MoonWorks.Collision.Fixed return DoSimplex(minkowskiDifference, new Simplex2D(b, c), d); } - private static (bool, Simplex2D) DoSimplex(MinkowskiDifference minkowskiDifference, Simplex2D simplex, Vector2 direction) + private static (bool, Simplex2D) DoSimplex(MinkowskiDifference minkowskiDifference, Simplex2D simplex, Vector2 direction) where T : IShape2D where U : IShape2D { var a = minkowskiDifference.Support(direction); var notPastOrigin = Vector2.Dot(a, direction) < Fix64.Zero; @@ -320,7 +436,8 @@ namespace MoonWorks.Collision.Fixed return collinear ? new Vector2(a.Y, -a.X) : d; } - private static bool SameDirection(Vector2 a, Vector2 b) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static bool SameDirection(Vector2 a, Vector2 b) { return Vector2.Dot(a, b) > Fix64.Zero; } diff --git a/src/Collision/Fixed/Shapes/Circle.cs b/src/Collision/Fixed/Shapes/Circle.cs index 91b4994..7511f0b 100644 --- a/src/Collision/Fixed/Shapes/Circle.cs +++ b/src/Collision/Fixed/Shapes/Circle.cs @@ -1,4 +1,3 @@ -using System.Collections.Generic; using MoonWorks.Math.Fixed; namespace MoonWorks.Collision.Fixed @@ -10,13 +9,6 @@ namespace MoonWorks.Collision.Fixed { public Fix64 Radius { get; } public AABB2D AABB { get; } - public IEnumerable Shapes - { - get - { - yield return this; - } - } public Circle(Fix64 radius) { diff --git a/src/Collision/Fixed/Shapes/Line.cs b/src/Collision/Fixed/Shapes/Line.cs index b17d424..c1f5949 100644 --- a/src/Collision/Fixed/Shapes/Line.cs +++ b/src/Collision/Fixed/Shapes/Line.cs @@ -1,4 +1,4 @@ -using System.Collections.Generic; +using System.Collections.Generic; using MoonWorks.Math.Fixed; namespace MoonWorks.Collision.Fixed @@ -13,14 +13,6 @@ namespace MoonWorks.Collision.Fixed public AABB2D AABB { get; } - public IEnumerable Shapes - { - get - { - yield return this; - } - } - public Line(Vector2 start, Vector2 end) { Start = start; diff --git a/src/Collision/Fixed/Simplex2D.cs b/src/Collision/Fixed/Simplex2D.cs index 7bfc50f..c5d47c2 100644 --- a/src/Collision/Fixed/Simplex2D.cs +++ b/src/Collision/Fixed/Simplex2D.cs @@ -1,4 +1,4 @@ -using System.Collections.Generic; +using System.Collections.Generic; using MoonWorks.Math.Fixed; namespace MoonWorks.Collision.Fixed @@ -54,33 +54,6 @@ namespace MoonWorks.Collision.Fixed } } - public IEnumerable Vertices - { - get - { - yield return (Vector2) a; - if (b.HasValue) { yield return (Vector2) b; } - if (c.HasValue) { yield return (Vector2) c; } - } - } - - public Vector2 Support(Vector2 direction, Transform2D transform) - { - var maxDotProduct = Fix64.MinValue; - var maxVertex = a; - foreach (var vertex in Vertices) - { - var transformed = Vector2.Transform(vertex, transform.TransformMatrix); - var dot = Vector2.Dot(transformed, direction); - if (dot > maxDotProduct) - { - maxVertex = transformed; - maxDotProduct = dot; - } - } - return maxVertex; - } - public void Insert(Vector2 point, int index) { if (index == 0) @@ -120,7 +93,7 @@ namespace MoonWorks.Collision.Fixed public override int GetHashCode() { - return System.HashCode.Combine(Vertices); + return System.HashCode.Combine(a, b, c); } public static bool operator ==(Simplex2D a, Simplex2D b) diff --git a/src/Collision/Fixed/SpatialHash2D.cs b/src/Collision/Fixed/SpatialHash2D.cs index 61d7951..d597cdd 100644 --- a/src/Collision/Fixed/SpatialHash2D.cs +++ b/src/Collision/Fixed/SpatialHash2D.cs @@ -1,4 +1,4 @@ -using System.Collections.Generic; +using System.Collections.Generic; using MoonWorks.Math.Fixed; namespace MoonWorks.Collision.Fixed @@ -7,13 +7,12 @@ namespace MoonWorks.Collision.Fixed /// Used to quickly check if two shapes are potentially overlapping. /// /// The type that will be used to uniquely identify shape-transform pairs. - public class SpatialHash2D where T : System.IEquatable + public class SpatialHash2D where T : struct, System.IEquatable where U : IHasAABB2D { private readonly Fix64 cellSize; private readonly Dictionary> hashDictionary = new Dictionary>(); - // FIXME: this ICollidable causes boxing which triggers garbage collection - private readonly Dictionary IDLookup = new Dictionary(); + private readonly Dictionary IDLookup = new Dictionary(); public int MinX { get; private set; } = 0; public int MaxX { get; private set; } = 0; @@ -39,9 +38,9 @@ namespace MoonWorks.Collision.Fixed /// /// /// A bitmask value specifying the groups this object belongs to. - public void Insert(T id, ICollidable shape, Transform2D transform2D, uint collisionGroups = uint.MaxValue) + public void Insert(T id, U shape, Transform2D transform2D, uint collisionGroups = uint.MaxValue) { - var box = shape.TransformedAABB(transform2D); + var box = AABB2D.Transformed(shape.AABB, transform2D); var minHash = Hash(box.Min); var maxHash = Hash(box.Max); @@ -65,11 +64,9 @@ namespace MoonWorks.Collision.Fixed /// /// Retrieves all the potential collisions of a shape-transform pair. Excludes any shape-transforms with the given ID. /// - public IEnumerable<(T, ICollidable, Transform2D, uint)> Retrieve(T id, ICollidable shape, Transform2D transform2D, uint collisionMask = uint.MaxValue) + public RetrieveEnumerator Retrieve(T id, V hasAABB, Transform2D transform2D, uint collisionMask = uint.MaxValue) where V : IHasAABB2D { - var returned = AcquireHashSet(); - - var box = shape.TransformedAABB(transform2D); + var box = AABB2D.Transformed(hasAABB.AABB, transform2D); var (minX, minY) = Hash(box.Min); var (maxX, maxY) = Hash(box.Max); @@ -78,64 +75,21 @@ namespace MoonWorks.Collision.Fixed if (minY < MinY) { minY = MinY; } if (maxY > MaxY) { maxY = MaxY; } - foreach (var key in Keys(minX, minY, maxX, maxY)) - { - if (hashDictionary.ContainsKey(key)) - { - foreach (var t in hashDictionary[key]) - { - if (!returned.Contains(t)) - { - var (otherShape, otherTransform, collisionGroups) = IDLookup[t]; - if (!id.Equals(t) && ((collisionGroups & collisionMask) > 0) && AABB2D.TestOverlap(box, otherShape.TransformedAABB(otherTransform))) - { - returned.Add(t); - yield return (t, otherShape, otherTransform, collisionGroups); - } - } - } - } - } - - FreeHashSet(returned); + return new RetrieveEnumerator( + this, + Keys(minX, minY, maxX, maxY), + id, + collisionMask + ); } /// /// Retrieves all the potential collisions of a shape-transform pair. /// - public IEnumerable<(T, ICollidable, Transform2D, uint)> Retrieve(ICollidable shape, Transform2D transform2D, uint collisionMask = uint.MaxValue) + public RetrieveEnumerator Retrieve(V hasAABB, Transform2D transform2D, uint collisionMask = uint.MaxValue) where V : IHasAABB2D { - var returned = AcquireHashSet(); - - var box = shape.TransformedAABB(transform2D); - var (minX, minY) = Hash(box.Min); - var (maxX, maxY) = Hash(box.Max); - - if (minX < MinX) { minX = MinX; } - if (maxX > MaxX) { maxX = MaxX; } - if (minY < MinY) { minY = MinY; } - if (maxY > MaxY) { maxY = MaxY; } - - foreach (var key in Keys(minX, minY, maxX, maxY)) - { - if (hashDictionary.ContainsKey(key)) - { - foreach (var t in hashDictionary[key]) - { - if (!returned.Contains(t)) - { - var (otherShape, otherTransform, collisionGroups) = IDLookup[t]; - if (((collisionGroups & collisionMask) > 0) && AABB2D.TestOverlap(box, otherShape.TransformedAABB(otherTransform))) - { - returned.Add(t); - yield return (t, otherShape, otherTransform, collisionGroups); - } - } - } - } - } - - FreeHashSet(returned); + var box = AABB2D.Transformed(hasAABB.AABB, transform2D); + return Retrieve(box, collisionMask); } /// @@ -143,10 +97,8 @@ namespace MoonWorks.Collision.Fixed /// /// A transformed AABB. /// - public IEnumerable<(T, ICollidable, Transform2D, uint)> Retrieve(AABB2D aabb, uint collisionMask = uint.MaxValue) + public RetrieveEnumerator Retrieve(AABB2D aabb, uint collisionMask = uint.MaxValue) { - var returned = AcquireHashSet(); - var (minX, minY) = Hash(aabb.Min); var (maxX, maxY) = Hash(aabb.Max); @@ -155,28 +107,14 @@ namespace MoonWorks.Collision.Fixed if (minY < MinY) { minY = MinY; } if (maxY > MaxY) { maxY = MaxY; } - foreach (var key in Keys(minX, minY, maxX, maxY)) - { - if (hashDictionary.ContainsKey(key)) - { - foreach (var t in hashDictionary[key]) - { - if (!returned.Contains(t)) - { - var (otherShape, otherTransform, collisionGroups) = IDLookup[t]; - if (((collisionGroups & collisionMask) > 0) && AABB2D.TestOverlap(aabb, otherShape.TransformedAABB(otherTransform))) - { - yield return (t, otherShape, otherTransform, collisionGroups); - } - } - } - } - } - - FreeHashSet(returned); + return new RetrieveEnumerator( + this, + Keys(minX, minY, maxX, maxY), + collisionMask + ); } - public void Update(T id, ICollidable shape, Transform2D transform2D, uint collisionGroups = uint.MaxValue) + public void Update(T id, U shape, Transform2D transform2D, uint collisionGroups = uint.MaxValue) { Remove(id); Insert(id, shape, transform2D, collisionGroups); @@ -189,7 +127,7 @@ namespace MoonWorks.Collision.Fixed { var (shape, transform, collisionGroups) = IDLookup[id]; - var box = shape.TransformedAABB(transform); + var box = AABB2D.Transformed(shape.AABB, transform); var minHash = Hash(box.Min); var maxHash = Hash(box.Max); @@ -222,18 +160,12 @@ namespace MoonWorks.Collision.Fixed return ((long) left << 32) | ((uint) right); } - private IEnumerable Keys(int minX, int minY, int maxX, int maxY) + internal static KeysEnumerator Keys(int minX, int minY, int maxX, int maxY) { - for (var i = minX; i <= maxX; i++) - { - for (var j = minY; j <= maxY; j++) - { - yield return MakeLong(i, j); - } - } + return new KeysEnumerator(minX, minY, maxX, maxY); } - private HashSet AcquireHashSet() + internal HashSet AcquireHashSet() { if (hashSetPool.Count == 0) { @@ -245,9 +177,147 @@ namespace MoonWorks.Collision.Fixed return hashSet; } - private void FreeHashSet(HashSet hashSet) + internal void FreeHashSet(HashSet hashSet) { hashSetPool.Enqueue(hashSet); } + + internal ref struct KeysEnumerator + { + private int MinX; + private int MinY; + private int MaxX; + private int MaxY; + private int i, j; + + public KeysEnumerator GetEnumerator() => this; + + public KeysEnumerator(int minX, int minY, int maxX, int maxY) + { + MinX = minX; + MinY = minY; + MaxX = maxX; + MaxY = maxY; + i = minX; + j = minY - 1; + } + + public bool MoveNext() + { + if (j < MaxY) + { + j += 1; + return true; + } + else if (i < MaxX) + { + i += 1; + j = MinY; + return true; + } + + return false; + } + + public long Current + { + get + { + return MakeLong(i, j); + } + } + } + + public ref struct RetrieveEnumerator + { + public SpatialHash2D SpatialHash; + private KeysEnumerator KeysEnumerator; + private HashSet.Enumerator HashSetEnumerator; + private bool HashSetEnumeratorActive; + private HashSet Duplicates; + private T? ID; + private uint CollisionMask; + + public RetrieveEnumerator GetEnumerator() => this; + + internal RetrieveEnumerator( + SpatialHash2D spatialHash, + KeysEnumerator keysEnumerator, + T id, + uint collisionMask + ) { + SpatialHash = spatialHash; + KeysEnumerator = keysEnumerator; + HashSetEnumerator = default; + HashSetEnumeratorActive = false; + Duplicates = SpatialHash.AcquireHashSet(); + ID = id; + CollisionMask = collisionMask; + } + + internal RetrieveEnumerator( + SpatialHash2D spatialHash, + KeysEnumerator keysEnumerator, + uint collisionMask + ) { + SpatialHash = spatialHash; + KeysEnumerator = keysEnumerator; + HashSetEnumerator = default; + HashSetEnumeratorActive = false; + Duplicates = SpatialHash.AcquireHashSet(); + ID = null; + CollisionMask = collisionMask; + } + + public bool MoveNext() + { + if (!HashSetEnumeratorActive || !HashSetEnumerator.MoveNext()) + { + if (!KeysEnumerator.MoveNext()) + { + SpatialHash.FreeHashSet(Duplicates); + return false; + } + + if (SpatialHash.hashDictionary.TryGetValue(KeysEnumerator.Current, out var hashset)) + { + HashSetEnumerator = hashset.GetEnumerator(); + HashSetEnumeratorActive = true; + } + + return MoveNext(); + } + + // conditions + var t = HashSetEnumerator.Current; + var collisionGroups = SpatialHash.IDLookup[t].Item3; + + if (Duplicates.Contains(t)) + { + return MoveNext(); + } + + if (ID.HasValue) + { + if (ID.Value.Equals(t) || (CollisionMask & collisionGroups) == 0) + { + return MoveNext(); + } + } + + Duplicates.Add(t); + return true; + } + + public (T, U, Transform2D, uint) Current + { + get + { + var t = HashSetEnumerator.Current; + var (u, transform, groups) = SpatialHash.IDLookup[t]; + return (t, u, transform, groups); + } + } + } } }