From d612e8538a074b351f9f4438ef91aaaa29e51e24 Mon Sep 17 00:00:00 2001 From: cosmonaut Date: Fri, 3 Feb 2023 11:44:04 -0800 Subject: [PATCH] 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); - } - } -}