From 4b783b22b310c2ee4cc9a039da170e1ea73ef227 Mon Sep 17 00:00:00 2001 From: evan Date: Sun, 15 Jan 2023 16:41:36 -0800 Subject: [PATCH] line test + improving AABB sweep --- src/Collision/Fixed/AABB2D.cs | 82 ++----- src/Collision/Fixed/NarrowPhase.cs | 370 ++++++++++++++++++----------- 2 files changed, 258 insertions(+), 194 deletions(-) diff --git a/src/Collision/Fixed/AABB2D.cs b/src/Collision/Fixed/AABB2D.cs index 4a3e8d2..f10471a 100644 --- a/src/Collision/Fixed/AABB2D.cs +++ b/src/Collision/Fixed/AABB2D.cs @@ -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); /// @@ -112,9 +120,9 @@ namespace MoonWorks.Collision.Fixed var extent = aabb.Max - center; 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) @@ -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; } - // FIXME: this is broken - public static bool SweepTest(AABB2D a, AABB2D b, Vector2 aMovement, Vector2 bMovement, out Fix64 normalizedTime) + private static AABB2D RotateAroundOrigin(AABB2D aabb) { - if (TestOverlap(a, b)) - { - normalizedTime = Fix64.Zero; - return true; - } + return Create(-aabb.Min.X, -aabb.Min.Y, -aabb.Max.X, -aabb.Max.Y); + } - 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; - if (a.Max.X < b.Min.X && relativeVelocity.X < 0) - { - 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; - } + var relativeMovement = aMovement - bMovement; + var line = new Line(Vector2.Zero, relativeMovement); - if (a.Max.Y < b.Min.Y && relativeVelocity.Y < 0) - { - 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; + return NarrowPhase.TestLineAABBOverlap(line, sum, out entry, out exit); } public override bool Equals(object obj) diff --git a/src/Collision/Fixed/NarrowPhase.cs b/src/Collision/Fixed/NarrowPhase.cs index aae4950..4339d20 100644 --- a/src/Collision/Fixed/NarrowPhase.cs +++ b/src/Collision/Fixed/NarrowPhase.cs @@ -93,6 +93,13 @@ namespace MoonWorks.Collision.Fixed 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; } @@ -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)] 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; } + [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; @@ -250,47 +348,47 @@ namespace MoonWorks.Collision.Fixed 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)); } + 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; + var b = simplex.B.Value; + var c = simplex.C.Value; - Vector2 intersection = default; + 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); + 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; + intersection = edge.Normal; + intersection *= distance; - if (Fix64.Abs(distance - edge.Distance) <= epsilon) - { - return intersection; - } - else - { + if (Fix64.Abs(distance - edge.Distance) <= epsilon) + { + return intersection; + } + else + { 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 closestNormal = Vector2.Zero; - var closestIndex = 0; + var closestNormal = Vector2.Zero; + var closestIndex = 0; for (var i = 0; i < 4; i += 1) { @@ -315,136 +413,136 @@ namespace MoonWorks.Collision.Fixed } } - return new Edge + return new Edge { Distance = closestDistance, Normal = closestNormal, Index = closestIndex }; - } + } [MethodImpl(MethodImplOptions.AggressiveInlining)] private static Vector2 CalculateSupport(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(MinkowskiDifference 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); - } + { + 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) 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); + private static (bool, Simplex2D) DoSimplex(MinkowskiDifference 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); - } - } + 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) 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) 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); + 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); - } - } + 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); + 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); + var first = Vector3.Cross(A, B); + 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) - { - var d = TripleProduct(a, b, a); - var collinear = d == Vector2.Zero; - return collinear ? new Vector2(a.Y, -a.X) : d; - } + 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; - } + { + return Vector2.Dot(a, b) > Fix64.Zero; + } - private static Vector2 Perpendicular(Vector2 a, Vector2 b) - { - return TripleProduct(a, b, a); - } + private static Vector2 Perpendicular(Vector2 a, Vector2 b) + { + return TripleProduct(a, b, a); + } } }