Add code for intersecting axis-aligned bounding boxes with segments & each other

GitOrigin-RevId: 99a855c1a813c0fcdd4fca0fd31456b62e964abb

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);
+    }
+  }
+}

Shared/Shared.projitems

@@ -11,6 +11,7 @@
   <ItemGroup>
     <Compile Include="$(MSBuildThisFileDirectory)Camera.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)Clock.cs" />
+    <Compile Include="$(MSBuildThisFileDirectory)Geometry.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)Input.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)Debug.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)FpsCounter.cs" />

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);
+    }
+  }
+}

SharedTests/SharedTests.csproj

@@ -52,6 +52,7 @@
     <Reference Include="System.Core" />
   </ItemGroup>
   <ItemGroup>
+    <Compile Include="GeometryTests.cs" />
     <Compile Include="HistoryTests.cs" />
     <Compile Include="LineTests.cs" />
     <Compile Include="Properties\AssemblyInfo.cs" />