Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add nearest to Pose2d and Translation2d (#4882)

Browse Source 
Co-authored-by: David Vo <auscompgeek@users.noreply.github.com>
This commit is contained in:
Ryan Blue
2023-02-03 18:27:16 -05:00
committed by GitHub
parent 08a536291b
commit 7b828ce84f
10 changed files with 279 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
View File

@@ -94,6 +94,37 @@ TEST(Translation2dTest, PolarConstructor) {
EXPECT_DOUBLE_EQ(std::sqrt(3.0), two.Y().value());
}
TEST(Translation2dTest, Nearest) {
const Translation2d origin{0_m, 0_m};
const Translation2d translation1{1_m, Rotation2d{45_deg}};
const Translation2d translation2{2_m, Rotation2d{90_deg}};
const Translation2d translation3{3_m, Rotation2d{135_deg}};
const Translation2d translation4{4_m, Rotation2d{180_deg}};
const Translation2d translation5{5_m, Rotation2d{270_deg}};
EXPECT_DOUBLE_EQ(
origin.Nearest({translation5, translation3, translation4}).X().value(),
translation3.X().value());
EXPECT_DOUBLE_EQ(
origin.Nearest({translation5, translation3, translation4}).Y().value(),
translation3.Y().value());
EXPECT_DOUBLE_EQ(
origin.Nearest({translation1, translation2, translation3}).X().value(),
translation1.X().value());
EXPECT_DOUBLE_EQ(
origin.Nearest({translation1, translation2, translation3}).Y().value(),
translation1.Y().value());
EXPECT_DOUBLE_EQ(
origin.Nearest({translation4, translation2, translation3}).X().value(),
translation2.X().value());
EXPECT_DOUBLE_EQ(
origin.Nearest({translation4, translation2, translation3}).Y().value(),
translation2.Y().value());
}
TEST(Translation2dTest, Constexpr) {
constexpr Translation2d defaultCtor;
constexpr Translation2d componentCtor{1_m, 2_m};