diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java index dcdfb06ab4..985b32c3b8 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java @@ -97,6 +97,15 @@ public class Translation2d implements Interpolatable { return Math.hypot(m_x, m_y); } + /** + * Returns the angle this translation forms with the positive X axis. + * + * @return The angle of the translation + */ + public Rotation2d getAngle() { + return new Rotation2d(m_x, m_y); + } + /** * Applies a rotation to the translation in 2D space. * diff --git a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp index 5a30ec2b81..37b8822725 100644 --- a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp @@ -24,6 +24,10 @@ units::meter_t Translation2d::Norm() const { return units::math::hypot(m_x, m_y); } +Rotation2d Translation2d::Angle() const { + return Rotation2d{m_x.value(), m_y.value()}; +} + Translation2d Translation2d::RotateBy(const Rotation2d& other) const { return {m_x * other.Cos() - m_y * other.Sin(), m_x * other.Sin() + m_y * other.Cos()}; diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h index 67180ddef0..7ee99aa264 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h @@ -80,6 +80,13 @@ class WPILIB_DLLEXPORT Translation2d { */ units::meter_t Norm() const; + /** + * Returns the angle this translation forms with the positive X axis. + * + * @return The angle of the translation + */ + Rotation2d Angle() const; + /** * Applies a rotation to the translation in 2D space. *