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

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

wpimath/CMakeLists.txt

@@ -22,6 +22,7 @@ file(
     src/main/native/cpp/jni/WPIMathJNI_ArmFeedforward.cpp
     src/main/native/cpp/jni/WPIMathJNI_DARE.cpp
     src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp
+    src/main/native/cpp/jni/WPIMathJNI_Ellipse2d.cpp
     src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp
     src/main/native/cpp/jni/WPIMathJNI_Pose3d.cpp
     src/main/native/cpp/jni/WPIMathJNI_StateSpaceUtil.cpp

wpimath/src/generated/main/java/edu/wpi/first/math/proto/Geometry2D.java

@@ -18,7 +18,7 @@ import us.hebi.quickbuf.ProtoUtil;
 import us.hebi.quickbuf.RepeatedByte;
 
 public final class Geometry2D {
-  private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(1256,
+  private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(1889,
     "ChBnZW9tZXRyeTJkLnByb3RvEgl3cGkucHJvdG8iMwoVUHJvdG9idWZUcmFuc2xhdGlvbjJkEgwKAXgY" + 
     "ASABKAFSAXgSDAoBeRgCIAEoAVIBeSIqChJQcm90b2J1ZlJvdGF0aW9uMmQSFAoFdmFsdWUYASABKAFS" + 
     "BXZhbHVlIo8BCg5Qcm90b2J1ZlBvc2UyZBJCCgt0cmFuc2xhdGlvbhgBIAEoCzIgLndwaS5wcm90by5Q" + 
@@ -27,19 +27,30 @@ public final class Geometry2D {
     "dHJhbnNsYXRpb24YASABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUgt0cmFuc2xh" + 
     "dGlvbhI5Cghyb3RhdGlvbhgCIAEoCzIdLndwaS5wcm90by5Qcm90b2J1ZlJvdGF0aW9uMmRSCHJvdGF0" + 
     "aW9uIkkKD1Byb3RvYnVmVHdpc3QyZBIOCgJkeBgBIAEoAVICZHgSDgoCZHkYAiABKAFSAmR5EhYKBmR0" + 
-    "aGV0YRgDIAEoAVIGZHRoZXRhQhoKGGVkdS53cGkuZmlyc3QubWF0aC5wcm90b0rPBQoGEgQAAB0BCggK" + 
-    "AQwSAwAAEgoICgECEgMCABIKCAoBCBIDBAAxCgkKAggBEgMEADEKCgoCBAASBAYACQEKCgoDBAABEgMG" + 
-    "CB0KCwoEBAACABIDBwIPCgwKBQQAAgAFEgMHAggKDAoFBAACAAESAwcJCgoMCgUEAAIAAxIDBw0OCgsK" + 
-    "BAQAAgESAwgCDwoMCgUEAAIBBRIDCAIICgwKBQQAAgEBEgMICQoKDAoFBAACAQMSAwgNDgoKCgIEARIE" + 
-    "CwANAQoKCgMEAQESAwsIGgoLCgQEAQIAEgMMAhMKDAoFBAECAAUSAwwCCAoMCgUEAQIAARIDDAkOCgwK" + 
-    "BQQBAgADEgMMERIKCgoCBAISBA8AEgEKCgoDBAIBEgMPCBYKCwoEBAICABIDEAIoCgwKBQQCAgAGEgMQ" + 
-    "AhcKDAoFBAICAAESAxAYIwoMCgUEAgIAAxIDECYnCgsKBAQCAgESAxECIgoMCgUEAgIBBhIDEQIUCgwK" + 
-    "BQQCAgEBEgMRFR0KDAoFBAICAQMSAxEgIQoKCgIEAxIEFAAXAQoKCgMEAwESAxQIGwoLCgQEAwIAEgMV" + 
-    "AigKDAoFBAMCAAYSAxUCFwoMCgUEAwIAARIDFRgjCgwKBQQDAgADEgMVJicKCwoEBAMCARIDFgIiCgwK" + 
-    "BQQDAgEGEgMWAhQKDAoFBAMCAQESAxYVHQoMCgUEAwIBAxIDFiAhCgoKAgQEEgQZAB0BCgoKAwQEARID" + 
-    "GQgXCgsKBAQEAgASAxoCEAoMCgUEBAIABRIDGgIICgwKBQQEAgABEgMaCQsKDAoFBAQCAAMSAxoODwoL" + 
-    "CgQEBAIBEgMbAhAKDAoFBAQCAQUSAxsCCAoMCgUEBAIBARIDGwkLCgwKBQQEAgEDEgMbDg8KCwoEBAQC",
-    "AhIDHAIUCgwKBQQEAgIFEgMcAggKDAoFBAQCAgESAxwJDwoMCgUEBAICAxIDHBITYgZwcm90bzM=");
+    "aGV0YRgDIAEoAVIGZHRoZXRhIngKE1Byb3RvYnVmUmVjdGFuZ2xlMmQSMQoGY2VudGVyGAEgASgLMhku" + 
+    "d3BpLnByb3RvLlByb3RvYnVmUG9zZTJkUgZjZW50ZXISFgoGeFdpZHRoGAIgASgBUgZ4V2lkdGgSFgoG" + 
+    "eVdpZHRoGAMgASgBUgZ5V2lkdGgiggEKEVByb3RvYnVmRWxsaXBzZTJkEjEKBmNlbnRlchgBIAEoCzIZ" + 
+    "LndwaS5wcm90by5Qcm90b2J1ZlBvc2UyZFIGY2VudGVyEhwKCXhTZW1pQXhpcxgCIAEoAVIJeFNlbWlB" + 
+    "eGlzEhwKCXlTZW1pQXhpcxgDIAEoAVIJeVNlbWlBeGlzQhoKGGVkdS53cGkuZmlyc3QubWF0aC5wcm90" + 
+    "b0rJCAoGEgQAACkBCggKAQwSAwAAEgoICgECEgMCABIKCAoBCBIDBAAxCgkKAggBEgMEADEKCgoCBAAS" + 
+    "BAYACQEKCgoDBAABEgMGCB0KCwoEBAACABIDBwIPCgwKBQQAAgAFEgMHAggKDAoFBAACAAESAwcJCgoM" + 
+    "CgUEAAIAAxIDBw0OCgsKBAQAAgESAwgCDwoMCgUEAAIBBRIDCAIICgwKBQQAAgEBEgMICQoKDAoFBAAC" + 
+    "AQMSAwgNDgoKCgIEARIECwANAQoKCgMEAQESAwsIGgoLCgQEAQIAEgMMAhMKDAoFBAECAAUSAwwCCAoM" + 
+    "CgUEAQIAARIDDAkOCgwKBQQBAgADEgMMERIKCgoCBAISBA8AEgEKCgoDBAIBEgMPCBYKCwoEBAICABID" + 
+    "EAIoCgwKBQQCAgAGEgMQAhcKDAoFBAICAAESAxAYIwoMCgUEAgIAAxIDECYnCgsKBAQCAgESAxECIgoM" + 
+    "CgUEAgIBBhIDEQIUCgwKBQQCAgEBEgMRFR0KDAoFBAICAQMSAxEgIQoKCgIEAxIEFAAXAQoKCgMEAwES",
+    "AxQIGwoLCgQEAwIAEgMVAigKDAoFBAMCAAYSAxUCFwoMCgUEAwIAARIDFRgjCgwKBQQDAgADEgMVJicK" + 
+    "CwoEBAMCARIDFgIiCgwKBQQDAgEGEgMWAhQKDAoFBAMCAQESAxYVHQoMCgUEAwIBAxIDFiAhCgoKAgQE" + 
+    "EgQZAB0BCgoKAwQEARIDGQgXCgsKBAQEAgASAxoCEAoMCgUEBAIABRIDGgIICgwKBQQEAgABEgMaCQsK" + 
+    "DAoFBAQCAAMSAxoODwoLCgQEBAIBEgMbAhAKDAoFBAQCAQUSAxsCCAoMCgUEBAIBARIDGwkLCgwKBQQE" + 
+    "AgEDEgMbDg8KCwoEBAQCAhIDHAIUCgwKBQQEAgIFEgMcAggKDAoFBAQCAgESAxwJDwoMCgUEBAICAxID" + 
+    "HBITCgoKAgQFEgQfACMBCgoKAwQFARIDHwgbCgsKBAQFAgASAyACHAoMCgUEBQIABhIDIAIQCgwKBQQF" + 
+    "AgABEgMgERcKDAoFBAUCAAMSAyAaGwoLCgQEBQIBEgMhAhQKDAoFBAUCAQUSAyECCAoMCgUEBQIBARID" + 
+    "IQkPCgwKBQQFAgEDEgMhEhMKCwoEBAUCAhIDIgIUCgwKBQQFAgIFEgMiAggKDAoFBAUCAgESAyIJDwoM" + 
+    "CgUEBQICAxIDIhITCgoKAgQGEgQlACkBCgoKAwQGARIDJQgZCgsKBAQGAgASAyYCHAoMCgUEBgIABhID" + 
+    "JgIQCgwKBQQGAgABEgMmERcKDAoFBAYCAAMSAyYaGwoLCgQEBgIBEgMnAhcKDAoFBAYCAQUSAycCCAoM" + 
+    "CgUEBgIBARIDJwkSCgwKBQQGAgEDEgMnFRYKCwoEBAYCAhIDKAIXCgwKBQQGAgIFEgMoAggKDAoFBAYC" + 
+    "AgESAygJEgoMCgUEBgICAxIDKBUWYgZwcm90bzM=");
 
   static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("geometry2d.proto", "wpi.proto", descriptorData);
 
@@ -53,6 +64,10 @@ public final class Geometry2D {
 
   static final Descriptors.Descriptor wpi_proto_ProtobufTwist2d_descriptor = descriptor.internalContainedType(425, 73, "ProtobufTwist2d", "wpi.proto.ProtobufTwist2d");
 
+  static final Descriptors.Descriptor wpi_proto_ProtobufRectangle2d_descriptor = descriptor.internalContainedType(500, 120, "ProtobufRectangle2d", "wpi.proto.ProtobufRectangle2d");
+
+  static final Descriptors.Descriptor wpi_proto_ProtobufEllipse2d_descriptor = descriptor.internalContainedType(623, 130, "ProtobufEllipse2d", "wpi.proto.ProtobufEllipse2d");
+
   /**
    * @return this proto file's descriptor.
    */
@@ -1803,4 +1818,870 @@ public final class Geometry2D {
       static final FieldName dtheta = FieldName.forField("dtheta");
     }
   }
+
+  /**
+   * Protobuf type {@code ProtobufRectangle2d}
+   */
+  public static final class ProtobufRectangle2d extends ProtoMessage<ProtobufRectangle2d> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional double xWidth = 2;</code>
+     */
+    private double xWidth;
+
+    /**
+     * <code>optional double yWidth = 3;</code>
+     */
+    private double yWidth;
+
+    /**
+     * <code>optional .wpi.proto.ProtobufPose2d center = 1;</code>
+     */
+    private final ProtobufPose2d center = ProtobufPose2d.newInstance();
+
+    private ProtobufRectangle2d() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufRectangle2d}
+     */
+    public static ProtobufRectangle2d newInstance() {
+      return new ProtobufRectangle2d();
+    }
+
+    /**
+     * <code>optional double xWidth = 2;</code>
+     * @return whether the xWidth field is set
+     */
+    public boolean hasXWidth() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional double xWidth = 2;</code>
+     * @return this
+     */
+    public ProtobufRectangle2d clearXWidth() {
+      bitField0_ &= ~0x00000001;
+      xWidth = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double xWidth = 2;</code>
+     * @return the xWidth
+     */
+    public double getXWidth() {
+      return xWidth;
+    }
+
+    /**
+     * <code>optional double xWidth = 2;</code>
+     * @param value the xWidth to set
+     * @return this
+     */
+    public ProtobufRectangle2d setXWidth(final double value) {
+      bitField0_ |= 0x00000001;
+      xWidth = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double yWidth = 3;</code>
+     * @return whether the yWidth field is set
+     */
+    public boolean hasYWidth() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional double yWidth = 3;</code>
+     * @return this
+     */
+    public ProtobufRectangle2d clearYWidth() {
+      bitField0_ &= ~0x00000002;
+      yWidth = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double yWidth = 3;</code>
+     * @return the yWidth
+     */
+    public double getYWidth() {
+      return yWidth;
+    }
+
+    /**
+     * <code>optional double yWidth = 3;</code>
+     * @param value the yWidth to set
+     * @return this
+     */
+    public ProtobufRectangle2d setYWidth(final double value) {
+      bitField0_ |= 0x00000002;
+      yWidth = value;
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufPose2d center = 1;</code>
+     * @return whether the center field is set
+     */
+    public boolean hasCenter() {
+      return (bitField0_ & 0x00000004) != 0;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufPose2d center = 1;</code>
+     * @return this
+     */
+    public ProtobufRectangle2d clearCenter() {
+      bitField0_ &= ~0x00000004;
+      center.clear();
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufPose2d center = 1;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableCenter()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public ProtobufPose2d getCenter() {
+      return center;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufPose2d center = 1;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public ProtobufPose2d getMutableCenter() {
+      bitField0_ |= 0x00000004;
+      return center;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufPose2d center = 1;</code>
+     * @param value the center to set
+     * @return this
+     */
+    public ProtobufRectangle2d setCenter(final ProtobufPose2d value) {
+      bitField0_ |= 0x00000004;
+      center.copyFrom(value);
+      return this;
+    }
+
+    @Override
+    public ProtobufRectangle2d copyFrom(final ProtobufRectangle2d other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        xWidth = other.xWidth;
+        yWidth = other.yWidth;
+        center.copyFrom(other.center);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufRectangle2d mergeFrom(final ProtobufRectangle2d other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasXWidth()) {
+        setXWidth(other.xWidth);
+      }
+      if (other.hasYWidth()) {
+        setYWidth(other.yWidth);
+      }
+      if (other.hasCenter()) {
+        getMutableCenter().mergeFrom(other.center);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufRectangle2d clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      xWidth = 0D;
+      yWidth = 0D;
+      center.clear();
+      return this;
+    }
+
+    @Override
+    public ProtobufRectangle2d clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      center.clearQuick();
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufRectangle2d)) {
+        return false;
+      }
+      ProtobufRectangle2d other = (ProtobufRectangle2d) o;
+      return bitField0_ == other.bitField0_
+        && (!hasXWidth() || ProtoUtil.isEqual(xWidth, other.xWidth))
+        && (!hasYWidth() || ProtoUtil.isEqual(yWidth, other.yWidth))
+        && (!hasCenter() || center.equals(other.center));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 17);
+        output.writeDoubleNoTag(xWidth);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 25);
+        output.writeDoubleNoTag(yWidth);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeRawByte((byte) 10);
+        output.writeMessageNoTag(center);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        size += 1 + ProtoSink.computeMessageSizeNoTag(center);
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufRectangle2d mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 17: {
+            // xWidth
+            xWidth = input.readDouble();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 25) {
+              break;
+            }
+          }
+          case 25: {
+            // yWidth
+            yWidth = input.readDouble();
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 10) {
+              break;
+            }
+          }
+          case 10: {
+            // center
+            input.readMessage(center);
+            bitField0_ |= 0x00000004;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeDouble(FieldNames.xWidth, xWidth);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeDouble(FieldNames.yWidth, yWidth);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeMessage(FieldNames.center, center);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufRectangle2d mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case -775894994: {
+            if (input.isAtField(FieldNames.xWidth)) {
+              if (!input.trySkipNullValue()) {
+                xWidth = input.readDouble();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -747265843: {
+            if (input.isAtField(FieldNames.yWidth)) {
+              if (!input.trySkipNullValue()) {
+                yWidth = input.readDouble();
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -1364013995: {
+            if (input.isAtField(FieldNames.center)) {
+              if (!input.trySkipNullValue()) {
+                input.readMessage(center);
+                bitField0_ |= 0x00000004;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufRectangle2d clone() {
+      return new ProtobufRectangle2d().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufRectangle2d parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufRectangle2d(), data).checkInitialized();
+    }
+
+    public static ProtobufRectangle2d parseFrom(final ProtoSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufRectangle2d(), input).checkInitialized();
+    }
+
+    public static ProtobufRectangle2d parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufRectangle2d(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufRectangle2d messages
+     */
+    public static MessageFactory<ProtobufRectangle2d> getFactory() {
+      return ProtobufRectangle2dFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Geometry2D.wpi_proto_ProtobufRectangle2d_descriptor;
+    }
+
+    private enum ProtobufRectangle2dFactory implements MessageFactory<ProtobufRectangle2d> {
+      INSTANCE;
+
+      @Override
+      public ProtobufRectangle2d create() {
+        return ProtobufRectangle2d.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName xWidth = FieldName.forField("xWidth");
+
+      static final FieldName yWidth = FieldName.forField("yWidth");
+
+      static final FieldName center = FieldName.forField("center");
+    }
+  }
+
+  /**
+   * Protobuf type {@code ProtobufEllipse2d}
+   */
+  public static final class ProtobufEllipse2d extends ProtoMessage<ProtobufEllipse2d> implements Cloneable {
+    private static final long serialVersionUID = 0L;
+
+    /**
+     * <code>optional double xSemiAxis = 2;</code>
+     */
+    private double xSemiAxis;
+
+    /**
+     * <code>optional double ySemiAxis = 3;</code>
+     */
+    private double ySemiAxis;
+
+    /**
+     * <code>optional .wpi.proto.ProtobufPose2d center = 1;</code>
+     */
+    private final ProtobufPose2d center = ProtobufPose2d.newInstance();
+
+    private ProtobufEllipse2d() {
+    }
+
+    /**
+     * @return a new empty instance of {@code ProtobufEllipse2d}
+     */
+    public static ProtobufEllipse2d newInstance() {
+      return new ProtobufEllipse2d();
+    }
+
+    /**
+     * <code>optional double xSemiAxis = 2;</code>
+     * @return whether the xSemiAxis field is set
+     */
+    public boolean hasXSemiAxis() {
+      return (bitField0_ & 0x00000001) != 0;
+    }
+
+    /**
+     * <code>optional double xSemiAxis = 2;</code>
+     * @return this
+     */
+    public ProtobufEllipse2d clearXSemiAxis() {
+      bitField0_ &= ~0x00000001;
+      xSemiAxis = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double xSemiAxis = 2;</code>
+     * @return the xSemiAxis
+     */
+    public double getXSemiAxis() {
+      return xSemiAxis;
+    }
+
+    /**
+     * <code>optional double xSemiAxis = 2;</code>
+     * @param value the xSemiAxis to set
+     * @return this
+     */
+    public ProtobufEllipse2d setXSemiAxis(final double value) {
+      bitField0_ |= 0x00000001;
+      xSemiAxis = value;
+      return this;
+    }
+
+    /**
+     * <code>optional double ySemiAxis = 3;</code>
+     * @return whether the ySemiAxis field is set
+     */
+    public boolean hasYSemiAxis() {
+      return (bitField0_ & 0x00000002) != 0;
+    }
+
+    /**
+     * <code>optional double ySemiAxis = 3;</code>
+     * @return this
+     */
+    public ProtobufEllipse2d clearYSemiAxis() {
+      bitField0_ &= ~0x00000002;
+      ySemiAxis = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double ySemiAxis = 3;</code>
+     * @return the ySemiAxis
+     */
+    public double getYSemiAxis() {
+      return ySemiAxis;
+    }
+
+    /**
+     * <code>optional double ySemiAxis = 3;</code>
+     * @param value the ySemiAxis to set
+     * @return this
+     */
+    public ProtobufEllipse2d setYSemiAxis(final double value) {
+      bitField0_ |= 0x00000002;
+      ySemiAxis = value;
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufPose2d center = 1;</code>
+     * @return whether the center field is set
+     */
+    public boolean hasCenter() {
+      return (bitField0_ & 0x00000004) != 0;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufPose2d center = 1;</code>
+     * @return this
+     */
+    public ProtobufEllipse2d clearCenter() {
+      bitField0_ &= ~0x00000004;
+      center.clear();
+      return this;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufPose2d center = 1;</code>
+     *
+     * This method returns the internal storage object without modifying any has state.
+     * The returned object should not be modified and be treated as read-only.
+     *
+     * Use {@link #getMutableCenter()} if you want to modify it.
+     *
+     * @return internal storage object for reading
+     */
+    public ProtobufPose2d getCenter() {
+      return center;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufPose2d center = 1;</code>
+     *
+     * This method returns the internal storage object and sets the corresponding
+     * has state. The returned object will become part of this message and its
+     * contents may be modified as long as the has state is not cleared.
+     *
+     * @return internal storage object for modifications
+     */
+    public ProtobufPose2d getMutableCenter() {
+      bitField0_ |= 0x00000004;
+      return center;
+    }
+
+    /**
+     * <code>optional .wpi.proto.ProtobufPose2d center = 1;</code>
+     * @param value the center to set
+     * @return this
+     */
+    public ProtobufEllipse2d setCenter(final ProtobufPose2d value) {
+      bitField0_ |= 0x00000004;
+      center.copyFrom(value);
+      return this;
+    }
+
+    @Override
+    public ProtobufEllipse2d copyFrom(final ProtobufEllipse2d other) {
+      cachedSize = other.cachedSize;
+      if ((bitField0_ | other.bitField0_) != 0) {
+        bitField0_ = other.bitField0_;
+        xSemiAxis = other.xSemiAxis;
+        ySemiAxis = other.ySemiAxis;
+        center.copyFrom(other.center);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufEllipse2d mergeFrom(final ProtobufEllipse2d other) {
+      if (other.isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      if (other.hasXSemiAxis()) {
+        setXSemiAxis(other.xSemiAxis);
+      }
+      if (other.hasYSemiAxis()) {
+        setYSemiAxis(other.ySemiAxis);
+      }
+      if (other.hasCenter()) {
+        getMutableCenter().mergeFrom(other.center);
+      }
+      return this;
+    }
+
+    @Override
+    public ProtobufEllipse2d clear() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      xSemiAxis = 0D;
+      ySemiAxis = 0D;
+      center.clear();
+      return this;
+    }
+
+    @Override
+    public ProtobufEllipse2d clearQuick() {
+      if (isEmpty()) {
+        return this;
+      }
+      cachedSize = -1;
+      bitField0_ = 0;
+      center.clearQuick();
+      return this;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (o == this) {
+        return true;
+      }
+      if (!(o instanceof ProtobufEllipse2d)) {
+        return false;
+      }
+      ProtobufEllipse2d other = (ProtobufEllipse2d) o;
+      return bitField0_ == other.bitField0_
+        && (!hasXSemiAxis() || ProtoUtil.isEqual(xSemiAxis, other.xSemiAxis))
+        && (!hasYSemiAxis() || ProtoUtil.isEqual(ySemiAxis, other.ySemiAxis))
+        && (!hasCenter() || center.equals(other.center));
+    }
+
+    @Override
+    public void writeTo(final ProtoSink output) throws IOException {
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeRawByte((byte) 17);
+        output.writeDoubleNoTag(xSemiAxis);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeRawByte((byte) 25);
+        output.writeDoubleNoTag(ySemiAxis);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeRawByte((byte) 10);
+        output.writeMessageNoTag(center);
+      }
+    }
+
+    @Override
+    protected int computeSerializedSize() {
+      int size = 0;
+      if ((bitField0_ & 0x00000001) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        size += 9;
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        size += 1 + ProtoSink.computeMessageSizeNoTag(center);
+      }
+      return size;
+    }
+
+    @Override
+    @SuppressWarnings("fallthrough")
+    public ProtobufEllipse2d mergeFrom(final ProtoSource input) throws IOException {
+      // Enabled Fall-Through Optimization (QuickBuffers)
+      int tag = input.readTag();
+      while (true) {
+        switch (tag) {
+          case 17: {
+            // xSemiAxis
+            xSemiAxis = input.readDouble();
+            bitField0_ |= 0x00000001;
+            tag = input.readTag();
+            if (tag != 25) {
+              break;
+            }
+          }
+          case 25: {
+            // ySemiAxis
+            ySemiAxis = input.readDouble();
+            bitField0_ |= 0x00000002;
+            tag = input.readTag();
+            if (tag != 10) {
+              break;
+            }
+          }
+          case 10: {
+            // center
+            input.readMessage(center);
+            bitField0_ |= 0x00000004;
+            tag = input.readTag();
+            if (tag != 0) {
+              break;
+            }
+          }
+          case 0: {
+            return this;
+          }
+          default: {
+            if (!input.skipField(tag)) {
+              return this;
+            }
+            tag = input.readTag();
+            break;
+          }
+        }
+      }
+    }
+
+    @Override
+    public void writeTo(final JsonSink output) throws IOException {
+      output.beginObject();
+      if ((bitField0_ & 0x00000001) != 0) {
+        output.writeDouble(FieldNames.xSemiAxis, xSemiAxis);
+      }
+      if ((bitField0_ & 0x00000002) != 0) {
+        output.writeDouble(FieldNames.ySemiAxis, ySemiAxis);
+      }
+      if ((bitField0_ & 0x00000004) != 0) {
+        output.writeMessage(FieldNames.center, center);
+      }
+      output.endObject();
+    }
+
+    @Override
+    public ProtobufEllipse2d mergeFrom(final JsonSource input) throws IOException {
+      if (!input.beginObject()) {
+        return this;
+      }
+      while (!input.isAtEnd()) {
+        switch (input.readFieldHash()) {
+          case -858640185: {
+            if (input.isAtField(FieldNames.xSemiAxis)) {
+              if (!input.trySkipNullValue()) {
+                xSemiAxis = input.readDouble();
+                bitField0_ |= 0x00000001;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case 1628872648: {
+            if (input.isAtField(FieldNames.ySemiAxis)) {
+              if (!input.trySkipNullValue()) {
+                ySemiAxis = input.readDouble();
+                bitField0_ |= 0x00000002;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          case -1364013995: {
+            if (input.isAtField(FieldNames.center)) {
+              if (!input.trySkipNullValue()) {
+                input.readMessage(center);
+                bitField0_ |= 0x00000004;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
+          default: {
+            input.skipUnknownField();
+            break;
+          }
+        }
+      }
+      input.endObject();
+      return this;
+    }
+
+    @Override
+    public ProtobufEllipse2d clone() {
+      return new ProtobufEllipse2d().copyFrom(this);
+    }
+
+    @Override
+    public boolean isEmpty() {
+      return ((bitField0_) == 0);
+    }
+
+    public static ProtobufEllipse2d parseFrom(final byte[] data) throws
+        InvalidProtocolBufferException {
+      return ProtoMessage.mergeFrom(new ProtobufEllipse2d(), data).checkInitialized();
+    }
+
+    public static ProtobufEllipse2d parseFrom(final ProtoSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufEllipse2d(), input).checkInitialized();
+    }
+
+    public static ProtobufEllipse2d parseFrom(final JsonSource input) throws IOException {
+      return ProtoMessage.mergeFrom(new ProtobufEllipse2d(), input).checkInitialized();
+    }
+
+    /**
+     * @return factory for creating ProtobufEllipse2d messages
+     */
+    public static MessageFactory<ProtobufEllipse2d> getFactory() {
+      return ProtobufEllipse2dFactory.INSTANCE;
+    }
+
+    /**
+     * @return this type's descriptor.
+     */
+    public static Descriptors.Descriptor getDescriptor() {
+      return Geometry2D.wpi_proto_ProtobufEllipse2d_descriptor;
+    }
+
+    private enum ProtobufEllipse2dFactory implements MessageFactory<ProtobufEllipse2d> {
+      INSTANCE;
+
+      @Override
+      public ProtobufEllipse2d create() {
+        return ProtobufEllipse2d.newInstance();
+      }
+    }
+
+    /**
+     * Contains name constants used for serializing JSON
+     */
+    static class FieldNames {
+      static final FieldName xSemiAxis = FieldName.forField("xSemiAxis");
+
+      static final FieldName ySemiAxis = FieldName.forField("ySemiAxis");
+
+      static final FieldName center = FieldName.forField("center");
+    }
+  }
 }

wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java

@@ -279,6 +279,32 @@ public final class WPIMathJNI {
   public static native void solveFullPivHouseholderQr(
       double[] A, int Arows, int Acols, double[] B, int Brows, int Bcols, double[] dst);
 
+  // Ellipse2d wrappers
+
+  /**
+   * Returns the nearest point that is contained within the ellipse.
+   *
+   * <p>Constructs an Ellipse2d object and runs its FindNearestPoint() method.
+   *
+   * @param centerX The x coordinate of the center of the ellipse in meters.
+   * @param centerY The y coordinate of the center of the ellipse in meters.
+   * @param centerHeading The ellipse's rotation in radians.
+   * @param xSemiAxis The x semi-axis in meters.
+   * @param ySemiAxis The y semi-axis in meters.
+   * @param pointX The x coordinate of the point that this will find the nearest point to.
+   * @param pointY The y coordinate of the point that this will find the nearest point to.
+   * @param nearestPoint Array to store nearest point into.
+   */
+  public static native void ellipse2dFindNearestPoint(
+      double centerX,
+      double centerY,
+      double centerHeading,
+      double xSemiAxis,
+      double ySemiAxis,
+      double pointX,
+      double pointY,
+      double[] nearestPoint);
+
   // Pose3d wrappers
 
   /**

wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java

@@ -0,0 +1,241 @@
+// 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 edu.wpi.first.math.Pair;
+import edu.wpi.first.math.WPIMathJNI;
+import edu.wpi.first.math.geometry.proto.Ellipse2dProto;
+import edu.wpi.first.math.geometry.struct.Ellipse2dStruct;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+import java.util.Objects;
+
+/** Represents a 2d ellipse space containing translational, rotational, and scaling components. */
+public class Ellipse2d implements ProtobufSerializable, StructSerializable {
+  private final Pose2d m_center;
+  private final double m_xSemiAxis;
+  private final double m_ySemiAxis;
+
+  /**
+   * Constructs an ellipse around a center point and two semi-axes, a horizontal and vertical one.
+   *
+   * @param center The center of the ellipse.
+   * @param xSemiAxis The x semi-axis.
+   * @param ySemiAxis The y semi-axis.
+   */
+  public Ellipse2d(Pose2d center, double xSemiAxis, double ySemiAxis) {
+    if (xSemiAxis <= 0 || ySemiAxis <= 0) {
+      throw new IllegalArgumentException("Ellipse2d semi-axes must be positive");
+    }
+
+    m_center = center;
+    m_xSemiAxis = xSemiAxis;
+    m_ySemiAxis = ySemiAxis;
+  }
+
+  /**
+   * Constructs a perfectly circular ellipse with the specified radius.
+   *
+   * @param center The center of the circle.
+   * @param radius The radius of the circle.
+   */
+  public Ellipse2d(Translation2d center, double radius) {
+    this(new Pose2d(center, Rotation2d.kZero), radius, radius);
+  }
+
+  /**
+   * Returns the center of the ellipse.
+   *
+   * @return The center of the ellipse.
+   */
+  public Pose2d getCenter() {
+    return m_center;
+  }
+
+  /**
+   * Returns the rotational component of the ellipse.
+   *
+   * @return The rotational component of the ellipse.
+   */
+  public Rotation2d getRotation() {
+    return m_center.getRotation();
+  }
+
+  /**
+   * Returns the x semi-axis.
+   *
+   * @return The x semi-axis.
+   */
+  public double getXSemiAxis() {
+    return m_xSemiAxis;
+  }
+
+  /**
+   * Returns the y semi-axis.
+   *
+   * @return The y semi-axis.
+   */
+  public double getYSemiAxis() {
+    return m_ySemiAxis;
+  }
+
+  /**
+   * Returns the focal points of the ellipse. In a perfect circle, this will always return the
+   * center.
+   *
+   * @return The focal points.
+   */
+  public Pair<Translation2d, Translation2d> getFocalPoints() {
+    // Major semi-axis
+    double a = Math.max(m_xSemiAxis, m_ySemiAxis);
+
+    // Minor semi-axis
+    double b = Math.min(m_xSemiAxis, m_ySemiAxis);
+
+    double c = Math.sqrt(a * a - b * b);
+
+    if (m_xSemiAxis > m_ySemiAxis) {
+      return new Pair<>(
+          m_center.plus(new Transform2d(-c, 0.0, Rotation2d.kZero)).getTranslation(),
+          m_center.plus(new Transform2d(c, 0.0, Rotation2d.kZero)).getTranslation());
+    } else {
+      return new Pair<>(
+          m_center.plus(new Transform2d(0.0, -c, Rotation2d.kZero)).getTranslation(),
+          m_center.plus(new Transform2d(0.0, c, Rotation2d.kZero)).getTranslation());
+    }
+  }
+
+  /**
+   * Transforms the center of the ellipse and returns the new ellipse.
+   *
+   * @param other The transform to transform by.
+   * @return The transformed ellipse.
+   */
+  public Ellipse2d transformBy(Transform2d other) {
+    return new Ellipse2d(m_center.transformBy(other), m_xSemiAxis, m_ySemiAxis);
+  }
+
+  /**
+   * Rotates the center of the ellipse and returns the new ellipse.
+   *
+   * @param other The rotation to transform by.
+   * @return The rotated ellipse.
+   */
+  public Ellipse2d rotateBy(Rotation2d other) {
+    return new Ellipse2d(m_center.rotateBy(other), m_xSemiAxis, m_ySemiAxis);
+  }
+
+  /**
+   * Checks if a point is intersected by this ellipse's circumference.
+   *
+   * @param point The point to check.
+   * @return True, if this ellipse's circumference intersects the point.
+   */
+  public boolean intersects(Translation2d point) {
+    return Math.abs(1.0 - solveEllipseEquation(point)) <= 1E-9;
+  }
+
+  /**
+   * Checks if a point is contained within this ellipse. This is inclusive, if the point lies on the
+   * circumference this will return {@code true}.
+   *
+   * @param point The point to check.
+   * @return True, if the point is within or on the ellipse.
+   */
+  public boolean contains(Translation2d point) {
+    return solveEllipseEquation(point) <= 1.0;
+  }
+
+  /**
+   * Returns the distance between the perimeter of the ellipse and the point.
+   *
+   * @param point The point to check.
+   * @return The distance (0, if the point is contained by the ellipse)
+   */
+  public double getDistance(Translation2d point) {
+    return findNearestPoint(point).getDistance(point);
+  }
+
+  /**
+   * Returns the nearest point that is contained within the ellipse.
+   *
+   * @param point The point that this will find the nearest point to.
+   * @return A new point that is nearest to {@code point} and contained in the ellipse.
+   */
+  public Translation2d findNearestPoint(Translation2d point) {
+    // Check if already in ellipse
+    if (contains(point)) {
+      return point;
+    }
+
+    // Find nearest point
+    var nearestPoint = new double[2];
+    WPIMathJNI.ellipse2dFindNearestPoint(
+        m_center.getX(),
+        m_center.getY(),
+        m_center.getRotation().getRadians(),
+        m_xSemiAxis,
+        m_ySemiAxis,
+        point.getX(),
+        point.getY(),
+        nearestPoint);
+    return new Translation2d(nearestPoint[0], nearestPoint[1]);
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "Ellipse2d(center: %s, x: %.2f, y:%.2f)", m_center, m_xSemiAxis, m_ySemiAxis);
+  }
+
+  /**
+   * Checks equality between this Ellipse2d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof Ellipse2d) {
+      return ((Ellipse2d) obj).getCenter().equals(m_center)
+          && ((Ellipse2d) obj).getXSemiAxis() == m_xSemiAxis
+          && ((Ellipse2d) obj).getYSemiAxis() == m_ySemiAxis;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_center, m_xSemiAxis, m_ySemiAxis);
+  }
+
+  /**
+   * Solves the equation of an ellipse from the given point. This is a helper function used to
+   * determine if that point lies inside of or on an ellipse.
+   *
+   * <pre>
+   * (x - h)²/a² + (y - k)²/b² = 1
+   * </pre>
+   *
+   * @param point The point to solve for.
+   * @return < 1.0 if the point lies inside the ellipse, == 1.0 if a point lies on the ellipse, and
+   *     > 1.0 if the point lies outsides the ellipse.
+   */
+  private double solveEllipseEquation(Translation2d point) {
+    // Rotate the point by the inverse of the ellipse's rotation
+    point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus());
+
+    double x = point.getX() - m_center.getX();
+    double y = point.getY() - m_center.getY();
+
+    return (x * x) / (m_xSemiAxis * m_xSemiAxis) + (y * y) / (m_ySemiAxis * m_ySemiAxis);
+  }
+
+  /** Ellipse2d protobuf for serialization. */
+  public static final Ellipse2dProto proto = new Ellipse2dProto();
+
+  /** Ellipse2d struct for serialization. */
+  public static final Ellipse2dStruct struct = new Ellipse2dStruct();
+}

wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java

@@ -0,0 +1,217 @@
+// 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 edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.proto.Rectangle2dProto;
+import edu.wpi.first.math.geometry.struct.Rectangle2dStruct;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+import java.util.Objects;
+
+/**
+ * Represents a 2d rectangular space containing translational, rotational, and scaling components.
+ */
+public class Rectangle2d implements ProtobufSerializable, StructSerializable {
+  private final Pose2d m_center;
+  private final double m_xWidth;
+  private final double m_yWidth;
+
+  /**
+   * Constructs a rectangle at the specified position with the specified width and height.
+   *
+   * @param center The position (translation and rotation) of the rectangle.
+   * @param xWidth The x size component of the rectangle, in unrotated coordinate frame.
+   * @param yWidth The y size component of the rectangle, in unrotated coordinate frame.
+   */
+  public Rectangle2d(Pose2d center, double xWidth, double yWidth) {
+    if (xWidth < 0 || yWidth < 0) {
+      throw new IllegalArgumentException("Rectangle2d dimensions cannot be less than 0!");
+    }
+
+    m_center = center;
+    m_xWidth = xWidth;
+    m_yWidth = yWidth;
+  }
+
+  /**
+   * Creates an unrotated rectangle from the given corners. The corners should be diagonally
+   * opposite of each other.
+   *
+   * @param cornerA The first corner of the rectangle.
+   * @param cornerB The second corner of the rectangle.
+   */
+  public Rectangle2d(Translation2d cornerA, Translation2d cornerB) {
+    this(
+        new Pose2d(cornerA.plus(cornerB).div(2.0), Rotation2d.kZero),
+        Math.abs(cornerA.getX() - cornerB.getX()),
+        Math.abs(cornerA.getY() - cornerB.getY()));
+  }
+
+  /**
+   * Returns the center of the rectangle.
+   *
+   * @return The center of the rectangle.
+   */
+  public Pose2d getCenter() {
+    return m_center;
+  }
+
+  /**
+   * Returns the rotational component of the rectangle.
+   *
+   * @return The rotational component of the rectangle.
+   */
+  public Rotation2d getRotation() {
+    return m_center.getRotation();
+  }
+
+  /**
+   * Returns the x size component of the rectangle.
+   *
+   * @return The x size component of the rectangle.
+   */
+  public double getXWidth() {
+    return m_xWidth;
+  }
+
+  /**
+   * Returns the y size component of the rectangle.
+   *
+   * @return The y size component of the rectangle.
+   */
+  public double getYWidth() {
+    return m_yWidth;
+  }
+
+  /**
+   * Transforms the center of the rectangle and returns the new rectangle.
+   *
+   * @param other The transform to transform by.
+   * @return The transformed rectangle
+   */
+  public Rectangle2d transformBy(Transform2d other) {
+    return new Rectangle2d(m_center.transformBy(other), m_xWidth, m_yWidth);
+  }
+
+  /**
+   * Rotates the center of the rectangle and returns the new rectangle.
+   *
+   * @param other The rotation to transform by.
+   * @return The rotated rectangle.
+   */
+  public Rectangle2d rotateBy(Rotation2d other) {
+    return new Rectangle2d(m_center.rotateBy(other), m_xWidth, m_yWidth);
+  }
+
+  /**
+   * Checks if a point is intersected by the rectangle's perimeter.
+   *
+   * @param point The point to check.
+   * @return True, if the rectangle's perimeter intersects the point.
+   */
+  public boolean intersects(Translation2d point) {
+    // Move the point into the rectangle's coordinate frame
+    point = point.minus(m_center.getTranslation());
+    point = point.rotateBy(m_center.getRotation().unaryMinus());
+
+    if (Math.abs(Math.abs(point.getX()) - m_xWidth / 2.0) <= 1E-9) {
+      // Point rests on left/right perimeter
+      return Math.abs(point.getY()) <= m_yWidth / 2.0;
+    } else if (Math.abs(Math.abs(point.getY()) - m_yWidth / 2.0) <= 1E-9) {
+      // Point rests on top/bottom perimeter
+      return Math.abs(point.getX()) <= m_xWidth / 2.0;
+    }
+
+    return false;
+  }
+
+  /**
+   * Checks if a point is contained within the rectangle. This is inclusive, if the point lies on
+   * the perimeter it will return true.
+   *
+   * @param point The point to check.
+   * @return True, if the rectangle contains the point or the perimeter intersects the point.
+   */
+  public boolean contains(Translation2d point) {
+    // Rotate the point into the rectangle's coordinate frame
+    point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus());
+
+    // Check if within bounding box
+    return point.getX() >= (m_center.getX() - m_xWidth / 2.0)
+        && point.getX() <= (m_center.getX() + m_xWidth / 2.0)
+        && point.getY() >= (m_center.getY() - m_yWidth / 2.0)
+        && point.getY() <= (m_center.getY() + m_yWidth / 2.0);
+  }
+
+  /**
+   * Returns the distance between the perimeter of the rectangle and the point.
+   *
+   * @param point The point to check.
+   * @return The distance (0, if the point is contained by the rectangle)
+   */
+  public double getDistance(Translation2d point) {
+    return findNearestPoint(point).getDistance(point);
+  }
+
+  /**
+   * Returns the nearest point that is contained within the rectangle.
+   *
+   * @param point The point that this will find the nearest point to.
+   * @return A new point that is nearest to {@code point} and contained in the rectangle.
+   */
+  public Translation2d findNearestPoint(Translation2d point) {
+    // Check if already in rectangle
+    if (contains(point)) {
+      return point;
+    }
+
+    // Rotate the point by the inverse of the rectangle's rotation
+    point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus());
+
+    // Find nearest point
+    point =
+        new Translation2d(
+            MathUtil.clamp(
+                point.getX(), m_center.getX() - m_xWidth / 2.0, m_center.getX() + m_xWidth / 2.0),
+            MathUtil.clamp(
+                point.getY(), m_center.getY() - m_yWidth / 2.0, m_center.getY() + m_yWidth / 2.0));
+
+    // Undo rotation
+    return point.rotateAround(m_center.getTranslation(), m_center.getRotation());
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Rectangle2d(center: %s, x: %.2f, y: %.2f)", m_center, m_xWidth, m_yWidth);
+  }
+
+  /**
+   * Checks equality between this Rectangle2d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof Rectangle2d) {
+      return ((Rectangle2d) obj).getCenter().equals(m_center)
+          && ((Rectangle2d) obj).getXWidth() == m_xWidth
+          && ((Rectangle2d) obj).getYWidth() == m_yWidth;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_center, m_xWidth, m_yWidth);
+  }
+
+  /** Rectangle2d protobuf for serialization. */
+  public static final Rectangle2dProto proto = new Rectangle2dProto();
+
+  /** Rectangle2d struct for serialization. */
+  public static final Rectangle2dStruct struct = new Rectangle2dStruct();
+}

wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java

@@ -179,6 +179,24 @@ public class Translation2d
         m_x * other.getCos() - m_y * other.getSin(), m_x * other.getSin() + m_y * other.getCos());
   }
 
+  /**
+   * Rotates this translation around another translation in 2D space.
+   *
+   * <pre>
+   * [x_new]   [rot.cos, -rot.sin][x - other.x]   [other.x]
+   * [y_new] = [rot.sin,  rot.cos][y - other.y] + [other.y]
+   * </pre>
+   *
+   * @param other The other translation to rotate around.
+   * @param rot The rotation to rotate the translation by.
+   * @return The new rotated translation.
+   */
+  public Translation2d rotateAround(Translation2d other, Rotation2d rot) {
+    return new Translation2d(
+        (m_x - other.getX()) * rot.getCos() - (m_y - other.getY()) * rot.getSin() + other.getX(),
+        (m_x - other.getX()) * rot.getSin() + (m_y - other.getY()) * rot.getCos() + other.getY());
+  }
+
   /**
    * Returns the sum of two translations in 2D space.
    *

wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Ellipse2dProto.java

@@ -0,0 +1,46 @@
+// 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 edu.wpi.first.math.geometry.Ellipse2d;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufEllipse2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Ellipse2dProto implements Protobuf<Ellipse2d, ProtobufEllipse2d> {
+  @Override
+  public Class<Ellipse2d> getTypeClass() {
+    return Ellipse2d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufEllipse2d.getDescriptor();
+  }
+
+  @Override
+  public Protobuf<?, ?>[] getNested() {
+    return new Protobuf<?, ?>[] {Pose2d.proto};
+  }
+
+  @Override
+  public ProtobufEllipse2d createMessage() {
+    return ProtobufEllipse2d.newInstance();
+  }
+
+  @Override
+  public Ellipse2d unpack(ProtobufEllipse2d msg) {
+    return new Ellipse2d(
+        Pose2d.proto.unpack(msg.getCenter()), msg.getXSemiAxis(), msg.getYSemiAxis());
+  }
+
+  @Override
+  public void pack(ProtobufEllipse2d msg, Ellipse2d value) {
+    Pose2d.proto.pack(msg.getMutableCenter(), value.getCenter());
+    msg.setXSemiAxis(value.getXSemiAxis());
+    msg.setYSemiAxis(value.getYSemiAxis());
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rectangle2dProto.java

@@ -0,0 +1,45 @@
+// 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 edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rectangle2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufRectangle2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Rectangle2dProto implements Protobuf<Rectangle2d, ProtobufRectangle2d> {
+  @Override
+  public Class<Rectangle2d> getTypeClass() {
+    return Rectangle2d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufRectangle2d.getDescriptor();
+  }
+
+  @Override
+  public Protobuf<?, ?>[] getNested() {
+    return new Protobuf<?, ?>[] {Pose2d.proto};
+  }
+
+  @Override
+  public ProtobufRectangle2d createMessage() {
+    return ProtobufRectangle2d.newInstance();
+  }
+
+  @Override
+  public Rectangle2d unpack(ProtobufRectangle2d msg) {
+    return new Rectangle2d(Pose2d.proto.unpack(msg.getCenter()), msg.getXWidth(), msg.getYWidth());
+  }
+
+  @Override
+  public void pack(ProtobufRectangle2d msg, Rectangle2d value) {
+    Pose2d.proto.pack(msg.getMutableCenter(), value.getCenter());
+    msg.setXWidth(value.getXWidth());
+    msg.setYWidth(value.getYWidth());
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Ellipse2dStruct.java

@@ -0,0 +1,52 @@
+// 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 edu.wpi.first.math.geometry.Ellipse2d;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Ellipse2dStruct implements Struct<Ellipse2d> {
+  @Override
+  public Class<Ellipse2d> getTypeClass() {
+    return Ellipse2d.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:Ellipse2d";
+  }
+
+  @Override
+  public int getSize() {
+    return Pose2d.struct.getSize() + kSizeDouble + kSizeDouble;
+  }
+
+  @Override
+  public String getSchema() {
+    return "Pose2d center;double xSemiAxis; double ySemiAxis";
+  }
+
+  @Override
+  public Struct<?>[] getNested() {
+    return new Struct<?>[] {Pose2d.struct};
+  }
+
+  @Override
+  public Ellipse2d unpack(ByteBuffer bb) {
+    Pose2d center = Pose2d.struct.unpack(bb);
+    double xSemiAxis = bb.getDouble();
+    double ySemiAxis = bb.getDouble();
+    return new Ellipse2d(center, xSemiAxis, ySemiAxis);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Ellipse2d value) {
+    Pose2d.struct.pack(bb, value.getCenter());
+    bb.putDouble(value.getXSemiAxis());
+    bb.putDouble(value.getYSemiAxis());
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rectangle2dStruct.java

@@ -0,0 +1,52 @@
+// 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 edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rectangle2d;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Rectangle2dStruct implements Struct<Rectangle2d> {
+  @Override
+  public Class<Rectangle2d> getTypeClass() {
+    return Rectangle2d.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:Rectangle2d";
+  }
+
+  @Override
+  public int getSize() {
+    return Pose2d.struct.getSize() + kSizeDouble + kSizeDouble;
+  }
+
+  @Override
+  public String getSchema() {
+    return "Pose2d center;double xWidth; double yWidth";
+  }
+
+  @Override
+  public Struct<?>[] getNested() {
+    return new Struct<?>[] {Pose2d.struct};
+  }
+
+  @Override
+  public Rectangle2d unpack(ByteBuffer bb) {
+    Pose2d center = Pose2d.struct.unpack(bb);
+    double width = bb.getDouble();
+    double height = bb.getDouble();
+    return new Rectangle2d(center, width, height);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Rectangle2d value) {
+    Pose2d.struct.pack(bb, value.getCenter());
+    bb.putDouble(value.getXWidth());
+    bb.putDouble(value.getYWidth());
+  }
+}

wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp

@@ -0,0 +1,57 @@
+// 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 "frc/geometry/Ellipse2d.h"
+
+#include <sleipnir/optimization/OptimizationProblem.hpp>
+
+#include "geometry2d.pb.h"
+
+using namespace frc;
+
+units::meter_t Ellipse2d::Distance(const Translation2d& point) const {
+  return FindNearestPoint(point).Distance(point);
+}
+
+Translation2d Ellipse2d::FindNearestPoint(const Translation2d& point) const {
+  // Check if already in ellipse
+  if (Contains(point)) {
+    return point;
+  }
+
+  // Rotate the point by the inverse of the ellipse's rotation
+  auto rotPoint =
+      point.RotateAround(m_center.Translation(), -m_center.Rotation());
+
+  // Find nearest point
+  {
+    namespace slp = sleipnir;
+
+    sleipnir::OptimizationProblem problem;
+
+    // Point on ellipse
+    auto x = problem.DecisionVariable();
+    x.SetValue(rotPoint.X().value());
+    auto y = problem.DecisionVariable();
+    y.SetValue(rotPoint.Y().value());
+
+    problem.Minimize(slp::pow(x - rotPoint.X().value(), 2) +
+                     slp::pow(y - rotPoint.Y().value(), 2));
+
+    // (x − x_c)²/a² + (y − y_c)²/b² = 1
+    problem.SubjectTo(slp::pow(x - m_center.X().value(), 2) /
+                              (m_xSemiAxis.value() * m_xSemiAxis.value()) +
+                          slp::pow(y - m_center.Y().value(), 2) /
+                              (m_ySemiAxis.value() * m_ySemiAxis.value()) ==
+                      1);
+
+    problem.Solve();
+
+    rotPoint = frc::Translation2d{units::meter_t{x.Value()},
+                                  units::meter_t{y.Value()}};
+  }
+
+  // Undo rotation
+  return rotPoint.RotateAround(m_center.Translation(), m_center.Rotation());
+}

wpimath/src/main/native/cpp/geometry/Rectangle2d.cpp

@@ -0,0 +1,7 @@
+// 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 "frc/geometry/Rectangle2d.h"
+
+#include "geometry2d.pb.h"

wpimath/src/main/native/cpp/geometry/Translation2d.cpp

@@ -14,19 +14,10 @@ using namespace frc;
 Translation2d::Translation2d(const Eigen::Vector2d& vector)
     : m_x{units::meter_t{vector.x()}}, m_y{units::meter_t{vector.y()}} {}
 
-units::meter_t Translation2d::Distance(const Translation2d& other) const {
-  return units::math::hypot(other.m_x - m_x, other.m_y - m_y);
-}
-
 units::meter_t Translation2d::Norm() const {
   return units::math::hypot(m_x, m_y);
 }
 
-bool Translation2d::operator==(const Translation2d& other) const {
-  return units::math::abs(m_x - other.m_x) < 1E-9_m &&
-         units::math::abs(m_y - other.m_y) < 1E-9_m;
-}
-
 Translation2d Translation2d::Nearest(
     std::span<const Translation2d> translations) const {
   return *std::min_element(translations.begin(), translations.end(),

wpimath/src/main/native/cpp/geometry/proto/Ellipse2dProto.cpp

@@ -0,0 +1,31 @@
+// 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 "frc/geometry/proto/Ellipse2dProto.h"
+
+#include "geometry2d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Ellipse2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufEllipse2d>(
+      arena);
+}
+
+frc::Ellipse2d wpi::Protobuf<frc::Ellipse2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufEllipse2d*>(&msg);
+  return frc::Ellipse2d{
+      wpi::UnpackProtobuf<frc::Pose2d>(m->center()),
+      units::meter_t{m->xsemiaxis()},
+      units::meter_t{m->ysemiaxis()},
+  };
+}
+
+void wpi::Protobuf<frc::Ellipse2d>::Pack(google::protobuf::Message* msg,
+                                         const frc::Ellipse2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufEllipse2d*>(msg);
+  wpi::PackProtobuf(m->mutable_center(), value.Center());
+  m->set_xsemiaxis(value.XSemiAxis().value());
+  m->set_ysemiaxis(value.YSemiAxis().value());
+}

wpimath/src/main/native/cpp/geometry/proto/Rectangle2dProto.cpp

@@ -0,0 +1,31 @@
+// 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 "frc/geometry/proto/Rectangle2dProto.h"
+
+#include "geometry2d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Rectangle2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufRectangle2d>(arena);
+}
+
+frc::Rectangle2d wpi::Protobuf<frc::Rectangle2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufRectangle2d*>(&msg);
+  return frc::Rectangle2d{
+      wpi::UnpackProtobuf<frc::Pose2d>(m->center()),
+      units::meter_t{m->xwidth()},
+      units::meter_t{m->ywidth()},
+  };
+}
+
+void wpi::Protobuf<frc::Rectangle2d>::Pack(google::protobuf::Message* msg,
+                                           const frc::Rectangle2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufRectangle2d*>(msg);
+  wpi::PackProtobuf(m->mutable_center(), value.Center());
+  m->set_xwidth(value.XWidth().value());
+  m->set_ywidth(value.YWidth().value());
+}

wpimath/src/main/native/cpp/geometry/struct/Ellipse2dStruct.cpp

@@ -0,0 +1,27 @@
+// 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 "frc/geometry/struct/Ellipse2dStruct.h"
+
+namespace {
+constexpr size_t kCenterOff = 0;
+constexpr size_t kXSemiAxisOff = kCenterOff + wpi::GetStructSize<frc::Pose2d>();
+constexpr size_t kYSemiAxisOff = kXSemiAxisOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::Ellipse2d>;
+
+frc::Ellipse2d StructType::Unpack(std::span<const uint8_t> data) {
+  return frc::Ellipse2d{
+      wpi::UnpackStruct<frc::Pose2d, kCenterOff>(data),
+      units::meter_t{wpi::UnpackStruct<double, kXSemiAxisOff>(data)},
+      units::meter_t{wpi::UnpackStruct<double, kYSemiAxisOff>(data)},
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data, const frc::Ellipse2d& value) {
+  wpi::PackStruct<kCenterOff>(data, value.Center());
+  wpi::PackStruct<kXSemiAxisOff>(data, value.XSemiAxis().value());
+  wpi::PackStruct<kYSemiAxisOff>(data, value.YSemiAxis().value());
+}

wpimath/src/main/native/cpp/geometry/struct/Rectangle2dStruct.cpp

@@ -0,0 +1,27 @@
+// 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 "frc/geometry/struct/Rectangle2dStruct.h"
+
+namespace {
+constexpr size_t kCenterOff = 0;
+constexpr size_t kXWidthOff = kCenterOff + wpi::GetStructSize<frc::Pose2d>();
+constexpr size_t kYWidthOff = kXWidthOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::Rectangle2d>;
+
+frc::Rectangle2d StructType::Unpack(std::span<const uint8_t> data) {
+  return frc::Rectangle2d{
+      wpi::UnpackStruct<frc::Pose2d, kCenterOff>(data),
+      units::meter_t{wpi::UnpackStruct<double, kXWidthOff>(data)},
+      units::meter_t{wpi::UnpackStruct<double, kYWidthOff>(data)},
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data, const frc::Rectangle2d& value) {
+  wpi::PackStruct<kCenterOff>(data, value.Center());
+  wpi::PackStruct<kXWidthOff>(data, value.XWidth().value());
+  wpi::PackStruct<kYWidthOff>(data, value.YWidth().value());
+}

wpimath/src/main/native/cpp/jni/WPIMathJNI_Ellipse2d.cpp

@@ -0,0 +1,39 @@
+// 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 <jni.h>
+
+#include <wpi/array.h>
+#include <wpi/jni_util.h>
+
+#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "frc/geometry/Ellipse2d.h"
+
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    ellipse2dFindNearestPoint
+ * Signature: (DDDDDDD[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_ellipse2dFindNearestPoint
+  (JNIEnv* env, jclass, jdouble centerX, jdouble centerY, jdouble centerHeading,
+   jdouble xSemiAxis, jdouble ySemiAxis, jdouble pointX, jdouble pointY,
+   jdoubleArray nearestPoint)
+{
+  auto point =
+      frc::Ellipse2d{
+          frc::Pose2d{units::meter_t{centerX}, units::meter_t{centerY},
+                      units::radian_t{centerHeading}},
+          units::meter_t{xSemiAxis}, units::meter_t{ySemiAxis}}
+          .FindNearestPoint({units::meter_t{pointX}, units::meter_t{pointY}});
+
+  wpi::array buf{point.X().value(), point.Y().value()};
+  env->SetDoubleArrayRegion(nearestPoint, 0, 2, buf.data());
+}
+
+}  // extern "C"

wpimath/src/main/native/include/frc/geometry/Ellipse2d.h

@@ -0,0 +1,212 @@
+// 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.
+
+#pragma once
+
+#include <stdexcept>
+
+#include <gcem.hpp>
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Transform2d.h"
+#include "frc/geometry/Translation2d.h"
+#include "units/length.h"
+#include "units/math.h"
+
+namespace frc {
+
+/**
+ * Represents a 2d ellipse space containing translational, rotational, and
+ * scaling components.
+ */
+class WPILIB_DLLEXPORT Ellipse2d {
+ public:
+  /**
+   * Constructs an ellipse around a center point and two semi-axes, a horizontal
+   * and vertical one.
+   *
+   * @param center The center of the ellipse.
+   * @param xSemiAxis The x semi-axis.
+   * @param ySemiAxis The y semi-axis.
+   */
+  constexpr Ellipse2d(const Pose2d& center, units::meter_t xSemiAxis,
+                      units::meter_t ySemiAxis)
+      : m_center{center}, m_xSemiAxis{xSemiAxis}, m_ySemiAxis{ySemiAxis} {
+    if (xSemiAxis <= 0_m || ySemiAxis <= 0_m) {
+      throw std::invalid_argument("Ellipse2d semi-axes must be positive");
+    }
+  }
+
+  /**
+   * Constructs a perfectly circular ellipse with the specified radius.
+   *
+   * @param center The center of the circle.
+   * @param radius The radius of the circle.
+   */
+  constexpr Ellipse2d(const Translation2d& center, double radius)
+      : m_center{center, Rotation2d{}},
+        m_xSemiAxis{radius},
+        m_ySemiAxis{radius} {}
+
+  /**
+   * Returns the center of the ellipse.
+   *
+   * @return The center of the ellipse.
+   */
+  constexpr const Pose2d& Center() const { return m_center; }
+
+  /**
+   * Returns the rotational component of the ellipse.
+   *
+   * @return The rotational component of the ellipse.
+   */
+  constexpr const Rotation2d& Rotation() const { return m_center.Rotation(); }
+
+  /**
+   * Returns the x semi-axis.
+   *
+   * @return The x semi-axis.
+   */
+  constexpr units::meter_t XSemiAxis() const { return m_xSemiAxis; }
+
+  /**
+   * Returns the y semi-axis.
+   *
+   * @return The y semi-axis.
+   */
+  constexpr units::meter_t YSemiAxis() const { return m_ySemiAxis; }
+
+  /**
+   * Returns the focal points of the ellipse. In a perfect circle, this will
+   * always return the center.
+   *
+   * @return The focal points.
+   */
+  constexpr wpi::array<Translation2d, 2> FocalPoints() const {
+    // Major semi-axis
+    auto a = units::math::max(m_xSemiAxis, m_ySemiAxis);
+
+    // Minor semi-axis
+    auto b = units::math::min(m_xSemiAxis, m_ySemiAxis);  // NOLINT
+
+    auto c = units::math::sqrt(a * a - b * b);
+
+    if (m_xSemiAxis > m_ySemiAxis) {
+      return wpi::array{
+          (m_center + Transform2d{-c, 0_m, Rotation2d{}}).Translation(),
+          (m_center + Transform2d{c, 0_m, Rotation2d{}}).Translation()};
+    } else {
+      return wpi::array{
+          (m_center + Transform2d{0_m, -c, Rotation2d{}}).Translation(),
+          (m_center + Transform2d{0_m, c, Rotation2d{}}).Translation()};
+    }
+  }
+
+  /**
+   * Transforms the center of the ellipse and returns the new ellipse.
+   *
+   * @param other The transform to transform by.
+   * @return The transformed ellipse.
+   */
+  constexpr Ellipse2d TransformBy(const Transform2d& other) const {
+    return Ellipse2d{m_center.TransformBy(other), m_xSemiAxis, m_ySemiAxis};
+  }
+
+  /**
+   * Rotates the center of the ellipse and returns the new ellipse.
+   *
+   * @param other The rotation to transform by.
+   * @return The rotated ellipse.
+   */
+  constexpr Ellipse2d RotateBy(const Rotation2d& other) const {
+    return Ellipse2d{m_center.RotateBy(other), m_xSemiAxis, m_ySemiAxis};
+  }
+
+  /**
+   * Checks if a point is intersected by this ellipse's circumference.
+   *
+   * @param point The point to check.
+   * @return True, if this ellipse's circumference intersects the point.
+   */
+  constexpr bool Intersects(const Translation2d& point) const {
+    return gcem::abs(1.0 - SolveEllipseEquation(point)) <= 1E-9;
+  }
+
+  /**
+   * Checks if a point is contained within this ellipse. This is inclusive, if
+   * the point lies on the circumference this will return {@code true}.
+   *
+   * @param point The point to check.
+   * @return True, if the point is within or on the ellipse.
+   */
+  constexpr bool Contains(const Translation2d& point) const {
+    return SolveEllipseEquation(point) <= 1.0;
+  }
+
+  /**
+   * Returns the distance between the perimeter of the ellipse and the point.
+   *
+   * @param point The point to check.
+   * @return The distance (0, if the point is contained by the ellipse)
+   */
+  units::meter_t Distance(const Translation2d& point) const;
+
+  /**
+   * Returns the nearest point that is contained within the ellipse.
+   *
+   * @param point The point that this will find the nearest point to.
+   * @return A new point that is nearest to {@code point} and contained in the
+   *   ellipse.
+   */
+  Translation2d FindNearestPoint(const Translation2d& point) const;
+
+  /**
+   * Checks equality between this Ellipse2d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  constexpr bool operator==(const Ellipse2d& other) const {
+    return m_center == other.m_center &&
+           units::math::abs(m_xSemiAxis - other.m_xSemiAxis) < 1E-9_m &&
+           units::math::abs(m_ySemiAxis - other.m_ySemiAxis) < 1E-9_m;
+  }
+
+ private:
+  Pose2d m_center;
+  units::meter_t m_xSemiAxis;
+  units::meter_t m_ySemiAxis;
+
+  /**
+   * Solves the equation of an ellipse from the given point. This is a helper
+   * function used to determine if that point lies inside of or on an ellipse.
+   *
+   * <pre>
+   * (x - h)²/a² + (y - k)²/b² = 1
+   * </pre>
+   *
+   * @param point The point to solve for.
+   * @return < 1.0 if the point lies inside the ellipse, == 1.0 if a point lies
+   * on the ellipse, and > 1.0 if the point lies outsides the ellipse.
+   */
+  constexpr double SolveEllipseEquation(const Translation2d& point) const {
+    // Rotate the point by the inverse of the ellipse's rotation
+    auto rotPoint =
+        point.RotateAround(m_center.Translation(), -m_center.Rotation());
+
+    auto x = rotPoint.X() - m_center.X();
+    auto y = rotPoint.Y() - m_center.Y();
+
+    return (x * x) / (m_xSemiAxis * m_xSemiAxis) +
+           (y * y) / (m_ySemiAxis * m_ySemiAxis);
+  }
+};
+
+}  // namespace frc
+
+#include "frc/geometry/proto/Ellipse2dProto.h"
+#include "frc/geometry/struct/Ellipse2dStruct.h"

wpimath/src/main/native/include/frc/geometry/Rectangle2d.h

@@ -0,0 +1,210 @@
+// 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.
+
+#pragma once
+
+#include <algorithm>
+#include <stdexcept>
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Transform2d.h"
+#include "frc/geometry/Translation2d.h"
+#include "units/length.h"
+#include "units/math.h"
+
+namespace frc {
+
+/**
+ * Represents a 2d rectangular space containing translational, rotational, and
+ * scaling components.
+ */
+class WPILIB_DLLEXPORT Rectangle2d {
+ public:
+  /**
+   * Constructs a rectangle at the specified position with the specified width
+   * and height.
+   *
+   * @param center The position (translation and rotation) of the rectangle.
+   * @param xWidth The x size component of the rectangle, in unrotated
+   * coordinate frame.
+   * @param yWidth The y size component of the rectangle, in unrotated
+   * coordinate frame.
+   */
+  constexpr Rectangle2d(const Pose2d& center, units::meter_t xWidth,
+                        units::meter_t yWidth)
+      : m_center{center}, m_xWidth{xWidth}, m_yWidth{yWidth} {
+    if (xWidth < 0_m || yWidth < 0_m) {
+      throw std::invalid_argument(
+          "Rectangle2d dimensions cannot be less than 0!");
+    }
+  }
+
+  /**
+   * Creates an unrotated rectangle from the given corners. The corners should
+   * be diagonally opposite of each other.
+   *
+   * @param cornerA The first corner of the rectangle.
+   * @param cornerB The second corner of the rectangle.
+   */
+  constexpr Rectangle2d(const Translation2d& cornerA,
+                        const Translation2d& cornerB)
+      : m_center{(cornerA + cornerB) / 2.0, Rotation2d{}},
+        m_xWidth{units::math::abs(cornerA.X() - cornerB.X())},
+        m_yWidth{units::math::abs(cornerA.Y() - cornerB.Y())} {}
+
+  /**
+   * Returns the center of the rectangle.
+   *
+   * @return The center of the rectangle.
+   */
+  constexpr const Pose2d& Center() const { return m_center; }
+
+  /**
+   * Returns the rotational component of the rectangle.
+   *
+   * @return The rotational component of the rectangle.
+   */
+  constexpr const Rotation2d& Rotation() const { return m_center.Rotation(); }
+
+  /**
+   * Returns the x size component of the rectangle.
+   *
+   * @return The x size component of the rectangle.
+   */
+  constexpr units::meter_t XWidth() const { return m_xWidth; }
+
+  /**
+   * Returns the y size component of the rectangle.
+   *
+   * @return The y size component of the rectangle.
+   */
+  constexpr units::meter_t YWidth() const { return m_yWidth; }
+
+  /**
+   * Transforms the center of the rectangle and returns the new rectangle.
+   *
+   * @param other The transform to transform by.
+   * @return The transformed rectangle
+   */
+  constexpr Rectangle2d TransformBy(const Transform2d& other) const {
+    return Rectangle2d{m_center.TransformBy(other), m_xWidth, m_yWidth};
+  }
+
+  /**
+   * Rotates the center of the rectangle and returns the new rectangle.
+   *
+   * @param other The rotation to transform by.
+   * @return The rotated rectangle.
+   */
+  constexpr Rectangle2d RotateBy(const Rotation2d& other) const {
+    return Rectangle2d{m_center.RotateBy(other), m_xWidth, m_yWidth};
+  }
+
+  /**
+   * Checks if a point is intersected by the rectangle's perimeter.
+   *
+   * @param point The point to check.
+   * @return True, if the rectangle's perimeter intersects the point.
+   */
+  constexpr bool Intersects(const Translation2d& point) const {
+    // Move the point into the rectangle's coordinate frame
+    auto pointInRect = point - m_center.Translation();
+    pointInRect = pointInRect.RotateBy(-m_center.Rotation());
+
+    if (units::math::abs(units::math::abs(pointInRect.X()) - m_xWidth / 2.0) <=
+        1E-9_m) {
+      // Point rests on left/right perimeter
+      return units::math::abs(pointInRect.Y()) <= m_yWidth / 2.0;
+    } else if (units::math::abs(units::math::abs(pointInRect.Y()) -
+                                m_yWidth / 2.0) <= 1E-9_m) {
+      // Point rests on top/bottom perimeter
+      return units::math::abs(pointInRect.X()) <= m_xWidth / 2.0;
+    }
+
+    return false;
+  }
+
+  /**
+   * Checks if a point is contained within the rectangle. This is inclusive, if
+   * the point lies on the perimeter it will return true.
+   *
+   * @param point The point to check.
+   * @return True, if the rectangle contains the point or the perimeter
+   * intersects the point.
+   */
+  constexpr bool Contains(const Translation2d& point) const {
+    // Rotate the point into the rectangle's coordinate frame
+    auto rotPoint =
+        point.RotateAround(m_center.Translation(), -m_center.Rotation());
+
+    // Check if within bounding box
+    return rotPoint.X() >= (m_center.X() - m_xWidth / 2.0) &&
+           rotPoint.X() <= (m_center.X() + m_xWidth / 2.0) &&
+           rotPoint.Y() >= (m_center.Y() - m_yWidth / 2.0) &&
+           rotPoint.Y() <= (m_center.Y() + m_yWidth / 2.0);
+  }
+
+  /**
+   * Returns the distance between the perimeter of the rectangle and the point.
+   *
+   * @param point The point to check.
+   * @return The distance (0, if the point is contained by the rectangle)
+   */
+  constexpr units::meter_t Distance(const Translation2d& point) const {
+    return FindNearestPoint(point).Distance(point);
+  }
+
+  /**
+   * Returns the nearest point that is contained within the rectangle.
+   *
+   * @param point The point that this will find the nearest point to.
+   * @return A new point that is nearest to {@code point} and contained in the
+   * rectangle.
+   */
+  constexpr Translation2d FindNearestPoint(const Translation2d& point) const {
+    // Check if already in rectangle
+    if (Contains(point)) {
+      return point;
+    }
+
+    // Rotate the point by the inverse of the rectangle's rotation
+    auto rotPoint =
+        point.RotateAround(m_center.Translation(), -m_center.Rotation());
+
+    // Find nearest point
+    rotPoint =
+        Translation2d{std::clamp(rotPoint.X(), m_center.X() - m_xWidth / 2.0,
+                                 m_center.X() + m_xWidth / 2.0),
+                      std::clamp(rotPoint.Y(), m_center.Y() - m_yWidth / 2.0,
+                                 m_center.Y() + m_yWidth / 2.0)};
+
+    // Undo rotation
+    return rotPoint.RotateAround(m_center.Translation(), m_center.Rotation());
+  }
+
+  /**
+   * Checks equality between this Rectangle2d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  constexpr bool operator==(const Rectangle2d& other) const {
+    return m_center == other.m_center &&
+           units::math::abs(m_xWidth - other.m_xWidth) < 1E-9_m &&
+           units::math::abs(m_yWidth - other.m_yWidth) < 1E-9_m;
+  }
+
+ private:
+  Pose2d m_center;
+  units::meter_t m_xWidth;
+  units::meter_t m_yWidth;
+};
+
+}  // namespace frc
+
+#include "frc/geometry/proto/Rectangle2dProto.h"
+#include "frc/geometry/struct/Rectangle2dStruct.h"

wpimath/src/main/native/include/frc/geometry/Translation2d.h

@@ -13,6 +13,7 @@
 
 #include "frc/geometry/Rotation2d.h"
 #include "units/length.h"
+#include "units/math.h"
 
 namespace frc {
 
@@ -66,7 +67,9 @@ class WPILIB_DLLEXPORT Translation2d {
    *
    * @return The distance between the two translations.
    */
-  units::meter_t Distance(const Translation2d& other) const;
+  constexpr units::meter_t Distance(const Translation2d& other) const {
+    return units::math::hypot(other.m_x - m_x, other.m_y - m_y);
+  }
 
   /**
    * Returns the X component of the translation.
@@ -123,6 +126,21 @@ class WPILIB_DLLEXPORT Translation2d {
    */
   constexpr Translation2d RotateBy(const Rotation2d& other) const;
 
+  /**
+   * Rotates this translation around another translation in 2D space.
+   *
+   * <pre>
+   * [x_new]   [rot.cos, -rot.sin][x - other.x]   [other.x]
+   * [y_new] = [rot.sin,  rot.cos][y - other.y] + [other.y]
+   * </pre>
+   *
+   * @param other The other translation to rotate around.
+   * @param rot The rotation to rotate the translation by.
+   * @return The new rotated translation.
+   */
+  constexpr Translation2d RotateAround(const Translation2d& other,
+                                       const Rotation2d& rot) const;
+
   /**
    * Returns the sum of two translations in 2D space.
    *
@@ -184,7 +202,7 @@ class WPILIB_DLLEXPORT Translation2d {
    * @param other The other object.
    * @return Whether the two objects are equal.
    */
-  bool operator==(const Translation2d& other) const;
+  constexpr bool operator==(const Translation2d& other) const;
 
   /**
    * Returns the nearest Translation2d from a collection of translations

wpimath/src/main/native/include/frc/geometry/Translation2d.inc

@@ -6,6 +6,7 @@
 
 #include "frc/geometry/Translation2d.h"
 #include "units/length.h"
+#include "units/math.h"
 
 namespace frc {
 
@@ -29,6 +30,14 @@ constexpr Translation2d Translation2d::RotateBy(const Rotation2d& other) const {
           m_x * other.Sin() + m_y * other.Cos()};
 }
 
+constexpr Translation2d Translation2d::RotateAround(
+    const Translation2d& other, const Rotation2d& rot) const {
+  return {
+      (m_x - other.X()) * rot.Cos() - (m_y - other.Y()) * rot.Sin() + other.X(),
+      (m_x - other.X()) * rot.Sin() + (m_y - other.Y()) * rot.Cos() +
+          other.Y()};
+}
+
 constexpr Translation2d Translation2d::operator+(
     const Translation2d& other) const {
   return {X() + other.X(), Y() + other.Y()};
@@ -51,4 +60,9 @@ constexpr Translation2d Translation2d::operator/(double scalar) const {
   return operator*(1.0 / scalar);
 }
 
+constexpr bool Translation2d::operator==(const Translation2d& other) const {
+  return units::math::abs(m_x - other.m_x) < 1E-9_m &&
+         units::math::abs(m_y - other.m_y) < 1E-9_m;
+}
+
 }  // namespace frc

wpimath/src/main/native/include/frc/geometry/proto/Ellipse2dProto.h

@@ -0,0 +1,17 @@
+// 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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Ellipse2d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Ellipse2d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Ellipse2d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg, const frc::Ellipse2d& value);
+};

wpimath/src/main/native/include/frc/geometry/proto/Rectangle2dProto.h

@@ -0,0 +1,18 @@
+// 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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Rectangle2d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Rectangle2d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Rectangle2d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Rectangle2d& value);
+};

wpimath/src/main/native/include/frc/geometry/struct/Ellipse2dStruct.h

@@ -0,0 +1,33 @@
+// 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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Ellipse2d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Ellipse2d> {
+  static constexpr std::string_view GetTypeString() {
+    return "struct:Ellipse2d";
+  }
+  static constexpr size_t GetSize() {
+    return wpi::GetStructSize<frc::Pose2d>() + 16;
+  }
+  static constexpr std::string_view GetSchema() {
+    return "Pose2d center;double xSemiAxis;double ySemiAxis";
+  }
+
+  static frc::Ellipse2d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Ellipse2d& value);
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Pose2d>(fn);
+  }
+};
+
+static_assert(wpi::StructSerializable<frc::Ellipse2d>);
+static_assert(wpi::HasNestedStruct<frc::Ellipse2d>);

wpimath/src/main/native/include/frc/geometry/struct/Rectangle2dStruct.h

@@ -0,0 +1,33 @@
+// 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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Rectangle2d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Rectangle2d> {
+  static constexpr std::string_view GetTypeString() {
+    return "struct:Rectangle2d";
+  }
+  static constexpr size_t GetSize() {
+    return wpi::GetStructSize<frc::Pose2d>() + 16;
+  }
+  static constexpr std::string_view GetSchema() {
+    return "Pose2d center;double xWidth;double yWidth";
+  }
+
+  static frc::Rectangle2d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Rectangle2d& value);
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Pose2d>(fn);
+  }
+};
+
+static_assert(wpi::StructSerializable<frc::Rectangle2d>);
+static_assert(wpi::HasNestedStruct<frc::Rectangle2d>);

wpimath/src/main/proto/geometry2d.proto

@@ -28,3 +28,15 @@ message ProtobufTwist2d {
   double dy = 2;
   double dtheta = 3;
 }
+
+message ProtobufRectangle2d {
+  ProtobufPose2d center = 1;
+  double xWidth = 2;
+  double yWidth = 3;
+}
+
+message ProtobufEllipse2d {
+  ProtobufPose2d center = 1;
+  double xSemiAxis = 2;
+  double ySemiAxis = 3;
+}

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