[wpimath] Add Pose3d(Pose2d) constructor (#4485)

2026-06-20 00:51:42 +00:00 · 2022-10-20 17:23:00 -07:00
parent 9d5055176d
commit 16cdc741cf
3 changed files with 21 additions and 0 deletions
--- 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);
 }