Add ObstructionCalculator

Signed-off-by: Jonah Snider <jonah@jonahsnider.com>

Author: jonahsnider

comp-bot/build.gradle

@@ -74,6 +74,9 @@ dependencies {
 
 	// https://central.sonatype.com/artifact/com.google.guava/guava
 	implementation 'com.google.guava:guava:33.5.0-jre'
+
+	// https://central.sonatype.com/artifact/org.apache.commons/commons-geometry-euclidean
+	implementation 'org.apache.commons:commons-geometry-euclidean:1.0'
 }
 
 test {

shared/build.gradle

@@ -21,6 +21,9 @@ dependencies {
 	// https://central.sonatype.com/artifact/com.google.guava/guava
 	implementation 'com.google.guava:guava:33.5.0-jre'
 
+	// https://central.sonatype.com/artifact/org.apache.commons/commons-geometry-euclidean
+	implementation 'org.apache.commons:commons-geometry-euclidean:1.0'
+
 	// https://central.sonatype.com/artifact/org.apache.commons/commons-math4-core
 	implementation 'org.apache.commons:commons-math4-legacy-core:4.0-beta1'
 	implementation("org.apache.commons:commons-math4-legacy:4.0-beta1")

shared/src/main/java/com/team581/math/ObstructionCalculator.java

@@ -0,0 +1,59 @@
+package com.team581.math;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import java.util.Arrays;
+import java.util.List;
+import org.apache.commons.geometry.euclidean.twod.ConvexArea;
+import org.apache.commons.geometry.euclidean.twod.Lines;
+import org.apache.commons.geometry.euclidean.twod.RegionBSPTree2D;
+import org.apache.commons.geometry.euclidean.twod.Vector2D;
+import org.apache.commons.numbers.core.Precision;
+
+public class ObstructionCalculator {
+  private static final Precision.DoubleEquivalence PRECISION =
+      Precision.doubleEquivalenceOfEpsilon(1e-6);
+
+  @SafeVarargs
+  public static ObstructionCalculator fromTranslations(List<Translation2d>... obstructionCorners) {
+    return new ObstructionCalculator(
+        Arrays.stream(obstructionCorners)
+            .map(corners -> corners.stream().map(ObstructionCalculator::vector2d).toList())
+            .toList());
+  }
+
+  private static final Vector2D vector2d(Translation2d translation) {
+    return Vector2D.of(translation.getX(), translation.getY());
+  }
+
+  private final RegionBSPTree2D region;
+
+  /**
+   * @param obstructionCorners Corners of polygons representing obstructions on the field.
+   */
+  public ObstructionCalculator(List<List<Vector2D>> obstructionCorners) {
+    region = RegionBSPTree2D.empty();
+
+    for (List<Vector2D> corners : obstructionCorners) {
+      ConvexArea area = ConvexArea.convexPolygonFromVertices(corners, PRECISION);
+      region.add(area);
+    }
+  }
+
+  public boolean isObstructed(Translation2d point) {
+    return isObstructed(vector2d(point));
+  }
+
+  public boolean isObstructed(Translation2d start, Translation2d end) {
+    return isObstructed(vector2d(start), vector2d(end));
+  }
+
+  public boolean isObstructed(Vector2D point) {
+    return region.contains((point));
+  }
+
+  public boolean isObstructed(Vector2D start, Vector2D end) {
+    return region.contains(start)
+        || region.contains(end)
+        || region.linecastFirst(Lines.segmentFromPoints(start, end, PRECISION)) != null;
+  }
+}

shared/src/test/java/com/team581/math/ObstructionCalculatorTest.java

@@ -0,0 +1,101 @@
+package com.team581.math;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+final class ObstructionCalculatorTest {
+  private static final ObstructionCalculator CALCULATOR =
+      ObstructionCalculator.fromTranslations(
+          List.of(
+              new Translation2d(0, 0),
+              new Translation2d(2, 0),
+              new Translation2d(2, 2),
+              new Translation2d(0, 2)));
+
+  @Test
+  void multipleObstructions() {
+    var calculator =
+        ObstructionCalculator.fromTranslations(
+            List.of(
+                new Translation2d(0, 0),
+                new Translation2d(1, 0),
+                new Translation2d(1, 1),
+                new Translation2d(0, 1)),
+            List.of(
+                new Translation2d(5, 5),
+                new Translation2d(6, 5),
+                new Translation2d(6, 6),
+                new Translation2d(5, 6)));
+
+    assertTrue(calculator.isObstructed(new Translation2d(0.5, 0.5)));
+    assertTrue(calculator.isObstructed(new Translation2d(5.5, 5.5)));
+    assertFalse(calculator.isObstructed(new Translation2d(3, 3)));
+  }
+
+  @Test
+  void pointInsideObstructionIsObstructed() {
+    assertTrue(CALCULATOR.isObstructed(new Translation2d(1, 1)));
+  }
+
+  @Test
+  void pointOnCornerIsObstructed() {
+    assertTrue(CALCULATOR.isObstructed(new Translation2d(0, 0)));
+  }
+
+  @Test
+  void pointOnEdgeIsObstructed() {
+    assertTrue(CALCULATOR.isObstructed(new Translation2d(1, 0)));
+  }
+
+  @Test
+  void pointOutsideObstructionIsNotObstructed() {
+    assertFalse(CALCULATOR.isObstructed(new Translation2d(5, 5)));
+  }
+
+  @Test
+  void segmentBetweenObstructionsIsNotObstructed() {
+    var calculator =
+        ObstructionCalculator.fromTranslations(
+            List.of(
+                new Translation2d(0, 0),
+                new Translation2d(1, 0),
+                new Translation2d(1, 1),
+                new Translation2d(0, 1)),
+            List.of(
+                new Translation2d(3, 0),
+                new Translation2d(4, 0),
+                new Translation2d(4, 1),
+                new Translation2d(3, 1)));
+
+    assertFalse(calculator.isObstructed(new Translation2d(1.5, 0.5), new Translation2d(2.5, 0.5)));
+  }
+
+  @Test
+  void segmentEndingAtEdgeIsObstructed() {
+    assertTrue(CALCULATOR.isObstructed(new Translation2d(-1, 1), new Translation2d(0, 1)));
+  }
+
+  @Test
+  void segmentEntirelyInsideObstructionIsObstructed() {
+    assertTrue(CALCULATOR.isObstructed(new Translation2d(0.5, 0.5), new Translation2d(1.5, 1.5)));
+  }
+
+  @Test
+  void segmentOutsideObstructionIsNotObstructed() {
+    assertFalse(CALCULATOR.isObstructed(new Translation2d(3, 3), new Translation2d(5, 5)));
+  }
+
+  @Test
+  void segmentStartingInsideIsObstructed() {
+    assertTrue(CALCULATOR.isObstructed(new Translation2d(1, 1), new Translation2d(5, 5)));
+  }
+
+  @Test
+  void segmentThroughObstructionIsObstructed() {
+    assertTrue(CALCULATOR.isObstructed(new Translation2d(-1, 1), new Translation2d(3, 1)));
+  }
+}