[wpimath] Add an operator for composing two Transform2ds (#3527)

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

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

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

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 transformed = initial.plus(transform);
-    var untransformed = transformed.plus(transformation.inverse());
+    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));
+  }
 }

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 Pose2d initial{1_m, 2_m, 45_deg};
-  const Transform2d transform{Translation2d{5.0_m, 0.0_m}, Rotation2d(5.0_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<double>(),
               untransformed.Rotation().Degrees().to<double>(), 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<double>(),
+              transformedCombined.X().to<double>(), kEpsilon);
+  EXPECT_NEAR(transformedSeparate.Y().to<double>(),
+              transformedCombined.Y().to<double>(), kEpsilon);
+  EXPECT_NEAR(transformedSeparate.Rotation().Degrees().to<double>(),
+              transformedCombined.Rotation().Degrees().to<double>(), kEpsilon);
+}