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

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

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"