diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java index efaf92a670..1aad1a892b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java +++ b/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 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); diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h index 743ccfbc04..e2ec7c4609 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h @@ -4,6 +4,9 @@ #pragma once +#include +#include +#include #include #include #include diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h index 72e220f08c..b9876c05a3 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h @@ -4,6 +4,10 @@ #pragma once +#include +#include +#include + #include #include #include @@ -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 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 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; diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java index 49b146fdd0..95a970ba6f 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java +++ b/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))); + } } diff --git a/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp index f463c0bfa4..b297f1217c 100644 --- a/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp +++ b/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()); +}