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

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2025-06-13 22:26:09 -07:00
parent fbee476fd2 05c080328b
commit 3355383fe9
13 changed files with 1031 additions and 107 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.
*/

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

@@ -4,6 +4,9 @@
#pragma once
#include <algorithm>
#include <initializer_list>
#include <span>
#include <stdexcept>
#include <type_traits>
#include <utility>
@@ -270,6 +273,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;

18
wpimath/src/main/native/include/frc/geometry/Translation2d.h
View File

@@ -238,10 +238,11 @@ class WPILIB_DLLEXPORT Translation2d {
*/
constexpr 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);
});
return *std::min_element(
translations.begin(), translations.end(),
[this](const Translation2d& a, const Translation2d& b) {
return this->Distance(a) < this->Distance(b);
});
}
/**
@@ -251,10 +252,11 @@ class WPILIB_DLLEXPORT Translation2d {
*/
constexpr 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);
});
return *std::min_element(
translations.begin(), translations.end(),
[this](const Translation2d& a, const Translation2d& b) {
return this->Distance(a) < this->Distance(b);
});
}
private:

32
wpimath/src/main/native/include/frc/geometry/Translation3d.h
View File

@@ -4,6 +4,10 @@
#pragma once
#include <algorithm>
#include <initializer_list>
#include <span>
#include <Eigen/Core>
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
@@ -244,6 +248,34 @@ class WPILIB_DLLEXPORT Translation3d {
units::math::abs(m_z - other.m_z) < 1E-9_m;
}
/**
* Returns the nearest Translation3d from a collection of translations
* @param translations The collection of translations.
* @return The nearest Translation3d from the collection.
*/
constexpr Translation3d Nearest(
std::span<const Translation3d> translations) const {
return *std::min_element(
translations.begin(), translations.end(),
[this](const Translation3d& a, const Translation3d& b) {
return this->Distance(a) < this->Distance(b);
});
}
/**
* Returns the nearest Translation3d from a collection of translations
* @param translations The collection of translations.
* @return The nearest Translation3d from the collection.
*/
constexpr Translation3d Nearest(
std::initializer_list<Translation3d> translations) const {
return *std::min_element(
translations.begin(), translations.end(),
[this](const Translation3d& a, const Translation3d& b) {
return this->Distance(a) < this->Distance(b);
});
}
private:
units::meter_t m_x = 0_m;
units::meter_t m_y = 0_m;