549 lines
16 KiB
C#
549 lines
16 KiB
C#
using MoonWorks.Math.Fixed;
|
|
using System.Runtime.CompilerServices;
|
|
|
|
namespace MoonWorks.Collision.Fixed
|
|
{
|
|
public static class NarrowPhase
|
|
{
|
|
private struct Edge
|
|
{
|
|
public Fix64 Distance;
|
|
public Vector2 Normal;
|
|
public int Index;
|
|
}
|
|
|
|
public static bool TestCollision<T, U>(Collider<T> colliderA, in Transform2D transformA, Collider<U> colliderB, in Transform2D transformB) where T : struct, IShape2D where U : struct, IShape2D
|
|
{
|
|
foreach (var shapeA in colliderA)
|
|
{
|
|
foreach (var shapeB in colliderB)
|
|
{
|
|
if (TestCollision(shapeA, transformA, shapeB, transformB))
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
public static bool TestCollision<T, U>(Collider<T> collider, in Transform2D transformA, U shape, in Transform2D transformB) where T : struct, IShape2D where U : struct, IShape2D
|
|
{
|
|
foreach (var colliderShape in collider)
|
|
{
|
|
if (TestCollision(colliderShape, transformA, shape, transformB))
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
public static bool TestCollision<T, U>(U shape, in Transform2D transformA, Collider<T> collider, in Transform2D transformB) where T : struct, IShape2D where U : struct, IShape2D
|
|
{
|
|
return TestCollision(collider, transformB, shape, transformA);
|
|
}
|
|
|
|
public static bool TestCollision<T, U>(in T shapeA, in Transform2D transformA, in U shapeB, in Transform2D transformB) where T : struct, IShape2D where U : struct, IShape2D
|
|
{
|
|
if (shapeA is Circle circle)
|
|
{
|
|
if (shapeB is Circle circleB)
|
|
{
|
|
return TestCollision(circle, transformA, circleB, transformB);
|
|
}
|
|
else if (shapeB is Point pointB)
|
|
{
|
|
return TestCollision(circle, transformA, pointB, transformB);
|
|
}
|
|
else if (shapeB is Rectangle rectangleB)
|
|
{
|
|
return TestCollision(circle, transformA, rectangleB, transformB);
|
|
}
|
|
}
|
|
else if (shapeA is Point point)
|
|
{
|
|
if (shapeB is Circle circleB)
|
|
{
|
|
return TestCollision(point, transformA, circleB, transformB);
|
|
}
|
|
else if (shapeB is Point pointB)
|
|
{
|
|
return TestCollision(point, transformA, pointB, transformB);
|
|
}
|
|
else if (shapeB is Rectangle rectangleB)
|
|
{
|
|
return TestCollision(point, transformA, rectangleB, transformB);
|
|
}
|
|
}
|
|
else if (shapeA is Rectangle rectangle)
|
|
{
|
|
if (shapeB is Circle circleB)
|
|
{
|
|
return TestCollision(rectangle, transformA, circleB, transformB);
|
|
}
|
|
else if (shapeB is Point pointB)
|
|
{
|
|
return TestCollision(rectangle, transformA, pointB, transformB);
|
|
}
|
|
else if (shapeB is Rectangle rectangleB)
|
|
{
|
|
return TestCollision(rectangle, transformA, rectangleB, transformB);
|
|
}
|
|
}
|
|
else if (shapeA is Line line)
|
|
{
|
|
if (shapeB is Line lineB)
|
|
{
|
|
return TestCollision(line, transformA, lineB, transformB);
|
|
}
|
|
}
|
|
|
|
return FindCollisionSimplex(shapeA, transformA, shapeB, transformB).Item1;
|
|
}
|
|
|
|
public static bool TestCollision(in Rectangle rectangleA, in Transform2D transformA, in Rectangle rectangleB, in Transform2D transformB)
|
|
{
|
|
if (transformA.IsAxisAligned && transformB.IsAxisAligned)
|
|
{
|
|
return TestRectangleOverlap(rectangleA, transformA, rectangleB, transformB);
|
|
}
|
|
else
|
|
{
|
|
return FindCollisionSimplex(rectangleA, transformA, rectangleB, transformB).Item1;
|
|
}
|
|
}
|
|
|
|
public static bool TestCollision(in Point point, in Transform2D transformA, in Rectangle rectangle, in Transform2D transformB)
|
|
{
|
|
if (transformB.IsAxisAligned)
|
|
{
|
|
return TestPointRectangleOverlap(point, transformA, rectangle, transformB);
|
|
}
|
|
else
|
|
{
|
|
return FindCollisionSimplex(point, transformA, rectangle, transformB).Item1;
|
|
}
|
|
}
|
|
|
|
public static bool TestCollision(in Rectangle rectangle, in Transform2D transformA, in Point point, in Transform2D transformB)
|
|
{
|
|
return TestCollision(point, transformB, rectangle, transformA);
|
|
}
|
|
|
|
public static bool TestCollision(in Rectangle rectangle, in Transform2D transformA, in Circle circle, in Transform2D transformB)
|
|
{
|
|
if (transformA.IsAxisAligned && transformB.IsUniformScale)
|
|
{
|
|
return TestCircleRectangleOverlap(circle, transformB, rectangle, transformA);
|
|
}
|
|
else
|
|
{
|
|
return FindCollisionSimplex(rectangle, transformA, circle, transformB).Item1;
|
|
}
|
|
}
|
|
|
|
public static bool TestCollision(in Circle circle, in Transform2D transformA, in Rectangle rectangle, in Transform2D transformB)
|
|
{
|
|
return TestCollision(rectangle, transformB, circle, transformA);
|
|
}
|
|
|
|
public static bool TestCollision(in Circle circle, in Transform2D transformA, in Point point, in Transform2D transformB)
|
|
{
|
|
if (transformA.IsUniformScale)
|
|
{
|
|
return TestCirclePointOverlap(circle, transformA, point, transformB);
|
|
}
|
|
else
|
|
{
|
|
return FindCollisionSimplex(circle, transformA, point, transformB).Item1;
|
|
}
|
|
}
|
|
|
|
public static bool TestCollision(in Point point, in Transform2D transformA, in Circle circle, in Transform2D transformB)
|
|
{
|
|
return TestCollision(circle, transformB, point, transformA);
|
|
}
|
|
|
|
public static bool TestCollision(in Circle circleA, in Transform2D transformA, in Circle circleB, in Transform2D transformB)
|
|
{
|
|
if (transformA.IsUniformScale && transformB.IsUniformScale)
|
|
{
|
|
return TestCircleOverlap(circleA, transformA, circleB, transformB);
|
|
}
|
|
else
|
|
{
|
|
return FindCollisionSimplex(circleA, transformA, circleB, transformB).Item1;
|
|
}
|
|
}
|
|
|
|
public static bool TestCollision(in Line lineA, in Transform2D transformA, in Line lineB, in Transform2D transformB)
|
|
{
|
|
return TestLineOverlap(lineA, transformA, lineB, transformB);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static bool TestRectangleOverlap(in Rectangle rectangleA, in Transform2D transformA, in Rectangle rectangleB, in Transform2D transformB)
|
|
{
|
|
var firstAABB = rectangleA.TransformedAABB(transformA);
|
|
var secondAABB = rectangleB.TransformedAABB(transformB);
|
|
|
|
return firstAABB.Left < secondAABB.Right && firstAABB.Right > secondAABB.Left && firstAABB.Top < secondAABB.Bottom && firstAABB.Bottom > secondAABB.Top;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static bool TestPointRectangleOverlap(in Point point, in Transform2D pointTransform, in Rectangle rectangle, in Transform2D rectangleTransform)
|
|
{
|
|
var transformedPoint = pointTransform.Position;
|
|
var AABB = rectangle.TransformedAABB(rectangleTransform);
|
|
|
|
return transformedPoint.X > AABB.Left && transformedPoint.X < AABB.Right && transformedPoint.Y < AABB.Bottom && transformedPoint.Y > AABB.Top;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static bool TestCirclePointOverlap(in Circle circle, in Transform2D circleTransform, in Point point, in Transform2D pointTransform)
|
|
{
|
|
var circleCenter = circleTransform.Position;
|
|
var circleRadius = circle.Radius * circleTransform.Scale.X;
|
|
|
|
var distanceX = circleCenter.X - pointTransform.Position.X;
|
|
var distanceY = circleCenter.Y - pointTransform.Position.Y;
|
|
|
|
return (distanceX * distanceX) + (distanceY * distanceY) < (circleRadius * circleRadius);
|
|
}
|
|
|
|
/// <summary>
|
|
/// NOTE: The rectangle must be axis aligned, and the scaling of the circle must be uniform.
|
|
/// </summary>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static bool TestCircleRectangleOverlap(in Circle circle, in Transform2D circleTransform, in Rectangle rectangle, in Transform2D rectangleTransform)
|
|
{
|
|
var circleCenter = circleTransform.Position;
|
|
var circleRadius = circle.Radius * circleTransform.Scale.X;
|
|
var AABB = rectangle.TransformedAABB(rectangleTransform);
|
|
|
|
var closestX = Fix64.Clamp(circleCenter.X, AABB.Left, AABB.Right);
|
|
var closestY = Fix64.Clamp(circleCenter.Y, AABB.Top, AABB.Bottom);
|
|
|
|
var distanceX = circleCenter.X - closestX;
|
|
var distanceY = circleCenter.Y - closestY;
|
|
|
|
var distanceSquared = (distanceX * distanceX) + (distanceY * distanceY);
|
|
return distanceSquared < (circleRadius * circleRadius);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static bool TestCircleOverlap(in Circle circleA, in Transform2D transformA, in Circle circleB, in Transform2D transformB)
|
|
{
|
|
var radiusA = circleA.Radius * transformA.Scale.X;
|
|
var radiusB = circleB.Radius * transformB.Scale.Y;
|
|
|
|
var centerA = transformA.Position;
|
|
var centerB = transformB.Position;
|
|
|
|
var distanceSquared = (centerA - centerB).LengthSquared();
|
|
var radiusSumSquared = (radiusA + radiusB) * (radiusA + radiusB);
|
|
|
|
return distanceSquared < radiusSumSquared;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static bool TestLineOverlap(in Line lineA, in Transform2D transformA, in Line lineB, in Transform2D transformB)
|
|
{
|
|
var b = lineA.End - lineA.Start;
|
|
var d = lineB.End - lineB.Start;
|
|
var bDotDPerp = b.X * d.Y - b.Y * d.X;
|
|
|
|
if (bDotDPerp == Fix64.Zero)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
var c = lineB.Start - lineA.Start;
|
|
var t = (c.X * d.Y - c.Y * d.X) / bDotDPerp;
|
|
if (t < 0 || t > 1)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
var u = (c.X * b.Y - c.Y * b.X) / bDotDPerp;
|
|
if (u < 0 || u > 1)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
public static bool TestLineOverlap(in Line lineA, in Transform2D transformA, in Line lineB, in Transform2D transformB, out Vector2 intersection)
|
|
{
|
|
intersection = Vector2.Zero;
|
|
|
|
var b = lineA.End - lineA.Start;
|
|
var d = lineB.End - lineB.Start;
|
|
var bDotDPerp = b.X * d.Y - b.Y * d.X;
|
|
|
|
if (bDotDPerp == Fix64.Zero)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
var c = lineB.Start - lineA.Start;
|
|
var t = (c.X * d.Y - c.Y * d.X) / bDotDPerp;
|
|
if (t < 0 || t > 1)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
var u = (c.X * b.Y - c.Y * b.X) / bDotDPerp;
|
|
if (u < 0 || u > 1)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
intersection = lineA.Start + t * b;
|
|
return true;
|
|
}
|
|
|
|
public static bool TestLineAABBOverlap(in Line line, in AABB2D aabb, out Fix64 entry, out Fix64 exit)
|
|
{
|
|
entry = Fix64.MinValue;
|
|
exit = Fix64.MaxValue;
|
|
|
|
var lineDirection = line.End - line.Start;
|
|
|
|
if (lineDirection.X != Fix64.Zero)
|
|
{
|
|
var tx1 = (aabb.Min.X - line.Start.X) / lineDirection.X;
|
|
var tx2 = (aabb.Max.X - line.Start.X) / lineDirection.X;
|
|
|
|
entry = Fix64.Max(entry, Fix64.Min(tx1, tx2));
|
|
exit = Fix64.Min(exit, Fix64.Max(tx1, tx2));
|
|
}
|
|
|
|
if (lineDirection.Y != Fix64.Zero)
|
|
{
|
|
var ty1 = (aabb.Min.Y - line.Start.Y) / lineDirection.Y;
|
|
var ty2 = (aabb.Max.Y - line.Start.Y) / lineDirection.Y;
|
|
|
|
entry = Fix64.Max(entry, Fix64.Min(ty1, ty2));
|
|
exit = Fix64.Min(exit, Fix64.Max(ty1, ty2));
|
|
}
|
|
|
|
return exit >= entry;
|
|
}
|
|
|
|
public static bool TestPointOverlap(in Point pointA, in Transform2D transformA, in Point pointB, in Transform2D transformB)
|
|
{
|
|
return transformA.Position == transformB.Position;
|
|
}
|
|
|
|
public static (bool, Simplex2D) FindCollisionSimplex<T, U>(T shapeA, Transform2D transformA, U shapeB, Transform2D transformB) where T : IShape2D where U : IShape2D
|
|
{
|
|
var minkowskiDifference = new MinkowskiDifference<T, U>(shapeA, transformA, shapeB, transformB);
|
|
var c = minkowskiDifference.Support(Vector2.UnitX);
|
|
var b = minkowskiDifference.Support(-Vector2.UnitX);
|
|
return Check(minkowskiDifference, c, b);
|
|
}
|
|
|
|
public unsafe static Vector2 Intersect(IShape2D shapeA, Transform2D Transform2DA, IShape2D shapeB, Transform2D Transform2DB, Simplex2D simplex)
|
|
{
|
|
if (shapeA == null) { throw new System.ArgumentNullException(nameof(shapeA)); }
|
|
if (shapeB == null) { throw new System.ArgumentNullException(nameof(shapeB)); }
|
|
if (!simplex.TwoSimplex) { throw new System.ArgumentException("Simplex must be a 2-Simplex.", nameof(simplex)); }
|
|
|
|
var epsilon = Fix64.FromFraction(1, 10000);
|
|
|
|
var a = simplex.A;
|
|
var b = simplex.B.Value;
|
|
var c = simplex.C.Value;
|
|
|
|
Vector2 intersection = default;
|
|
|
|
for (var i = 0; i < 32; i++)
|
|
{
|
|
var edge = FindClosestEdge(simplex);
|
|
var support = CalculateSupport(shapeA, Transform2DA, shapeB, Transform2DB, edge.Normal);
|
|
var distance = Vector2.Dot(support, edge.Normal);
|
|
|
|
intersection = edge.Normal;
|
|
intersection *= distance;
|
|
|
|
if (Fix64.Abs(distance - edge.Distance) <= epsilon)
|
|
{
|
|
return intersection;
|
|
}
|
|
else
|
|
{
|
|
simplex.Insert(support, edge.Index);
|
|
}
|
|
}
|
|
|
|
return intersection; // close enough
|
|
}
|
|
|
|
private static unsafe Edge FindClosestEdge(Simplex2D simplex)
|
|
{
|
|
var closestDistance = Fix64.MaxValue;
|
|
var closestNormal = Vector2.Zero;
|
|
var closestIndex = 0;
|
|
|
|
for (var i = 0; i < 4; i += 1)
|
|
{
|
|
var j = (i + 1 == 3) ? 0 : i + 1;
|
|
|
|
var a = simplex[i];
|
|
var b = simplex[j];
|
|
|
|
var e = b - a;
|
|
|
|
var oa = a;
|
|
|
|
var n = Vector2.Normalize(TripleProduct(e, oa, e));
|
|
|
|
var d = Vector2.Dot(n, a);
|
|
|
|
if (d < closestDistance)
|
|
{
|
|
closestDistance = d;
|
|
closestNormal = n;
|
|
closestIndex = j;
|
|
}
|
|
}
|
|
|
|
return new Edge
|
|
{
|
|
Distance = closestDistance,
|
|
Normal = closestNormal,
|
|
Index = closestIndex
|
|
};
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
private static Vector2 CalculateSupport<T, U>(T shapeA, Transform2D Transform2DA, U shapeB, Transform2D Transform2DB, Vector2 direction) where T : IShape2D where U : IShape2D
|
|
{
|
|
return shapeA.Support(direction, Transform2DA) - shapeB.Support(-direction, Transform2DB);
|
|
}
|
|
|
|
private static (bool, Simplex2D) Check<T, U>(MinkowskiDifference<T, U> minkowskiDifference, Vector2 c, Vector2 b) where T : IShape2D where U : IShape2D
|
|
{
|
|
var cb = c - b;
|
|
var c0 = -c;
|
|
var d = Direction(cb, c0);
|
|
return DoSimplex(minkowskiDifference, new Simplex2D(b, c), d);
|
|
}
|
|
|
|
private static (bool, Simplex2D) DoSimplex<T, U>(MinkowskiDifference<T, U> minkowskiDifference, Simplex2D simplex, Vector2 direction) where T : IShape2D where U : IShape2D
|
|
{
|
|
var a = minkowskiDifference.Support(direction);
|
|
var notPastOrigin = Vector2.Dot(a, direction) < Fix64.Zero;
|
|
var (intersects, newSimplex, newDirection) = EnclosesOrigin(a, simplex);
|
|
|
|
if (notPastOrigin)
|
|
{
|
|
return (false, default(Simplex2D));
|
|
}
|
|
else if (intersects)
|
|
{
|
|
return (true, new Simplex2D(simplex.A, simplex.B.Value, a));
|
|
}
|
|
else
|
|
{
|
|
return DoSimplex(minkowskiDifference, newSimplex, newDirection);
|
|
}
|
|
}
|
|
|
|
private static (bool, Simplex2D, Vector2) EnclosesOrigin(Vector2 a, Simplex2D simplex)
|
|
{
|
|
if (simplex.ZeroSimplex)
|
|
{
|
|
return HandleZeroSimplex(a, simplex.A);
|
|
}
|
|
else if (simplex.OneSimplex)
|
|
{
|
|
return HandleOneSimplex(a, simplex.A, simplex.B.Value);
|
|
}
|
|
else
|
|
{
|
|
return (false, simplex, Vector2.Zero);
|
|
}
|
|
}
|
|
|
|
private static (bool, Simplex2D, Vector2) HandleZeroSimplex(Vector2 a, Vector2 b)
|
|
{
|
|
var ab = b - a;
|
|
var a0 = -a;
|
|
var (newSimplex, newDirection) = SameDirection(ab, a0) ? (new Simplex2D(a, b), Perpendicular(ab, a0)) : (new Simplex2D(a), a0);
|
|
return (false, newSimplex, newDirection);
|
|
}
|
|
|
|
private static (bool, Simplex2D, Vector2) HandleOneSimplex(Vector2 a, Vector2 b, Vector2 c)
|
|
{
|
|
var a0 = -a;
|
|
var ab = b - a;
|
|
var ac = c - a;
|
|
var abp = Perpendicular(ab, -ac);
|
|
var acp = Perpendicular(ac, -ab);
|
|
|
|
if (SameDirection(abp, a0))
|
|
{
|
|
if (SameDirection(ab, a0))
|
|
{
|
|
return (false, new Simplex2D(a, b), abp);
|
|
}
|
|
else
|
|
{
|
|
return (false, new Simplex2D(a), a0);
|
|
}
|
|
}
|
|
else if (SameDirection(acp, a0))
|
|
{
|
|
if (SameDirection(ac, a0))
|
|
{
|
|
return (false, new Simplex2D(a, c), acp);
|
|
}
|
|
else
|
|
{
|
|
return (false, new Simplex2D(a), a0);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
return (true, new Simplex2D(b, c), a0);
|
|
}
|
|
}
|
|
|
|
private static Vector2 TripleProduct(Vector2 a, Vector2 b, Vector2 c)
|
|
{
|
|
var A = new Vector3(a.X, a.Y, Fix64.Zero);
|
|
var B = new Vector3(b.X, b.Y, Fix64.Zero);
|
|
var C = new Vector3(c.X, c.Y, Fix64.Zero);
|
|
|
|
var first = Vector3.Cross(A, B);
|
|
var second = Vector3.Cross(first, C);
|
|
|
|
return new Vector2(second.X, second.Y);
|
|
}
|
|
|
|
private static Vector2 Direction(Vector2 a, Vector2 b)
|
|
{
|
|
var d = TripleProduct(a, b, a);
|
|
var collinear = d == Vector2.Zero;
|
|
return collinear ? new Vector2(a.Y, -a.X) : d;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
private static bool SameDirection(Vector2 a, Vector2 b)
|
|
{
|
|
return Vector2.Dot(a, b) > Fix64.Zero;
|
|
}
|
|
|
|
private static Vector2 Perpendicular(Vector2 a, Vector2 b)
|
|
{
|
|
return TripleProduct(a, b, a);
|
|
}
|
|
}
|
|
}
|