initial collision implementation
parent
cc876b2132
commit
def50f305c
|
|
@ -0,0 +1,147 @@
|
|||
using System.Collections.Generic;
|
||||
using MoonWorks.Math;
|
||||
|
||||
namespace MoonWorks.Collision
|
||||
{
|
||||
/// <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);
|
||||
}
|
||||
|
||||
/// <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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
using MoonWorks.Math;
|
||||
|
||||
namespace MoonWorks.Collision
|
||||
{
|
||||
public interface IHasAABB2D
|
||||
{
|
||||
AABB2D AABB { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Returns a bounding box based on the shape.
|
||||
/// </summary>
|
||||
/// <param name="transform">A Transform for transforming the shape vertices.</param>
|
||||
/// <returns>Returns a bounding box based on the shape.</returns>
|
||||
AABB2D TransformedAABB(Transform2D transform);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
using MoonWorks.Math;
|
||||
|
||||
namespace MoonWorks.Collision
|
||||
{
|
||||
public interface IShape2D : IHasAABB2D, 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);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,57 @@
|
|||
using MoonWorks.Math;
|
||||
|
||||
namespace MoonWorks.Collision
|
||||
{
|
||||
/// <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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,267 @@
|
|||
using MoonWorks.Math;
|
||||
|
||||
namespace MoonWorks.Collision
|
||||
{
|
||||
public static class NarrowPhase
|
||||
{
|
||||
private struct Edge
|
||||
{
|
||||
public float Distance;
|
||||
public Vector2 Normal;
|
||||
public int Index;
|
||||
}
|
||||
|
||||
public static bool TestCollision(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB)
|
||||
{
|
||||
if (shapeA is Rectangle rectangleA && shapeB is Rectangle rectangleB && transformA.Rotation == 0 && transformB.Rotation == 0)
|
||||
{
|
||||
return TestRectangleOverlap(rectangleA, transformA, rectangleB, transformB);
|
||||
}
|
||||
else if (shapeA is Point && shapeB is Rectangle && transformB.Rotation == 0)
|
||||
{
|
||||
return TestPointRectangleOverlap((Point) shapeA, transformA, (Rectangle) shapeB, transformB);
|
||||
}
|
||||
else if (shapeA is Rectangle && shapeB is Point && transformA.Rotation == 0)
|
||||
{
|
||||
return TestPointRectangleOverlap((Point) shapeB, transformB, (Rectangle) shapeA, transformA);
|
||||
}
|
||||
else if (shapeA is Circle circleA && shapeB is Circle circleB && transformA.Scale.X == transformA.Scale.Y && transformB.Scale.X == transformB.Scale.Y)
|
||||
{
|
||||
return TestCircleOverlap(circleA, transformA, circleB, transformB);
|
||||
}
|
||||
|
||||
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 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,59 @@
|
|||
using MoonWorks.Math;
|
||||
|
||||
namespace MoonWorks.Collision
|
||||
{
|
||||
/// <summary>
|
||||
/// A Circle is a shape defined by a radius.
|
||||
/// </summary>
|
||||
public struct Circle : IShape2D, System.IEquatable<Circle>
|
||||
{
|
||||
public int Radius { get; }
|
||||
public AABB2D AABB { get; }
|
||||
|
||||
public Circle(int 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,74 @@
|
|||
using MoonWorks.Math;
|
||||
|
||||
namespace MoonWorks.Collision
|
||||
{
|
||||
/// <summary>
|
||||
/// A line is a shape defined by exactly two points in space.
|
||||
/// </summary>
|
||||
public struct Line : IShape2D, System.IEquatable<Line>
|
||||
{
|
||||
private Vector2 Start { get; }
|
||||
private Vector2 End { get; }
|
||||
|
||||
public AABB2D AABB { get; }
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,53 @@
|
|||
using MoonWorks.Math;
|
||||
|
||||
namespace MoonWorks.Collision
|
||||
{
|
||||
/// <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 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,106 @@
|
|||
using MoonWorks.Math;
|
||||
|
||||
namespace MoonWorks.Collision
|
||||
{
|
||||
/// <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 int Width { get; }
|
||||
public int Height { get; }
|
||||
|
||||
public float Right { get; }
|
||||
public float Left { get; }
|
||||
public float Top { get; }
|
||||
public float Bottom { get; }
|
||||
public Vector2 BottomLeft { get; }
|
||||
public Vector2 TopRight { get; }
|
||||
|
||||
public Vector2 Min { get; }
|
||||
public Vector2 Max { get; }
|
||||
|
||||
public Rectangle(int left, int top, int width, int height)
|
||||
{
|
||||
Width = width;
|
||||
Height = height;
|
||||
Left = left;
|
||||
Right = left + width;
|
||||
Top = top;
|
||||
Bottom = top + height;
|
||||
AABB = new AABB2D(left, top, Right, Bottom);
|
||||
BottomLeft = new Vector2(Left, Bottom);
|
||||
TopRight = new Vector2(Top, Right);
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,136 @@
|
|||
using System.Collections.Generic;
|
||||
using MoonWorks.Math;
|
||||
|
||||
namespace MoonWorks.Collision
|
||||
{
|
||||
/// <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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,154 @@
|
|||
using System.Collections.Generic;
|
||||
using MoonWorks.Math;
|
||||
|
||||
namespace MoonWorks.Collision
|
||||
{
|
||||
/// <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>>();
|
||||
private readonly Dictionary<T, (IHasAABB2D, Transform2D)> IDLookup = new Dictionary<T, (IHasAABB2D, Transform2D)>();
|
||||
|
||||
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;
|
||||
|
||||
|
||||
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>
|
||||
public void Insert(T id, IHasAABB2D shape, Transform2D transform2D)
|
||||
{
|
||||
var box = shape.TransformedAABB(transform2D);
|
||||
var minHash = Hash(box.Min);
|
||||
var maxHash = Hash(box.Max);
|
||||
|
||||
for (var i = minHash.Item1; i <= maxHash.Item1; i++)
|
||||
{
|
||||
for (var j = minHash.Item2; j <= maxHash.Item2; j++)
|
||||
{
|
||||
var key = MakeLong(i, j);
|
||||
if (!hashDictionary.ContainsKey(key))
|
||||
{
|
||||
hashDictionary.Add(key, new HashSet<T>());
|
||||
}
|
||||
|
||||
hashDictionary[key].Add(id);
|
||||
IDLookup[id] = (shape, transform2D);
|
||||
}
|
||||
}
|
||||
|
||||
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, IHasAABB2D, Transform2D)> Retrieve(T id, IHasAABB2D shape, Transform2D transform2D)
|
||||
{
|
||||
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; }
|
||||
|
||||
for (var i = minX; i <= maxX; i++)
|
||||
{
|
||||
for (var j = minY; j <= maxY; j++)
|
||||
{
|
||||
var key = MakeLong(i, j);
|
||||
if (hashDictionary.ContainsKey(key))
|
||||
{
|
||||
foreach (var t in hashDictionary[key])
|
||||
{
|
||||
var (otherShape, otherTransform) = IDLookup[t];
|
||||
if (!id.Equals(t) && AABB2D.TestOverlap(box, otherShape.TransformedAABB(otherTransform)))
|
||||
{
|
||||
yield return (t, otherShape, otherTransform);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// Retrieves objects based on a pre-transformed AABB.
|
||||
/// </summary>
|
||||
/// <param name="aabb">A transformed AABB.</param>
|
||||
/// <returns></returns>
|
||||
public IEnumerable<(T, IHasAABB2D, Transform2D)> Retrieve(AABB2D aabb)
|
||||
{
|
||||
var (minX, minY) = Hash(aabb.Min);
|
||||
var (maxX, maxY) = Hash(aabb.Max);
|
||||
|
||||
if (minX < MinX) { minX = MinX; }
|
||||
if (maxX > MaxX) { maxX = MaxX; }
|
||||
if (minY < MinY) { minY = MinY; }
|
||||
if (maxY > MaxY) { maxY = MaxY; }
|
||||
|
||||
for (var i = minX; i <= maxX; i++)
|
||||
{
|
||||
for (var j = minY; j <= maxY; j++)
|
||||
{
|
||||
var key = MakeLong(i, j);
|
||||
if (hashDictionary.ContainsKey(key))
|
||||
{
|
||||
foreach (var t in hashDictionary[key])
|
||||
{
|
||||
var (otherShape, otherTransform) = IDLookup[t];
|
||||
if (AABB2D.TestOverlap(aabb, otherShape.TransformedAABB(otherTransform)))
|
||||
{
|
||||
yield return (t, otherShape, otherTransform);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,82 @@
|
|||
namespace MoonWorks.Math
|
||||
{
|
||||
public struct Transform2D : System.IEquatable<Transform2D>
|
||||
{
|
||||
public Vector2 Position { get; }
|
||||
public float Rotation { get; }
|
||||
public Vector2 Scale { get; }
|
||||
|
||||
public Matrix3x2 TransformMatrix;
|
||||
|
||||
public Transform2D(Vector2 position)
|
||||
{
|
||||
Position = position;
|
||||
Rotation = 0;
|
||||
Scale = Vector2.One;
|
||||
TransformMatrix = CreateTransformMatrix(Position, Rotation, Scale);
|
||||
}
|
||||
|
||||
public Transform2D(Vector2 position, float rotation)
|
||||
{
|
||||
Position = position;
|
||||
Rotation = rotation;
|
||||
Scale = Vector2.One;
|
||||
TransformMatrix = CreateTransformMatrix(Position, Rotation, Scale);
|
||||
}
|
||||
|
||||
public Transform2D(Vector2 position, float rotation, Vector2 scale)
|
||||
{
|
||||
Position = position;
|
||||
Rotation = rotation;
|
||||
Scale = scale;
|
||||
TransformMatrix = CreateTransformMatrix(Position, Rotation, Scale);
|
||||
}
|
||||
|
||||
public Transform2D Compose(Transform2D other)
|
||||
{
|
||||
return new Transform2D(Position + other.Position, Rotation + other.Rotation, Scale * other.Scale);
|
||||
}
|
||||
|
||||
private static Matrix3x2 CreateTransformMatrix(Vector2 position, float rotation, Vector2 scale)
|
||||
{
|
||||
return
|
||||
Matrix3x2.CreateScale(scale) *
|
||||
Matrix3x2.CreateRotation(rotation) *
|
||||
Matrix3x2.CreateTranslation(position);
|
||||
}
|
||||
|
||||
public bool Equals(Transform2D other)
|
||||
{
|
||||
return
|
||||
Position == other.Position &&
|
||||
Rotation == other.Rotation &&
|
||||
Scale == other.Scale;
|
||||
}
|
||||
|
||||
|
||||
public override bool Equals(System.Object other)
|
||||
{
|
||||
if (other is Transform2D otherTransform)
|
||||
{
|
||||
return Equals(otherTransform);
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
public override int GetHashCode()
|
||||
{
|
||||
return System.HashCode.Combine(Position, Rotation, Scale);
|
||||
}
|
||||
|
||||
public static bool operator ==(Transform2D a, Transform2D b)
|
||||
{
|
||||
return a.Equals(b);
|
||||
}
|
||||
|
||||
public static bool operator !=(Transform2D a, Transform2D b)
|
||||
{
|
||||
return !a.Equals(b);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -878,10 +878,10 @@ namespace MoonWorks.Math
|
|||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates a new <see cref="Vector2"/> that contains a transformation of 2d-vector by the specified <see cref="Matrix4x4"/>.
|
||||
/// Creates a new <see cref="Vector2"/> that contains a transformation of 2d-vector by the specified <see cref="Matrix3x2"/>.
|
||||
/// </summary>
|
||||
/// <param name="position">Source <see cref="Vector2"/>.</param>
|
||||
/// <param name="matrix">The transformation <see cref="Matrix4x4"/>.</param>
|
||||
/// <param name="matrix">The transformation <see cref="Matrix3x2"/>.</param>
|
||||
/// <returns>Transformed <see cref="Vector2"/>.</returns>
|
||||
public static Vector2 Transform(Vector2 position, Matrix3x2 matrix)
|
||||
{
|
||||
|
|
@ -999,6 +999,19 @@ namespace MoonWorks.Math
|
|||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates a new <see cref="Vector2"/> that contains a transformation of the specified normal by the specified <see cref="Matrix3x2"/>.
|
||||
/// </summary>
|
||||
/// <param name="normal">Source <see cref="Vector2"/> which represents a normal vector.</param>
|
||||
/// <param name="matrix">The transformation <see cref="Matrix3x2"/>.</param>
|
||||
/// <returns>Transformed normal.</returns>
|
||||
public static Vector2 TransformNormal(Vector2 normal, Matrix3x2 matrix)
|
||||
{
|
||||
return new Vector2(
|
||||
normal.X * matrix.M11 + normal.Y * matrix.M21,
|
||||
normal.X * matrix.M12 + normal.Y * matrix.M22);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates a new <see cref="Vector2"/> that contains a transformation of the specified normal by the specified <see cref="Matrix4x4"/>.
|
||||
/// </summary>
|
||||
|
|
|
|||
Loading…
Reference in New Issue