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 ab744a1408..ada3e9059d 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 @@ -83,6 +83,8 @@ public class Pose3d implements Interpolatable, 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); diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java index 6d12bbd185..8883e66538 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java +++ b/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. * 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 b14f2e0ad4..8332b607c2 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 @@ -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. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java index 5ea1ab2c02..48a70c7f45 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java +++ b/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. diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h index dba738f777..779d3522f6 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h +++ b/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}, diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h index 661a0875f2..8393f67558 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h +++ b/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. * diff --git a/wpimath/src/main/native/include/frc/geometry/Transform3d.h b/wpimath/src/main/native/include/frc/geometry/Transform3d.h index 690906aee6..4716b7181e 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform3d.h @@ -8,6 +8,7 @@ #include +#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. * diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h index 75d6fae503..fb3194b8ce 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h +++ b/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. *