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

32
wpimath/src/main/native/cpp/geometry/Pose2d.cpp
View File

@@ -8,6 +8,8 @@
#include <wpi/json.h>
#include "frc/MathUtil.h"
using namespace frc;
Transform2d Pose2d::operator-(const Pose2d& other) const {
@@ -67,6 +69,36 @@ Twist2d Pose2d::Log(const Pose2d& end) const {
return {translationPart.X(), translationPart.Y(), units::radian_t{dtheta}};
}
Pose2d Pose2d::Nearest(std::span<const Pose2d> poses) const {
return *std::min_element(
poses.begin(), poses.end(), [this](const Pose2d& a, const Pose2d& b) {
auto aDistance = this->Translation().Distance(a.Translation());
auto bDistance = this->Translation().Distance(b.Translation());
// If the distances are equal sort by difference in rotation
if (aDistance == bDistance) {
return std::abs((this->Rotation() - a.Rotation()).Radians().value()) <
std::abs((this->Rotation() - b.Rotation()).Radians().value());
}
return aDistance < bDistance;
});
}
Pose2d Pose2d::Nearest(std::initializer_list<Pose2d> poses) const {
return *std::min_element(
poses.begin(), poses.end(), [this](const Pose2d& a, const Pose2d& b) {
auto aDistance = this->Translation().Distance(a.Translation());
auto bDistance = this->Translation().Distance(b.Translation());
// If the distances are equal sort by difference in rotation
if (aDistance == bDistance) {
return std::abs((this->Rotation() - a.Rotation()).Radians().value()) <
std::abs((this->Rotation() - b.Rotation()).Radians().value());
}
return aDistance < bDistance;
});
}
void frc::to_json(wpi::json& json, const Pose2d& pose) {
json = wpi::json{{"translation", pose.Translation()},
{"rotation", pose.Rotation()}};

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