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

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

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

13
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
View File

@@ -20,6 +20,9 @@ import edu.wpi.first.math.numbers.N3;
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;
/**
@@ -296,6 +299,16 @@ public class Translation3d
return new Translation3d(m_x / scalar, m_y / scalar, m_z / scalar);
}
/**
* Returns the nearest Translation3d from a collection of translations.
*
* @param translations The collection of translations to find the nearest.
* @return The nearest Translation3d from the collection.
*/
public Translation3d nearest(List<Translation3d> translations) {
return Collections.min(translations, Comparator.comparing(this::getDistance));
}
@Override
public String toString() {
return String.format("Translation3d(X: %.2f, Y: %.2f, Z: %.2f)", m_x, m_y, m_z);