From b81780e2589f6e8f213de395da770634c8025f52 Mon Sep 17 00:00:00 2001 From: cosmonaut Date: Thu, 22 Dec 2022 18:56:06 -0800 Subject: [PATCH 1/7] garbage optimization changes --- src/Collision/Fixed/Collider.cs | 33 +++ src/Collision/Fixed/ICollidable.cs | 12 - src/Collision/Fixed/IHasAABB2D.cs | 7 + src/Collision/Fixed/IShape2D.cs | 4 +- src/Collision/Fixed/MinkowskiDifference.cs | 39 +-- src/Collision/Fixed/NarrowPhase.cs | 223 +++++++++++++----- src/Collision/Fixed/Shapes/Circle.cs | 8 - src/Collision/Fixed/Shapes/Line.cs | 10 +- src/Collision/Fixed/Simplex2D.cs | 31 +-- src/Collision/Fixed/SpatialHash2D.cs | 262 +++++++++++++-------- 10 files changed, 386 insertions(+), 243 deletions(-) create mode 100644 src/Collision/Fixed/Collider.cs delete mode 100644 src/Collision/Fixed/ICollidable.cs create mode 100644 src/Collision/Fixed/IHasAABB2D.cs 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); + } + } + } } } -- 2.25.1 From c04d0212375e86e8bff42f8d4dd51cfae385a64c Mon Sep 17 00:00:00 2001 From: cosmonaut Date: Mon, 26 Dec 2022 20:03:20 -0800 Subject: [PATCH 2/7] fix spatial hash remove crash if key does not exist --- src/Collision/Fixed/SpatialHash2D.cs | 29 +++++++++++++++------------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/src/Collision/Fixed/SpatialHash2D.cs b/src/Collision/Fixed/SpatialHash2D.cs index d597cdd..0821799 100644 --- a/src/Collision/Fixed/SpatialHash2D.cs +++ b/src/Collision/Fixed/SpatialHash2D.cs @@ -125,21 +125,24 @@ namespace MoonWorks.Collision.Fixed /// public void Remove(T id) { - var (shape, transform, collisionGroups) = IDLookup[id]; - - var box = AABB2D.Transformed(shape.AABB, transform); - var minHash = Hash(box.Min); - var maxHash = Hash(box.Max); - - foreach (var key in Keys(minHash.Item1, minHash.Item2, maxHash.Item1, maxHash.Item2)) + if (IDLookup.TryGetValue(id, out var data)) { - if (hashDictionary.ContainsKey(key)) - { - hashDictionary[key].Remove(id); - } - } + var (shape, transform, collisionGroups) = data; - IDLookup.Remove(id); + var box = AABB2D.Transformed(shape.AABB, transform); + var minHash = Hash(box.Min); + var maxHash = Hash(box.Max); + + foreach (var key in Keys(minHash.Item1, minHash.Item2, maxHash.Item1, maxHash.Item2)) + { + if (hashDictionary.ContainsKey(key)) + { + hashDictionary[key].Remove(id); + } + } + + IDLookup.Remove(id); + } } /// -- 2.25.1 From 9704072ab280017955b4bf951c644e63a585d3ed Mon Sep 17 00:00:00 2001 From: cosmonaut Date: Sun, 15 Jan 2023 12:40:45 -0800 Subject: [PATCH 3/7] rework spatial hash to store arbitrary data --- src/Collision/Fixed/SpatialHash2D.cs | 117 +++++++++++++++++---------- 1 file changed, 73 insertions(+), 44 deletions(-) diff --git a/src/Collision/Fixed/SpatialHash2D.cs b/src/Collision/Fixed/SpatialHash2D.cs index 0821799..9d1a87a 100644 --- a/src/Collision/Fixed/SpatialHash2D.cs +++ b/src/Collision/Fixed/SpatialHash2D.cs @@ -7,17 +7,20 @@ 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 : struct, System.IEquatable where U : IHasAABB2D + public class SpatialHash2D where T : struct, System.IEquatable { private readonly Fix64 cellSize; private readonly Dictionary> hashDictionary = new Dictionary>(); - private readonly Dictionary IDLookup = new Dictionary(); + private readonly Dictionary IDBoxLookup = new Dictionary(); + private readonly Dictionary IDDataLookup = new Dictionary(); - public int MinX { get; private set; } = 0; - public int MaxX { get; private set; } = 0; - public int MinY { get; private set; } = 0; - public int MaxY { get; private set; } = 0; + private readonly HashSet DynamicIDs = new HashSet(); + + private int MinX; + private int MaxX; + private int MinY; + private int MaxY; private Queue> hashSetPool = new Queue>(); @@ -35,12 +38,11 @@ namespace MoonWorks.Collision.Fixed /// Inserts an element into the SpatialHash. /// /// A unique ID for the shape-transform pair. - /// - /// - /// A bitmask value specifying the groups this object belongs to. - public void Insert(T id, U shape, Transform2D transform2D, uint collisionGroups = uint.MaxValue) + public void Insert(T id, AABB2D aabb, Transform2D transform2D, U data, bool dynamic = true) { - var box = AABB2D.Transformed(shape.AABB, transform2D); + Remove(id); + + var box = AABB2D.Transformed(aabb, transform2D); var minHash = Hash(box.Min); var maxHash = Hash(box.Max); @@ -52,19 +54,26 @@ namespace MoonWorks.Collision.Fixed } hashDictionary[key].Add(id); - IDLookup[id] = (shape, transform2D, collisionGroups); + IDDataLookup[id] = data; } MinX = System.Math.Min(MinX, minHash.Item1); MinY = System.Math.Min(MinY, minHash.Item2); MaxX = System.Math.Max(MaxX, maxHash.Item1); MaxY = System.Math.Max(MaxY, maxHash.Item2); + + if (dynamic) + { + DynamicIDs.Add(id); + } + + IDBoxLookup[id] = box; } /// /// Retrieves all the potential collisions of a shape-transform pair. Excludes any shape-transforms with the given ID. /// - public RetrieveEnumerator Retrieve(T id, V hasAABB, Transform2D transform2D, uint collisionMask = uint.MaxValue) where V : IHasAABB2D + public RetrieveEnumerator Retrieve(T id, V hasAABB, Transform2D transform2D) where V : IHasAABB2D { var box = AABB2D.Transformed(hasAABB.AABB, transform2D); var (minX, minY) = Hash(box.Min); @@ -78,18 +87,17 @@ namespace MoonWorks.Collision.Fixed return new RetrieveEnumerator( this, Keys(minX, minY, maxX, maxY), - id, - collisionMask + id ); } /// /// Retrieves all the potential collisions of a shape-transform pair. /// - public RetrieveEnumerator Retrieve(V hasAABB, Transform2D transform2D, uint collisionMask = uint.MaxValue) where V : IHasAABB2D + public RetrieveEnumerator Retrieve(V hasAABB, Transform2D transform2D) where V : IHasAABB2D { var box = AABB2D.Transformed(hasAABB.AABB, transform2D); - return Retrieve(box, collisionMask); + return Retrieve(box); } /// @@ -97,7 +105,7 @@ namespace MoonWorks.Collision.Fixed /// /// A transformed AABB. /// - public RetrieveEnumerator Retrieve(AABB2D aabb, uint collisionMask = uint.MaxValue) + public RetrieveEnumerator Retrieve(T id, AABB2D aabb) { var (minX, minY) = Hash(aabb.Min); var (maxX, maxY) = Hash(aabb.Max); @@ -110,14 +118,29 @@ namespace MoonWorks.Collision.Fixed return new RetrieveEnumerator( this, Keys(minX, minY, maxX, maxY), - collisionMask + id ); } - public void Update(T id, U shape, Transform2D transform2D, uint collisionGroups = uint.MaxValue) + /// + /// Retrieves objects based on a pre-transformed AABB. + /// + /// A transformed AABB. + /// + public RetrieveEnumerator Retrieve(AABB2D aabb) { - Remove(id); - Insert(id, shape, transform2D, collisionGroups); + var (minX, minY) = Hash(aabb.Min); + var (maxX, maxY) = Hash(aabb.Max); + + if (minX < MinX) { minX = MinX; } + if (maxX > MaxX) { maxX = MaxX; } + if (minY < MinY) { minY = MinY; } + if (maxY > MaxY) { maxY = MaxY; } + + return new RetrieveEnumerator( + this, + Keys(minX, minY, maxX, maxY) + ); } /// @@ -125,24 +148,24 @@ namespace MoonWorks.Collision.Fixed /// public void Remove(T id) { - if (IDLookup.TryGetValue(id, out var data)) + if (IDBoxLookup.TryGetValue(id, out var aabb)) { - var (shape, transform, collisionGroups) = data; - - var box = AABB2D.Transformed(shape.AABB, transform); - var minHash = Hash(box.Min); - var maxHash = Hash(box.Max); + var minHash = Hash(aabb.Min); + var maxHash = Hash(aabb.Max); foreach (var key in Keys(minHash.Item1, minHash.Item2, maxHash.Item1, maxHash.Item2)) { - if (hashDictionary.ContainsKey(key)) + if (hashDictionary.TryGetValue(key, out HashSet value)) { - hashDictionary[key].Remove(id); + value.Remove(id); } } - IDLookup.Remove(id); + IDDataLookup.Remove(id); + IDBoxLookup.Remove(id); } + + DynamicIDs.Remove(id); } /// @@ -155,7 +178,19 @@ namespace MoonWorks.Collision.Fixed hash.Clear(); } - IDLookup.Clear(); + IDDataLookup.Clear(); + IDBoxLookup.Clear(); + } + + /// + /// Removes + /// + public void ClearDynamic() + { + foreach (var id in DynamicIDs) + { + Remove(id); + } } private static long MakeLong(int left, int right) @@ -239,15 +274,13 @@ namespace MoonWorks.Collision.Fixed 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 + T id ) { SpatialHash = spatialHash; KeysEnumerator = keysEnumerator; @@ -255,13 +288,11 @@ namespace MoonWorks.Collision.Fixed HashSetEnumeratorActive = false; Duplicates = SpatialHash.AcquireHashSet(); ID = id; - CollisionMask = collisionMask; } internal RetrieveEnumerator( SpatialHash2D spatialHash, - KeysEnumerator keysEnumerator, - uint collisionMask + KeysEnumerator keysEnumerator ) { SpatialHash = spatialHash; KeysEnumerator = keysEnumerator; @@ -269,7 +300,6 @@ namespace MoonWorks.Collision.Fixed HashSetEnumeratorActive = false; Duplicates = SpatialHash.AcquireHashSet(); ID = null; - CollisionMask = collisionMask; } public bool MoveNext() @@ -293,7 +323,6 @@ namespace MoonWorks.Collision.Fixed // conditions var t = HashSetEnumerator.Current; - var collisionGroups = SpatialHash.IDLookup[t].Item3; if (Duplicates.Contains(t)) { @@ -302,7 +331,7 @@ namespace MoonWorks.Collision.Fixed if (ID.HasValue) { - if (ID.Value.Equals(t) || (CollisionMask & collisionGroups) == 0) + if (ID.Value.Equals(t)) { return MoveNext(); } @@ -312,13 +341,13 @@ namespace MoonWorks.Collision.Fixed return true; } - public (T, U, Transform2D, uint) Current + public (T, U) Current { get { var t = HashSetEnumerator.Current; - var (u, transform, groups) = SpatialHash.IDLookup[t]; - return (t, u, transform, groups); + var u = SpatialHash.IDDataLookup[t]; + return (t, u); } } } -- 2.25.1 From 10db4f95c8851698d3bba8634ce4870d98332d8c Mon Sep 17 00:00:00 2001 From: cosmonaut Date: Sun, 15 Jan 2023 12:41:02 -0800 Subject: [PATCH 4/7] AABB transform shortcut + sweep test --- src/Collision/Fixed/AABB2D.cs | 104 ++++++++++++++++++++++++++++++++-- 1 file changed, 100 insertions(+), 4 deletions(-) diff --git a/src/Collision/Fixed/AABB2D.cs b/src/Collision/Fixed/AABB2D.cs index 025ddaf..4a3e8d2 100644 --- a/src/Collision/Fixed/AABB2D.cs +++ b/src/Collision/Fixed/AABB2D.cs @@ -66,6 +66,8 @@ namespace MoonWorks.Collision.Fixed ); } + static readonly Fix64 Half = Fix64.FromFraction(1, 2); + /// /// Efficiently transforms the AABB by a Transform2D. /// @@ -74,11 +76,42 @@ namespace MoonWorks.Collision.Fixed /// public static AABB2D Transformed(AABB2D aabb, Transform2D transform) { - var two = new Fix64(2); - var center = (aabb.Min + aabb.Max) / two; - var extent = (aabb.Max - aabb.Min) / two; + if (transform.IsAxisAligned) + { + var min = aabb.Min * transform.Scale + transform.Position; + var max = aabb.Max * transform.Scale + transform.Position; - var newCenter = Vector2.Transform(center, transform.TransformMatrix); + Fix64 minX, minY, maxX, maxY; + + if (min.X <= max.X) + { + minX = min.X; + maxX = max.X; + } + else + { + minX = max.X; + maxX = min.X; + } + + if (min.Y <= max.Y) + { + minY = min.Y; + maxY = max.Y; + } + else + { + minY = max.Y; + maxY = min.Y; + } + + return new AABB2D(minX, minY, maxX, maxY); + } + + var center = (aabb.Min + aabb.Max) * Half; + var extent = aabb.Max - center; + + var newCenter = Vector2.Transform(center, transform.TransformMatrix); var newExtent = Vector2.TransformNormal(extent, AbsoluteMatrix(transform.TransformMatrix)); return new AABB2D(newCenter - newExtent, newCenter + newExtent); @@ -152,6 +185,69 @@ namespace MoonWorks.Collision.Fixed return a.Left < b.Right && a.Right > b.Left && a.Top < b.Bottom && a.Bottom > b.Top; } + // FIXME: this is broken + public static bool SweepTest(AABB2D a, AABB2D b, Vector2 aMovement, Vector2 bMovement, out Fix64 normalizedTime) + { + if (TestOverlap(a, b)) + { + normalizedTime = Fix64.Zero; + return true; + } + + normalizedTime = Fix64.One; + + var relativeVelocity = bMovement - aMovement; + + Vector2 entry = Vector2.Zero; + if (a.Max.X < b.Min.X && relativeVelocity.X < 0) + { + entry.X = (a.Max.X - b.Min.X) / relativeVelocity.X; + } + else if (b.Max.X < a.Min.X && relativeVelocity.X > 0) + { + entry.X = (a.Min.X - b.Max.X) / relativeVelocity.X; + } + + if (a.Max.Y < b.Min.Y && relativeVelocity.Y < 0) + { + entry.Y = (a.Max.Y - b.Min.Y) / relativeVelocity.Y; + } + else if (b.Max.Y > a.Min.Y && relativeVelocity.Y > 0) + { + entry.Y = (a.Min.Y - b.Max.Y) / relativeVelocity.Y; + } + + Vector2 exit = new Vector2(Fix64.MaxValue, Fix64.MaxValue); + if (b.Max.X > a.Min.X && relativeVelocity.X < 0) + { + exit.X = (a.Min.X - b.Max.X) / relativeVelocity.X; + } + else if (a.Max.X > b.Min.X && relativeVelocity.X > 0) + { + exit.Y = (a.Max.X - b.Min.X) / relativeVelocity.X; + } + + if (b.Max.Y > a.Min.Y && relativeVelocity.Y < 0) + { + exit.Y = (a.Min.Y - b.Max.Y) / relativeVelocity.Y; + } + else if (a.Max.Y > b.Min.Y && relativeVelocity.Y > 0) + { + exit.Y = (a.Max.Y - b.Min.Y) / relativeVelocity.Y; + } + + Fix64 firstTime = Fix64.Max(entry.X, entry.Y); + Fix64 lastTime = Fix64.Min(exit.X, exit.Y); + + if (firstTime <= lastTime && firstTime > 0) + { + normalizedTime = firstTime; + return true; + } + + return false; + } + public override bool Equals(object obj) { return obj is AABB2D aabb && Equals(aabb); -- 2.25.1 From 72573219eda42b2b93f390223bd078f229b6df9b Mon Sep 17 00:00:00 2001 From: cosmonaut Date: Sun, 15 Jan 2023 12:41:08 -0800 Subject: [PATCH 5/7] fix uniform scale check --- src/Math/Fixed/Transform2D.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Math/Fixed/Transform2D.cs b/src/Math/Fixed/Transform2D.cs index a43081c..208b358 100644 --- a/src/Math/Fixed/Transform2D.cs +++ b/src/Math/Fixed/Transform2D.cs @@ -24,7 +24,7 @@ } public bool IsAxisAligned => Rotation % Fix64.PiOver2 == Fix64.Zero; - public bool IsUniformScale => Scale.X == Scale.Y; + public bool IsUniformScale => Scale.X == Scale.Y || Scale.X == -Scale.Y; public static readonly Transform2D Identity = new Transform2D(Vector2.Zero, Fix64.Zero, Vector2.One); -- 2.25.1 From 1916415fb4c71869d4ccd7170638ed96f06fb6a9 Mon Sep 17 00:00:00 2001 From: evan Date: Sun, 15 Jan 2023 16:41:36 -0800 Subject: [PATCH 6/7] line test + improving AABB sweep --- src/Collision/Fixed/AABB2D.cs | 82 ++----- src/Collision/Fixed/NarrowPhase.cs | 370 ++++++++++++++++++----------- 2 files changed, 258 insertions(+), 194 deletions(-) diff --git a/src/Collision/Fixed/AABB2D.cs b/src/Collision/Fixed/AABB2D.cs index 4a3e8d2..f10471a 100644 --- a/src/Collision/Fixed/AABB2D.cs +++ b/src/Collision/Fixed/AABB2D.cs @@ -66,6 +66,14 @@ namespace MoonWorks.Collision.Fixed ); } + // When you don't know which values are smaller! + public static AABB2D Create(Fix64 x1, Fix64 y1, Fix64 x2, Fix64 y2) + { + var min = new Vector2(Fix64.Min(x1, x2), Fix64.Min(y1, y2)); + var max = new Vector2(Fix64.Max(x1, x2), Fix64.Max(y1, y2)); + return new AABB2D(min, max); + } + static readonly Fix64 Half = Fix64.FromFraction(1, 2); /// @@ -112,9 +120,9 @@ namespace MoonWorks.Collision.Fixed var extent = aabb.Max - center; var newCenter = Vector2.Transform(center, transform.TransformMatrix); - var newExtent = Vector2.TransformNormal(extent, AbsoluteMatrix(transform.TransformMatrix)); + var newExtent = Vector2.TransformNormal(extent, AbsoluteMatrix(transform.TransformMatrix)); - return new AABB2D(newCenter - newExtent, newCenter + newExtent); + return new AABB2D(newCenter - newExtent, newCenter + newExtent); } public AABB2D Compose(AABB2D aabb) @@ -185,67 +193,25 @@ namespace MoonWorks.Collision.Fixed return a.Left < b.Right && a.Right > b.Left && a.Top < b.Bottom && a.Bottom > b.Top; } - // FIXME: this is broken - public static bool SweepTest(AABB2D a, AABB2D b, Vector2 aMovement, Vector2 bMovement, out Fix64 normalizedTime) + private static AABB2D RotateAroundOrigin(AABB2D aabb) { - if (TestOverlap(a, b)) - { - normalizedTime = Fix64.Zero; - return true; - } + return Create(-aabb.Min.X, -aabb.Min.Y, -aabb.Max.X, -aabb.Max.Y); + } - normalizedTime = Fix64.One; + private static AABB2D MinkowskiSum(AABB2D a, AABB2D b) + { + return new AABB2D(a.Min.X + b.Min.X, a.Min.Y + b.Min.Y, a.Max.X + b.Max.X, a.Max.Y + b.Max.Y); + } - var relativeVelocity = bMovement - aMovement; + public static bool SweepTest(AABB2D a, AABB2D b, Vector2 aMovement, Vector2 bMovement, out Fix64 entry, out Fix64 exit) + { + var rotatedA = RotateAroundOrigin(a); + var sum = MinkowskiSum(rotatedA, b); - Vector2 entry = Vector2.Zero; - if (a.Max.X < b.Min.X && relativeVelocity.X < 0) - { - entry.X = (a.Max.X - b.Min.X) / relativeVelocity.X; - } - else if (b.Max.X < a.Min.X && relativeVelocity.X > 0) - { - entry.X = (a.Min.X - b.Max.X) / relativeVelocity.X; - } + var relativeMovement = aMovement - bMovement; + var line = new Line(Vector2.Zero, relativeMovement); - if (a.Max.Y < b.Min.Y && relativeVelocity.Y < 0) - { - entry.Y = (a.Max.Y - b.Min.Y) / relativeVelocity.Y; - } - else if (b.Max.Y > a.Min.Y && relativeVelocity.Y > 0) - { - entry.Y = (a.Min.Y - b.Max.Y) / relativeVelocity.Y; - } - - Vector2 exit = new Vector2(Fix64.MaxValue, Fix64.MaxValue); - if (b.Max.X > a.Min.X && relativeVelocity.X < 0) - { - exit.X = (a.Min.X - b.Max.X) / relativeVelocity.X; - } - else if (a.Max.X > b.Min.X && relativeVelocity.X > 0) - { - exit.Y = (a.Max.X - b.Min.X) / relativeVelocity.X; - } - - if (b.Max.Y > a.Min.Y && relativeVelocity.Y < 0) - { - exit.Y = (a.Min.Y - b.Max.Y) / relativeVelocity.Y; - } - else if (a.Max.Y > b.Min.Y && relativeVelocity.Y > 0) - { - exit.Y = (a.Max.Y - b.Min.Y) / relativeVelocity.Y; - } - - Fix64 firstTime = Fix64.Max(entry.X, entry.Y); - Fix64 lastTime = Fix64.Min(exit.X, exit.Y); - - if (firstTime <= lastTime && firstTime > 0) - { - normalizedTime = firstTime; - return true; - } - - return false; + return NarrowPhase.TestLineAABBOverlap(line, sum, out entry, out exit); } public override bool Equals(object obj) diff --git a/src/Collision/Fixed/NarrowPhase.cs b/src/Collision/Fixed/NarrowPhase.cs index aae4950..4339d20 100644 --- a/src/Collision/Fixed/NarrowPhase.cs +++ b/src/Collision/Fixed/NarrowPhase.cs @@ -93,6 +93,13 @@ namespace MoonWorks.Collision.Fixed return TestCollision(rectangle, transformA, rectangleB, transformB); } } + else if (shapeA is Line line) + { + if (shapeB is Line lineB) + { + return TestCollision(line, transformA, lineB, transformB); + } + } return FindCollisionSimplex(shapeA, transformA, shapeB, transformB).Item1; } @@ -172,6 +179,11 @@ namespace MoonWorks.Collision.Fixed } } + public static bool TestCollision(in Line lineA, in Transform2D transformA, in Line lineB, in Transform2D transformB) + { + return TestLineOverlap(lineA, transformA, lineB, transformB); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static bool TestRectangleOverlap(in Rectangle rectangleA, in Transform2D transformA, in Rectangle rectangleB, in Transform2D transformB) { @@ -237,6 +249,92 @@ namespace MoonWorks.Collision.Fixed return distanceSquared < radiusSumSquared; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool TestLineOverlap(in Line lineA, in Transform2D transformA, in Line lineB, in Transform2D transformB) + { + var b = lineA.End - lineA.Start; + var d = lineB.End - lineB.Start; + var bDotDPerp = b.X * d.Y - b.Y * d.X; + + if (bDotDPerp == Fix64.Zero) + { + return false; + } + + var c = lineB.Start - lineA.Start; + var t = (c.X * d.Y - c.Y * d.X) / bDotDPerp; + if (t < 0 || t > 1) + { + return false; + } + + var u = (c.X * b.Y - c.Y * b.X) / bDotDPerp; + if (u < 0 || u > 1) + { + return false; + } + + return true; + } + + public static bool TestLineOverlap(in Line lineA, in Transform2D transformA, in Line lineB, in Transform2D transformB, out Vector2 intersection) + { + intersection = Vector2.Zero; + + var b = lineA.End - lineA.Start; + var d = lineB.End - lineB.Start; + var bDotDPerp = b.X * d.Y - b.Y * d.X; + + if (bDotDPerp == Fix64.Zero) + { + return false; + } + + var c = lineB.Start - lineA.Start; + var t = (c.X * d.Y - c.Y * d.X) / bDotDPerp; + if (t < 0 || t > 1) + { + return false; + } + + var u = (c.X * b.Y - c.Y * b.X) / bDotDPerp; + if (u < 0 || u > 1) + { + return false; + } + + intersection = lineA.Start + t * b; + return true; + } + + public static bool TestLineAABBOverlap(in Line line, in AABB2D aabb, out Fix64 entry, out Fix64 exit) + { + entry = Fix64.MinValue; + exit = Fix64.MaxValue; + + var lineDirection = line.End - line.Start; + + if (lineDirection.X != Fix64.Zero) + { + var tx1 = (aabb.Min.X - line.Start.X) / lineDirection.X; + var tx2 = (aabb.Max.X - line.Start.X) / lineDirection.X; + + entry = Fix64.Max(entry, Fix64.Min(tx1, tx2)); + exit = Fix64.Min(exit, Fix64.Max(tx1, tx2)); + } + + if (lineDirection.Y != Fix64.Zero) + { + var ty1 = (aabb.Min.Y - line.Start.Y) / lineDirection.Y; + var ty2 = (aabb.Max.Y - line.Start.Y) / lineDirection.Y; + + entry = Fix64.Max(entry, Fix64.Min(ty1, ty2)); + exit = Fix64.Min(exit, Fix64.Max(ty1, ty2)); + } + + return exit >= entry; + } + public static bool TestPointOverlap(in Point pointA, in Transform2D transformA, in Point pointB, in Transform2D transformB) { return transformA.Position == transformB.Position; @@ -250,47 +348,47 @@ namespace MoonWorks.Collision.Fixed return Check(minkowskiDifference, c, b); } - 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)); } + 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 epsilon = Fix64.FromFraction(1, 10000); var a = simplex.A; - var b = simplex.B.Value; - var c = simplex.C.Value; + var b = simplex.B.Value; + var c = simplex.C.Value; - Vector2 intersection = default; + Vector2 intersection = default; - for (var i = 0; i < 32; i++) - { - var edge = FindClosestEdge(simplex); - var support = CalculateSupport(shapeA, Transform2DA, shapeB, Transform2DB, edge.Normal); - var distance = Vector2.Dot(support, edge.Normal); + for (var i = 0; i < 32; i++) + { + var edge = FindClosestEdge(simplex); + var support = CalculateSupport(shapeA, Transform2DA, shapeB, Transform2DB, edge.Normal); + var distance = Vector2.Dot(support, edge.Normal); - intersection = edge.Normal; - intersection *= distance; + intersection = edge.Normal; + intersection *= distance; - if (Fix64.Abs(distance - edge.Distance) <= epsilon) - { - return intersection; - } - else - { + if (Fix64.Abs(distance - edge.Distance) <= epsilon) + { + return intersection; + } + else + { simplex.Insert(support, edge.Index); - } - } + } + } - return intersection; // close enough - } + return intersection; // close enough + } - private static unsafe Edge FindClosestEdge(Simplex2D simplex) - { + private static unsafe Edge FindClosestEdge(Simplex2D simplex) + { var closestDistance = Fix64.MaxValue; - var closestNormal = Vector2.Zero; - var closestIndex = 0; + var closestNormal = Vector2.Zero; + var closestIndex = 0; for (var i = 0; i < 4; i += 1) { @@ -315,136 +413,136 @@ namespace MoonWorks.Collision.Fixed } } - return new Edge + return new Edge { Distance = closestDistance, Normal = closestNormal, Index = closestIndex }; - } + } [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); - } + { + return shapeA.Support(direction, Transform2DA) - shapeB.Support(-direction, Transform2DB); + } private static (bool, Simplex2D) Check(MinkowskiDifference minkowskiDifference, Vector2 c, Vector2 b) where T : IShape2D where U : IShape2D - { - var cb = c - b; - var c0 = -c; - var d = Direction(cb, c0); - return DoSimplex(minkowskiDifference, new Simplex2D(b, c), d); - } + { + 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) where T : IShape2D where U : IShape2D - { - var a = minkowskiDifference.Support(direction); - var notPastOrigin = Vector2.Dot(a, direction) < Fix64.Zero; - var (intersects, newSimplex, newDirection) = EnclosesOrigin(a, simplex); + 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; + 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); - } - } + 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) 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) 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); + 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); - } - } + 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, Fix64.Zero); - var B = new Vector3(b.X, b.Y, Fix64.Zero); - var C = new Vector3(c.X, c.Y, Fix64.Zero); + private static Vector2 TripleProduct(Vector2 a, Vector2 b, Vector2 c) + { + var A = new Vector3(a.X, a.Y, Fix64.Zero); + var B = new Vector3(b.X, b.Y, Fix64.Zero); + var C = new Vector3(c.X, c.Y, Fix64.Zero); - var first = Vector3.Cross(A, B); - var second = Vector3.Cross(first, C); + var first = Vector3.Cross(A, B); + var second = Vector3.Cross(first, C); - return new Vector2(second.X, second.Y); - } + 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 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; + } [MethodImpl(MethodImplOptions.AggressiveInlining)] private static bool SameDirection(Vector2 a, Vector2 b) - { - return Vector2.Dot(a, b) > Fix64.Zero; - } + { + return Vector2.Dot(a, b) > Fix64.Zero; + } - private static Vector2 Perpendicular(Vector2 a, Vector2 b) - { - return TripleProduct(a, b, a); - } + private static Vector2 Perpendicular(Vector2 a, Vector2 b) + { + return TripleProduct(a, b, a); + } } } -- 2.25.1 From d612e8538a074b351f9f4438ef91aaaa29e51e24 Mon Sep 17 00:00:00 2001 From: cosmonaut Date: Fri, 3 Feb 2023 11:44:04 -0800 Subject: [PATCH 7/7] delete entire collision system --- src/Collision/Fixed/AABB2D.cs | 243 --------- src/Collision/Fixed/Collider.cs | 33 -- src/Collision/Fixed/IHasAABB2D.cs | 7 - src/Collision/Fixed/IShape2D.cs | 15 - src/Collision/Fixed/MinkowskiDifference.cs | 28 -- src/Collision/Fixed/NarrowPhase.cs | 548 --------------------- src/Collision/Fixed/Shapes/Circle.cs | 65 --- src/Collision/Fixed/Shapes/Line.cs | 75 --- src/Collision/Fixed/Shapes/Point.cs | 61 --- src/Collision/Fixed/Shapes/Rectangle.cs | 130 ----- src/Collision/Fixed/Simplex2D.cs | 109 ---- src/Collision/Fixed/SpatialHash2D.cs | 355 ------------- src/Collision/Float/AABB2D.cs | 174 ------- src/Collision/Float/ICollidable.cs | 12 - src/Collision/Float/IShape2D.cs | 15 - src/Collision/Float/MinkowskiDifference.cs | 57 --- src/Collision/Float/NarrowPhase.cs | 331 ------------- src/Collision/Float/Shapes/Circle.cs | 67 --- src/Collision/Float/Shapes/Line.cs | 83 ---- src/Collision/Float/Shapes/Point.cs | 61 --- src/Collision/Float/Shapes/Rectangle.cs | 115 ----- src/Collision/Float/Simplex2D.cs | 136 ----- src/Collision/Float/SpatialHash2D.cs | 253 ---------- 23 files changed, 2973 deletions(-) delete mode 100644 src/Collision/Fixed/AABB2D.cs delete mode 100644 src/Collision/Fixed/Collider.cs delete mode 100644 src/Collision/Fixed/IHasAABB2D.cs delete mode 100644 src/Collision/Fixed/IShape2D.cs delete mode 100644 src/Collision/Fixed/MinkowskiDifference.cs delete mode 100644 src/Collision/Fixed/NarrowPhase.cs delete mode 100644 src/Collision/Fixed/Shapes/Circle.cs delete mode 100644 src/Collision/Fixed/Shapes/Line.cs delete mode 100644 src/Collision/Fixed/Shapes/Point.cs delete mode 100644 src/Collision/Fixed/Shapes/Rectangle.cs delete mode 100644 src/Collision/Fixed/Simplex2D.cs delete mode 100644 src/Collision/Fixed/SpatialHash2D.cs delete mode 100644 src/Collision/Float/AABB2D.cs delete mode 100644 src/Collision/Float/ICollidable.cs delete mode 100644 src/Collision/Float/IShape2D.cs delete mode 100644 src/Collision/Float/MinkowskiDifference.cs delete mode 100644 src/Collision/Float/NarrowPhase.cs delete mode 100644 src/Collision/Float/Shapes/Circle.cs delete mode 100644 src/Collision/Float/Shapes/Line.cs delete mode 100644 src/Collision/Float/Shapes/Point.cs delete mode 100644 src/Collision/Float/Shapes/Rectangle.cs delete mode 100644 src/Collision/Float/Simplex2D.cs delete mode 100644 src/Collision/Float/SpatialHash2D.cs diff --git a/src/Collision/Fixed/AABB2D.cs b/src/Collision/Fixed/AABB2D.cs deleted file mode 100644 index f10471a..0000000 --- a/src/Collision/Fixed/AABB2D.cs +++ /dev/null @@ -1,243 +0,0 @@ -using System.Collections.Generic; -using MoonWorks.Math.Fixed; - -namespace MoonWorks.Collision.Fixed -{ - /// - /// Axis-aligned bounding box. - /// - public struct AABB2D : System.IEquatable - { - /// - /// The top-left position of the AABB. - /// - /// - public Vector2 Min { get; private set; } - - /// - /// The bottom-right position of the AABB. - /// - /// - public Vector2 Max { get; private set; } - - public Fix64 Width { get { return Max.X - Min.X; } } - public Fix64 Height { get { return Max.Y - Min.Y; } } - - public Fix64 Right { get { return Max.X; } } - public Fix64 Left { get { return Min.X; } } - - /// - /// The top of the AABB. Assumes a downward-aligned Y axis, so this value will be smaller than Bottom. - /// - /// - public Fix64 Top { get { return Min.Y; } } - - /// - /// The bottom of the AABB. Assumes a downward-aligned Y axis, so this value will be larger than Top. - /// - /// - public Fix64 Bottom { get { return Max.Y; } } - - public AABB2D(Fix64 minX, Fix64 minY, Fix64 maxX, Fix64 maxY) - { - Min = new Vector2(minX, minY); - Max = new Vector2(maxX, maxY); - } - - public AABB2D(int minX, int minY, int maxX, int maxY) - { - Min = new Vector2(minX, minY); - Max = new Vector2(maxX, maxY); - } - - public AABB2D(Vector2 min, Vector2 max) - { - Min = min; - Max = max; - } - - private static Matrix3x2 AbsoluteMatrix(Matrix3x2 matrix) - { - return new Matrix3x2 - ( - Fix64.Abs(matrix.M11), Fix64.Abs(matrix.M12), - Fix64.Abs(matrix.M21), Fix64.Abs(matrix.M22), - Fix64.Abs(matrix.M31), Fix64.Abs(matrix.M32) - ); - } - - // When you don't know which values are smaller! - public static AABB2D Create(Fix64 x1, Fix64 y1, Fix64 x2, Fix64 y2) - { - var min = new Vector2(Fix64.Min(x1, x2), Fix64.Min(y1, y2)); - var max = new Vector2(Fix64.Max(x1, x2), Fix64.Max(y1, y2)); - return new AABB2D(min, max); - } - - static readonly Fix64 Half = Fix64.FromFraction(1, 2); - - /// - /// Efficiently transforms the AABB by a Transform2D. - /// - /// - /// - /// - public static AABB2D Transformed(AABB2D aabb, Transform2D transform) - { - if (transform.IsAxisAligned) - { - var min = aabb.Min * transform.Scale + transform.Position; - var max = aabb.Max * transform.Scale + transform.Position; - - Fix64 minX, minY, maxX, maxY; - - if (min.X <= max.X) - { - minX = min.X; - maxX = max.X; - } - else - { - minX = max.X; - maxX = min.X; - } - - if (min.Y <= max.Y) - { - minY = min.Y; - maxY = max.Y; - } - else - { - minY = max.Y; - maxY = min.Y; - } - - return new AABB2D(minX, minY, maxX, maxY); - } - - var center = (aabb.Min + aabb.Max) * Half; - var extent = aabb.Max - center; - - var newCenter = Vector2.Transform(center, transform.TransformMatrix); - var newExtent = Vector2.TransformNormal(extent, AbsoluteMatrix(transform.TransformMatrix)); - - return new AABB2D(newCenter - newExtent, newCenter + newExtent); - } - - public AABB2D Compose(AABB2D aabb) - { - Fix64 left = Left; - Fix64 top = Top; - Fix64 right = Right; - Fix64 bottom = Bottom; - - if (aabb.Left < left) - { - left = aabb.Left; - } - if (aabb.Right > right) - { - right = aabb.Right; - } - if (aabb.Top < top) - { - top = aabb.Top; - } - if (aabb.Bottom > bottom) - { - bottom = aabb.Bottom; - } - - return new AABB2D(left, top, right, bottom); - } - - /// - /// Creates an AABB for an arbitrary collection of positions. - /// This is less efficient than defining a custom AABB method for most shapes, so avoid using this if possible. - /// - /// - /// - public static AABB2D FromVertices(IEnumerable vertices) - { - var minX = Fix64.MaxValue; - var minY = Fix64.MaxValue; - var maxX = Fix64.MinValue; - var maxY = Fix64.MinValue; - - foreach (var vertex in vertices) - { - if (vertex.X < minX) - { - minX = vertex.X; - } - if (vertex.Y < minY) - { - minY = vertex.Y; - } - if (vertex.X > maxX) - { - maxX = vertex.X; - } - if (vertex.Y > maxY) - { - maxY = vertex.Y; - } - } - - return new AABB2D(minX, minY, maxX, maxY); - } - - public static bool TestOverlap(AABB2D a, AABB2D b) - { - return a.Left < b.Right && a.Right > b.Left && a.Top < b.Bottom && a.Bottom > b.Top; - } - - private static AABB2D RotateAroundOrigin(AABB2D aabb) - { - return Create(-aabb.Min.X, -aabb.Min.Y, -aabb.Max.X, -aabb.Max.Y); - } - - private static AABB2D MinkowskiSum(AABB2D a, AABB2D b) - { - return new AABB2D(a.Min.X + b.Min.X, a.Min.Y + b.Min.Y, a.Max.X + b.Max.X, a.Max.Y + b.Max.Y); - } - - public static bool SweepTest(AABB2D a, AABB2D b, Vector2 aMovement, Vector2 bMovement, out Fix64 entry, out Fix64 exit) - { - var rotatedA = RotateAroundOrigin(a); - var sum = MinkowskiSum(rotatedA, b); - - var relativeMovement = aMovement - bMovement; - var line = new Line(Vector2.Zero, relativeMovement); - - return NarrowPhase.TestLineAABBOverlap(line, sum, out entry, out exit); - } - - public override bool Equals(object obj) - { - return obj is AABB2D aabb && Equals(aabb); - } - - public bool Equals(AABB2D other) - { - return Min == other.Min && - Max == other.Max; - } - - public override int GetHashCode() - { - return System.HashCode.Combine(Min, Max); - } - - public static bool operator ==(AABB2D left, AABB2D right) - { - return left.Equals(right); - } - - public static bool operator !=(AABB2D left, AABB2D right) - { - return !(left == right); - } - } -} diff --git a/src/Collision/Fixed/Collider.cs b/src/Collision/Fixed/Collider.cs deleted file mode 100644 index db27e38..0000000 --- a/src/Collision/Fixed/Collider.cs +++ /dev/null @@ -1,33 +0,0 @@ -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/IHasAABB2D.cs b/src/Collision/Fixed/IHasAABB2D.cs deleted file mode 100644 index e8c17cd..0000000 --- a/src/Collision/Fixed/IHasAABB2D.cs +++ /dev/null @@ -1,7 +0,0 @@ -namespace MoonWorks.Collision.Fixed -{ - public interface IHasAABB2D - { - AABB2D AABB { get; } - } -} diff --git a/src/Collision/Fixed/IShape2D.cs b/src/Collision/Fixed/IShape2D.cs deleted file mode 100644 index 05f0fc6..0000000 --- a/src/Collision/Fixed/IShape2D.cs +++ /dev/null @@ -1,15 +0,0 @@ -using MoonWorks.Math.Fixed; - -namespace MoonWorks.Collision.Fixed -{ - public interface IShape2D : IHasAABB2D, System.IEquatable - { - /// - /// A Minkowski support function. Gives the farthest point on the edge of a shape along the given direction. - /// - /// A normalized Vector2. - /// A Transform for transforming the shape vertices. - /// The farthest point on the edge of the shape along the given direction. - Vector2 Support(Vector2 direction, Transform2D transform); - } -} diff --git a/src/Collision/Fixed/MinkowskiDifference.cs b/src/Collision/Fixed/MinkowskiDifference.cs deleted file mode 100644 index e9af4c6..0000000 --- a/src/Collision/Fixed/MinkowskiDifference.cs +++ /dev/null @@ -1,28 +0,0 @@ -using MoonWorks.Math.Fixed; - -namespace MoonWorks.Collision.Fixed -{ - /// - /// A Minkowski difference between two shapes. - /// - public struct MinkowskiDifference where T : IShape2D where U : IShape2D - { - private T ShapeA { get; } - private Transform2D TransformA { get; } - private U ShapeB { get; } - private Transform2D TransformB { get; } - - public MinkowskiDifference(T shapeA, Transform2D transformA, U shapeB, Transform2D transformB) - { - ShapeA = shapeA; - TransformA = transformA; - ShapeB = shapeB; - TransformB = transformB; - } - - public Vector2 Support(Vector2 direction) - { - return ShapeA.Support(direction, TransformA) - ShapeB.Support(-direction, TransformB); - } - } -} diff --git a/src/Collision/Fixed/NarrowPhase.cs b/src/Collision/Fixed/NarrowPhase.cs deleted file mode 100644 index 4339d20..0000000 --- a/src/Collision/Fixed/NarrowPhase.cs +++ /dev/null @@ -1,548 +0,0 @@ -using MoonWorks.Math.Fixed; -using System.Runtime.CompilerServices; - -namespace MoonWorks.Collision.Fixed -{ - public static class NarrowPhase - { - private struct Edge - { - public Fix64 Distance; - public Vector2 Normal; - public int Index; - } - - 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(Collider collider, in Transform2D transformA, U shape, in Transform2D transformB) where T : struct, IShape2D where U : struct, IShape2D - { - foreach (var colliderShape in collider) - { - 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); - } - } - else if (shapeA is Line line) - { - if (shapeB is Line lineB) - { - return TestCollision(line, transformA, lineB, transformB); - } - } - - return FindCollisionSimplex(shapeA, transformA, shapeB, transformB).Item1; - } - - 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; - } - } - - public static bool TestCollision(in Line lineA, in Transform2D transformA, in Line lineB, in Transform2D transformB) - { - return TestLineOverlap(lineA, transformA, lineB, transformB); - } - - [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); - - return firstAABB.Left < secondAABB.Right && firstAABB.Right > secondAABB.Left && firstAABB.Top < secondAABB.Bottom && firstAABB.Bottom > secondAABB.Top; - } - - [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); - - return transformedPoint.X > AABB.Left && transformedPoint.X < AABB.Right && transformedPoint.Y < AABB.Bottom && transformedPoint.Y > AABB.Top; - } - - [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; - - var distanceX = circleCenter.X - pointTransform.Position.X; - var distanceY = circleCenter.Y - pointTransform.Position.Y; - - return (distanceX * distanceX) + (distanceY * distanceY) < (circleRadius * circleRadius); - } - - /// - /// NOTE: The rectangle must be axis aligned, and the scaling of the circle must be uniform. - /// - [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; - var AABB = rectangle.TransformedAABB(rectangleTransform); - - var closestX = Fix64.Clamp(circleCenter.X, AABB.Left, AABB.Right); - var closestY = Fix64.Clamp(circleCenter.Y, AABB.Top, AABB.Bottom); - - var distanceX = circleCenter.X - closestX; - var distanceY = circleCenter.Y - closestY; - - var distanceSquared = (distanceX * distanceX) + (distanceY * distanceY); - return distanceSquared < (circleRadius * circleRadius); - } - - [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; - - var centerA = transformA.Position; - var centerB = transformB.Position; - - var distanceSquared = (centerA - centerB).LengthSquared(); - var radiusSumSquared = (radiusA + radiusB) * (radiusA + radiusB); - - return distanceSquared < radiusSumSquared; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static bool TestLineOverlap(in Line lineA, in Transform2D transformA, in Line lineB, in Transform2D transformB) - { - var b = lineA.End - lineA.Start; - var d = lineB.End - lineB.Start; - var bDotDPerp = b.X * d.Y - b.Y * d.X; - - if (bDotDPerp == Fix64.Zero) - { - return false; - } - - var c = lineB.Start - lineA.Start; - var t = (c.X * d.Y - c.Y * d.X) / bDotDPerp; - if (t < 0 || t > 1) - { - return false; - } - - var u = (c.X * b.Y - c.Y * b.X) / bDotDPerp; - if (u < 0 || u > 1) - { - return false; - } - - return true; - } - - public static bool TestLineOverlap(in Line lineA, in Transform2D transformA, in Line lineB, in Transform2D transformB, out Vector2 intersection) - { - intersection = Vector2.Zero; - - var b = lineA.End - lineA.Start; - var d = lineB.End - lineB.Start; - var bDotDPerp = b.X * d.Y - b.Y * d.X; - - if (bDotDPerp == Fix64.Zero) - { - return false; - } - - var c = lineB.Start - lineA.Start; - var t = (c.X * d.Y - c.Y * d.X) / bDotDPerp; - if (t < 0 || t > 1) - { - return false; - } - - var u = (c.X * b.Y - c.Y * b.X) / bDotDPerp; - if (u < 0 || u > 1) - { - return false; - } - - intersection = lineA.Start + t * b; - return true; - } - - public static bool TestLineAABBOverlap(in Line line, in AABB2D aabb, out Fix64 entry, out Fix64 exit) - { - entry = Fix64.MinValue; - exit = Fix64.MaxValue; - - var lineDirection = line.End - line.Start; - - if (lineDirection.X != Fix64.Zero) - { - var tx1 = (aabb.Min.X - line.Start.X) / lineDirection.X; - var tx2 = (aabb.Max.X - line.Start.X) / lineDirection.X; - - entry = Fix64.Max(entry, Fix64.Min(tx1, tx2)); - exit = Fix64.Min(exit, Fix64.Max(tx1, tx2)); - } - - if (lineDirection.Y != Fix64.Zero) - { - var ty1 = (aabb.Min.Y - line.Start.Y) / lineDirection.Y; - var ty2 = (aabb.Max.Y - line.Start.Y) / lineDirection.Y; - - entry = Fix64.Max(entry, Fix64.Min(ty1, ty2)); - exit = Fix64.Min(exit, Fix64.Max(ty1, ty2)); - } - - return exit >= entry; - } - - public static bool TestPointOverlap(in Point pointA, in Transform2D transformA, in Point pointB, in Transform2D 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); - } - - 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 epsilon = Fix64.FromFraction(1, 10000); - - var a = simplex.A; - var b = simplex.B.Value; - var c = simplex.C.Value; - - Vector2 intersection = default; - - for (var i = 0; i < 32; i++) - { - var edge = FindClosestEdge(simplex); - var support = CalculateSupport(shapeA, Transform2DA, shapeB, Transform2DB, edge.Normal); - var distance = Vector2.Dot(support, edge.Normal); - - intersection = edge.Normal; - intersection *= distance; - - if (Fix64.Abs(distance - edge.Distance) <= epsilon) - { - return intersection; - } - else - { - simplex.Insert(support, edge.Index); - } - } - - return intersection; // close enough - } - - private static unsafe Edge FindClosestEdge(Simplex2D simplex) - { - var closestDistance = Fix64.MaxValue; - var closestNormal = Vector2.Zero; - var closestIndex = 0; - - for (var i = 0; i < 4; i += 1) - { - var j = (i + 1 == 3) ? 0 : i + 1; - - var a = simplex[i]; - var b = simplex[j]; - - var e = b - a; - - var oa = a; - - var n = Vector2.Normalize(TripleProduct(e, oa, e)); - - var d = Vector2.Dot(n, a); - - if (d < closestDistance) - { - closestDistance = d; - closestNormal = n; - closestIndex = j; - } - } - - return new Edge - { - Distance = closestDistance, - Normal = closestNormal, - Index = closestIndex - }; - } - - [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) where T : IShape2D where U : IShape2D - { - 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) where T : IShape2D where U : IShape2D - { - var a = minkowskiDifference.Support(direction); - var notPastOrigin = Vector2.Dot(a, direction) < Fix64.Zero; - 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, Fix64.Zero); - var B = new Vector3(b.X, b.Y, Fix64.Zero); - var C = new Vector3(c.X, c.Y, Fix64.Zero); - - 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; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static bool SameDirection(Vector2 a, Vector2 b) - { - return Vector2.Dot(a, b) > Fix64.Zero; - } - - private static Vector2 Perpendicular(Vector2 a, Vector2 b) - { - return TripleProduct(a, b, a); - } - } -} diff --git a/src/Collision/Fixed/Shapes/Circle.cs b/src/Collision/Fixed/Shapes/Circle.cs deleted file mode 100644 index 7511f0b..0000000 --- a/src/Collision/Fixed/Shapes/Circle.cs +++ /dev/null @@ -1,65 +0,0 @@ -using MoonWorks.Math.Fixed; - -namespace MoonWorks.Collision.Fixed -{ - /// - /// A Circle is a shape defined by a radius. - /// - public struct Circle : IShape2D, System.IEquatable - { - public Fix64 Radius { get; } - public AABB2D AABB { get; } - - public Circle(Fix64 radius) - { - Radius = radius; - AABB = new AABB2D(-Radius, -Radius, Radius, Radius); - } - - public Circle(int radius) - { - Radius = (Fix64) radius; - AABB = new AABB2D(-Radius, -Radius, Radius, Radius); - } - - public Vector2 Support(Vector2 direction, Transform2D transform) - { - return Vector2.Transform(Vector2.Normalize(direction) * Radius, transform.TransformMatrix); - } - - public AABB2D TransformedAABB(Transform2D transform2D) - { - return AABB2D.Transformed(AABB, transform2D); - } - - public override bool Equals(object obj) - { - return obj is IShape2D other && Equals(other); - } - - public bool Equals(IShape2D other) - { - return other is Circle circle && Equals(circle); - } - - public bool Equals(Circle other) - { - return Radius == other.Radius; - } - - public override int GetHashCode() - { - return System.HashCode.Combine(Radius); - } - - public static bool operator ==(Circle a, Circle b) - { - return a.Equals(b); - } - - public static bool operator !=(Circle a, Circle b) - { - return !(a == b); - } - } -} diff --git a/src/Collision/Fixed/Shapes/Line.cs b/src/Collision/Fixed/Shapes/Line.cs deleted file mode 100644 index c1f5949..0000000 --- a/src/Collision/Fixed/Shapes/Line.cs +++ /dev/null @@ -1,75 +0,0 @@ -using System.Collections.Generic; -using MoonWorks.Math.Fixed; - -namespace MoonWorks.Collision.Fixed -{ - /// - /// A line is a shape defined by exactly two points in space. - /// - public struct Line : IShape2D, System.IEquatable - { - public Vector2 Start { get; } - public Vector2 End { get; } - - public AABB2D AABB { get; } - - public Line(Vector2 start, Vector2 end) - { - Start = start; - End = end; - - AABB = new AABB2D( - Fix64.Min(Start.X, End.X), - Fix64.Min(Start.Y, End.Y), - Fix64.Max(Start.X, End.X), - Fix64.Max(Start.Y, End.Y) - ); - } - - public Vector2 Support(Vector2 direction, Transform2D transform) - { - var transformedStart = Vector2.Transform(Start, transform.TransformMatrix); - var transformedEnd = Vector2.Transform(End, transform.TransformMatrix); - return Vector2.Dot(transformedStart, direction) > Vector2.Dot(transformedEnd, direction) ? - transformedStart : - transformedEnd; - } - - public AABB2D TransformedAABB(Transform2D transform) - { - return AABB2D.Transformed(AABB, transform); - } - - public override bool Equals(object obj) - { - return obj is IShape2D other && Equals(other); - } - - public bool Equals(IShape2D other) - { - return other is Line otherLine && Equals(otherLine); - } - - public bool Equals(Line other) - { - return - (Start == other.Start && End == other.End) || - (End == other.Start && Start == other.End); - } - - public override int GetHashCode() - { - return System.HashCode.Combine(Start, End); - } - - public static bool operator ==(Line a, Line b) - { - return a.Equals(b); - } - - public static bool operator !=(Line a, Line b) - { - return !(a == b); - } - } -} diff --git a/src/Collision/Fixed/Shapes/Point.cs b/src/Collision/Fixed/Shapes/Point.cs deleted file mode 100644 index 8783fd6..0000000 --- a/src/Collision/Fixed/Shapes/Point.cs +++ /dev/null @@ -1,61 +0,0 @@ -using System.Collections.Generic; -using MoonWorks.Math.Fixed; - -namespace MoonWorks.Collision.Fixed -{ - /// - /// A Point is "that which has no part". - /// All points by themselves are identical. - /// - public struct Point : IShape2D, System.IEquatable - { - public AABB2D AABB { get; } - public IEnumerable Shapes - { - get - { - yield return this; - } - } - - public AABB2D TransformedAABB(Transform2D transform) - { - return AABB2D.Transformed(AABB, transform); - } - - public Vector2 Support(Vector2 direction, Transform2D transform) - { - return Vector2.Transform(Vector2.Zero, transform.TransformMatrix); - } - - public override bool Equals(object obj) - { - return obj is IShape2D other && Equals(other); - } - - public bool Equals(IShape2D other) - { - return other is Point otherPoint && Equals(otherPoint); - } - - public bool Equals(Point other) - { - return true; - } - - public override int GetHashCode() - { - return 0; - } - - public static bool operator ==(Point a, Point b) - { - return true; - } - - public static bool operator !=(Point a, Point b) - { - return false; - } - } -} diff --git a/src/Collision/Fixed/Shapes/Rectangle.cs b/src/Collision/Fixed/Shapes/Rectangle.cs deleted file mode 100644 index 4da7bdd..0000000 --- a/src/Collision/Fixed/Shapes/Rectangle.cs +++ /dev/null @@ -1,130 +0,0 @@ -using System.Collections.Generic; -using MoonWorks.Math.Fixed; - -namespace MoonWorks.Collision.Fixed -{ - /// - /// A rectangle is a shape defined by a width and height. The origin is the center of the rectangle. - /// - public struct Rectangle : IShape2D, System.IEquatable - { - public AABB2D AABB { get; } - public Fix64 Width { get; } - public Fix64 Height { get; } - - public Fix64 Right { get; } - public Fix64 Left { get; } - public Fix64 Top { get; } - public Fix64 Bottom { get; } - public Vector2 TopLeft { get; } - public Vector2 BottomRight { get; } - - public Vector2 Min { get; } - public Vector2 Max { get; } - - public IEnumerable Shapes - { - get - { - yield return this; - } - } - - public Rectangle(Fix64 left, Fix64 top, Fix64 width, Fix64 height) - { - Width = width; - Height = height; - Left = left; - Right = left + width; - Top = top; - Bottom = top + height; - AABB = new AABB2D(left, top, Right, Bottom); - TopLeft = new Vector2(Left, Top); - BottomRight = new Vector2(Right, Bottom); - Min = AABB.Min; - Max = AABB.Max; - } - - public Rectangle(int left, int top, int width, int height) - { - Width = (Fix64) width; - Height = (Fix64) height; - Left = (Fix64) left; - Right = (Fix64) (left + width); - Top = (Fix64) top; - Bottom = (Fix64) (top + height); - AABB = new AABB2D(Left, Top, Right, Bottom); - TopLeft = new Vector2(Left, Top); - BottomRight = new Vector2(Right, Bottom); - Min = AABB.Min; - Max = AABB.Max; - } - - private Vector2 Support(Vector2 direction) - { - if (direction.X >= Fix64.Zero && direction.Y >= Fix64.Zero) - { - return Max; - } - else if (direction.X >= Fix64.Zero && direction.Y < Fix64.Zero) - { - return new Vector2(Max.X, Min.Y); - } - else if (direction.X < Fix64.Zero && direction.Y >= Fix64.Zero) - { - return new Vector2(Min.X, Max.Y); - } - else if (direction.X < Fix64.Zero && direction.Y < Fix64.Zero) - { - return new Vector2(Min.X, Min.Y); - } - else - { - throw new System.ArgumentException("Support vector direction cannot be zero."); - } - } - - public Vector2 Support(Vector2 direction, Transform2D transform) - { - Matrix3x2 inverseTransform; - Matrix3x2.Invert(transform.TransformMatrix, out inverseTransform); - var inverseDirection = Vector2.TransformNormal(direction, inverseTransform); - return Vector2.Transform(Support(inverseDirection), transform.TransformMatrix); - } - - public AABB2D TransformedAABB(Transform2D transform) - { - return AABB2D.Transformed(AABB, transform); - } - - public override bool Equals(object obj) - { - return obj is IShape2D other && Equals(other); - } - - public bool Equals(IShape2D other) - { - return (other is Rectangle rectangle && Equals(rectangle)); - } - - public bool Equals(Rectangle other) - { - return Min == other.Min && Max == other.Max; - } - - public override int GetHashCode() - { - return System.HashCode.Combine(Min, Max); - } - - public static bool operator ==(Rectangle a, Rectangle b) - { - return a.Equals(b); - } - - public static bool operator !=(Rectangle a, Rectangle b) - { - return !(a == b); - } - } -} diff --git a/src/Collision/Fixed/Simplex2D.cs b/src/Collision/Fixed/Simplex2D.cs deleted file mode 100644 index c5d47c2..0000000 --- a/src/Collision/Fixed/Simplex2D.cs +++ /dev/null @@ -1,109 +0,0 @@ -using System.Collections.Generic; -using MoonWorks.Math.Fixed; - -namespace MoonWorks.Collision.Fixed -{ - /// - /// A simplex is a shape with up to n - 2 vertices in the nth dimension. - /// - public struct Simplex2D : System.IEquatable - { - private Vector2 a; - private Vector2? b; - private Vector2? c; - - public Vector2 A => a; - public Vector2? B => b; - public Vector2? C => c; - - public bool ZeroSimplex { get { return !b.HasValue && !c.HasValue; } } - public bool OneSimplex { get { return b.HasValue && !c.HasValue; } } - public bool TwoSimplex { get { return b.HasValue && c.HasValue; } } - - public int Count => TwoSimplex ? 3 : (OneSimplex ? 2 : 1); - - public Simplex2D(Vector2 a) - { - this.a = a; - b = null; - c = null; - } - - public Simplex2D(Vector2 a, Vector2 b) - { - this.a = a; - this.b = b; - c = null; - } - - public Simplex2D(Vector2 a, Vector2 b, Vector2 c) - { - this.a = a; - this.b = b; - this.c = c; - } - - public Vector2 this[int index] - { - get - { - if (index == 0) { return a; } - if (index == 1) { return b.Value; } - if (index == 2) { return c.Value; } - throw new System.IndexOutOfRangeException(); - } - } - - public void Insert(Vector2 point, int index) - { - if (index == 0) - { - c = b; - b = a; - a = point; - } - else if (index == 1) - { - c = b; - b = point; - } - else - { - c = point; - } - } - - public override bool Equals(object obj) - { - return obj is Simplex2D other && Equals(other); - } - - public bool Equals(Simplex2D other) - { - if (Count != other.Count) { return false; } - - return - (A == other.A && B == other.B && C == other.C) || - (A == other.A && B == other.C && C == other.B) || - (A == other.B && B == other.A && C == other.C) || - (A == other.B && B == other.C && C == other.A) || - (A == other.C && B == other.A && C == other.B) || - (A == other.C && B == other.B && C == other.A); - } - - public override int GetHashCode() - { - return System.HashCode.Combine(a, b, c); - } - - public static bool operator ==(Simplex2D a, Simplex2D b) - { - return a.Equals(b); - } - - public static bool operator !=(Simplex2D a, Simplex2D b) - { - return !(a == b); - } - } -} diff --git a/src/Collision/Fixed/SpatialHash2D.cs b/src/Collision/Fixed/SpatialHash2D.cs deleted file mode 100644 index 9d1a87a..0000000 --- a/src/Collision/Fixed/SpatialHash2D.cs +++ /dev/null @@ -1,355 +0,0 @@ -using System.Collections.Generic; -using MoonWorks.Math.Fixed; - -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 : struct, System.IEquatable - { - private readonly Fix64 cellSize; - - private readonly Dictionary> hashDictionary = new Dictionary>(); - private readonly Dictionary IDBoxLookup = new Dictionary(); - private readonly Dictionary IDDataLookup = new Dictionary(); - - private readonly HashSet DynamicIDs = new HashSet(); - - private int MinX; - private int MaxX; - private int MinY; - private int MaxY; - - private Queue> hashSetPool = new Queue>(); - - public SpatialHash2D(int cellSize) - { - this.cellSize = new Fix64(cellSize); - } - - private (int, int) Hash(Vector2 position) - { - return ((int) (position.X / cellSize), (int) (position.Y / cellSize)); - } - - /// - /// Inserts an element into the SpatialHash. - /// - /// A unique ID for the shape-transform pair. - public void Insert(T id, AABB2D aabb, Transform2D transform2D, U data, bool dynamic = true) - { - Remove(id); - - var box = AABB2D.Transformed(aabb, transform2D); - var minHash = Hash(box.Min); - var maxHash = Hash(box.Max); - - foreach (var key in Keys(minHash.Item1, minHash.Item2, maxHash.Item1, maxHash.Item2)) - { - if (!hashDictionary.ContainsKey(key)) - { - hashDictionary.Add(key, new HashSet()); - } - - hashDictionary[key].Add(id); - IDDataLookup[id] = data; - } - - MinX = System.Math.Min(MinX, minHash.Item1); - MinY = System.Math.Min(MinY, minHash.Item2); - MaxX = System.Math.Max(MaxX, maxHash.Item1); - MaxY = System.Math.Max(MaxY, maxHash.Item2); - - if (dynamic) - { - DynamicIDs.Add(id); - } - - IDBoxLookup[id] = box; - } - - /// - /// Retrieves all the potential collisions of a shape-transform pair. Excludes any shape-transforms with the given ID. - /// - public RetrieveEnumerator Retrieve(T id, V hasAABB, Transform2D transform2D) where V : IHasAABB2D - { - var box = AABB2D.Transformed(hasAABB.AABB, 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; } - - return new RetrieveEnumerator( - this, - Keys(minX, minY, maxX, maxY), - id - ); - } - - /// - /// Retrieves all the potential collisions of a shape-transform pair. - /// - public RetrieveEnumerator Retrieve(V hasAABB, Transform2D transform2D) where V : IHasAABB2D - { - var box = AABB2D.Transformed(hasAABB.AABB, transform2D); - return Retrieve(box); - } - - /// - /// Retrieves objects based on a pre-transformed AABB. - /// - /// A transformed AABB. - /// - public RetrieveEnumerator Retrieve(T id, AABB2D aabb) - { - var (minX, minY) = Hash(aabb.Min); - var (maxX, maxY) = Hash(aabb.Max); - - if (minX < MinX) { minX = MinX; } - if (maxX > MaxX) { maxX = MaxX; } - if (minY < MinY) { minY = MinY; } - if (maxY > MaxY) { maxY = MaxY; } - - return new RetrieveEnumerator( - this, - Keys(minX, minY, maxX, maxY), - id - ); - } - - /// - /// Retrieves objects based on a pre-transformed AABB. - /// - /// A transformed AABB. - /// - public RetrieveEnumerator Retrieve(AABB2D aabb) - { - var (minX, minY) = Hash(aabb.Min); - var (maxX, maxY) = Hash(aabb.Max); - - if (minX < MinX) { minX = MinX; } - if (maxX > MaxX) { maxX = MaxX; } - if (minY < MinY) { minY = MinY; } - if (maxY > MaxY) { maxY = MaxY; } - - return new RetrieveEnumerator( - this, - Keys(minX, minY, maxX, maxY) - ); - } - - /// - /// Removes a specific ID from the SpatialHash. - /// - public void Remove(T id) - { - if (IDBoxLookup.TryGetValue(id, out var aabb)) - { - var minHash = Hash(aabb.Min); - var maxHash = Hash(aabb.Max); - - foreach (var key in Keys(minHash.Item1, minHash.Item2, maxHash.Item1, maxHash.Item2)) - { - if (hashDictionary.TryGetValue(key, out HashSet value)) - { - value.Remove(id); - } - } - - IDDataLookup.Remove(id); - IDBoxLookup.Remove(id); - } - - DynamicIDs.Remove(id); - } - - /// - /// Removes everything that has been inserted into the SpatialHash. - /// - public void Clear() - { - foreach (var hash in hashDictionary.Values) - { - hash.Clear(); - } - - IDDataLookup.Clear(); - IDBoxLookup.Clear(); - } - - /// - /// Removes - /// - public void ClearDynamic() - { - foreach (var id in DynamicIDs) - { - Remove(id); - } - } - - private static long MakeLong(int left, int right) - { - return ((long) left << 32) | ((uint) right); - } - - internal static KeysEnumerator Keys(int minX, int minY, int maxX, int maxY) - { - return new KeysEnumerator(minX, minY, maxX, maxY); - } - - internal HashSet AcquireHashSet() - { - if (hashSetPool.Count == 0) - { - hashSetPool.Enqueue(new HashSet()); - } - - var hashSet = hashSetPool.Dequeue(); - hashSet.Clear(); - return 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; - - public RetrieveEnumerator GetEnumerator() => this; - - internal RetrieveEnumerator( - SpatialHash2D spatialHash, - KeysEnumerator keysEnumerator, - T id - ) { - SpatialHash = spatialHash; - KeysEnumerator = keysEnumerator; - HashSetEnumerator = default; - HashSetEnumeratorActive = false; - Duplicates = SpatialHash.AcquireHashSet(); - ID = id; - } - - internal RetrieveEnumerator( - SpatialHash2D spatialHash, - KeysEnumerator keysEnumerator - ) { - SpatialHash = spatialHash; - KeysEnumerator = keysEnumerator; - HashSetEnumerator = default; - HashSetEnumeratorActive = false; - Duplicates = SpatialHash.AcquireHashSet(); - ID = null; - } - - 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; - - if (Duplicates.Contains(t)) - { - return MoveNext(); - } - - if (ID.HasValue) - { - if (ID.Value.Equals(t)) - { - return MoveNext(); - } - } - - Duplicates.Add(t); - return true; - } - - public (T, U) Current - { - get - { - var t = HashSetEnumerator.Current; - var u = SpatialHash.IDDataLookup[t]; - return (t, u); - } - } - } - } -} diff --git a/src/Collision/Float/AABB2D.cs b/src/Collision/Float/AABB2D.cs deleted file mode 100644 index f7684bd..0000000 --- a/src/Collision/Float/AABB2D.cs +++ /dev/null @@ -1,174 +0,0 @@ -using System.Collections.Generic; -using MoonWorks.Math.Float; - -namespace MoonWorks.Collision.Float -{ - /// - /// Axis-aligned bounding box. - /// - public struct AABB2D : System.IEquatable - { - /// - /// The top-left position of the AABB. - /// - /// - public Vector2 Min { get; private set; } - - /// - /// The bottom-right position of the AABB. - /// - /// - public Vector2 Max { get; private set; } - - 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; } } - - /// - /// The top of the AABB. Assumes a downward-aligned Y axis, so this value will be smaller than Bottom. - /// - /// - public float Top { get { return Min.Y; } } - - /// - /// The bottom of the AABB. Assumes a downward-aligned Y axis, so this value will be larger than Top. - /// - /// - public float Bottom { get { return Max.Y; } } - - public AABB2D(float minX, float minY, float maxX, float maxY) - { - Min = new Vector2(minX, minY); - Max = new Vector2(maxX, maxY); - } - - public AABB2D(Vector2 min, Vector2 max) - { - Min = min; - Max = max; - } - - private static Matrix3x2 AbsoluteMatrix(Matrix3x2 matrix) - { - return new Matrix3x2 - ( - System.Math.Abs(matrix.M11), System.Math.Abs(matrix.M12), - System.Math.Abs(matrix.M21), System.Math.Abs(matrix.M22), - System.Math.Abs(matrix.M31), System.Math.Abs(matrix.M32) - ); - } - - /// - /// Efficiently transforms the AABB by a Transform2D. - /// - /// - /// - /// - public static AABB2D Transformed(AABB2D aabb, Transform2D transform) - { - var center = (aabb.Min + aabb.Max) / 2f; - var extent = (aabb.Max - aabb.Min) / 2f; - - var newCenter = Vector2.Transform(center, transform.TransformMatrix); - var newExtent = Vector2.TransformNormal(extent, AbsoluteMatrix(transform.TransformMatrix)); - - return new AABB2D(newCenter - newExtent, newCenter + newExtent); - } - - public AABB2D Compose(AABB2D aabb) - { - float left = Left; - float top = Top; - float right = Right; - float bottom = Bottom; - - if (aabb.Left < left) - { - left = aabb.Left; - } - if (aabb.Right > right) - { - right = aabb.Right; - } - if (aabb.Top < top) - { - top = aabb.Top; - } - if (aabb.Bottom > bottom) - { - bottom = aabb.Bottom; - } - - return new AABB2D(left, top, right, bottom); - } - - /// - /// Creates an AABB for an arbitrary collection of positions. - /// This is less efficient than defining a custom AABB method for most shapes, so avoid using this if possible. - /// - /// - /// - public static AABB2D FromVertices(IEnumerable vertices) - { - var minX = float.MaxValue; - var minY = float.MaxValue; - var maxX = float.MinValue; - var maxY = float.MinValue; - - foreach (var vertex in vertices) - { - if (vertex.X < minX) - { - minX = vertex.X; - } - if (vertex.Y < minY) - { - minY = vertex.Y; - } - if (vertex.X > maxX) - { - maxX = vertex.X; - } - if (vertex.Y > maxY) - { - maxY = vertex.Y; - } - } - - return new AABB2D(minX, minY, maxX, maxY); - } - - public static bool TestOverlap(AABB2D a, AABB2D b) - { - return a.Left < b.Right && a.Right > b.Left && a.Top < b.Bottom && a.Bottom > b.Top; - } - - public override bool Equals(object obj) - { - return obj is AABB2D aabb && Equals(aabb); - } - - public bool Equals(AABB2D other) - { - return Min == other.Min && - Max == other.Max; - } - - public override int GetHashCode() - { - return System.HashCode.Combine(Min, Max); - } - - public static bool operator ==(AABB2D left, AABB2D right) - { - return left.Equals(right); - } - - public static bool operator !=(AABB2D left, AABB2D right) - { - return !(left == right); - } - } -} diff --git a/src/Collision/Float/ICollidable.cs b/src/Collision/Float/ICollidable.cs deleted file mode 100644 index 52355aa..0000000 --- a/src/Collision/Float/ICollidable.cs +++ /dev/null @@ -1,12 +0,0 @@ -using System.Collections.Generic; -using MoonWorks.Math.Float; - -namespace MoonWorks.Collision.Float -{ - public interface ICollidable - { - IEnumerable Shapes { get; } - AABB2D AABB { get; } - AABB2D TransformedAABB(Transform2D transform); - } -} diff --git a/src/Collision/Float/IShape2D.cs b/src/Collision/Float/IShape2D.cs deleted file mode 100644 index cb8ee80..0000000 --- a/src/Collision/Float/IShape2D.cs +++ /dev/null @@ -1,15 +0,0 @@ -using MoonWorks.Math.Float; - -namespace MoonWorks.Collision.Float -{ - public interface IShape2D : ICollidable, System.IEquatable - { - /// - /// A Minkowski support function. Gives the farthest point on the edge of a shape along the given direction. - /// - /// A normalized Vector2. - /// A Transform for transforming the shape vertices. - /// The farthest point on the edge of the shape along the given direction. - Vector2 Support(Vector2 direction, Transform2D transform); - } -} diff --git a/src/Collision/Float/MinkowskiDifference.cs b/src/Collision/Float/MinkowskiDifference.cs deleted file mode 100644 index 81c494d..0000000 --- a/src/Collision/Float/MinkowskiDifference.cs +++ /dev/null @@ -1,57 +0,0 @@ -using MoonWorks.Math.Float; - -namespace MoonWorks.Collision.Float -{ - /// - /// A Minkowski difference between two shapes. - /// - public struct MinkowskiDifference : System.IEquatable - { - private IShape2D ShapeA { get; } - private Transform2D TransformA { get; } - private IShape2D ShapeB { get; } - private Transform2D TransformB { get; } - - public MinkowskiDifference(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB) - { - ShapeA = shapeA; - TransformA = transformA; - ShapeB = shapeB; - TransformB = transformB; - } - - public Vector2 Support(Vector2 direction) - { - 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/Float/NarrowPhase.cs b/src/Collision/Float/NarrowPhase.cs deleted file mode 100644 index 690a7d2..0000000 --- a/src/Collision/Float/NarrowPhase.cs +++ /dev/null @@ -1,331 +0,0 @@ -using MoonWorks.Math.Float; - -namespace MoonWorks.Collision.Float -{ - public static class NarrowPhase - { - private struct Edge - { - public float Distance; - public Vector2 Normal; - 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) - { - if (TestCollision(shapeA, transformA, shapeB, transformB)) - { - return true; - } - } - } - - return false; - } - - public static bool TestCollision(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB) - { - // 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) - { - 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); - } - - // 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) - { - 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; - } - - public static bool TestPointRectangleOverlap(Point point, Transform2D pointTransform, Rectangle rectangle, Transform2D rectangleTransform) - { - var transformedPoint = pointTransform.Position; - var AABB = rectangle.TransformedAABB(rectangleTransform); - - 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) - { - var circleCenter = circleTransform.Position; - var circleRadius = circle.Radius * circleTransform.Scale.X; - - var distanceX = circleCenter.X - pointTransform.Position.X; - var distanceY = circleCenter.Y - pointTransform.Position.Y; - - return (distanceX * distanceX) + (distanceY * distanceY) < (circleRadius * circleRadius); - } - - /// - /// 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) - { - var circleCenter = circleTransform.Position; - var circleRadius = circle.Radius * circleTransform.Scale.X; - var AABB = rectangle.TransformedAABB(rectangleTransform); - - var closestX = Math.MathHelper.Clamp(circleCenter.X, AABB.Left, AABB.Right); - var closestY = Math.MathHelper.Clamp(circleCenter.Y, AABB.Top, AABB.Bottom); - - var distanceX = circleCenter.X - closestX; - var distanceY = circleCenter.Y - closestY; - - var distanceSquared = (distanceX * distanceX) + (distanceY * distanceY); - return distanceSquared < (circleRadius * circleRadius); - } - - public static bool TestCircleOverlap(Circle circleA, Transform2D transformA, Circle circleB, Transform2D transformB) - { - var radiusA = circleA.Radius * transformA.Scale.X; - var radiusB = circleB.Radius * transformB.Scale.Y; - - var centerA = transformA.Position; - var centerB = transformB.Position; - - var distanceSquared = (centerA - centerB).LengthSquared(); - var radiusSumSquared = (radiusA + radiusB) * (radiusA + radiusB); - - return distanceSquared < radiusSumSquared; - } - - 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); - } - - 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; - - Vector2 intersection = default; - - for (var i = 0; i < 32; i++) - { - var edge = FindClosestEdge(simplex); - 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) <= 0.00001f) - { - return intersection; - } - else - { - simplex.Insert(support, edge.Index); - } - } - - return intersection; // close enough - } - - private static unsafe Edge FindClosestEdge(Simplex2D simplex) - { - var closestDistance = float.PositiveInfinity; - var closestNormal = Vector2.Zero; - var closestIndex = 0; - - for (var i = 0; i < 4; i += 1) - { - var j = (i + 1 == 3) ? 0 : i + 1; - - var a = simplex[i]; - var b = simplex[j]; - - var e = b - a; - - var oa = a; - - var n = Vector2.Normalize(TripleProduct(e, oa, e)); - - var d = Vector2.Dot(n, a); - - if (d < closestDistance) - { - closestDistance = d; - closestNormal = n; - closestIndex = j; - } - } - - return new Edge - { - Distance = closestDistance, - Normal = closestNormal, - Index = 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); - } - } -} diff --git a/src/Collision/Float/Shapes/Circle.cs b/src/Collision/Float/Shapes/Circle.cs deleted file mode 100644 index d5cdff7..0000000 --- a/src/Collision/Float/Shapes/Circle.cs +++ /dev/null @@ -1,67 +0,0 @@ -using System.Collections.Generic; -using MoonWorks.Math.Float; - -namespace MoonWorks.Collision.Float -{ - /// - /// A Circle is a shape defined by a radius. - /// - public struct Circle : IShape2D, System.IEquatable - { - public float Radius { get; } - public AABB2D AABB { get; } - public IEnumerable Shapes - { - get - { - yield return this; - } - } - - public Circle(float radius) - { - Radius = radius; - AABB = new AABB2D(-Radius, -Radius, Radius, Radius); - } - - public Vector2 Support(Vector2 direction, Transform2D transform) - { - return Vector2.Transform(Vector2.Normalize(direction) * Radius, transform.TransformMatrix); - } - - public AABB2D TransformedAABB(Transform2D transform2D) - { - return AABB2D.Transformed(AABB, transform2D); - } - - public override bool Equals(object obj) - { - return obj is IShape2D other && Equals(other); - } - - public bool Equals(IShape2D other) - { - return other is Circle circle && Equals(circle); - } - - public bool Equals(Circle other) - { - return Radius == other.Radius; - } - - public override int GetHashCode() - { - return System.HashCode.Combine(Radius); - } - - public static bool operator ==(Circle a, Circle b) - { - return a.Equals(b); - } - - public static bool operator !=(Circle a, Circle b) - { - return !(a == b); - } - } -} diff --git a/src/Collision/Float/Shapes/Line.cs b/src/Collision/Float/Shapes/Line.cs deleted file mode 100644 index 00355b4..0000000 --- a/src/Collision/Float/Shapes/Line.cs +++ /dev/null @@ -1,83 +0,0 @@ -using System.Collections.Generic; -using MoonWorks.Math.Float; - -namespace MoonWorks.Collision.Float -{ - /// - /// A line is a shape defined by exactly two points in space. - /// - public struct Line : IShape2D, System.IEquatable - { - public Vector2 Start { get; } - public Vector2 End { get; } - - public AABB2D AABB { get; } - - public IEnumerable Shapes - { - get - { - yield return this; - } - } - - public Line(Vector2 start, Vector2 end) - { - Start = start; - End = end; - - AABB = new AABB2D( - System.Math.Min(Start.X, End.X), - System.Math.Min(Start.Y, End.Y), - System.Math.Max(Start.X, End.X), - System.Math.Max(Start.Y, End.Y) - ); - } - - public Vector2 Support(Vector2 direction, Transform2D transform) - { - var transformedStart = Vector2.Transform(Start, transform.TransformMatrix); - var transformedEnd = Vector2.Transform(End, transform.TransformMatrix); - return Vector2.Dot(transformedStart, direction) > Vector2.Dot(transformedEnd, direction) ? - transformedStart : - transformedEnd; - } - - public AABB2D TransformedAABB(Transform2D transform) - { - return AABB2D.Transformed(AABB, transform); - } - - public override bool Equals(object obj) - { - return obj is IShape2D other && Equals(other); - } - - public bool Equals(IShape2D other) - { - return other is Line otherLine && Equals(otherLine); - } - - public bool Equals(Line other) - { - return - (Start == other.Start && End == other.End) || - (End == other.Start && Start == other.End); - } - - public override int GetHashCode() - { - return System.HashCode.Combine(Start, End); - } - - public static bool operator ==(Line a, Line b) - { - return a.Equals(b); - } - - public static bool operator !=(Line a, Line b) - { - return !(a == b); - } - } -} diff --git a/src/Collision/Float/Shapes/Point.cs b/src/Collision/Float/Shapes/Point.cs deleted file mode 100644 index 5b9ec61..0000000 --- a/src/Collision/Float/Shapes/Point.cs +++ /dev/null @@ -1,61 +0,0 @@ -using System.Collections.Generic; -using MoonWorks.Math.Float; - -namespace MoonWorks.Collision.Float -{ - /// - /// A Point is "that which has no part". - /// All points by themselves are identical. - /// - public struct Point : IShape2D, System.IEquatable - { - public AABB2D AABB { get; } - public IEnumerable Shapes - { - get - { - yield return this; - } - } - - public AABB2D TransformedAABB(Transform2D transform) - { - return AABB2D.Transformed(AABB, transform); - } - - public Vector2 Support(Vector2 direction, Transform2D transform) - { - return Vector2.Transform(Vector2.Zero, transform.TransformMatrix); - } - - public override bool Equals(object obj) - { - return obj is IShape2D other && Equals(other); - } - - public bool Equals(IShape2D other) - { - return other is Point otherPoint && Equals(otherPoint); - } - - public bool Equals(Point other) - { - return true; - } - - public override int GetHashCode() - { - return 0; - } - - public static bool operator ==(Point a, Point b) - { - return true; - } - - public static bool operator !=(Point a, Point b) - { - return false; - } - } -} diff --git a/src/Collision/Float/Shapes/Rectangle.cs b/src/Collision/Float/Shapes/Rectangle.cs deleted file mode 100644 index 073fff9..0000000 --- a/src/Collision/Float/Shapes/Rectangle.cs +++ /dev/null @@ -1,115 +0,0 @@ -using System.Collections.Generic; -using MoonWorks.Math.Float; - -namespace MoonWorks.Collision.Float -{ - /// - /// A rectangle is a shape defined by a width and height. The origin is the center of the rectangle. - /// - public struct Rectangle : IShape2D, System.IEquatable - { - public AABB2D AABB { get; } - public float Width { get; } - public float Height { get; } - - public float Right { get; } - public float Left { get; } - public float Top { get; } - public float Bottom { get; } - public Vector2 TopLeft { get; } - public Vector2 BottomRight { get; } - - public Vector2 Min { get; } - public Vector2 Max { get; } - - public IEnumerable Shapes - { - get - { - yield return this; - } - } - - public Rectangle(float left, float top, float width, float height) - { - Width = width; - Height = height; - Left = left; - Right = left + width; - Top = top; - Bottom = top + height; - AABB = new AABB2D(left, top, Right, Bottom); - TopLeft = new Vector2(Left, Top); - BottomRight = new Vector2(Right, Bottom); - Min = AABB.Min; - Max = AABB.Max; - } - - private Vector2 Support(Vector2 direction) - { - if (direction.X >= 0 && direction.Y >= 0) - { - return Max; - } - else if (direction.X >= 0 && direction.Y < 0) - { - return new Vector2(Max.X, Min.Y); - } - else if (direction.X < 0 && direction.Y >= 0) - { - return new Vector2(Min.X, Max.Y); - } - else if (direction.X < 0 && direction.Y < 0) - { - return new Vector2(Min.X, Min.Y); - } - else - { - throw new System.ArgumentException("Support vector direction cannot be zero."); - } - } - - public Vector2 Support(Vector2 direction, Transform2D transform) - { - Matrix3x2 inverseTransform; - Matrix3x2.Invert(transform.TransformMatrix, out inverseTransform); - var inverseDirection = Vector2.TransformNormal(direction, inverseTransform); - return Vector2.Transform(Support(inverseDirection), transform.TransformMatrix); - } - - public AABB2D TransformedAABB(Transform2D transform) - { - return AABB2D.Transformed(AABB, transform); - } - - public override bool Equals(object obj) - { - return obj is IShape2D other && Equals(other); - } - - public bool Equals(IShape2D other) - { - return (other is Rectangle rectangle && Equals(rectangle)); - } - - public bool Equals(Rectangle other) - { - return Min == other.Min && Max == other.Max; - } - - public override int GetHashCode() - { - return System.HashCode.Combine(Min, Max); - } - - public static bool operator ==(Rectangle a, Rectangle b) - { - return a.Equals(b); - } - - public static bool operator !=(Rectangle a, Rectangle b) - { - return !(a == b); - } - } -} diff --git a/src/Collision/Float/Simplex2D.cs b/src/Collision/Float/Simplex2D.cs deleted file mode 100644 index 4aadf4b..0000000 --- a/src/Collision/Float/Simplex2D.cs +++ /dev/null @@ -1,136 +0,0 @@ -using System.Collections.Generic; -using MoonWorks.Math.Float; - -namespace MoonWorks.Collision.Float -{ - /// - /// A simplex is a shape with up to n - 2 vertices in the nth dimension. - /// - public struct Simplex2D : System.IEquatable - { - private Vector2 a; - private Vector2? b; - private Vector2? c; - - public Vector2 A => a; - public Vector2? B => b; - public Vector2? C => c; - - public bool ZeroSimplex { get { return !b.HasValue && !c.HasValue; } } - public bool OneSimplex { get { return b.HasValue && !c.HasValue; } } - public bool TwoSimplex { get { return b.HasValue && c.HasValue; } } - - public int Count => TwoSimplex ? 3 : (OneSimplex ? 2 : 1); - - public Simplex2D(Vector2 a) - { - this.a = a; - b = null; - c = null; - } - - public Simplex2D(Vector2 a, Vector2 b) - { - this.a = a; - this.b = b; - c = null; - } - - public Simplex2D(Vector2 a, Vector2 b, Vector2 c) - { - this.a = a; - this.b = b; - this.c = c; - } - - public Vector2 this[int index] - { - get - { - if (index == 0) { return a; } - if (index == 1) { return b.Value; } - if (index == 2) { return c.Value; } - throw new System.IndexOutOfRangeException(); - } - } - - 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 = float.NegativeInfinity; - 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) - { - c = b; - b = a; - a = point; - } - else if (index == 1) - { - c = b; - b = point; - } - else - { - c = point; - } - } - - public override bool Equals(object obj) - { - return obj is Simplex2D other && Equals(other); - } - - public bool Equals(Simplex2D other) - { - if (Count != other.Count) { return false; } - - return - (A == other.A && B == other.B && C == other.C) || - (A == other.A && B == other.C && C == other.B) || - (A == other.B && B == other.A && C == other.C) || - (A == other.B && B == other.C && C == other.A) || - (A == other.C && B == other.A && C == other.B) || - (A == other.C && B == other.B && C == other.A); - } - - public override int GetHashCode() - { - return System.HashCode.Combine(Vertices); - } - - public static bool operator ==(Simplex2D a, Simplex2D b) - { - return a.Equals(b); - } - - public static bool operator !=(Simplex2D a, Simplex2D b) - { - return !(a == b); - } - } -} diff --git a/src/Collision/Float/SpatialHash2D.cs b/src/Collision/Float/SpatialHash2D.cs deleted file mode 100644 index 52bbd34..0000000 --- a/src/Collision/Float/SpatialHash2D.cs +++ /dev/null @@ -1,253 +0,0 @@ -using System.Collections.Generic; -using MoonWorks.Math.Float; - -namespace MoonWorks.Collision.Float -{ - /// - /// 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 - { - private readonly int cellSize; - - private readonly Dictionary> hashDictionary = new Dictionary>(); - // FIXME: this ICollidable causes boxing which triggers garbage collection - private readonly Dictionary IDLookup = new Dictionary(); - - public int MinX { get; private set; } = 0; - public int MaxX { get; private set; } = 0; - public int MinY { get; private set; } = 0; - public int MaxY { get; private set; } = 0; - - private Queue> hashSetPool = new Queue>(); - - public SpatialHash2D(int cellSize) - { - this.cellSize = cellSize; - } - - private (int, int) Hash(Vector2 position) - { - return ((int) System.Math.Floor(position.X / cellSize), (int) System.Math.Floor(position.Y / cellSize)); - } - - /// - /// Inserts an element into the SpatialHash. - /// - /// A unique ID for the shape-transform pair. - /// - /// - /// A bitmask value specifying the groups this object belongs to. - public void Insert(T id, ICollidable shape, Transform2D transform2D, uint collisionGroups = uint.MaxValue) - { - var box = shape.TransformedAABB(transform2D); - var minHash = Hash(box.Min); - var maxHash = Hash(box.Max); - - foreach (var key in Keys(minHash.Item1, minHash.Item2, maxHash.Item1, maxHash.Item2)) - { - if (!hashDictionary.ContainsKey(key)) - { - hashDictionary.Add(key, new HashSet()); - } - - hashDictionary[key].Add(id); - IDLookup[id] = (shape, transform2D, collisionGroups); - } - - MinX = System.Math.Min(MinX, minHash.Item1); - MinY = System.Math.Min(MinY, minHash.Item2); - MaxX = System.Math.Max(MaxX, maxHash.Item1); - MaxY = System.Math.Max(MaxY, maxHash.Item2); - } - - /// - /// 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) - { - 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 (!id.Equals(t) && ((collisionGroups & collisionMask) > 0) && AABB2D.TestOverlap(box, otherShape.TransformedAABB(otherTransform))) - { - returned.Add(t); - yield return (t, otherShape, otherTransform, collisionGroups); - } - } - } - } - } - - FreeHashSet(returned); - } - - /// - /// 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) - { - 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); - } - - /// - /// Retrieves objects based on a pre-transformed AABB. - /// - /// A transformed AABB. - /// - public IEnumerable<(T, ICollidable, Transform2D, uint)> Retrieve(AABB2D aabb, uint collisionMask = uint.MaxValue) - { - var returned = AcquireHashSet(); - - var (minX, minY) = Hash(aabb.Min); - var (maxX, maxY) = Hash(aabb.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(aabb, otherShape.TransformedAABB(otherTransform))) - { - yield return (t, otherShape, otherTransform, collisionGroups); - } - } - } - } - } - - FreeHashSet(returned); - } - - public void Update(T id, ICollidable shape, Transform2D transform2D, uint collisionGroups = uint.MaxValue) - { - Remove(id); - Insert(id, shape, transform2D, collisionGroups); - } - - /// - /// Removes a specific ID from the SpatialHash. - /// - public void Remove(T id) - { - var (shape, transform, collisionGroups) = IDLookup[id]; - - var box = shape.TransformedAABB(transform); - var minHash = Hash(box.Min); - var maxHash = Hash(box.Max); - - foreach (var key in Keys(minHash.Item1, minHash.Item2, maxHash.Item1, maxHash.Item2)) - { - if (hashDictionary.ContainsKey(key)) - { - hashDictionary[key].Remove(id); - } - } - - IDLookup.Remove(id); - } - - /// - /// Removes everything that has been inserted into the SpatialHash. - /// - public void Clear() - { - foreach (var hash in hashDictionary.Values) - { - hash.Clear(); - } - - IDLookup.Clear(); - } - - private static long MakeLong(int left, int right) - { - return ((long) left << 32) | ((uint) right); - } - - private IEnumerable 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); - } - } - } - - private HashSet AcquireHashSet() - { - if (hashSetPool.Count == 0) - { - hashSetPool.Enqueue(new HashSet()); - } - - var hashSet = hashSetPool.Dequeue(); - hashSet.Clear(); - return hashSet; - } - - private void FreeHashSet(HashSet hashSet) - { - hashSetPool.Enqueue(hashSet); - } - } -} -- 2.25.1