Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +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

68
wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
View File

@@ -3,6 +3,7 @@
// the WPILib BSD license file in the root directory of this project.
#include <cmath>
#include <cstdlib>
#include "frc/geometry/Pose2d.h"
#include "gtest/gtest.h"
@@ -54,6 +55,73 @@ TEST(Pose2dTest, Minus) {
EXPECT_DOUBLE_EQ(0.0, transform.Rotation().Degrees().value());
}
TEST(Pose2dTest, Nearest) {
const Pose2d origin{0_m, 0_m, 0_deg};
const Pose2d pose1{Translation2d{1_m, Rotation2d{45_deg}}, 0_deg};
const Pose2d pose2{Translation2d{2_m, Rotation2d{90_deg}}, 0_deg};
const Pose2d pose3{Translation2d{3_m, Rotation2d{135_deg}}, 0_deg};
const Pose2d pose4{Translation2d{4_m, Rotation2d{180_deg}}, 0_deg};
const Pose2d pose5{Translation2d{5_m, Rotation2d{270_deg}}, 0_deg};
EXPECT_DOUBLE_EQ(pose3.X().value(),
origin.Nearest({pose5, pose3, pose4}).X().value());
EXPECT_DOUBLE_EQ(pose3.Y().value(),
origin.Nearest({pose5, pose3, pose4}).Y().value());
EXPECT_DOUBLE_EQ(pose1.X().value(),
origin.Nearest({pose1, pose2, pose3}).X().value());
EXPECT_DOUBLE_EQ(pose1.Y().value(),
origin.Nearest({pose1, pose2, pose3}).Y().value());
EXPECT_DOUBLE_EQ(pose2.X().value(),
origin.Nearest({pose4, pose2, pose3}).X().value());
EXPECT_DOUBLE_EQ(pose2.Y().value(),
origin.Nearest({pose4, pose2, pose3}).Y().value());
// Rotation component sort (when distance is the same)
// Use the same translation because using different angles at the same
// distance can cause rounding error.
const Translation2d translation{1_m, Rotation2d{0_deg}};
const Pose2d poseA{translation, 0_deg};
const Pose2d poseB{translation, Rotation2d{30_deg}};
const Pose2d poseC{translation, Rotation2d{120_deg}};
const Pose2d poseD{translation, Rotation2d{90_deg}};
const Pose2d poseE{translation, Rotation2d{-180_deg}};
EXPECT_DOUBLE_EQ(poseA.Rotation().Degrees().value(),
Pose2d(0_m, 0_m, Rotation2d{360_deg})
.Nearest({poseA, poseB, poseD})
.Rotation()
.Degrees()
.value());
EXPECT_DOUBLE_EQ(poseB.Rotation().Degrees().value(),
Pose2d(0_m, 0_m, Rotation2d{-335_deg})
.Nearest({poseB, poseC, poseD})
.Rotation()
.Degrees()
.value());
EXPECT_DOUBLE_EQ(poseC.Rotation().Degrees().value(),
Pose2d(0_m, 0_m, Rotation2d{-120_deg})
.Nearest({poseB, poseC, poseD})
.Rotation()
.Degrees()
.value());
EXPECT_DOUBLE_EQ(poseD.Rotation().Degrees().value(),
Pose2d(0_m, 0_m, Rotation2d{85_deg})
.Nearest({poseA, poseC, poseD})
.Rotation()
.Degrees()
.value());
EXPECT_DOUBLE_EQ(poseE.Rotation().Degrees().value(),
Pose2d(0_m, 0_m, Rotation2d{170_deg})
.Nearest({poseA, poseD, poseE})
.Rotation()
.Degrees()
.value());
}
TEST(Pose2dTest, Constexpr) {
constexpr Pose2d defaultConstructed;
constexpr Pose2d translationRotation{Translation2d{0_m, 1_m},