[wpimath] Add nearest() method to Pose3d (mirroring Pose2d) (#8010)

wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java

@@ -21,6 +21,9 @@ import edu.wpi.first.math.numbers.N4;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.List;
 import java.util.Objects;
 
 /** Represents a 3D pose containing translational and rotational elements. */
@@ -396,6 +399,22 @@ public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, Str
     return new Pose2d(m_translation.toTranslation2d(), m_rotation.toRotation2d());
   }
 
+  /**
+   * Returns the nearest Pose3d from a list of poses. If two or more poses in the list have the same
+   * distance from this pose, return the one with the closest rotation component.
+   *
+   * @param poses The list of poses to find the nearest.
+   * @return The nearest Pose3d from the list.
+   */
+  public Pose3d nearest(List<Pose3d> poses) {
+    return Collections.min(
+        poses,
+        Comparator.comparing(
+                (Pose3d other) -> this.getTranslation().getDistance(other.getTranslation()))
+            .thenComparing(
+                (Pose3d other) -> this.getRotation().minus(other.getRotation()).getAngle()));
+  }
+
   @Override
   public String toString() {
     return String.format("Pose3d(%s, %s)", m_translation, m_rotation);

wpimath/src/main/native/include/frc/geometry/Pose2d.h

@@ -242,7 +242,11 @@ class WPILIB_DLLEXPORT Pose2d {
   }
 
   /**
-   * Returns the nearest Pose2d from a collection of poses
+   * Returns the nearest Pose2d from a collection of poses.
+   *
+   * If two or more poses in the collection have the same distance from this
+   * pose, return the one with the closest rotation component.
+   *
    * @param poses The collection of poses.
    * @return The nearest Pose2d from the collection.
    */
@@ -264,7 +268,11 @@ class WPILIB_DLLEXPORT Pose2d {
   }
 
   /**
-   * Returns the nearest Pose2d from a collection of poses
+   * Returns the nearest Pose2d from a collection of poses.
+   *
+   * If two or more poses in the collection have the same distance from this
+   * pose, return the one with the closest rotation component.
+   *
    * @param poses The collection of poses.
    * @return The nearest Pose2d from the collection.
    */

wpimath/src/main/native/include/frc/geometry/Pose3d.h

@@ -270,6 +270,56 @@ class WPILIB_DLLEXPORT Pose3d {
     return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()};
   }
 
+  /**
+   * Returns the nearest Pose3d from a collection of poses.
+   *
+   * If two or more poses in the collection have the same distance from this
+   * pose, return the one with the closest rotation component.
+   *
+   * @param poses The collection of poses.
+   * @return The nearest Pose3d from the collection.
+   */
+  constexpr Pose3d Nearest(std::span<const Pose3d> poses) const {
+    return *std::min_element(
+        poses.begin(), poses.end(), [this](const Pose3d& a, const Pose3d& b) {
+          auto aDistance = this->Translation().Distance(a.Translation());
+          auto bDistance = this->Translation().Distance(b.Translation());
+
+          // If the distances are equal sort by difference in rotation
+          if (aDistance == bDistance) {
+            return gcem::abs(
+                       (this->Rotation() - a.Rotation()).Angle().value()) <
+                   gcem::abs((this->Rotation() - b.Rotation()).Angle().value());
+          }
+          return aDistance < bDistance;
+        });
+  }
+
+  /**
+   * Returns the nearest Pose3d from a collection of poses.
+   *
+   * If two or more poses in the collection have the same distance from this
+   * pose, return the one with the closest rotation component.
+   *
+   * @param poses The collection of poses.
+   * @return The nearest Pose3d from the collection.
+   */
+  constexpr Pose3d Nearest(std::initializer_list<Pose3d> poses) const {
+    return *std::min_element(
+        poses.begin(), poses.end(), [this](const Pose3d& a, const Pose3d& b) {
+          auto aDistance = this->Translation().Distance(a.Translation());
+          auto bDistance = this->Translation().Distance(b.Translation());
+
+          // If the distances are equal sort by difference in rotation
+          if (aDistance == bDistance) {
+            return gcem::abs(
+                       (this->Rotation() - a.Rotation()).Angle().value()) <
+                   gcem::abs((this->Rotation() - b.Rotation()).Angle().value());
+          }
+          return aDistance < bDistance;
+        });
+  }
+
  private:
   Translation3d m_translation;
   Rotation3d m_rotation;