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

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