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

19
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
View File

@@ -21,6 +21,9 @@ import edu.wpi.first.math.numbers.N4;
import edu.wpi.first.units.measure.Distance;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
import java.util.Collections;
import java.util.Comparator;
import java.util.List;
import java.util.Objects;
/** Represents a 3D pose containing translational and rotational elements. */
@@ -396,6 +399,22 @@ public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, Str
return new Pose2d(m_translation.toTranslation2d(), m_rotation.toRotation2d());
}
/**
* Returns the nearest Pose3d from a list of poses. If two or more poses in the list have the same
* distance from this pose, return the one with the closest rotation component.
*
* @param poses The list of poses to find the nearest.
* @return The nearest Pose3d from the list.
*/
public Pose3d nearest(List<Pose3d> poses) {
return Collections.min(
poses,
Comparator.comparing(
(Pose3d other) -> this.getTranslation().getDistance(other.getTranslation()))
.thenComparing(
(Pose3d other) -> this.getRotation().minus(other.getRotation()).getAngle()));
}
@Override
public String toString() {
return String.format("Pose3d(%s, %s)", m_translation, m_rotation);