Merge branch 'main' into 2027

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/java/edu/wpi/first/math/geometry/Translation3d.java

@@ -20,6 +20,9 @@ import edu.wpi.first.math.numbers.N3;
 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;
 
 /**
@@ -296,6 +299,16 @@ public class Translation3d
     return new Translation3d(m_x / scalar, m_y / scalar, m_z / scalar);
   }
 
+  /**
+   * Returns the nearest Translation3d from a collection of translations.
+   *
+   * @param translations The collection of translations to find the nearest.
+   * @return The nearest Translation3d from the collection.
+   */
+  public Translation3d nearest(List<Translation3d> translations) {
+    return Collections.min(translations, Comparator.comparing(this::getDistance));
+  }
+
   @Override
   public String toString() {
     return String.format("Translation3d(X: %.2f, Y: %.2f, Z: %.2f)", m_x, m_y, m_z);

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

@@ -4,6 +4,9 @@
 
 #pragma once
 
+#include <algorithm>
+#include <initializer_list>
+#include <span>
 #include <stdexcept>
 #include <type_traits>
 #include <utility>
@@ -270,6 +273,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;

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

@@ -238,10 +238,11 @@ class WPILIB_DLLEXPORT Translation2d {
    */
   constexpr Translation2d Nearest(
       std::span<const Translation2d> translations) const {
-    return *std::min_element(translations.begin(), translations.end(),
-                             [this](Translation2d a, Translation2d b) {
-                               return this->Distance(a) < this->Distance(b);
-                             });
+    return *std::min_element(
+        translations.begin(), translations.end(),
+        [this](const Translation2d& a, const Translation2d& b) {
+          return this->Distance(a) < this->Distance(b);
+        });
   }
 
   /**
@@ -251,10 +252,11 @@ class WPILIB_DLLEXPORT Translation2d {
    */
   constexpr Translation2d Nearest(
       std::initializer_list<Translation2d> translations) const {
-    return *std::min_element(translations.begin(), translations.end(),
-                             [this](Translation2d a, Translation2d b) {
-                               return this->Distance(a) < this->Distance(b);
-                             });
+    return *std::min_element(
+        translations.begin(), translations.end(),
+        [this](const Translation2d& a, const Translation2d& b) {
+          return this->Distance(a) < this->Distance(b);
+        });
   }
 
  private:

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

@@ -4,6 +4,10 @@
 
 #pragma once
 
+#include <algorithm>
+#include <initializer_list>
+#include <span>
+
 #include <Eigen/Core>
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
@@ -244,6 +248,34 @@ class WPILIB_DLLEXPORT Translation3d {
            units::math::abs(m_z - other.m_z) < 1E-9_m;
   }
 
+  /**
+   * Returns the nearest Translation3d from a collection of translations
+   * @param translations The collection of translations.
+   * @return The nearest Translation3d from the collection.
+   */
+  constexpr Translation3d Nearest(
+      std::span<const Translation3d> translations) const {
+    return *std::min_element(
+        translations.begin(), translations.end(),
+        [this](const Translation3d& a, const Translation3d& b) {
+          return this->Distance(a) < this->Distance(b);
+        });
+  }
+
+  /**
+   * Returns the nearest Translation3d from a collection of translations
+   * @param translations The collection of translations.
+   * @return The nearest Translation3d from the collection.
+   */
+  constexpr Translation3d Nearest(
+      std::initializer_list<Translation3d> translations) const {
+    return *std::min_element(
+        translations.begin(), translations.end(),
+        [this](const Translation3d& a, const Translation3d& b) {
+          return this->Distance(a) < this->Distance(b);
+        });
+  }
+
  private:
   units::meter_t m_x = 0_m;
   units::meter_t m_y = 0_m;