From 878d434b22d5bddddd796117521c81131ef045fb Mon Sep 17 00:00:00 2001 From: Colin McMillen Date: Wed, 29 Jan 2020 13:54:16 -0500 Subject: [PATCH] Add code for intersecting axis-aligned bounding boxes with segments & each other GitOrigin-RevId: 99a855c1a813c0fcdd4fca0fd31456b62e964abb --- Shared/Geometry.cs | 151 +++++++++++++++++++++++++++++++++ Shared/Shared.projitems | 1 + SharedTests/GeometryTests.cs | 126 +++++++++++++++++++++++++++ SharedTests/SharedTests.csproj | 1 + 4 files changed, 279 insertions(+) create mode 100644 Shared/Geometry.cs create mode 100644 SharedTests/GeometryTests.cs diff --git a/Shared/Geometry.cs b/Shared/Geometry.cs new file mode 100644 index 0000000..085e144 --- /dev/null +++ b/Shared/Geometry.cs @@ -0,0 +1,151 @@ +using Microsoft.Xna.Framework; +using System; + +// Design largely from https://noonat.github.io/intersect/. + +namespace SemiColinGames { + + public readonly struct Hit { + public readonly Aabb Collider; + public readonly Vector2 Position; + public readonly Vector2 Delta; + public readonly Vector2 Normal; + public readonly float Time; // ranges from [0, 1]. + + public Hit(Aabb collider, Vector2 position, Vector2 delta, Vector2 normal) : + this(collider, position, delta, normal, 0.0f) { + } + + public Hit(Aabb collider, Vector2 position, Vector2 delta, Vector2 normal, float time) { + Collider = collider; + Position = position; + Delta = delta; + Normal = normal; + Time = time; + } + } + + public readonly struct Sweep { + public readonly Hit? Hit; + public readonly Vector2 Position; + public readonly float Time; + + public Sweep(Hit? hit, Vector2 position, float time) { + Hit = hit; + Position = position; + Time = time; + } + } + + public readonly struct Aabb { + public readonly Vector2 Position; // centroid + public readonly Vector2 HalfSize; + + static float Clamp(float value, float min, float max) { + if (value < min) { + return min; + } else if (value > max) { + return max; + } else { + return value; + } + } + + public Aabb(Vector2 position, Vector2 halfSize) { + Position = position; + HalfSize = halfSize; + } + + public Hit? Intersect(Aabb box) { + float dx = box.Position.X - Position.X; + float px = box.HalfSize.X + HalfSize.X - Math.Abs(dx); + if (px <= 0) { + return null; + } + + float dy = box.Position.Y - Position.Y; + float py = box.HalfSize.Y + HalfSize.Y - Math.Abs(dy); + if (py <= 0) { + return null; + } + + // TODO: which of delta/normal/hitPos do we actually care about? + if (px < py) { + int sign = Math.Sign(dx); + Vector2 delta = new Vector2(px * sign, 0); + Vector2 normal = new Vector2(sign, 0); + Vector2 hitPos = new Vector2(Position.X + HalfSize.X * sign, box.Position.Y); + return new Hit(box, hitPos, delta, normal); + } else { + int sign = Math.Sign(dy); + Vector2 delta = new Vector2(0, py * sign); + Vector2 normal = new Vector2(0, sign); + Vector2 hitPos = new Vector2(box.Position.X, Position.Y + HalfSize.Y * sign); + return new Hit(this, hitPos, delta, normal); + } + } + + public Hit? IntersectSegment(Vector2 pos, Vector2 delta) { + return IntersectSegment(pos, delta, Vector2.Zero); + } + + public Hit? IntersectSegment(Vector2 pos, Vector2 delta, Vector2 padding) { + float scaleX = 1.0f / delta.X; + float scaleY = 1.0f / delta.Y; + int signX = Math.Sign(scaleX); + int signY = Math.Sign(scaleY); + float nearTimeX = (Position.X - signX * (HalfSize.X + padding.X) - pos.X) * scaleX; + float nearTimeY = (Position.Y - signY * (HalfSize.Y + padding.Y) - pos.Y) * scaleY; + float farTimeX = (Position.X + signX * (HalfSize.X + padding.X) - pos.X) * scaleX; + float farTimeY = (Position.Y + signY * (HalfSize.Y + padding.Y) - pos.Y) * scaleY; + if (nearTimeX > farTimeY || nearTimeY > farTimeX) { + return null; + } + + float nearTime = Math.Max(nearTimeX, nearTimeY); + float farTime = Math.Min(farTimeX, farTimeY); + if (nearTime >= 1 || farTime <= 0) { + return null; + } + + // If we've gotten this far, a collision is happening. If the near time is greater than zero, + // the segment starts outside and is entering the box. Otherwise, the segment starts inside + // the box, so we set the hit time to zero. + float hitTime = Math.Max(0, nearTime); + Vector2 normal = nearTimeX > nearTimeY ? + new Vector2(-signX, 0) : + new Vector2(0, -signY); + // TODO: replace these with Vector2.Multiply (etc) + Vector2 hitDelta = new Vector2((1.0f - hitTime) * -delta.X, (1.0f - hitTime) * -delta.Y); + Vector2 hitPos = new Vector2(pos.X + delta.X * hitTime, pos.Y + delta.Y * hitTime); + return new Hit(this, hitPos, hitDelta, normal, hitTime); + } + + public Sweep Sweep(Aabb box, Vector2 delta) { + // fast-path case if the other box is static + if (delta.X == 0 && delta.Y == 0) { + Hit? staticHit = Intersect(box); + // TODO: I don't understand the original source here, but I think this is correct. + return new Sweep(staticHit, box.Position, staticHit?.Time ?? 1); + } + Hit? maybeHit = IntersectSegment(box.Position, delta, box.HalfSize); + if (maybeHit == null) { + return new Sweep(null, Vector2.Add(box.Position, delta), 1); + } + Hit hit = (Hit) maybeHit; + Vector2 hitPos = new Vector2( + box.Position.X + delta.X * hit.Time, + box.Position.Y + delta.Y * hit.Time); + Vector2 direction = Vector2.Normalize(delta); + // TODO: why is this calculation made, and then thrown away? + Vector2 sweepHitPos = new Vector2( + Clamp(hit.Position.X + direction.X * box.HalfSize.X, + Position.X - HalfSize.X, + Position.X + HalfSize.X), + Clamp(hit.Position.Y + direction.Y * box.HalfSize.Y, + Position.Y - HalfSize.Y, + Position.Y + HalfSize.Y)); + return new Sweep(hit, hitPos, hit.Time); + } + } +} \ No newline at end of file diff --git a/Shared/Shared.projitems b/Shared/Shared.projitems index 1a9b199..f9651ae 100644 --- a/Shared/Shared.projitems +++ b/Shared/Shared.projitems @@ -11,6 +11,7 @@ + diff --git a/SharedTests/GeometryTests.cs b/SharedTests/GeometryTests.cs new file mode 100644 index 0000000..f1f9e92 --- /dev/null +++ b/SharedTests/GeometryTests.cs @@ -0,0 +1,126 @@ +using Microsoft.Xna.Framework; +using Microsoft.VisualStudio.TestTools.UnitTesting; +using System; + +namespace SemiColinGames.Tests { + [TestClass] + public class GeometryTests { + [TestMethod] + public void TestIntersectSegmentNotColliding() { + Aabb box = new Aabb(new Vector2(0, 0), new Vector2(8, 8)); + Assert.IsNull(box.IntersectSegment(new Vector2(-16, -16), new Vector2(32, 0))); + } + + [TestMethod] + public void TestIntersectSegmentHit() { + Aabb box = new Aabb(new Vector2(0, 0), new Vector2(8, 8)); + var point = new Vector2(-16, 4); + var delta = new Vector2(32, 0); + Hit? maybeHit = box.IntersectSegment(point, delta); + + Assert.IsNotNull(maybeHit); + Hit hit = (Hit) maybeHit; + + Assert.AreEqual(box, hit.Collider); + Assert.AreEqual(0.25, hit.Time); + + Assert.AreEqual(point.X + delta.X * hit.Time, hit.Position.X); + Assert.AreEqual(point.Y + delta.Y * hit.Time, hit.Position.Y); + + Assert.AreEqual((1.0f - hit.Time) * -delta.X, hit.Delta.X); + Assert.AreEqual((1.0f - hit.Time) * -delta.Y, hit.Delta.Y); + + Assert.AreEqual(-1, hit.Normal.X); + Assert.AreEqual(0, hit.Normal.Y); + } + + [TestMethod] + public void TestIntersectSegmentFromInsideBox() { + Aabb box = new Aabb(new Vector2(0, 0), new Vector2(8, 8)); + var point = new Vector2(-4, 4); + var delta = new Vector2(32, 0); + Hit? maybeHit = box.IntersectSegment(point, delta); + + Assert.IsNotNull(maybeHit); + Hit hit = (Hit) maybeHit; + + Assert.AreEqual(box, hit.Collider); + Assert.AreEqual(0.0, hit.Time); + + Assert.AreEqual(-4, hit.Position.X); + Assert.AreEqual(4, hit.Position.Y); + + Assert.AreEqual(-delta.X, hit.Delta.X); + Assert.AreEqual(-delta.Y, hit.Delta.Y); + + Assert.AreEqual(-1, hit.Normal.X); + Assert.AreEqual(0, hit.Normal.Y); + } + + [TestMethod] + public void TestIntersectSegmentWithPadding() { + Aabb box = new Aabb(new Vector2(0, 0), new Vector2(8, 8)); + var point = new Vector2(-16, 4); + var delta = new Vector2(32, 0); + int padding = 4; + Hit? maybeHit = box.IntersectSegment(point, delta, new Vector2(padding, padding)); + + Assert.IsNotNull(maybeHit); + Hit hit = (Hit) maybeHit; + + Assert.AreEqual(box, hit.Collider); + Assert.AreEqual(0.125, hit.Time); + + Assert.AreEqual(point.X + delta.X * hit.Time, hit.Position.X); + Assert.AreEqual(point.Y + delta.Y * hit.Time, hit.Position.Y); + + Assert.AreEqual((1.0f - hit.Time) * -delta.X, hit.Delta.X); + Assert.AreEqual((1.0f - hit.Time) * -delta.Y, hit.Delta.Y); + + Assert.AreEqual(-1, hit.Normal.X); + Assert.AreEqual(0, hit.Normal.Y); + } + + [TestMethod] + public void TestIntersectSegmentFromTwoDirections() { + Aabb box = new Aabb(new Vector2(0, 0), new Vector2(32, 32)); + var farPos = new Vector2(64, 0); + var farToNearDelta = new Vector2(-32, 0); + Assert.IsNull(box.IntersectSegment(farPos, farToNearDelta)); + + var nearPos = new Vector2(32, 0); + var nearToFarDelta = new Vector2(32, 0); + Assert.IsNull(box.IntersectSegment(nearPos, nearToFarDelta)); + } + + [TestMethod] + public void TestIntersectSegmentXAxisAligned() { + Aabb box = new Aabb(new Vector2(0, 0), new Vector2(16, 16)); + var pos = new Vector2(-32, 0); + var delta = new Vector2(64, 0); + Hit? maybeHit = box.IntersectSegment(pos, delta); + + Assert.IsNotNull(maybeHit); + Hit hit = (Hit) maybeHit; + + Assert.AreEqual(0.25, hit.Time); + Assert.AreEqual(-1, hit.Normal.X); + Assert.AreEqual(0, hit.Normal.Y); + } + + [TestMethod] + public void TestIntersectSegmentYAxisAligned() { + Aabb box = new Aabb(new Vector2(0, 0), new Vector2(16, 16)); + var pos = new Vector2(0, -32); + var delta = new Vector2(0, 64); + Hit? maybeHit = box.IntersectSegment(pos, delta); + + Assert.IsNotNull(maybeHit); + Hit hit = (Hit) maybeHit; + + Assert.AreEqual(0.25, hit.Time); + Assert.AreEqual(0, hit.Normal.X); + Assert.AreEqual(-1, hit.Normal.Y); + } + } +} \ No newline at end of file diff --git a/SharedTests/SharedTests.csproj b/SharedTests/SharedTests.csproj index c60ff41..6412016 100644 --- a/SharedTests/SharedTests.csproj +++ b/SharedTests/SharedTests.csproj @@ -52,6 +52,7 @@ +