diff --git a/src/Collision/Fixed/AABB2D.cs b/src/Collision/Fixed/AABB2D.cs deleted file mode 100644 index 025ddaf2..00000000 --- a/src/Collision/Fixed/AABB2D.cs +++ /dev/null @@ -1,181 +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) - ); - } - - /// - /// Efficiently transforms the AABB by a Transform2D. - /// - /// - /// - /// - 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; - - 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; - } - - 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/ICollidable.cs b/src/Collision/Fixed/ICollidable.cs deleted file mode 100644 index f1aa27c2..00000000 --- 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/IShape2D.cs b/src/Collision/Fixed/IShape2D.cs deleted file mode 100644 index 819069c1..00000000 --- a/src/Collision/Fixed/IShape2D.cs +++ /dev/null @@ -1,15 +0,0 @@ -using MoonWorks.Math.Fixed; - -namespace MoonWorks.Collision.Fixed -{ - 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/Fixed/MinkowskiDifference.cs b/src/Collision/Fixed/MinkowskiDifference.cs deleted file mode 100644 index a7b05e86..00000000 --- a/src/Collision/Fixed/MinkowskiDifference.cs +++ /dev/null @@ -1,57 +0,0 @@ -using MoonWorks.Math.Fixed; - -namespace MoonWorks.Collision.Fixed -{ - /// - /// 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/Fixed/NarrowPhase.cs b/src/Collision/Fixed/NarrowPhase.cs deleted file mode 100644 index 731e9698..00000000 --- a/src/Collision/Fixed/NarrowPhase.cs +++ /dev/null @@ -1,333 +0,0 @@ -using MoonWorks.Math.Fixed; - -namespace MoonWorks.Collision.Fixed -{ - public static class NarrowPhase - { - private struct Edge - { - public Fix64 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 = 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); - } - - 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 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 - }; - } - - 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) < 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; - } - - 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 91b4994c..00000000 --- a/src/Collision/Fixed/Shapes/Circle.cs +++ /dev/null @@ -1,73 +0,0 @@ -using System.Collections.Generic; -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 IEnumerable Shapes - { - get - { - yield return this; - } - } - - 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 b17d4249..00000000 --- a/src/Collision/Fixed/Shapes/Line.cs +++ /dev/null @@ -1,83 +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 IEnumerable Shapes - { - get - { - yield return this; - } - } - - 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 8783fd6d..00000000 --- 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 4da7bdd1..00000000 --- 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 7bfc50f2..00000000 --- a/src/Collision/Fixed/Simplex2D.cs +++ /dev/null @@ -1,136 +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 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) - { - 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/Fixed/SpatialHash2D.cs b/src/Collision/Fixed/SpatialHash2D.cs deleted file mode 100644 index 61d7951e..00000000 --- a/src/Collision/Fixed/SpatialHash2D.cs +++ /dev/null @@ -1,253 +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 : System.IEquatable - { - 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(); - - 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 = 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. - /// - /// - /// 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); - } - } -} diff --git a/src/Collision/Float/AABB2D.cs b/src/Collision/Float/AABB2D.cs deleted file mode 100644 index f7684bdc..00000000 --- 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 52355aad..00000000 --- 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 cb8ee802..00000000 --- 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 81c494d3..00000000 --- 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 690a7d2c..00000000 --- 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 d5cdff77..00000000 --- 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 00355b48..00000000 --- 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 5b9ec616..00000000 --- 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 073fff98..00000000 --- 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 4aadf4b1..00000000 --- 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 52bbd34a..00000000 --- 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); - } - } -} diff --git a/src/Math/Fixed/Transform2D.cs b/src/Math/Fixed/Transform2D.cs index a43081c7..208b3581 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);