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

wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp

@@ -0,0 +1,100 @@
+// 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.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Ellipse2d.h"
+
+TEST(Ellipse2dTest, FocalPoints) {
+  constexpr frc::Pose2d center{1_m, 2_m, 0_deg};
+  constexpr frc::Ellipse2d ellipse{center, 5_m, 4_m};
+
+  const auto [a, b] = ellipse.FocalPoints();
+
+  EXPECT_EQ(frc::Translation2d(-2_m, 2_m), a);
+  EXPECT_EQ(frc::Translation2d(4_m, 2_m), b);
+}
+
+TEST(Ellipse2dTest, IntersectsPoint) {
+  constexpr frc::Pose2d center{1_m, 2_m, 0_deg};
+  constexpr frc::Ellipse2d ellipse{center, 2_m, 1_m};
+
+  constexpr frc::Translation2d pointA{1_m, 3_m};
+  constexpr frc::Translation2d pointB{0_m, 3_m};
+
+  EXPECT_TRUE(ellipse.Intersects(pointA));
+  EXPECT_FALSE(ellipse.Intersects(pointB));
+}
+
+TEST(Ellipse2dTest, ContainsPoint) {
+  constexpr frc::Pose2d center{-1_m, -2_m, 45_deg};
+  constexpr frc::Ellipse2d ellipse{center, 2_m, 1_m};
+
+  constexpr frc::Translation2d pointA{0_m, -1_m};
+  constexpr frc::Translation2d pointB{0.5_m, -2_m};
+
+  EXPECT_TRUE(ellipse.Contains(pointA));
+  EXPECT_FALSE(ellipse.Contains(pointB));
+}
+
+TEST(Ellipse2dTest, DistanceToPoint) {
+  constexpr double kEpsilon = 1E-9;
+
+  constexpr frc::Pose2d center{1_m, 2_m, 270_deg};
+  constexpr frc::Ellipse2d ellipse{center, 1_m, 2_m};
+
+  constexpr frc::Translation2d point1{2.5_m, 2_m};
+  EXPECT_NEAR(0, ellipse.Distance(point1).value(), kEpsilon);
+
+  constexpr frc::Translation2d point2{1_m, 2_m};
+  EXPECT_NEAR(0, ellipse.Distance(point2).value(), kEpsilon);
+
+  constexpr frc::Translation2d point3{1_m, 1_m};
+  EXPECT_NEAR(0, ellipse.Distance(point3).value(), kEpsilon);
+
+  constexpr frc::Translation2d point4{-1_m, 2.5_m};
+  EXPECT_NEAR(0.19210128384806818, ellipse.Distance(point4).value(), kEpsilon);
+}
+
+TEST(Ellipse2dTest, FindNearestPoint) {
+  constexpr double kEpsilon = 1E-9;
+
+  constexpr frc::Pose2d center{1_m, 2_m, 270_deg};
+  constexpr frc::Ellipse2d ellipse{center, 1_m, 2_m};
+
+  constexpr frc::Translation2d point1{2.5_m, 2_m};
+  auto nearestPoint1 = ellipse.FindNearestPoint(point1);
+  EXPECT_NEAR(2.5, nearestPoint1.X().value(), kEpsilon);
+  EXPECT_NEAR(2.0, nearestPoint1.Y().value(), kEpsilon);
+
+  constexpr frc::Translation2d point2{1_m, 2_m};
+  auto nearestPoint2 = ellipse.FindNearestPoint(point2);
+  EXPECT_NEAR(1.0, nearestPoint2.X().value(), kEpsilon);
+  EXPECT_NEAR(2.0, nearestPoint2.Y().value(), kEpsilon);
+
+  constexpr frc::Translation2d point3{1_m, 1_m};
+  auto nearestPoint3 = ellipse.FindNearestPoint(point3);
+  EXPECT_NEAR(1.0, nearestPoint3.X().value(), kEpsilon);
+  EXPECT_NEAR(1.0, nearestPoint3.Y().value(), kEpsilon);
+
+  constexpr frc::Translation2d point4{-1_m, 2.5_m};
+  auto nearestPoint4 = ellipse.FindNearestPoint(point4);
+  EXPECT_NEAR(-0.8512799937611617, nearestPoint4.X().value(), kEpsilon);
+  EXPECT_NEAR(2.378405333174535, nearestPoint4.Y().value(), kEpsilon);
+}
+
+TEST(Ellipse2dTest, Equals) {
+  constexpr frc::Pose2d center1{1_m, 2_m, 90_deg};
+  constexpr frc::Ellipse2d ellipse1{center1, 2_m, 3_m};
+
+  constexpr frc::Pose2d center2{1_m, 2_m, 90_deg};
+  constexpr frc::Ellipse2d ellipse2{center2, 2_m, 3_m};
+
+  constexpr frc::Pose2d center3{1_m, 2_m, 90_deg};
+  constexpr frc::Ellipse2d ellipse3{center3, 3_m, 2_m};
+
+  EXPECT_EQ(ellipse1, ellipse2);
+  EXPECT_NE(ellipse1, ellipse3);
+  EXPECT_NE(ellipse3, ellipse2);
+}

wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp

@@ -33,7 +33,7 @@ TEST(Pose3dTest, RotateBy) {
 }
 
 TEST(Pose3dTest, TestTransformByRotations) {
-  const double kEpsilon = 1E-9;
+  constexpr double kEpsilon = 1E-9;
 
   const Pose3d initialPose{0_m, 0_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}};
   const Transform3d transform1{Translation3d{0_m, 0_m, 0_m},

wpimath/src/test/native/cpp/geometry/Rectangle2dTest.cpp

@@ -0,0 +1,88 @@
+// 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.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Rectangle2d.h"
+
+TEST(Rectangle2dTest, NewWithCorners) {
+  constexpr frc::Translation2d cornerA{1_m, 2_m};
+  constexpr frc::Translation2d cornerB{4_m, 6_m};
+
+  frc::Rectangle2d rect{cornerA, cornerB};
+
+  EXPECT_EQ(3.0, rect.XWidth().value());
+  EXPECT_EQ(4.0, rect.YWidth().value());
+  EXPECT_EQ(2.5, rect.Center().X().value());
+  EXPECT_EQ(4.0, rect.Center().Y().value());
+}
+
+TEST(Rectangle2dTest, IntersectsPoint) {
+  constexpr frc::Pose2d center{4_m, 3_m, 90_deg};
+  constexpr frc::Rectangle2d rect{center, 2_m, 3_m};
+
+  EXPECT_TRUE(rect.Intersects(frc::Translation2d{5.5_m, 4_m}));
+  EXPECT_TRUE(rect.Intersects(frc::Translation2d{3_m, 2_m}));
+  EXPECT_FALSE(rect.Intersects(frc::Translation2d{4_m, 1.5_m}));
+  EXPECT_FALSE(rect.Intersects(frc::Translation2d{4_m, 3.5_m}));
+}
+
+TEST(Rectangle2dTest, ContainsPoint) {
+  constexpr frc::Pose2d center{2_m, 3_m, 45_deg};
+  constexpr frc::Rectangle2d rect{center, 3_m, 1_m};
+
+  EXPECT_TRUE(rect.Contains(frc::Translation2d{2_m, 3_m}));
+  EXPECT_TRUE(rect.Contains(frc::Translation2d{3_m, 4_m}));
+  EXPECT_FALSE(rect.Contains(frc::Translation2d{3_m, 3_m}));
+}
+
+TEST(Rectangle2dTest, DistanceToPoint) {
+  constexpr double kEpsilon = 1E-9;
+
+  constexpr frc::Pose2d center{1_m, 2_m, 270_deg};
+  constexpr frc::Rectangle2d rect{center, 1_m, 2_m};
+
+  constexpr frc::Translation2d point1{2.5_m, 2_m};
+  EXPECT_NEAR(0.5, rect.Distance(point1).value(), kEpsilon);
+
+  constexpr frc::Translation2d point2{1_m, 2_m};
+  EXPECT_NEAR(0, rect.Distance(point2).value(), kEpsilon);
+
+  constexpr frc::Translation2d point3{1_m, 1_m};
+  EXPECT_NEAR(0.5, rect.Distance(point3).value(), kEpsilon);
+
+  constexpr frc::Translation2d point4{-1_m, 2.5_m};
+  EXPECT_NEAR(1, rect.Distance(point4).value(), kEpsilon);
+}
+
+TEST(Rectangle2dTest, FindNearestPoint) {
+  constexpr double kEpsilon = 1E-9;
+
+  constexpr frc::Pose2d center{1_m, 1_m, 90_deg};
+  constexpr frc::Rectangle2d rect{center, 3_m, 4_m};
+
+  constexpr frc::Translation2d point1{1_m, 3_m};
+  auto nearestPoint1 = rect.FindNearestPoint(point1);
+  EXPECT_NEAR(1.0, nearestPoint1.X().value(), kEpsilon);
+  EXPECT_NEAR(2.5, nearestPoint1.Y().value(), kEpsilon);
+
+  constexpr frc::Translation2d point2{0_m, 0_m};
+  auto nearestPoint2 = rect.FindNearestPoint(point2);
+  EXPECT_NEAR(0.0, nearestPoint2.X().value(), kEpsilon);
+  EXPECT_NEAR(0.0, nearestPoint2.Y().value(), kEpsilon);
+}
+
+TEST(Rectangle2dTest, Equals) {
+  constexpr frc::Pose2d center1{2_m, 3_m, 0_deg};
+  constexpr frc::Rectangle2d rect1{center1, 5_m, 3_m};
+
+  constexpr frc::Pose2d center2{2_m, 3_m, 0_deg};
+  constexpr frc::Rectangle2d rect2{center2, 5_m, 3_m};
+
+  constexpr frc::Pose2d center3{2_m, 3_m, 0_deg};
+  constexpr frc::Rectangle2d rect3{center3, 3_m, 3_m};
+
+  EXPECT_EQ(rect1, rect2);
+  EXPECT_NE(rect2, rect3);
+}

wpimath/src/test/native/cpp/geometry/proto/Ellipse2dProtoTest.cpp

@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Ellipse2d.h"
+#include "geometry2d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Ellipse2d>;
+
+const Ellipse2d kExpectedData{
+    Pose2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{22.9_rad}}, 1.2_m, 2.3_m};
+}  // namespace
+
+TEST(Ellipse2dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Ellipse2d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.Center(), unpacked_data.Center());
+  EXPECT_EQ(kExpectedData.XSemiAxis(), unpacked_data.XSemiAxis());
+  EXPECT_EQ(kExpectedData.YSemiAxis(), unpacked_data.YSemiAxis());
+}

wpimath/src/test/native/cpp/geometry/proto/Rectangle2dProtoTest.cpp

@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Rectangle2d.h"
+#include "geometry2d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Rectangle2d>;
+
+const Rectangle2d kExpectedData{
+    Pose2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{22.9_rad}}, 1.2_m, 2.3_m};
+}  // namespace
+
+TEST(Rectangle2dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Rectangle2d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.Center(), unpacked_data.Center());
+  EXPECT_EQ(kExpectedData.XWidth(), unpacked_data.XWidth());
+  EXPECT_EQ(kExpectedData.YWidth(), unpacked_data.YWidth());
+}

wpimath/src/test/native/cpp/geometry/struct/Ellipse2dStructTest.cpp

@@ -0,0 +1,28 @@
+// 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.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Ellipse2d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Ellipse2d>;
+const Ellipse2d kExpectedData{
+    Pose2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{22.9_rad}}, 1.2_m, 2.3_m};
+}  // namespace
+
+TEST(Ellipse2dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  Ellipse2d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.Center(), unpacked_data.Center());
+  EXPECT_EQ(kExpectedData.XSemiAxis(), unpacked_data.XSemiAxis());
+  EXPECT_EQ(kExpectedData.YSemiAxis(), unpacked_data.YSemiAxis());
+}

wpimath/src/test/native/cpp/geometry/struct/Rectangle2dStructTest.cpp

@@ -0,0 +1,28 @@
+// 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.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Rectangle2d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Rectangle2d>;
+const Rectangle2d kExpectedData{
+    Pose2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{22.9_rad}}, 1.2_m, 2.3_m};
+}  // namespace
+
+TEST(Rectangle2dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  Rectangle2d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.Center(), unpacked_data.Center());
+  EXPECT_EQ(kExpectedData.XWidth(), unpacked_data.XWidth());
+  EXPECT_EQ(kExpectedData.YWidth(), unpacked_data.YWidth());
+}