Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11: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);
}
}

68
wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
View File

@@ -3,6 +3,7 @@
// the WPILib BSD license file in the root directory of this project.
#include <cmath>
#include <cstdlib>
#include "frc/geometry/Pose2d.h"
#include "gtest/gtest.h"
@@ -54,6 +55,73 @@ TEST(Pose2dTest, Minus) {
EXPECT_DOUBLE_EQ(0.0, transform.Rotation().Degrees().value());
}
TEST(Pose2dTest, Nearest) {
const Pose2d origin{0_m, 0_m, 0_deg};
const Pose2d pose1{Translation2d{1_m, Rotation2d{45_deg}}, 0_deg};
const Pose2d pose2{Translation2d{2_m, Rotation2d{90_deg}}, 0_deg};
const Pose2d pose3{Translation2d{3_m, Rotation2d{135_deg}}, 0_deg};
const Pose2d pose4{Translation2d{4_m, Rotation2d{180_deg}}, 0_deg};
const Pose2d pose5{Translation2d{5_m, Rotation2d{270_deg}}, 0_deg};
EXPECT_DOUBLE_EQ(pose3.X().value(),
origin.Nearest({pose5, pose3, pose4}).X().value());
EXPECT_DOUBLE_EQ(pose3.Y().value(),
origin.Nearest({pose5, pose3, pose4}).Y().value());
EXPECT_DOUBLE_EQ(pose1.X().value(),
origin.Nearest({pose1, pose2, pose3}).X().value());
EXPECT_DOUBLE_EQ(pose1.Y().value(),
origin.Nearest({pose1, pose2, pose3}).Y().value());
EXPECT_DOUBLE_EQ(pose2.X().value(),
origin.Nearest({pose4, pose2, pose3}).X().value());
EXPECT_DOUBLE_EQ(pose2.Y().value(),
origin.Nearest({pose4, pose2, pose3}).Y().value());
// Rotation component sort (when distance is the same)
// Use the same translation because using different angles at the same
// distance can cause rounding error.
const Translation2d translation{1_m, Rotation2d{0_deg}};
const Pose2d poseA{translation, 0_deg};
const Pose2d poseB{translation, Rotation2d{30_deg}};
const Pose2d poseC{translation, Rotation2d{120_deg}};
const Pose2d poseD{translation, Rotation2d{90_deg}};
const Pose2d poseE{translation, Rotation2d{-180_deg}};
EXPECT_DOUBLE_EQ(poseA.Rotation().Degrees().value(),
Pose2d(0_m, 0_m, Rotation2d{360_deg})
.Nearest({poseA, poseB, poseD})
.Rotation()
.Degrees()
.value());
EXPECT_DOUBLE_EQ(poseB.Rotation().Degrees().value(),
Pose2d(0_m, 0_m, Rotation2d{-335_deg})
.Nearest({poseB, poseC, poseD})
.Rotation()
.Degrees()
.value());
EXPECT_DOUBLE_EQ(poseC.Rotation().Degrees().value(),
Pose2d(0_m, 0_m, Rotation2d{-120_deg})
.Nearest({poseB, poseC, poseD})
.Rotation()
.Degrees()
.value());
EXPECT_DOUBLE_EQ(poseD.Rotation().Degrees().value(),
Pose2d(0_m, 0_m, Rotation2d{85_deg})
.Nearest({poseA, poseC, poseD})
.Rotation()
.Degrees()
.value());
EXPECT_DOUBLE_EQ(poseE.Rotation().Degrees().value(),
Pose2d(0_m, 0_m, Rotation2d{170_deg})
.Nearest({poseA, poseD, poseE})
.Rotation()
.Degrees()
.value());
}
TEST(Pose2dTest, Constexpr) {
constexpr Pose2d defaultConstructed;
constexpr Pose2d translationRotation{Translation2d{0_m, 1_m},

31
wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
View File

@@ -94,6 +94,37 @@ TEST(Translation2dTest, PolarConstructor) {
EXPECT_DOUBLE_EQ(std::sqrt(3.0), two.Y().value());
}
TEST(Translation2dTest, Nearest) {
const Translation2d origin{0_m, 0_m};
const Translation2d translation1{1_m, Rotation2d{45_deg}};
const Translation2d translation2{2_m, Rotation2d{90_deg}};
const Translation2d translation3{3_m, Rotation2d{135_deg}};
const Translation2d translation4{4_m, Rotation2d{180_deg}};
const Translation2d translation5{5_m, Rotation2d{270_deg}};
EXPECT_DOUBLE_EQ(
origin.Nearest({translation5, translation3, translation4}).X().value(),
translation3.X().value());
EXPECT_DOUBLE_EQ(
origin.Nearest({translation5, translation3, translation4}).Y().value(),
translation3.Y().value());
EXPECT_DOUBLE_EQ(
origin.Nearest({translation1, translation2, translation3}).X().value(),
translation1.X().value());
EXPECT_DOUBLE_EQ(
origin.Nearest({translation1, translation2, translation3}).Y().value(),
translation1.Y().value());
EXPECT_DOUBLE_EQ(
origin.Nearest({translation4, translation2, translation3}).X().value(),
translation2.X().value());
EXPECT_DOUBLE_EQ(
origin.Nearest({translation4, translation2, translation3}).Y().value(),
translation2.Y().value());
}
TEST(Translation2dTest, Constexpr) {
constexpr Translation2d defaultCtor;
constexpr Translation2d componentCtor{1_m, 2_m};