Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add nearest() method to Pose3d (mirroring Pose2d) (#8010)

Browse Source
This commit is contained in:
Michael Lesirge
2025-06-05 22:11:49 -07:00
committed by GitHub
parent 7a3df6175e
commit 075cc4a20f
5 changed files with 254 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
wpimath/src/main/native/include/frc/geometry/Pose2d.h
View File

@@ -242,7 +242,11 @@ class WPILIB_DLLEXPORT Pose2d {
}
/**
* Returns the nearest Pose2d from a collection of poses
* Returns the nearest Pose2d from a collection of poses.
*
* If two or more poses in the collection have the same distance from this
* pose, return the one with the closest rotation component.
*
* @param poses The collection of poses.
* @return The nearest Pose2d from the collection.
*/
@@ -264,7 +268,11 @@ class WPILIB_DLLEXPORT Pose2d {
}
/**
* Returns the nearest Pose2d from a collection of poses
* Returns the nearest Pose2d from a collection of poses.
*
* If two or more poses in the collection have the same distance from this
* pose, return the one with the closest rotation component.
*
* @param poses The collection of poses.
* @return The nearest Pose2d from the collection.
*/

50
wpimath/src/main/native/include/frc/geometry/Pose3d.h
View File

@@ -270,6 +270,56 @@ class WPILIB_DLLEXPORT Pose3d {
return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()};
}
/**
* Returns the nearest Pose3d from a collection of poses.
*
* If two or more poses in the collection have the same distance from this
* pose, return the one with the closest rotation component.
*
* @param poses The collection of poses.
* @return The nearest Pose3d from the collection.
*/
constexpr Pose3d Nearest(std::span<const Pose3d> poses) const {
return *std::min_element(
poses.begin(), poses.end(), [this](const Pose3d& a, const Pose3d& 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 gcem::abs(
(this->Rotation() - a.Rotation()).Angle().value()) <
gcem::abs((this->Rotation() - b.Rotation()).Angle().value());
}
return aDistance < bDistance;
});
}
/**
* Returns the nearest Pose3d from a collection of poses.
*
* If two or more poses in the collection have the same distance from this
* pose, return the one with the closest rotation component.
*
* @param poses The collection of poses.
* @return The nearest Pose3d from the collection.
*/
constexpr Pose3d Nearest(std::initializer_list<Pose3d> poses) const {
return *std::min_element(
poses.begin(), poses.end(), [this](const Pose3d& a, const Pose3d& 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 gcem::abs(
(this->Rotation() - a.Rotation()).Angle().value()) <
gcem::abs((this->Rotation() - b.Rotation()).Angle().value());
}
return aDistance < bDistance;
});
}
private:
Translation3d m_translation;
Rotation3d m_rotation;