Remove MoonWorks.Collision #46
|
|
@ -1,181 +0,0 @@
|
|||
using System.Collections.Generic;
|
||||
using MoonWorks.Math.Fixed;
|
||||
|
||||
namespace MoonWorks.Collision.Fixed
|
||||
{
|
||||
/// <summary>
|
||||
/// Axis-aligned bounding box.
|
||||
/// </summary>
|
||||
public struct AABB2D : System.IEquatable<AABB2D>
|
||||
{
|
||||
/// <summary>
|
||||
/// The top-left position of the AABB.
|
||||
/// </summary>
|
||||
/// <value></value>
|
||||
public Vector2 Min { get; private set; }
|
||||
|
||||
/// <summary>
|
||||
/// The bottom-right position of the AABB.
|
||||
/// </summary>
|
||||
/// <value></value>
|
||||
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; } }
|
||||
|
||||
/// <summary>
|
||||
/// The top of the AABB. Assumes a downward-aligned Y axis, so this value will be smaller than Bottom.
|
||||
/// </summary>
|
||||
/// <value></value>
|
||||
public Fix64 Top { get { return Min.Y; } }
|
||||
|
||||
/// <summary>
|
||||
/// The bottom of the AABB. Assumes a downward-aligned Y axis, so this value will be larger than Top.
|
||||
/// </summary>
|
||||
/// <value></value>
|
||||
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)
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Efficiently transforms the AABB by a Transform2D.
|
||||
/// </summary>
|
||||
/// <param name="aabb"></param>
|
||||
/// <param name="transform"></param>
|
||||
/// <returns></returns>
|
||||
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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 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.
|
||||
/// </summary>
|
||||
/// <param name="vertices"></param>
|
||||
/// <returns></returns>
|
||||
public static AABB2D FromVertices(IEnumerable<Vector2> 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,12 +0,0 @@
|
|||
using System.Collections.Generic;
|
||||
using MoonWorks.Math.Fixed;
|
||||
|
||||
namespace MoonWorks.Collision.Fixed
|
||||
{
|
||||
public interface ICollidable
|
||||
{
|
||||
IEnumerable<IShape2D> Shapes { get; }
|
||||
AABB2D AABB { get; }
|
||||
AABB2D TransformedAABB(Transform2D transform);
|
||||
}
|
||||
}
|
||||
|
|
@ -1,15 +0,0 @@
|
|||
using MoonWorks.Math.Fixed;
|
||||
|
||||
namespace MoonWorks.Collision.Fixed
|
||||
{
|
||||
public interface IShape2D : ICollidable, System.IEquatable<IShape2D>
|
||||
{
|
||||
/// <summary>
|
||||
/// A Minkowski support function. Gives the farthest point on the edge of a shape along the given direction.
|
||||
/// </summary>
|
||||
/// <param name="direction">A normalized Vector2.</param>
|
||||
/// <param name="transform">A Transform for transforming the shape vertices.</param>
|
||||
/// <returns>The farthest point on the edge of the shape along the given direction.</returns>
|
||||
Vector2 Support(Vector2 direction, Transform2D transform);
|
||||
}
|
||||
}
|
||||
|
|
@ -1,57 +0,0 @@
|
|||
using MoonWorks.Math.Fixed;
|
||||
|
||||
namespace MoonWorks.Collision.Fixed
|
||||
{
|
||||
/// <summary>
|
||||
/// A Minkowski difference between two shapes.
|
||||
/// </summary>
|
||||
public struct MinkowskiDifference : System.IEquatable<MinkowskiDifference>
|
||||
{
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// NOTE: The rectangle must be axis aligned, and the scaling of the circle must be uniform.
|
||||
/// </summary>
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,73 +0,0 @@
|
|||
using System.Collections.Generic;
|
||||
using MoonWorks.Math.Fixed;
|
||||
|
||||
namespace MoonWorks.Collision.Fixed
|
||||
{
|
||||
/// <summary>
|
||||
/// A Circle is a shape defined by a radius.
|
||||
/// </summary>
|
||||
public struct Circle : IShape2D, System.IEquatable<Circle>
|
||||
{
|
||||
public Fix64 Radius { get; }
|
||||
public AABB2D AABB { get; }
|
||||
public IEnumerable<IShape2D> 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,83 +0,0 @@
|
|||
using System.Collections.Generic;
|
||||
using MoonWorks.Math.Fixed;
|
||||
|
||||
namespace MoonWorks.Collision.Fixed
|
||||
{
|
||||
/// <summary>
|
||||
/// A line is a shape defined by exactly two points in space.
|
||||
/// </summary>
|
||||
public struct Line : IShape2D, System.IEquatable<Line>
|
||||
{
|
||||
public Vector2 Start { get; }
|
||||
public Vector2 End { get; }
|
||||
|
||||
public AABB2D AABB { get; }
|
||||
|
||||
public IEnumerable<IShape2D> 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,61 +0,0 @@
|
|||
using System.Collections.Generic;
|
||||
using MoonWorks.Math.Fixed;
|
||||
|
||||
namespace MoonWorks.Collision.Fixed
|
||||
{
|
||||
/// <summary>
|
||||
/// A Point is "that which has no part".
|
||||
/// All points by themselves are identical.
|
||||
/// </summary>
|
||||
public struct Point : IShape2D, System.IEquatable<Point>
|
||||
{
|
||||
public AABB2D AABB { get; }
|
||||
public IEnumerable<IShape2D> 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,130 +0,0 @@
|
|||
using System.Collections.Generic;
|
||||
using MoonWorks.Math.Fixed;
|
||||
|
||||
namespace MoonWorks.Collision.Fixed
|
||||
{
|
||||
/// <summary>
|
||||
/// A rectangle is a shape defined by a width and height. The origin is the center of the rectangle.
|
||||
/// </summary>
|
||||
public struct Rectangle : IShape2D, System.IEquatable<Rectangle>
|
||||
{
|
||||
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<IShape2D> 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,136 +0,0 @@
|
|||
using System.Collections.Generic;
|
||||
using MoonWorks.Math.Fixed;
|
||||
|
||||
namespace MoonWorks.Collision.Fixed
|
||||
{
|
||||
/// <summary>
|
||||
/// A simplex is a shape with up to n - 2 vertices in the nth dimension.
|
||||
/// </summary>
|
||||
public struct Simplex2D : System.IEquatable<Simplex2D>
|
||||
{
|
||||
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<Vector2> 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,253 +0,0 @@
|
|||
using System.Collections.Generic;
|
||||
using MoonWorks.Math.Fixed;
|
||||
|
||||
namespace MoonWorks.Collision.Fixed
|
||||
{
|
||||
/// <summary>
|
||||
/// Used to quickly check if two shapes are potentially overlapping.
|
||||
/// </summary>
|
||||
/// <typeparam name="T">The type that will be used to uniquely identify shape-transform pairs.</typeparam>
|
||||
public class SpatialHash2D<T> where T : System.IEquatable<T>
|
||||
{
|
||||
private readonly Fix64 cellSize;
|
||||
|
||||
private readonly Dictionary<long, HashSet<T>> hashDictionary = new Dictionary<long, HashSet<T>>();
|
||||
// FIXME: this ICollidable causes boxing which triggers garbage collection
|
||||
private readonly Dictionary<T, (ICollidable, Transform2D, uint)> IDLookup = new Dictionary<T, (ICollidable, Transform2D, uint)>();
|
||||
|
||||
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<HashSet<T>> hashSetPool = new Queue<HashSet<T>>();
|
||||
|
||||
public SpatialHash2D(int cellSize)
|
||||
{
|
||||
this.cellSize = new Fix64(cellSize);
|
||||
}
|
||||
|
||||
private (int, int) Hash(Vector2 position)
|
||||
{
|
||||
return ((int) (position.X / cellSize), (int) (position.Y / cellSize));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Inserts an element into the SpatialHash.
|
||||
/// </summary>
|
||||
/// <param name="id">A unique ID for the shape-transform pair.</param>
|
||||
/// <param name="shape"></param>
|
||||
/// <param name="transform2D"></param>
|
||||
/// <param name="collisionGroups">A bitmask value specifying the groups this object belongs to.</param>
|
||||
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<T>());
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Retrieves all the potential collisions of a shape-transform pair. Excludes any shape-transforms with the given ID.
|
||||
/// </summary>
|
||||
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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Retrieves all the potential collisions of a shape-transform pair.
|
||||
/// </summary>
|
||||
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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Retrieves objects based on a pre-transformed AABB.
|
||||
/// </summary>
|
||||
/// <param name="aabb">A transformed AABB.</param>
|
||||
/// <returns></returns>
|
||||
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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Removes a specific ID from the SpatialHash.
|
||||
/// </summary>
|
||||
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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Removes everything that has been inserted into the SpatialHash.
|
||||
/// </summary>
|
||||
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<long> 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<T> AcquireHashSet()
|
||||
{
|
||||
if (hashSetPool.Count == 0)
|
||||
{
|
||||
hashSetPool.Enqueue(new HashSet<T>());
|
||||
}
|
||||
|
||||
var hashSet = hashSetPool.Dequeue();
|
||||
hashSet.Clear();
|
||||
return hashSet;
|
||||
}
|
||||
|
||||
private void FreeHashSet(HashSet<T> hashSet)
|
||||
{
|
||||
hashSetPool.Enqueue(hashSet);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,174 +0,0 @@
|
|||
using System.Collections.Generic;
|
||||
using MoonWorks.Math.Float;
|
||||
|
||||
namespace MoonWorks.Collision.Float
|
||||
{
|
||||
/// <summary>
|
||||
/// Axis-aligned bounding box.
|
||||
/// </summary>
|
||||
public struct AABB2D : System.IEquatable<AABB2D>
|
||||
{
|
||||
/// <summary>
|
||||
/// The top-left position of the AABB.
|
||||
/// </summary>
|
||||
/// <value></value>
|
||||
public Vector2 Min { get; private set; }
|
||||
|
||||
/// <summary>
|
||||
/// The bottom-right position of the AABB.
|
||||
/// </summary>
|
||||
/// <value></value>
|
||||
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; } }
|
||||
|
||||
/// <summary>
|
||||
/// The top of the AABB. Assumes a downward-aligned Y axis, so this value will be smaller than Bottom.
|
||||
/// </summary>
|
||||
/// <value></value>
|
||||
public float Top { get { return Min.Y; } }
|
||||
|
||||
/// <summary>
|
||||
/// The bottom of the AABB. Assumes a downward-aligned Y axis, so this value will be larger than Top.
|
||||
/// </summary>
|
||||
/// <value></value>
|
||||
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)
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Efficiently transforms the AABB by a Transform2D.
|
||||
/// </summary>
|
||||
/// <param name="aabb"></param>
|
||||
/// <param name="transform"></param>
|
||||
/// <returns></returns>
|
||||
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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 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.
|
||||
/// </summary>
|
||||
/// <param name="vertices"></param>
|
||||
/// <returns></returns>
|
||||
public static AABB2D FromVertices(IEnumerable<Vector2> 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,12 +0,0 @@
|
|||
using System.Collections.Generic;
|
||||
using MoonWorks.Math.Float;
|
||||
|
||||
namespace MoonWorks.Collision.Float
|
||||
{
|
||||
public interface ICollidable
|
||||
{
|
||||
IEnumerable<IShape2D> Shapes { get; }
|
||||
AABB2D AABB { get; }
|
||||
AABB2D TransformedAABB(Transform2D transform);
|
||||
}
|
||||
}
|
||||
|
|
@ -1,15 +0,0 @@
|
|||
using MoonWorks.Math.Float;
|
||||
|
||||
namespace MoonWorks.Collision.Float
|
||||
{
|
||||
public interface IShape2D : ICollidable, System.IEquatable<IShape2D>
|
||||
{
|
||||
/// <summary>
|
||||
/// A Minkowski support function. Gives the farthest point on the edge of a shape along the given direction.
|
||||
/// </summary>
|
||||
/// <param name="direction">A normalized Vector2.</param>
|
||||
/// <param name="transform">A Transform for transforming the shape vertices.</param>
|
||||
/// <returns>The farthest point on the edge of the shape along the given direction.</returns>
|
||||
Vector2 Support(Vector2 direction, Transform2D transform);
|
||||
}
|
||||
}
|
||||
|
|
@ -1,57 +0,0 @@
|
|||
using MoonWorks.Math.Float;
|
||||
|
||||
namespace MoonWorks.Collision.Float
|
||||
{
|
||||
/// <summary>
|
||||
/// A Minkowski difference between two shapes.
|
||||
/// </summary>
|
||||
public struct MinkowskiDifference : System.IEquatable<MinkowskiDifference>
|
||||
{
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// NOTE: The rectangle must be axis aligned, and the scaling of the circle must be uniform.
|
||||
/// </summary>
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,67 +0,0 @@
|
|||
using System.Collections.Generic;
|
||||
using MoonWorks.Math.Float;
|
||||
|
||||
namespace MoonWorks.Collision.Float
|
||||
{
|
||||
/// <summary>
|
||||
/// A Circle is a shape defined by a radius.
|
||||
/// </summary>
|
||||
public struct Circle : IShape2D, System.IEquatable<Circle>
|
||||
{
|
||||
public float Radius { get; }
|
||||
public AABB2D AABB { get; }
|
||||
public IEnumerable<IShape2D> 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,83 +0,0 @@
|
|||
using System.Collections.Generic;
|
||||
using MoonWorks.Math.Float;
|
||||
|
||||
namespace MoonWorks.Collision.Float
|
||||
{
|
||||
/// <summary>
|
||||
/// A line is a shape defined by exactly two points in space.
|
||||
/// </summary>
|
||||
public struct Line : IShape2D, System.IEquatable<Line>
|
||||
{
|
||||
public Vector2 Start { get; }
|
||||
public Vector2 End { get; }
|
||||
|
||||
public AABB2D AABB { get; }
|
||||
|
||||
public IEnumerable<IShape2D> 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,61 +0,0 @@
|
|||
using System.Collections.Generic;
|
||||
using MoonWorks.Math.Float;
|
||||
|
||||
namespace MoonWorks.Collision.Float
|
||||
{
|
||||
/// <summary>
|
||||
/// A Point is "that which has no part".
|
||||
/// All points by themselves are identical.
|
||||
/// </summary>
|
||||
public struct Point : IShape2D, System.IEquatable<Point>
|
||||
{
|
||||
public AABB2D AABB { get; }
|
||||
public IEnumerable<IShape2D> 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,115 +0,0 @@
|
|||
using System.Collections.Generic;
|
||||
using MoonWorks.Math.Float;
|
||||
|
||||
namespace MoonWorks.Collision.Float
|
||||
{
|
||||
/// <summary>
|
||||
/// A rectangle is a shape defined by a width and height. The origin is the center of the rectangle.
|
||||
/// </summary>
|
||||
public struct Rectangle : IShape2D, System.IEquatable<Rectangle>
|
||||
{
|
||||
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<IShape2D> 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,136 +0,0 @@
|
|||
using System.Collections.Generic;
|
||||
using MoonWorks.Math.Float;
|
||||
|
||||
namespace MoonWorks.Collision.Float
|
||||
{
|
||||
/// <summary>
|
||||
/// A simplex is a shape with up to n - 2 vertices in the nth dimension.
|
||||
/// </summary>
|
||||
public struct Simplex2D : System.IEquatable<Simplex2D>
|
||||
{
|
||||
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<Vector2> 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,253 +0,0 @@
|
|||
using System.Collections.Generic;
|
||||
using MoonWorks.Math.Float;
|
||||
|
||||
namespace MoonWorks.Collision.Float
|
||||
{
|
||||
/// <summary>
|
||||
/// Used to quickly check if two shapes are potentially overlapping.
|
||||
/// </summary>
|
||||
/// <typeparam name="T">The type that will be used to uniquely identify shape-transform pairs.</typeparam>
|
||||
public class SpatialHash2D<T> where T : System.IEquatable<T>
|
||||
{
|
||||
private readonly int cellSize;
|
||||
|
||||
private readonly Dictionary<long, HashSet<T>> hashDictionary = new Dictionary<long, HashSet<T>>();
|
||||
// FIXME: this ICollidable causes boxing which triggers garbage collection
|
||||
private readonly Dictionary<T, (ICollidable, Transform2D, uint)> IDLookup = new Dictionary<T, (ICollidable, Transform2D, uint)>();
|
||||
|
||||
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<HashSet<T>> hashSetPool = new Queue<HashSet<T>>();
|
||||
|
||||
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));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Inserts an element into the SpatialHash.
|
||||
/// </summary>
|
||||
/// <param name="id">A unique ID for the shape-transform pair.</param>
|
||||
/// <param name="shape"></param>
|
||||
/// <param name="transform2D"></param>
|
||||
/// <param name="collisionGroups">A bitmask value specifying the groups this object belongs to.</param>
|
||||
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<T>());
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Retrieves all the potential collisions of a shape-transform pair. Excludes any shape-transforms with the given ID.
|
||||
/// </summary>
|
||||
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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Retrieves all the potential collisions of a shape-transform pair.
|
||||
/// </summary>
|
||||
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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Retrieves objects based on a pre-transformed AABB.
|
||||
/// </summary>
|
||||
/// <param name="aabb">A transformed AABB.</param>
|
||||
/// <returns></returns>
|
||||
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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Removes a specific ID from the SpatialHash.
|
||||
/// </summary>
|
||||
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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Removes everything that has been inserted into the SpatialHash.
|
||||
/// </summary>
|
||||
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<long> 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<T> AcquireHashSet()
|
||||
{
|
||||
if (hashSetPool.Count == 0)
|
||||
{
|
||||
hashSetPool.Enqueue(new HashSet<T>());
|
||||
}
|
||||
|
||||
var hashSet = hashSetPool.Dequeue();
|
||||
hashSet.Clear();
|
||||
return hashSet;
|
||||
}
|
||||
|
||||
private void FreeHashSet(HashSet<T> hashSet)
|
||||
{
|
||||
hashSetPool.Enqueue(hashSet);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue