diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java index 7265e2582f..93d8a0e9af 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java @@ -4,14 +4,11 @@ package edu.wpi.first.math.geometry; -import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; class Transform2dTest { - private static final double kEpsilon = 1E-9; - @Test void testInverse() { var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0)); @@ -20,14 +17,7 @@ class Transform2dTest { var transformed = initial.plus(transform); var untransformed = transformed.plus(transform.inverse()); - assertAll( - () -> assertEquals(initial.getX(), untransformed.getX(), kEpsilon), - () -> assertEquals(initial.getY(), untransformed.getY(), kEpsilon), - () -> - assertEquals( - initial.getRotation().getDegrees(), - untransformed.getRotation().getDegrees(), - kEpsilon)); + assertEquals(initial, untransformed); } @Test @@ -39,13 +29,6 @@ class Transform2dTest { var transformedSeparate = initial.plus(transform1).plus(transform2); var transformedCombined = initial.plus(transform1.plus(transform2)); - assertAll( - () -> assertEquals(transformedSeparate.getX(), transformedCombined.getX(), kEpsilon), - () -> assertEquals(transformedSeparate.getY(), transformedCombined.getY(), kEpsilon), - () -> - assertEquals( - transformedSeparate.getRotation().getDegrees(), - transformedCombined.getRotation().getDegrees(), - kEpsilon)); + assertEquals(transformedSeparate, transformedCombined); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java index 122466c51e..51d5f071fc 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java @@ -4,7 +4,6 @@ package edu.wpi.first.math.geometry; -import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import edu.wpi.first.math.VecBuilder; @@ -12,29 +11,21 @@ import edu.wpi.first.math.util.Units; import org.junit.jupiter.api.Test; class Transform3dTest { - private static final double kEpsilon = 1E-9; - @Test void testInverse() { var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); var initial = new Pose3d( - new Translation3d(1.0, 2.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(45.0))); + new Translation3d(1.0, 2.0, 3.0), new Rotation3d(zAxis, Units.degreesToRadians(45.0))); var transform = new Transform3d( - new Translation3d(5.0, 0.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(5.0))); + new Translation3d(5.0, 4.0, 3.0), new Rotation3d(zAxis, Units.degreesToRadians(5.0))); var transformed = initial.plus(transform); var untransformed = transformed.plus(transform.inverse()); - assertAll( - () -> assertEquals(initial.getX(), untransformed.getX(), kEpsilon), - () -> assertEquals(initial.getY(), untransformed.getY(), kEpsilon), - () -> assertEquals(initial.getZ(), untransformed.getZ(), kEpsilon), - () -> - assertEquals( - initial.getRotation().getZ(), untransformed.getRotation().getZ(), kEpsilon)); + assertEquals(initial, untransformed); } @Test @@ -43,7 +34,7 @@ class Transform3dTest { var initial = new Pose3d( - new Translation3d(1.0, 2.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(45.0))); + new Translation3d(1.0, 2.0, 3.0), new Rotation3d(zAxis, Units.degreesToRadians(45.0))); var transform1 = new Transform3d( new Translation3d(5.0, 0.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(5.0))); @@ -54,14 +45,6 @@ class Transform3dTest { var transformedSeparate = initial.plus(transform1).plus(transform2); var transformedCombined = initial.plus(transform1.plus(transform2)); - assertAll( - () -> assertEquals(transformedSeparate.getX(), transformedCombined.getX(), kEpsilon), - () -> assertEquals(transformedSeparate.getY(), transformedCombined.getY(), kEpsilon), - () -> assertEquals(transformedSeparate.getZ(), transformedCombined.getZ(), kEpsilon), - () -> - assertEquals( - transformedSeparate.getRotation().getZ(), - transformedCombined.getRotation().getZ(), - kEpsilon)); + assertEquals(transformedSeparate, transformedCombined); } } diff --git a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp index 2dd619d182..1b0934d6ca 100644 --- a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp @@ -19,10 +19,7 @@ TEST(Transform2dTest, Inverse) { auto transformed = initial + transform; auto untransformed = transformed + transform.Inverse(); - EXPECT_NEAR(initial.X().value(), untransformed.X().value(), 1e-9); - EXPECT_DOUBLE_EQ(initial.Y().value(), untransformed.Y().value()); - EXPECT_DOUBLE_EQ(initial.Rotation().Degrees().value(), - untransformed.Rotation().Degrees().value()); + EXPECT_EQ(initial, untransformed); } TEST(Transform2dTest, Composition) { @@ -33,12 +30,7 @@ TEST(Transform2dTest, Composition) { auto transformedSeparate = initial + transform1 + transform2; auto transformedCombined = initial + (transform1 + transform2); - EXPECT_DOUBLE_EQ(transformedSeparate.X().value(), - transformedCombined.X().value()); - EXPECT_DOUBLE_EQ(transformedSeparate.Y().value(), - transformedCombined.Y().value()); - EXPECT_DOUBLE_EQ(transformedSeparate.Rotation().Degrees().value(), - transformedCombined.Rotation().Degrees().value()); + EXPECT_EQ(transformedSeparate, transformedCombined); } TEST(Transform2dTest, Constexpr) { diff --git a/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp index 7361096d7f..904ec9c06e 100644 --- a/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp @@ -15,35 +15,24 @@ using namespace frc; TEST(Transform3dTest, Inverse) { Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; - const Pose3d initial{1_m, 2_m, 0_m, Rotation3d{zAxis, 45_deg}}; - const Transform3d transform{{5_m, 0_m, 0_m}, Rotation3d{zAxis, 5_deg}}; + const Pose3d initial{1_m, 2_m, 3_m, Rotation3d{zAxis, 45_deg}}; + const Transform3d transform{{5_m, 4_m, 3_m}, Rotation3d{zAxis, 5_deg}}; auto transformed = initial + transform; auto untransformed = transformed + transform.Inverse(); - EXPECT_NEAR(initial.X().value(), untransformed.X().value(), 1e-9); - EXPECT_DOUBLE_EQ(initial.Y().value(), untransformed.Y().value()); - EXPECT_DOUBLE_EQ(initial.Z().value(), untransformed.Z().value()); - EXPECT_DOUBLE_EQ(initial.Rotation().Z().value(), - untransformed.Rotation().Z().value()); + EXPECT_EQ(initial, untransformed); } TEST(Transform3dTest, Composition) { Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; - const Pose3d initial{1_m, 2_m, 0_m, Rotation3d{zAxis, 45_deg}}; + const Pose3d initial{1_m, 2_m, 3_m, Rotation3d{zAxis, 45_deg}}; const Transform3d transform1{{5_m, 0_m, 0_m}, Rotation3d{zAxis, 5_deg}}; const Transform3d transform2{{0_m, 2_m, 0_m}, Rotation3d{zAxis, 5_deg}}; auto transformedSeparate = initial + transform1 + transform2; auto transformedCombined = initial + (transform1 + transform2); - EXPECT_DOUBLE_EQ(transformedSeparate.X().value(), - transformedCombined.X().value()); - EXPECT_DOUBLE_EQ(transformedSeparate.Y().value(), - transformedCombined.Y().value()); - EXPECT_DOUBLE_EQ(transformedSeparate.Z().value(), - transformedCombined.Z().value()); - EXPECT_DOUBLE_EQ(transformedSeparate.Rotation().Z().value(), - transformedCombined.Rotation().Z().value()); + EXPECT_EQ(transformedSeparate, transformedCombined); }