Merge branch 'main' into 2027

wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp

@@ -221,3 +221,111 @@ TEST(Pose3dTest, TwistNaN) {
     EXPECT_FALSE(std::isnan(twist.rz.value()));
   }
 }
+
+TEST(Pose3dTest, Nearest) {
+  const Pose3d origin{0_m, 0_m, 0_m, Rotation3d{}};
+
+  // Distance sort
+  // poses are in order of closest to farthest away from the origin at
+  // various positions in 3D space.
+  const Pose3d pose1{1_m, 0_m, 0_m, Rotation3d{}};
+  const Pose3d pose2{0_m, 2_m, 0_m, Rotation3d{}};
+  const Pose3d pose3{0_m, 0_m, 3_m, Rotation3d{}};
+  const Pose3d pose4{2_m, 2_m, 2_m, Rotation3d{}};
+  const Pose3d pose5{3_m, 3_m, 3_m, Rotation3d{}};
+
+  EXPECT_DOUBLE_EQ(pose3.X().value(),
+                   origin.Nearest({pose5, pose3, pose4}).X().value());
+  EXPECT_DOUBLE_EQ(pose3.Y().value(),
+                   origin.Nearest({pose5, pose3, pose4}).Y().value());
+  EXPECT_DOUBLE_EQ(pose3.Z().value(),
+                   origin.Nearest({pose5, pose3, pose4}).Z().value());
+
+  EXPECT_DOUBLE_EQ(pose1.X().value(),
+                   origin.Nearest({pose1, pose2, pose3}).X().value());
+  EXPECT_DOUBLE_EQ(pose1.Y().value(),
+                   origin.Nearest({pose1, pose2, pose3}).Y().value());
+  EXPECT_DOUBLE_EQ(pose1.Z().value(),
+                   origin.Nearest({pose1, pose2, pose3}).Z().value());
+
+  EXPECT_DOUBLE_EQ(pose2.X().value(),
+                   origin.Nearest({pose4, pose2, pose3}).X().value());
+  EXPECT_DOUBLE_EQ(pose2.Y().value(),
+                   origin.Nearest({pose4, pose2, pose3}).Y().value());
+  EXPECT_DOUBLE_EQ(pose2.Z().value(),
+                   origin.Nearest({pose4, pose2, pose3}).Z().value());
+
+  // Rotation component sort (when distance is the same)
+  // Use the same translation to avoid distance differences
+  const Translation3d translation{1_m, 0_m, 0_m};
+
+  const Pose3d poseA{translation, Rotation3d{}};  // No rotation
+  const Pose3d poseB{translation, Rotation3d{30_deg, 0_deg, 0_deg}};
+  const Pose3d poseC{translation, Rotation3d{0_deg, 45_deg, 0_deg}};
+  const Pose3d poseD{translation, Rotation3d{0_deg, 0_deg, 90_deg}};
+  const Pose3d poseE{translation, Rotation3d{180_deg, 0_deg, 0_deg}};
+
+  auto result1 =
+      Pose3d{0_m, 0_m, 0_m, Rotation3d{}}.Nearest({poseA, poseB, poseD});
+  EXPECT_DOUBLE_EQ(poseA.Rotation().X().value(),
+                   result1.Rotation().X().value());
+  EXPECT_DOUBLE_EQ(poseA.Rotation().Y().value(),
+                   result1.Rotation().Y().value());
+  EXPECT_DOUBLE_EQ(poseA.Rotation().Z().value(),
+                   result1.Rotation().Z().value());
+
+  auto result2 =
+      Pose3d{0_m, 0_m, 0_m, Rotation3d{25_deg, 0_deg, 0_deg}}.Nearest(
+          {poseB, poseC, poseD});
+  EXPECT_DOUBLE_EQ(poseB.Rotation().X().value(),
+                   result2.Rotation().X().value());
+  EXPECT_DOUBLE_EQ(poseB.Rotation().Y().value(),
+                   result2.Rotation().Y().value());
+  EXPECT_DOUBLE_EQ(poseB.Rotation().Z().value(),
+                   result2.Rotation().Z().value());
+
+  auto result3 =
+      Pose3d{0_m, 0_m, 0_m, Rotation3d{0_deg, 50_deg, 0_deg}}.Nearest(
+          {poseB, poseC, poseD});
+  EXPECT_DOUBLE_EQ(poseC.Rotation().X().value(),
+                   result3.Rotation().X().value());
+  EXPECT_DOUBLE_EQ(poseC.Rotation().Y().value(),
+                   result3.Rotation().Y().value());
+  EXPECT_DOUBLE_EQ(poseC.Rotation().Z().value(),
+                   result3.Rotation().Z().value());
+
+  auto result4 =
+      Pose3d{0_m, 0_m, 0_m, Rotation3d{0_deg, 0_deg, 85_deg}}.Nearest(
+          {poseA, poseC, poseD});
+  EXPECT_DOUBLE_EQ(poseD.Rotation().X().value(),
+                   result4.Rotation().X().value());
+  EXPECT_DOUBLE_EQ(poseD.Rotation().Y().value(),
+                   result4.Rotation().Y().value());
+  EXPECT_DOUBLE_EQ(poseD.Rotation().Z().value(),
+                   result4.Rotation().Z().value());
+
+  auto result5 =
+      Pose3d{0_m, 0_m, 0_m, Rotation3d{170_deg, 0_deg, 0_deg}}.Nearest(
+          {poseA, poseD, poseE});
+  EXPECT_DOUBLE_EQ(poseE.Rotation().X().value(),
+                   result5.Rotation().X().value());
+  EXPECT_DOUBLE_EQ(poseE.Rotation().Y().value(),
+                   result5.Rotation().Y().value());
+  EXPECT_DOUBLE_EQ(poseE.Rotation().Z().value(),
+                   result5.Rotation().Z().value());
+
+  // Test with complex 3D rotations (combining roll, pitch, yaw)
+  const Pose3d complexPose1{translation, Rotation3d{45_deg, 30_deg, 60_deg}};
+  const Pose3d complexPose2{translation, Rotation3d{90_deg, 45_deg, 90_deg}};
+  const Pose3d complexPose3{translation, Rotation3d{10_deg, 15_deg, 20_deg}};
+
+  auto complexResult =
+      Pose3d{0_m, 0_m, 0_m, Rotation3d{5_deg, 10_deg, 15_deg}}.Nearest(
+          {complexPose1, complexPose2, complexPose3});
+  EXPECT_DOUBLE_EQ(complexPose3.Rotation().X().value(),
+                   complexResult.Rotation().X().value());
+  EXPECT_DOUBLE_EQ(complexPose3.Rotation().Y().value(),
+                   complexResult.Rotation().Y().value());
+  EXPECT_DOUBLE_EQ(complexPose3.Rotation().Z().value(),
+                   complexResult.Rotation().Z().value());
+}

wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp

@@ -186,3 +186,31 @@ TEST(Translation3dTest, Constexpr) {
   static_assert(projected.X() == 1_m);
   static_assert(projected.Y() == 2_m);
 }
+
+TEST(Translation3dTest, Nearest) {
+  const Translation3d origin{0_m, 0_m, 0_m};
+
+  // Distance sort
+  // translations are in order of closest to farthest away from the origin at
+  // various positions in 3D space.
+  const Translation3d translation1{1_m, 0_m, 0_m};
+  const Translation3d translation2{0_m, 2_m, 0_m};
+  const Translation3d translation3{0_m, 0_m, 3_m};
+  const Translation3d translation4{2_m, 2_m, 2_m};
+  const Translation3d translation5{3_m, 3_m, 3_m};
+
+  auto nearest1 = origin.Nearest({translation5, translation3, translation4});
+  EXPECT_DOUBLE_EQ(nearest1.X().value(), translation3.X().value());
+  EXPECT_DOUBLE_EQ(nearest1.Y().value(), translation3.Y().value());
+  EXPECT_DOUBLE_EQ(nearest1.Z().value(), translation3.Z().value());
+
+  auto nearest2 = origin.Nearest({translation1, translation2, translation3});
+  EXPECT_DOUBLE_EQ(nearest2.X().value(), translation1.X().value());
+  EXPECT_DOUBLE_EQ(nearest2.Y().value(), translation1.Y().value());
+  EXPECT_DOUBLE_EQ(nearest2.Z().value(), translation1.Z().value());
+
+  auto nearest3 = origin.Nearest({translation4, translation2, translation3});
+  EXPECT_DOUBLE_EQ(nearest3.X().value(), translation2.X().value());
+  EXPECT_DOUBLE_EQ(nearest3.Y().value(), translation2.Y().value());
+  EXPECT_DOUBLE_EQ(nearest3.Z().value(), translation2.Z().value());
+}