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

line test + improving AABB sweep

evan 2023-01-15 16:41:36 -08:00
parent 27d7e23b3f
commit 4b783b22b3
2 changed files with 258 additions and 194 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -66,6 +66,14 @@ namespace MoonWorks.Collision.Fixed
); );
} }
// When you don't know which values are smaller!
public static AABB2D Create(Fix64 x1, Fix64 y1, Fix64 x2, Fix64 y2)
{
var min = new Vector2(Fix64.Min(x1, x2), Fix64.Min(y1, y2));
var max = new Vector2(Fix64.Max(x1, x2), Fix64.Max(y1, y2));
return new AABB2D(min, max);
}
static readonly Fix64 Half = Fix64.FromFraction(1, 2); static readonly Fix64 Half = Fix64.FromFraction(1, 2);
/// <summary> /// <summary>
@ -112,9 +120,9 @@ namespace MoonWorks.Collision.Fixed
var extent = aabb.Max - center; var extent = aabb.Max - center;
var newCenter = Vector2.Transform(center, transform.TransformMatrix); var newCenter = Vector2.Transform(center, transform.TransformMatrix);
var newExtent = Vector2.TransformNormal(extent, AbsoluteMatrix(transform.TransformMatrix)); var newExtent = Vector2.TransformNormal(extent, AbsoluteMatrix(transform.TransformMatrix));
return new AABB2D(newCenter - newExtent, newCenter + newExtent); return new AABB2D(newCenter - newExtent, newCenter + newExtent);
} }
public AABB2D Compose(AABB2D aabb) public AABB2D Compose(AABB2D aabb)
@ -185,67 +193,25 @@ namespace MoonWorks.Collision.Fixed
return a.Left < b.Right && a.Right > b.Left && a.Top < b.Bottom && a.Bottom > b.Top; return a.Left < b.Right && a.Right > b.Left && a.Top < b.Bottom && a.Bottom > b.Top;
} }
// FIXME: this is broken private static AABB2D RotateAroundOrigin(AABB2D aabb)
public static bool SweepTest(AABB2D a, AABB2D b, Vector2 aMovement, Vector2 bMovement, out Fix64 normalizedTime)
{ {
if (TestOverlap(a, b)) return Create(-aabb.Min.X, -aabb.Min.Y, -aabb.Max.X, -aabb.Max.Y);
{ }
normalizedTime = Fix64.Zero;
return true;
}
normalizedTime = Fix64.One; private static AABB2D MinkowskiSum(AABB2D a, AABB2D b)
{
return new AABB2D(a.Min.X + b.Min.X, a.Min.Y + b.Min.Y, a.Max.X + b.Max.X, a.Max.Y + b.Max.Y);
}
var relativeVelocity = bMovement - aMovement; public static bool SweepTest(AABB2D a, AABB2D b, Vector2 aMovement, Vector2 bMovement, out Fix64 entry, out Fix64 exit)
{
var rotatedA = RotateAroundOrigin(a);
var sum = MinkowskiSum(rotatedA, b);
Vector2 entry = Vector2.Zero; var relativeMovement = aMovement - bMovement;
if (a.Max.X < b.Min.X && relativeVelocity.X < 0) var line = new Line(Vector2.Zero, relativeMovement);
{
entry.X = (a.Max.X - b.Min.X) / relativeVelocity.X;
}
else if (b.Max.X < a.Min.X && relativeVelocity.X > 0)
{
entry.X = (a.Min.X - b.Max.X) / relativeVelocity.X;
}
if (a.Max.Y < b.Min.Y && relativeVelocity.Y < 0) return NarrowPhase.TestLineAABBOverlap(line, sum, out entry, out exit);
{
entry.Y = (a.Max.Y - b.Min.Y) / relativeVelocity.Y;
}
else if (b.Max.Y > a.Min.Y && relativeVelocity.Y > 0)
{
entry.Y = (a.Min.Y - b.Max.Y) / relativeVelocity.Y;
}
Vector2 exit = new Vector2(Fix64.MaxValue, Fix64.MaxValue);
if (b.Max.X > a.Min.X && relativeVelocity.X < 0)
{
exit.X = (a.Min.X - b.Max.X) / relativeVelocity.X;
}
else if (a.Max.X > b.Min.X && relativeVelocity.X > 0)
{
exit.Y = (a.Max.X - b.Min.X) / relativeVelocity.X;
}
if (b.Max.Y > a.Min.Y && relativeVelocity.Y < 0)
{
exit.Y = (a.Min.Y - b.Max.Y) / relativeVelocity.Y;
}
else if (a.Max.Y > b.Min.Y && relativeVelocity.Y > 0)
{
exit.Y = (a.Max.Y - b.Min.Y) / relativeVelocity.Y;
}
Fix64 firstTime = Fix64.Max(entry.X, entry.Y);
Fix64 lastTime = Fix64.Min(exit.X, exit.Y);
if (firstTime <= lastTime && firstTime > 0)
{
normalizedTime = firstTime;
return true;
}
return false;
} }
public override bool Equals(object obj) public override bool Equals(object obj)

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

@ -93,6 +93,13 @@ namespace MoonWorks.Collision.Fixed
return TestCollision(rectangle, transformA, rectangleB, transformB); 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; return FindCollisionSimplex(shapeA, transformA, shapeB, transformB).Item1;
} }
@ -172,6 +179,11 @@ namespace MoonWorks.Collision.Fixed
} }
} }
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)] [MethodImpl(MethodImplOptions.AggressiveInlining)]
public static bool TestRectangleOverlap(in Rectangle rectangleA, in Transform2D transformA, in Rectangle rectangleB, in Transform2D transformB) public static bool TestRectangleOverlap(in Rectangle rectangleA, in Transform2D transformA, in Rectangle rectangleB, in Transform2D transformB)
{ {
@ -237,6 +249,92 @@ namespace MoonWorks.Collision.Fixed
return distanceSquared < radiusSumSquared; 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) public static bool TestPointOverlap(in Point pointA, in Transform2D transformA, in Point pointB, in Transform2D transformB)
{ {
return transformA.Position == transformB.Position; return transformA.Position == transformB.Position;
@ -250,47 +348,47 @@ namespace MoonWorks.Collision.Fixed
return Check(minkowskiDifference, c, b); return Check(minkowskiDifference, c, b);
} }
public unsafe static Vector2 Intersect(IShape2D shapeA, Transform2D Transform2DA, IShape2D shapeB, Transform2D Transform2DB, Simplex2D simplex) 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 (shapeA == null) { throw new System.ArgumentNullException(nameof(shapeA)); }
if (shapeB == null) { throw new System.ArgumentNullException(nameof(shapeB)); } if (shapeB == null) { throw new System.ArgumentNullException(nameof(shapeB)); }
if (!simplex.TwoSimplex) { throw new System.ArgumentException("Simplex must be a 2-Simplex.", nameof(simplex)); } if (!simplex.TwoSimplex) { throw new System.ArgumentException("Simplex must be a 2-Simplex.", nameof(simplex)); }
var epsilon = Fix64.FromFraction(1, 10000); var epsilon = Fix64.FromFraction(1, 10000);
var a = simplex.A; var a = simplex.A;
var b = simplex.B.Value; var b = simplex.B.Value;
var c = simplex.C.Value; var c = simplex.C.Value;
Vector2 intersection = default; Vector2 intersection = default;
for (var i = 0; i < 32; i++) for (var i = 0; i < 32; i++)
{ {
var edge = FindClosestEdge(simplex); var edge = FindClosestEdge(simplex);
var support = CalculateSupport(shapeA, Transform2DA, shapeB, Transform2DB, edge.Normal); var support = CalculateSupport(shapeA, Transform2DA, shapeB, Transform2DB, edge.Normal);
var distance = Vector2.Dot(support, edge.Normal); var distance = Vector2.Dot(support, edge.Normal);
intersection = edge.Normal; intersection = edge.Normal;
intersection *= distance; intersection *= distance;
if (Fix64.Abs(distance - edge.Distance) <= epsilon) if (Fix64.Abs(distance - edge.Distance) <= epsilon)
{ {
return intersection; return intersection;
} }
else else
{ {
simplex.Insert(support, edge.Index); simplex.Insert(support, edge.Index);
} }
} }
return intersection; // close enough return intersection; // close enough
} }
private static unsafe Edge FindClosestEdge(Simplex2D simplex) private static unsafe Edge FindClosestEdge(Simplex2D simplex)
{ {
var closestDistance = Fix64.MaxValue; var closestDistance = Fix64.MaxValue;
var closestNormal = Vector2.Zero; var closestNormal = Vector2.Zero;
var closestIndex = 0; var closestIndex = 0;
for (var i = 0; i < 4; i += 1) for (var i = 0; i < 4; i += 1)
{ {
@ -315,136 +413,136 @@ namespace MoonWorks.Collision.Fixed
} }
} }
return new Edge return new Edge
{ {
Distance = closestDistance, Distance = closestDistance,
Normal = closestNormal, Normal = closestNormal,
Index = closestIndex Index = closestIndex
}; };
} }
[MethodImpl(MethodImplOptions.AggressiveInlining)] [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 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); 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 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 cb = c - b;
var c0 = -c; var c0 = -c;
var d = Direction(cb, c0); var d = Direction(cb, c0);
return DoSimplex(minkowskiDifference, new Simplex2D(b, c), d); 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 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 a = minkowskiDifference.Support(direction);
var notPastOrigin = Vector2.Dot(a, direction) < Fix64.Zero; var notPastOrigin = Vector2.Dot(a, direction) < Fix64.Zero;
var (intersects, newSimplex, newDirection) = EnclosesOrigin(a, simplex); var (intersects, newSimplex, newDirection) = EnclosesOrigin(a, simplex);
if (notPastOrigin) if (notPastOrigin)
{ {
return (false, default(Simplex2D)); return (false, default(Simplex2D));
} }
else if (intersects) else if (intersects)
{ {
return (true, new Simplex2D(simplex.A, simplex.B.Value, a)); return (true, new Simplex2D(simplex.A, simplex.B.Value, a));
} }
else else
{ {
return DoSimplex(minkowskiDifference, newSimplex, newDirection); return DoSimplex(minkowskiDifference, newSimplex, newDirection);
} }
} }
private static (bool, Simplex2D, Vector2) EnclosesOrigin(Vector2 a, Simplex2D simplex) private static (bool, Simplex2D, Vector2) EnclosesOrigin(Vector2 a, Simplex2D simplex)
{ {
if (simplex.ZeroSimplex) if (simplex.ZeroSimplex)
{ {
return HandleZeroSimplex(a, simplex.A); return HandleZeroSimplex(a, simplex.A);
} }
else if (simplex.OneSimplex) else if (simplex.OneSimplex)
{ {
return HandleOneSimplex(a, simplex.A, simplex.B.Value); return HandleOneSimplex(a, simplex.A, simplex.B.Value);
} }
else else
{ {
return (false, simplex, Vector2.Zero); return (false, simplex, Vector2.Zero);
} }
} }
private static (bool, Simplex2D, Vector2) HandleZeroSimplex(Vector2 a, Vector2 b) private static (bool, Simplex2D, Vector2) HandleZeroSimplex(Vector2 a, Vector2 b)
{ {
var ab = b - a; var ab = b - a;
var a0 = -a; var a0 = -a;
var (newSimplex, newDirection) = SameDirection(ab, a0) ? (new Simplex2D(a, b), Perpendicular(ab, a0)) : (new Simplex2D(a), a0); var (newSimplex, newDirection) = SameDirection(ab, a0) ? (new Simplex2D(a, b), Perpendicular(ab, a0)) : (new Simplex2D(a), a0);
return (false, newSimplex, newDirection); return (false, newSimplex, newDirection);
} }
private static (bool, Simplex2D, Vector2) HandleOneSimplex(Vector2 a, Vector2 b, Vector2 c) private static (bool, Simplex2D, Vector2) HandleOneSimplex(Vector2 a, Vector2 b, Vector2 c)
{ {
var a0 = -a; var a0 = -a;
var ab = b - a; var ab = b - a;
var ac = c - a; var ac = c - a;
var abp = Perpendicular(ab, -ac); var abp = Perpendicular(ab, -ac);
var acp = Perpendicular(ac, -ab); var acp = Perpendicular(ac, -ab);
if (SameDirection(abp, a0)) if (SameDirection(abp, a0))
{ {
if (SameDirection(ab, a0)) if (SameDirection(ab, a0))
{ {
return (false, new Simplex2D(a, b), abp); return (false, new Simplex2D(a, b), abp);
} }
else else
{ {
return (false, new Simplex2D(a), a0); return (false, new Simplex2D(a), a0);
} }
} }
else if (SameDirection(acp, a0)) else if (SameDirection(acp, a0))
{ {
if (SameDirection(ac, a0)) if (SameDirection(ac, a0))
{ {
return (false, new Simplex2D(a, c), acp); return (false, new Simplex2D(a, c), acp);
} }
else else
{ {
return (false, new Simplex2D(a), a0); return (false, new Simplex2D(a), a0);
} }
} }
else else
{ {
return (true, new Simplex2D(b, c), a0); return (true, new Simplex2D(b, c), a0);
} }
} }
private static Vector2 TripleProduct(Vector2 a, Vector2 b, Vector2 c) private static Vector2 TripleProduct(Vector2 a, Vector2 b, Vector2 c)
{ {
var A = new Vector3(a.X, a.Y, Fix64.Zero); var A = new Vector3(a.X, a.Y, Fix64.Zero);
var B = new Vector3(b.X, b.Y, Fix64.Zero); var B = new Vector3(b.X, b.Y, Fix64.Zero);
var C = new Vector3(c.X, c.Y, Fix64.Zero); var C = new Vector3(c.X, c.Y, Fix64.Zero);
var first = Vector3.Cross(A, B); var first = Vector3.Cross(A, B);
var second = Vector3.Cross(first, C); var second = Vector3.Cross(first, C);
return new Vector2(second.X, second.Y); return new Vector2(second.X, second.Y);
} }
private static Vector2 Direction(Vector2 a, Vector2 b) private static Vector2 Direction(Vector2 a, Vector2 b)
{ {
var d = TripleProduct(a, b, a); var d = TripleProduct(a, b, a);
var collinear = d == Vector2.Zero; var collinear = d == Vector2.Zero;
return collinear ? new Vector2(a.Y, -a.X) : d; return collinear ? new Vector2(a.Y, -a.X) : d;
} }
[MethodImpl(MethodImplOptions.AggressiveInlining)] [MethodImpl(MethodImplOptions.AggressiveInlining)]
private static bool SameDirection(Vector2 a, Vector2 b) private static bool SameDirection(Vector2 a, Vector2 b)
{ {
return Vector2.Dot(a, b) > Fix64.Zero; return Vector2.Dot(a, b) > Fix64.Zero;
} }
private static Vector2 Perpendicular(Vector2 a, Vector2 b) private static Vector2 Perpendicular(Vector2 a, Vector2 b)
{ {
return TripleProduct(a, b, a); return TripleProduct(a, b, a);
} }
} }
} }