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 Translation3d.nearest() (#8015)

Browse Source
This commit is contained in:
Michael Lesirge
2025-06-12 22:14:00 -07:00
committed by GitHub
parent 075cc4a20f
commit c01e318370
5 changed files with 95 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
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>

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;