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

16
wpimath/src/main/native/cpp/geometry/Translation2d.cpp
View File

@@ -23,6 +23,22 @@ bool Translation2d::operator==(const Translation2d& other) const {
units::math::abs(m_y - other.m_y) < 1E-9_m;
}
Translation2d Translation2d::Nearest(
std::span<const Translation2d> translations) const {
return *std::min_element(translations.begin(), translations.end(),
[this](Translation2d a, Translation2d b) {
return this->Distance(a) < this->Distance(b);
});
}
Translation2d Translation2d::Nearest(
std::initializer_list<Translation2d> translations) const {
return *std::min_element(translations.begin(), translations.end(),
[this](Translation2d a, Translation2d b) {
return this->Distance(a) < this->Distance(b);
});
}
void frc::to_json(wpi::json& json, const Translation2d& translation) {
json =
wpi::json{{"x", translation.X().value()}, {"y", translation.Y().value()}};