[wpimath] Add distance/angle constructor to Translation2d (#2791)

wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java

@@ -50,6 +50,18 @@ public class Translation2d {
     m_y = y;
   }
 
+  /**
+   * Constructs a Translation2d with the provided distance and angle. This is
+   * essentially converting from polar coordinates to Cartesian coordinates.
+   *
+   * @param distance The distance from the origin to the end of the translation.
+   * @param angle    The angle between the x-axis and the translation vector.
+   */
+  public Translation2d(double distance, Rotation2d angle) {
+    m_x = distance * angle.getCos();
+    m_y = distance * angle.getSin();
+  }
+
   /**
    * Calculates the distance between two translations in 2d space.
    *

wpimath/src/main/native/cpp/geometry/Translation2d.cpp

@@ -16,6 +16,9 @@ using namespace frc;
 Translation2d::Translation2d(units::meter_t x, units::meter_t y)
     : m_x(x), m_y(y) {}
 
+Translation2d::Translation2d(units::meter_t distance, const Rotation2d& angle)
+    : m_x(distance * angle.Cos()), m_y(distance * angle.Sin()) {}
+
 units::meter_t Translation2d::Distance(const Translation2d& other) const {
   return units::math::hypot(other.m_x - m_x, other.m_y - m_y);
 }

wpimath/src/main/native/include/frc/geometry/Translation2d.h

@@ -40,6 +40,15 @@ class Translation2d {
    */
   Translation2d(units::meter_t x, units::meter_t y);
 
+  /**
+   * Constructs a Translation2d with the provided distance and angle. This is
+   * essentially converting from polar coordinates to Cartesian coordinates.
+   *
+   * @param distance The distance from the origin to the end of the translation.
+   * @param angle The angle between the x-axis and the translation vector.
+   */
+  Translation2d(units::meter_t distance, const Rotation2d& angle);
+
   /**
    * Calculates the distance between two translations in 2d space.
    *

wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java

@@ -13,6 +13,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;
 
+@SuppressWarnings("PMD.TooManyMethods")
 class Translation2dTest {
   private static final double kEpsilon = 1E-9;
 
@@ -112,4 +113,16 @@ class Translation2dTest {
     var two = new Translation2d(9, 5.7);
     assertNotEquals(one, two);
   }
+
+  @Test
+  void testPolarConstructor() {
+    var one = new Translation2d(Math.sqrt(2), Rotation2d.fromDegrees(45.0));
+    var two = new Translation2d(2, Rotation2d.fromDegrees(60.0));
+    assertAll(
+        () -> assertEquals(one.getX(), 1.0, kEpsilon),
+        () -> assertEquals(one.getY(), 1.0, kEpsilon),
+        () -> assertEquals(two.getX(), 1.0, kEpsilon),
+        () -> assertEquals(two.getY(), Math.sqrt(3), kEpsilon)
+    );
+  }
 }

wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp

@@ -88,3 +88,13 @@ TEST(Translation2dTest, Inequality) {
   const Translation2d two{9_m, 5.7_m};
   EXPECT_TRUE(one != two);
 }
+
+TEST(Translation2dTest, PolarConstructor) {
+  Translation2d one{std::sqrt(2) * 1_m, Rotation2d(45_deg)};
+  EXPECT_NEAR(one.X().to<double>(), 1.0, kEpsilon);
+  EXPECT_NEAR(one.Y().to<double>(), 1.0, kEpsilon);
+
+  Translation2d two{2_m, Rotation2d(60_deg)};
+  EXPECT_NEAR(two.X().to<double>(), 1.0, kEpsilon);
+  EXPECT_NEAR(two.Y().to<double>(), std::sqrt(3.0), kEpsilon);
+}