[wpimath] Add Translation3d.nearest() (#8015)

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/Pose3d.h

@@ -4,6 +4,9 @@
 
 #pragma once
 
+#include <algorithm>
+#include <initializer_list>
+#include <span>
 #include <stdexcept>
 #include <type_traits>
 #include <utility>

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;