[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;

wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java

@@ -11,6 +11,7 @@ import static org.junit.jupiter.api.Assertions.assertNotEquals;
 
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.util.Units;
+import java.util.List;
 import org.junit.jupiter.api.Test;
 
 class Translation3dTest {
@@ -206,4 +207,22 @@ class Translation3dTest {
 
     assertEquals(vec, translation.toVector());
   }
+
+  @Test
+  void testNearest() {
+    var origin = Translation3d.kZero;
+
+    // Distance sort
+    // translations are in order of closest to farthest away from the origin at various positions
+    // in 3D space.
+    final var translation1 = new Translation3d(1, 0, 0);
+    final var translation2 = new Translation3d(0, 2, 0);
+    final var translation3 = new Translation3d(0, 0, 3);
+    final var translation4 = new Translation3d(2, 2, 2);
+    final var translation5 = new Translation3d(3, 3, 3);
+
+    assertEquals(translation3, origin.nearest(List.of(translation5, translation3, translation4)));
+    assertEquals(translation1, origin.nearest(List.of(translation1, translation2, translation3)));
+    assertEquals(translation2, origin.nearest(List.of(translation4, translation2, translation3)));
+  }
 }

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