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 ecead7405d..04119b954d 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 @@ -40,6 +40,18 @@ public class Transform2d { m_rotation = rotation; } + /** + * Constructs a transform with x and y translations instead of a separate Translation2d. + * + * @param x The x component of the translational component of the transform. + * @param y The y component of the translational component of the transform. + * @param rotation The rotational component of the transform. + */ + public Transform2d(double x, double y, Rotation2d rotation) { + m_translation = new Translation2d(x, y); + m_rotation = rotation; + } + /** Constructs the identity transform -- maps an initial pose to itself. */ public Transform2d() { m_translation = new Translation2d(); diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java index 0fbfb79e68..3e56a0a4d3 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java @@ -40,6 +40,19 @@ public class Transform3d { m_rotation = rotation; } + /** + * Constructs a transform with x, y, and z translations instead of a separate Translation3d. + * + * @param x The x component of the translational component of the transform. + * @param y The y component of the translational component of the transform. + * @param z The z component of the translational component of the transform. + * @param rotation The rotational component of the transform. + */ + public Transform3d(double x, double y, double z, Rotation3d rotation) { + m_translation = new Translation3d(x, y, z); + m_rotation = rotation; + } + /** Constructs the identity transform -- maps an initial pose to itself. */ public Transform3d() { m_translation = new Translation3d(); diff --git a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp index 1cfabaa1c4..556a3027d0 100644 --- a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp @@ -21,6 +21,10 @@ Transform3d::Transform3d(Pose3d initial, Pose3d final) { Transform3d::Transform3d(Translation3d translation, Rotation3d rotation) : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {} +Transform3d::Transform3d(units::meter_t x, units::meter_t y, units::meter_t z, + Rotation3d rotation) + : m_translation(x, y, z), m_rotation(std::move(rotation)) {} + Transform3d Transform3d::Inverse() const { // We are rotating the difference between the translations // using a clockwise rotation matrix. This transforms the global diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.h b/wpimath/src/main/native/include/frc/geometry/Transform2d.h index 6e8ec1a8fd..f91fbc7cdf 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.h @@ -33,6 +33,17 @@ class WPILIB_DLLEXPORT Transform2d { */ constexpr Transform2d(Translation2d translation, Rotation2d rotation); + /** + * Constructs a transform with x and y translations instead of a separate + * Translation2d. + * + * @param x The x component of the translational component of the transform. + * @param y The y component of the translational component of the transform. + * @param rotation The rotational component of the transform. + */ + constexpr Transform2d(units::meter_t x, units::meter_t y, + Rotation2d rotation); + /** * Constructs the identity transform -- maps an initial pose to itself. */ diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.inc b/wpimath/src/main/native/include/frc/geometry/Transform2d.inc index f851a05207..862b8d5c60 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform2d.inc +++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.inc @@ -16,6 +16,10 @@ constexpr Transform2d::Transform2d(Translation2d translation, Rotation2d rotation) : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {} +constexpr Transform2d::Transform2d(units::meter_t x, units::meter_t y, + Rotation2d rotation) + : m_translation(x, y), m_rotation(std::move(rotation)) {} + constexpr Transform2d Transform2d::Inverse() const { // We are rotating the difference between the translations // using a clockwise rotation matrix. This transforms the global diff --git a/wpimath/src/main/native/include/frc/geometry/Transform3d.h b/wpimath/src/main/native/include/frc/geometry/Transform3d.h index e9a5b2375f..9e4683fa1d 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform3d.h @@ -33,6 +33,18 @@ class WPILIB_DLLEXPORT Transform3d { */ Transform3d(Translation3d translation, Rotation3d rotation); + /** + * Constructs a transform with x, y, and z translations instead of a separate + * Translation3d. + * + * @param x The x component of the translational component of the transform. + * @param y The y component of the translational component of the transform. + * @param z The z component of the translational component of the transform. + * @param rotation The rotational component of the transform. + */ + Transform3d(units::meter_t x, units::meter_t y, units::meter_t z, + Rotation3d rotation); + /** * Constructs the identity transform -- maps an initial pose to itself. */