diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java index 2eb69dab60..59b8b95cf0 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java @@ -56,6 +56,15 @@ public class Transform2d { return new Transform2d(m_translation.times(scalar), m_rotation.times(scalar)); } + /** + * Composes two transformations. + * + * @param other The transform to compose with this one. + */ + public Transform2d plus(Transform2d other) { + return new Transform2d(new Pose2d(), new Pose2d().transformBy(this).transformBy(other)); + } + /** * Returns the translation component of the transformation. * diff --git a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp index a53d6bcbf0..0808f351fa 100644 --- a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp @@ -28,6 +28,10 @@ Transform2d Transform2d::Inverse() const { return Transform2d{(-Translation()).RotateBy(-Rotation()), -Rotation()}; } +Transform2d Transform2d::operator+(const Transform2d& other) const { + return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)}; +} + bool Transform2d::operator==(const Transform2d& other) const { return m_translation == other.m_translation && m_rotation == other.m_rotation; } diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.h b/wpimath/src/main/native/include/frc/geometry/Transform2d.h index 62c53f05a1..296ebc31ba 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.h @@ -81,6 +81,13 @@ class Transform2d { return Transform2d(m_translation * scalar, m_rotation * scalar); } + /** + * Composes two transformations. + * + * @param other The transform to compose with this one. + */ + Transform2d operator+(const Transform2d& other) const; + /** * Checks equality between this Transform2d and another object. * 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 1a2f8e795d..7265e2582f 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 @@ -15,10 +15,10 @@ class Transform2dTest { @Test void testInverse() { var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0)); - var transformation = new Transform2d(new Translation2d(5.0, 0.0), Rotation2d.fromDegrees(5.0)); + var transform = new Transform2d(new Translation2d(5.0, 0.0), Rotation2d.fromDegrees(5.0)); - var transformed = initial.plus(transformation); - var untransformed = transformed.plus(transformation.inverse()); + var transformed = initial.plus(transform); + var untransformed = transformed.plus(transform.inverse()); assertAll( () -> assertEquals(initial.getX(), untransformed.getX(), kEpsilon), @@ -29,4 +29,23 @@ class Transform2dTest { untransformed.getRotation().getDegrees(), kEpsilon)); } + + @Test + void testComposition() { + var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0)); + var transform1 = new Transform2d(new Translation2d(5.0, 0.0), Rotation2d.fromDegrees(5.0)); + var transform2 = new Transform2d(new Translation2d(0.0, 2.0), Rotation2d.fromDegrees(5.0)); + + 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)); + } } diff --git a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp index 9324a1d255..2014a6c624 100644 --- a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp @@ -15,8 +15,8 @@ using namespace frc; static constexpr double kEpsilon = 1E-9; TEST(Transform2dTest, Inverse) { - const Pose2d initial{1_m, 2_m, Rotation2d(45.0_deg)}; - const Transform2d transform{Translation2d{5.0_m, 0.0_m}, Rotation2d(5.0_deg)}; + const Pose2d initial{1_m, 2_m, 45_deg}; + const Transform2d transform{{5_m, 0_m}, 5_deg}; auto transformed = initial + transform; auto untransformed = transformed + transform.Inverse(); @@ -28,3 +28,19 @@ TEST(Transform2dTest, Inverse) { EXPECT_NEAR(initial.Rotation().Degrees().to(), untransformed.Rotation().Degrees().to(), kEpsilon); } + +TEST(Transform2dTest, Composition) { + const Pose2d initial{1_m, 2_m, 45_deg}; + const Transform2d transform1{{5_m, 0_m}, 5_deg}; + const Transform2d transform2{{0_m, 2_m}, 5_deg}; + + auto transformedSeparate = initial + transform1 + transform2; + auto transformedCombined = initial + (transform1 + transform2); + + EXPECT_NEAR(transformedSeparate.X().to(), + transformedCombined.X().to(), kEpsilon); + EXPECT_NEAR(transformedSeparate.Y().to(), + transformedCombined.Y().to(), kEpsilon); + EXPECT_NEAR(transformedSeparate.Rotation().Degrees().to(), + transformedCombined.Rotation().Degrees().to(), kEpsilon); +}