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

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

@@ -83,6 +83,8 @@ public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, Str
    * 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 class Rotation3d
     }
   }
 
+  /**
+   * 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 class Transform3d implements ProtobufSerializable, StructSerializable {
     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 class Translation3d
     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.