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

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

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