[wpimath] Add geometry classes for Rectangle2d and Ellipse2d (#6555)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java

@@ -0,0 +1,121 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.junit.jupiter.api.Test;
+
+class Ellipse2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testGetFocalPoints() {
+    var center = new Pose2d(1, 2, new Rotation2d());
+    var ellipse = new Ellipse2d(center, 5.0, 4.0);
+
+    var pair = ellipse.getFocalPoints();
+    var a = pair.getFirst();
+    var b = pair.getSecond();
+
+    assertAll(
+        () -> assertEquals(new Translation2d(-2.0, 2.0), a),
+        () -> assertEquals(new Translation2d(4.0, 2.0), b));
+  }
+
+  @Test
+  void testIntersectsPoint() {
+    var center = new Pose2d(1.0, 2.0, new Rotation2d());
+    var ellipse = new Ellipse2d(center, 2.0, 1.0);
+
+    var pointA = new Translation2d(1.0, 3.0);
+    var pointB = new Translation2d(0.0, 3.0);
+
+    assertAll(
+        () -> assertTrue(ellipse.intersects(pointA)),
+        () -> assertFalse(ellipse.intersects(pointB)));
+  }
+
+  @Test
+  void testContainsPoint() {
+    var center = new Pose2d(-1.0, -2.0, Rotation2d.fromDegrees(45.0));
+    var ellipse = new Ellipse2d(center, 2.0, 1.0);
+
+    var pointA = new Translation2d(0.0, -1.0);
+    var pointB = new Translation2d(0.5, -2.0);
+
+    assertAll(
+        () -> assertTrue(ellipse.contains(pointA)), () -> assertFalse(ellipse.contains(pointB)));
+  }
+
+  @Test
+  void testDistanceToPoint() {
+    var center = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(270.0));
+    var ellipse = new Ellipse2d(center, 1.0, 2.0);
+
+    var point1 = new Translation2d(2.5, 2.0);
+    assertEquals(0.0, ellipse.getDistance(point1), kEpsilon);
+
+    var point2 = new Translation2d(1.0, 2.0);
+    assertEquals(0.0, ellipse.getDistance(point2), kEpsilon);
+
+    var point3 = new Translation2d(1.0, 1.0);
+    assertEquals(0.0, ellipse.getDistance(point3), kEpsilon);
+
+    var point4 = new Translation2d(-1.0, 2.5);
+    assertEquals(0.19210128384806818, ellipse.getDistance(point4), kEpsilon);
+  }
+
+  @Test
+  void testFindNearestPoint() {
+    var center = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(270.0));
+    var ellipse = new Ellipse2d(center, 1.0, 2.0);
+
+    var point1 = new Translation2d(2.5, 2.0);
+    var nearestPoint1 = ellipse.findNearestPoint(point1);
+    assertAll(
+        () -> assertEquals(2.5, nearestPoint1.getX(), kEpsilon),
+        () -> assertEquals(2.0, nearestPoint1.getY(), kEpsilon));
+
+    var point2 = new Translation2d(1.0, 2.0);
+    var nearestPoint2 = ellipse.findNearestPoint(point2);
+    assertAll(
+        () -> assertEquals(1.0, nearestPoint2.getX(), kEpsilon),
+        () -> assertEquals(2.0, nearestPoint2.getY(), kEpsilon));
+
+    var point3 = new Translation2d(1.0, 1.0);
+    var nearestPoint3 = ellipse.findNearestPoint(point3);
+    assertAll(
+        () -> assertEquals(1.0, nearestPoint3.getX(), kEpsilon),
+        () -> assertEquals(1.0, nearestPoint3.getY(), kEpsilon));
+
+    var point4 = new Translation2d(-1.0, 2.5);
+    var nearestPoint4 = ellipse.findNearestPoint(point4);
+    assertAll(
+        () -> assertEquals(-0.8512799937611617, nearestPoint4.getX(), kEpsilon),
+        () -> assertEquals(2.378405333174535, nearestPoint4.getY(), kEpsilon));
+  }
+
+  @Test
+  void testEquals() {
+    var center1 = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(90.0));
+    var ellipse1 = new Ellipse2d(center1, 2.0, 3.0);
+
+    var center2 = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(90.0));
+    var ellipse2 = new Ellipse2d(center2, 2.0, 3.0);
+
+    var center3 = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(90.0));
+    var ellipse3 = new Ellipse2d(center3, 3.0, 2.0);
+
+    assertAll(
+        () -> assertEquals(ellipse1, ellipse2),
+        () -> assertNotEquals(ellipse1, ellipse3),
+        () -> assertNotEquals(ellipse3, ellipse2));
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java

@@ -0,0 +1,104 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.junit.jupiter.api.Test;
+
+class Rectangle2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testNewWithCorners() {
+    var cornerA = new Translation2d(1.0, 2.0);
+    var cornerB = new Translation2d(4.0, 6.0);
+
+    var rect = new Rectangle2d(cornerA, cornerB);
+
+    assertAll(
+        () -> assertEquals(3.0, rect.getXWidth()),
+        () -> assertEquals(4.0, rect.getYWidth()),
+        () -> assertEquals(2.5, rect.getCenter().getX()),
+        () -> assertEquals(4.0, rect.getCenter().getY()));
+  }
+
+  @Test
+  void testIntersectsPoint() {
+    var center = new Pose2d(4.0, 3.0, Rotation2d.fromDegrees(90.0));
+    var rect = new Rectangle2d(center, 2.0, 3.0);
+
+    assertAll(
+        () -> assertTrue(rect.intersects(new Translation2d(5.5, 4.0))),
+        () -> assertTrue(rect.intersects(new Translation2d(3.0, 2.0))),
+        () -> assertFalse(rect.intersects(new Translation2d(4.0, 1.5))),
+        () -> assertFalse(rect.intersects(new Translation2d(4.0, 3.5))));
+  }
+
+  @Test
+  void testContainsPoint() {
+    var center = new Pose2d(2.0, 3.0, Rotation2d.fromDegrees(45.0));
+    var rect = new Rectangle2d(center, 3.0, 1.0);
+
+    assertAll(
+        () -> assertTrue(rect.contains(new Translation2d(2.0, 3.0))),
+        () -> assertTrue(rect.contains(new Translation2d(3.0, 4.0))),
+        () -> assertFalse(rect.contains(new Translation2d(3.0, 3.0))));
+  }
+
+  @Test
+  void testDistanceToPoint() {
+    var center = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(270.0));
+    var rect = new Rectangle2d(center, 1.0, 2.0);
+
+    var point1 = new Translation2d(2.5, 2.0);
+    assertEquals(0.5, rect.getDistance(point1), kEpsilon);
+
+    var point2 = new Translation2d(1.0, 2.0);
+    assertEquals(0.0, rect.getDistance(point2), kEpsilon);
+
+    var point3 = new Translation2d(1.0, 1.0);
+    assertEquals(0.5, rect.getDistance(point3), kEpsilon);
+
+    var point4 = new Translation2d(-1.0, 2.5);
+    assertEquals(1.0, rect.getDistance(point4), kEpsilon);
+  }
+
+  @Test
+  void testFindNearestPoint() {
+    var center = new Pose2d(1.0, 1.0, Rotation2d.fromDegrees(90.0));
+    var rect = new Rectangle2d(center, 3.0, 4.0);
+
+    var point1 = new Translation2d(1.0, 3.0);
+    var nearestPoint1 = rect.findNearestPoint(point1);
+    assertAll(
+        () -> assertEquals(1.0, nearestPoint1.getX(), kEpsilon),
+        () -> assertEquals(2.5, nearestPoint1.getY(), kEpsilon));
+
+    var point2 = new Translation2d(0.0, 0.0);
+    var nearestPoint2 = rect.findNearestPoint(point2);
+    assertAll(
+        () -> assertEquals(0.0, nearestPoint2.getX(), kEpsilon),
+        () -> assertEquals(0.0, nearestPoint2.getY(), kEpsilon));
+  }
+
+  @Test
+  void testEquals() {
+    var center1 = new Pose2d(2.0, 3.0, new Rotation2d());
+    var rect1 = new Rectangle2d(center1, 5.0, 3.0);
+
+    var center2 = new Pose2d(2.0, 3.0, new Rotation2d());
+    var rect2 = new Rectangle2d(center2, 5.0, 3.0);
+
+    var center3 = new Pose2d(2.0, 3.0, new Rotation2d());
+    var rect3 = new Rectangle2d(center3, 3.0, 3.0);
+
+    assertAll(() -> assertEquals(rect1, rect2), () -> assertNotEquals(rect2, rect3));
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java

@@ -49,6 +49,17 @@ class Translation2dTest {
         () -> assertEquals(3.0, rotated.getY(), kEpsilon));
   }
 
+  @Test
+  void testRotateAround() {
+    var original = new Translation2d(2.0, 1.0);
+    var other = new Translation2d(3.0, 2.0);
+    var rotated = original.rotateAround(other, Rotation2d.fromDegrees(180.0));
+
+    assertAll(
+        () -> assertEquals(4.0, rotated.getX(), kEpsilon),
+        () -> assertEquals(3.0, rotated.getY(), kEpsilon));
+  }
+
   @Test
   void testMultiplication() {
     var original = new Translation2d(3.0, 5.0);

wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Ellipse2dProtoTest.java

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Ellipse2d;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufEllipse2d;
+import org.junit.jupiter.api.Test;
+
+class Ellipse2dProtoTest {
+  private static final Ellipse2d DATA =
+      new Ellipse2d(new Pose2d(1.0, 3.6, new Rotation2d(4.1)), 2.0, 1.0);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufEllipse2d proto = Ellipse2d.proto.createMessage();
+    Ellipse2d.proto.pack(proto, DATA);
+
+    Ellipse2d data = Ellipse2d.proto.unpack(proto);
+    assertEquals(DATA.getCenter(), data.getCenter());
+    assertEquals(DATA.getXSemiAxis(), data.getXSemiAxis());
+    assertEquals(DATA.getYSemiAxis(), data.getYSemiAxis());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rectangle2dProtoTest.java

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rectangle2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufRectangle2d;
+import org.junit.jupiter.api.Test;
+
+class Rectangle2dProtoTest {
+  private static final Rectangle2d DATA =
+      new Rectangle2d(new Pose2d(0.1, 0.2, new Rotation2d()), 4.0, 3.0);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufRectangle2d proto = Rectangle2d.proto.createMessage();
+    Rectangle2d.proto.pack(proto, DATA);
+
+    Rectangle2d data = Rectangle2d.proto.unpack(proto);
+    assertEquals(DATA.getCenter(), data.getCenter());
+    assertEquals(DATA.getXWidth(), data.getXWidth());
+    assertEquals(DATA.getYWidth(), data.getYWidth());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Ellipse2dStructTest.java

@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Ellipse2d;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Ellipse2dStructTest {
+  private static final Ellipse2d DATA =
+      new Ellipse2d(new Pose2d(0.0, 1.0, new Rotation2d(2.4)), 3.0, 4.0);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Ellipse2d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Ellipse2d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Ellipse2d data = Ellipse2d.struct.unpack(buffer);
+    assertEquals(DATA.getCenter(), data.getCenter());
+    assertEquals(DATA.getXSemiAxis(), data.getXSemiAxis());
+    assertEquals(DATA.getYSemiAxis(), data.getYSemiAxis());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rectangle2dStructTest.java

@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rectangle2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Rectangle2dStructTest {
+  private static final Rectangle2d DATA =
+      new Rectangle2d(new Pose2d(1.0, 2.0, new Rotation2d(3.1)), 5.0, 3.0);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Rectangle2d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Rectangle2d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Rectangle2d data = Rectangle2d.struct.unpack(buffer);
+    assertEquals(DATA.getCenter(), data.getCenter());
+    assertEquals(DATA.getXWidth(), data.getXWidth());
+    assertEquals(DATA.getYWidth(), data.getYWidth());
+  }
+}