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

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

@@ -4,6 +4,9 @@
#pragma once
#include <initializer_list>
#include <span>
#include <wpi/SymbolExports.h>
#include "Transform2d.h"
@@ -176,6 +179,20 @@ class WPILIB_DLLEXPORT Pose2d {
*/
Twist2d Log(const Pose2d& end) const;
/**
* Returns the nearest Pose2d from a collection of poses
* @param poses The collection of poses.
* @return The nearest Pose2d from the collection.
*/
Pose2d Nearest(std::span<const Pose2d> poses) const;
/**
* Returns the nearest Pose2d from a collection of poses
* @param poses The collection of poses.
* @return The nearest Pose2d from the collection.
*/
Pose2d Nearest(std::initializer_list<Pose2d> poses) const;
private:
Translation2d m_translation;
Rotation2d m_rotation;

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

@@ -4,6 +4,9 @@
#pragma once
#include <initializer_list>
#include <span>
#include <wpi/SymbolExports.h>
#include "Rotation2d.h"
@@ -170,6 +173,21 @@ class WPILIB_DLLEXPORT Translation2d {
*/
bool operator==(const Translation2d& other) const;
/**
* Returns the nearest Translation2d from a collection of translations
* @param translations The collection of translations.
* @return The nearest Translation2d from the collection.
*/
Translation2d Nearest(std::span<const Translation2d> translations) const;
/**
* Returns the nearest Translation2d from a collection of translations
* @param translations The collection of translations.
* @return The nearest Translation2d from the collection.
*/
Translation2d Nearest(
std::initializer_list<Translation2d> translations) const;
private:
units::meter_t m_x = 0_m;
units::meter_t m_y = 0_m;