Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Michael Lesirge
2025-06-12 22:14:00 -07:00
committed by GitHub
parent 075cc4a20f
commit c01e318370
5 changed files with 95 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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)));
}
}

28
wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
View File

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