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

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

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>);