Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +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

47
wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
View File

@@ -8,6 +8,7 @@ import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
import java.util.List;
import org.junit.jupiter.api.Test;
class Pose2dTest {
@@ -65,4 +66,50 @@ class Pose2dTest {
() -> assertEquals(0.0, transform.getY(), kEpsilon),
() -> assertEquals(0.0, transform.getRotation().getDegrees(), kEpsilon));
}
@Test
void testNearest() {
var origin = new Pose2d();
// Distance sort
// each poseX is X units away from the origin at a random angle.
final var pose1 =
new Pose2d(new Translation2d(1, Rotation2d.fromDegrees(45)), new Rotation2d());
final var pose2 =
new Pose2d(new Translation2d(2, Rotation2d.fromDegrees(90)), new Rotation2d());
final var pose3 =
new Pose2d(new Translation2d(3, Rotation2d.fromDegrees(135)), new Rotation2d());
final var pose4 =
new Pose2d(new Translation2d(4, Rotation2d.fromDegrees(180)), new Rotation2d());
final var pose5 =
new Pose2d(new Translation2d(5, Rotation2d.fromDegrees(270)), new Rotation2d());
assertEquals(pose3, origin.nearest(List.of(pose5, pose3, pose4)));
assertEquals(pose1, origin.nearest(List.of(pose1, pose2, pose3)));
assertEquals(pose2, origin.nearest(List.of(pose4, pose2, pose3)));
// Rotation component sort (when distance is the same)
// Use the same translation because using different angles at the same distance can cause
// rounding error.
final var translation = new Translation2d(1, new Rotation2d());
final var poseA = new Pose2d(translation, Rotation2d.fromDegrees(0));
final var poseB = new Pose2d(translation, Rotation2d.fromDegrees(30));
final var poseC = new Pose2d(translation, Rotation2d.fromDegrees(120));
final var poseD = new Pose2d(translation, Rotation2d.fromDegrees(90));
final var poseE = new Pose2d(translation, Rotation2d.fromDegrees(-180));
assertEquals(
poseA, new Pose2d(0, 0, Rotation2d.fromDegrees(360)).nearest(List.of(poseA, poseB, poseD)));
assertEquals(
poseB,
new Pose2d(0, 0, Rotation2d.fromDegrees(-335)).nearest(List.of(poseB, poseC, poseD)));
assertEquals(
poseC,
new Pose2d(0, 0, Rotation2d.fromDegrees(-120)).nearest(List.of(poseB, poseC, poseD)));
assertEquals(
poseD, new Pose2d(0, 0, Rotation2d.fromDegrees(85)).nearest(List.of(poseA, poseC, poseD)));
assertEquals(
poseE, new Pose2d(0, 0, Rotation2d.fromDegrees(170)).nearest(List.of(poseA, poseD, poseE)));
}
}

17
wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
View File

@@ -8,6 +8,7 @@ import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
import java.util.List;
import org.junit.jupiter.api.Test;
class Translation2dTest {
@@ -114,4 +115,20 @@ class Translation2dTest {
() -> assertEquals(1.0, two.getX(), kEpsilon),
() -> assertEquals(Math.sqrt(3.0), two.getY(), kEpsilon));
}
@Test
void testNearest() {
var origin = new Translation2d();
// each translationX is X units away from the origin at a random angle.
var translation1 = new Translation2d(1, Rotation2d.fromDegrees(45));
var translation2 = new Translation2d(2, Rotation2d.fromDegrees(90));
var translation3 = new Translation2d(3, Rotation2d.fromDegrees(135));
var translation4 = new Translation2d(4, Rotation2d.fromDegrees(180));
var translation5 = new Translation2d(5, Rotation2d.fromDegrees(270));
assertEquals(origin.nearest(List.of(translation5, translation3, translation4)), translation3);
assertEquals(origin.nearest(List.of(translation1, translation2, translation3)), translation1);
assertEquals(origin.nearest(List.of(translation4, translation2, translation3)), translation2);
}
}