MoonsideGames
/
MoonWorks
1
0
Fork 1
Code Issues 4 Pull Requests Projects Releases 1 Activity

Remove MoonWorks.Collision #46

Merged
cosmonaut merged 7 commits from collision_rework into main 2023-02-03 19:51:36 +00:00
Conversation 0 Commits 7 Files Changed 23
23 changed files with 1 additions and 2639 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

181
src/Collision/Fixed/AABB2D.cs
View File

@ -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);
}
}
}

12
src/Collision/Fixed/ICollidable.cs
View File

@ -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);
}
}

15
src/Collision/Fixed/IShape2D.cs
View File

@ -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);
}
}

57
src/Collision/Fixed/MinkowskiDifference.cs
View File

@ -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);
}
}
}

333
src/Collision/Fixed/NarrowPhase.cs
View File

@ -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);
}
}
}

73
src/Collision/Fixed/Shapes/Circle.cs
View File

@ -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);
}
}
}

83
src/Collision/Fixed/Shapes/Line.cs
View File

@ -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);
}
}
}

61
src/Collision/Fixed/Shapes/Point.cs
View File

@ -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;
}
}
}

130
src/Collision/Fixed/Shapes/Rectangle.cs
View File

@ -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);
}
}
}

136
src/Collision/Fixed/Simplex2D.cs
View File

@ -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);
}
}
}

253
src/Collision/Fixed/SpatialHash2D.cs
View File

@ -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);
}
}
}

174
src/Collision/Float/AABB2D.cs
View File

@ -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);
}
}
}

12
src/Collision/Float/ICollidable.cs
View File

@ -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);
}
}

15
src/Collision/Float/IShape2D.cs
View File

@ -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);
}
}

57
src/Collision/Float/MinkowskiDifference.cs
View File

@ -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);
}
}
}

331
src/Collision/Float/NarrowPhase.cs
View File

@ -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);
}
}
}

67
src/Collision/Float/Shapes/Circle.cs
View File

@ -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);
}
}
}

83
src/Collision/Float/Shapes/Line.cs
View File

@ -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);
}
}
}

61
src/Collision/Float/Shapes/Point.cs
View File

@ -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;
}
}
}

115
src/Collision/Float/Shapes/Rectangle.cs
View File

@ -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);
}
}
}

136
src/Collision/Float/Simplex2D.cs
View File

@ -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);
}
}
}

253
src/Collision/Float/SpatialHash2D.cs
View File

@ -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);
}
}
}

2
src/Math/Fixed/Transform2D.cs
View File

@ -24,7 +24,7 @@
} }
public bool IsAxisAligned => Rotation % Fix64.PiOver2 == Fix64.Zero; 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); public static readonly Transform2D Identity = new Transform2D(Vector2.Zero, Fix64.Zero, Vector2.One);