diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java index 803a3469cd..34dde8e767 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java @@ -48,6 +48,16 @@ public class Pose3d implements Interpolatable { m_rotation = rotation; } + /** + * Constructs a 3D pose from a 2D pose in the X-Y plane. + * + * @param pose The 2D pose. + */ + public Pose3d(Pose2d pose) { + m_translation = new Translation3d(pose.getX(), pose.getY(), 0.0); + m_rotation = new Rotation3d(0.0, 0.0, pose.getRotation().getRadians()); + } + /** * Transforms the pose by the given transformation and returns the new transformed pose. * diff --git a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp index f3bb96e42f..1f4cb6ae76 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp @@ -37,6 +37,10 @@ Pose3d::Pose3d(units::meter_t x, units::meter_t y, units::meter_t z, Rotation3d rotation) : m_translation(x, y, z), m_rotation(std::move(rotation)) {} +Pose3d::Pose3d(const Pose2d& pose) + : m_translation(pose.X(), pose.Y(), 0_m), + m_rotation(0_rad, 0_rad, pose.Rotation().Radians()) {} + Pose3d Pose3d::operator+(const Transform3d& other) const { return TransformBy(other); } diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h index 8e9633f968..b75e845d98 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h @@ -43,6 +43,13 @@ class WPILIB_DLLEXPORT Pose3d { Pose3d(units::meter_t x, units::meter_t y, units::meter_t z, Rotation3d rotation); + /** + * Constructs a 3D pose from a 2D pose in the X-Y plane. + * + * @param pose The 2D pose. + */ + explicit Pose3d(const Pose2d& pose); + /** * Transforms the pose by the given transformation and returns the new * transformed pose.