[wpimath] Add 2D to 3D geometry constructors (#7380)

wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java

@@ -83,6 +83,8 @@ public Pose3d(Distance x, Distance y, Distance z, Rotation3d rotation) {
    * Constructs a 3D pose from a 2D pose in the X-Y plane.
    *
    * @param pose The 2D pose.
+   * @see Rotation3d#Rotation3d(Rotation2d)
+   * @see Translation3d#Translation3d(Translation2d)
    */
   public Pose3d(Pose2d pose) {
     m_translation = new Translation3d(pose.getX(), pose.getY(), 0.0);

wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java

@@ -273,6 +273,17 @@ public Rotation3d(Vector<N3> initial, Vector<N3> last) {
     }
   }
 
+  /**
+   * Constructs a 3D rotation from a 2D rotation in the X-Y plane.
+   *
+   * @param rotation The 2D rotation.
+   * @see Pose3d#Pose3d(Pose2d)
+   * @see Transform3d#Transform3d(Transform2d)
+   */
+  public Rotation3d(Rotation2d rotation) {
+    this(0.0, 0.0, rotation.getRadians());
+  }
+
   /**
    * Adds two rotations together.
    *

wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java

@@ -86,6 +86,18 @@ public Transform3d() {
     m_rotation = Rotation3d.kZero;
   }
 
+  /**
+   * Constructs a 3D transform from a 2D transform in the X-Y plane.
+   *
+   * @param transform The 2D transform.
+   * @see Rotation3d#Rotation3d(Rotation2d)
+   * @see Translation3d#Translation3d(Translation2d)
+   */
+  public Transform3d(Transform2d transform) {
+    m_translation = new Translation3d(transform.getTranslation());
+    m_rotation = new Rotation3d(transform.getRotation());
+  }
+
   /**
    * Multiplies the transform by the scalar.
    *

wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java

@@ -92,6 +92,17 @@ public Translation3d(Distance x, Distance y, Distance z) {
     this(x.in(Meters), y.in(Meters), z.in(Meters));
   }
 
+  /**
+   * Constructs a 3D translation from a 2D translation in the X-Y plane.
+   *
+   * @param translation The 2D translation.
+   * @see Pose3d#Pose3d(Pose2d)
+   * @see Transform3d#Transform3d(Transform2d)
+   */
+  public Translation3d(Translation2d translation) {
+    this(translation.getX(), translation.getY(), 0.0);
+  }
+
   /**
    * Constructs a Translation3d from the provided translation vector's X, Y, and Z components. The
    * values are assumed to be in meters.

wpimath/src/main/native/include/frc/geometry/Pose3d.h

@@ -58,6 +58,8 @@ class WPILIB_DLLEXPORT Pose3d {
    * Constructs a 3D pose from a 2D pose in the X-Y plane.
    *
    * @param pose The 2D pose.
+   * @see Rotation3d(Rotation2d)
+   * @see Translation3d(Translation2d)
    */
   constexpr explicit Pose3d(const Pose2d& pose)
       : m_translation{pose.X(), pose.Y(), 0_m},

wpimath/src/main/native/include/frc/geometry/Rotation3d.h

@@ -229,6 +229,16 @@ class WPILIB_DLLEXPORT Rotation3d {
     }
   }
 
+  /**
+   * Constructs a 3D rotation from a 2D rotation in the X-Y plane.
+   *
+   * @param rotation The 2D rotation.
+   * @see Pose3d(Pose2d)
+   * @see Transform3d(Transform2d)
+   */
+  constexpr explicit Rotation3d(const Rotation2d& rotation)
+      : Rotation3d{0_rad, 0_rad, rotation.Radians()} {}
+
   /**
    * Adds two rotations together.
    *

wpimath/src/main/native/include/frc/geometry/Transform3d.h

@@ -8,6 +8,7 @@
 
 #include <wpi/SymbolExports.h>
 
+#include "frc/geometry/Transform2d.h"
 #include "frc/geometry/Translation3d.h"
 
 namespace frc {
@@ -55,6 +56,17 @@ class WPILIB_DLLEXPORT Transform3d {
    */
   constexpr Transform3d() = default;
 
+  /**
+   * Constructs a 3D transform from a 2D transform in the X-Y plane.
+   **
+   * @param transform The 2D transform.
+   * @see Rotation3d(Rotation2d)
+   * @see Translation3d(Translation2d)
+   */
+  constexpr explicit Transform3d(const frc::Transform2d& transform)
+      : m_translation{frc::Translation3d{transform.Translation()}},
+        m_rotation{frc::Rotation3d{transform.Rotation()}} {}
+
   /**
    * Returns the translation component of the transformation.
    *

wpimath/src/main/native/include/frc/geometry/Translation3d.h

@@ -66,6 +66,16 @@ class WPILIB_DLLEXPORT Translation3d {
         m_y{units::meter_t{vector.y()}},
         m_z{units::meter_t{vector.z()}} {}
 
+  /**
+   * Constructs a 3D translation from a 2D translation in the X-Y plane.
+   *
+   * @param translation The 2D translation.
+   * @see Pose3d(Pose2d)
+   * @see Transform3d(Transform2d)
+   */
+  constexpr explicit Translation3d(const Translation2d& translation)
+      : Translation3d{translation.X(), translation.Y(), 0_m} {}
+
   /**
    * Calculates the distance between two translations in 3D space.
    *