diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java index 3ec5f18b06..5365759652 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java +++ b/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. * diff --git a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp index f081ac0fd4..6f4551cbe2 100644 --- a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp +++ b/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); } diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h index aa9fb48ac4..0c3ee3d0f0 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h +++ b/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. * diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java index dc109692bc..4be8565cb4 100644 --- a/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java +++ b/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) + ); + } } diff --git a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp index 3eb5c78345..8e487f3ae3 100644 --- a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp +++ b/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(), 1.0, kEpsilon); + EXPECT_NEAR(one.Y().to(), 1.0, kEpsilon); + + Translation2d two{2_m, Rotation2d(60_deg)}; + EXPECT_NEAR(two.X().to(), 1.0, kEpsilon); + EXPECT_NEAR(two.Y().to(), std::sqrt(3.0), kEpsilon); +}