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

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

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