[wpimath] Add 3D geometry classes (#4175)

Also clean up 2D geometry documentation.

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

@@ -17,13 +17,12 @@ class json;
 namespace frc {
 
 /**
- * Represents a 2d pose containing translational and rotational elements.
+ * Represents a 2D pose containing translational and rotational elements.
  */
 class WPILIB_DLLEXPORT Pose2d {
  public:
   /**
    * Constructs a pose at the origin facing toward the positive X axis.
-   * (Translation2d{0, 0} and Rotation{0})
    */
   constexpr Pose2d() = default;
 
@@ -36,8 +35,8 @@ class WPILIB_DLLEXPORT Pose2d {
   Pose2d(Translation2d translation, Rotation2d rotation);
 
   /**
-   * Convenience constructors that takes in x and y values directly instead of
-   * having to construct a Translation2d.
+   * Constructs a pose with x and y translations instead of a separate
+   * Translation2d.
    *
    * @param x The x component of the translational component of the pose.
    * @param y The y component of the translational component of the pose.
@@ -49,9 +48,11 @@ class WPILIB_DLLEXPORT Pose2d {
    * Transforms the pose by the given transformation and returns the new
    * transformed pose.
    *
+   * <pre>
    * [x_new]    [cos, -sin, 0][transform.x]
    * [y_new] += [sin,  cos, 0][transform.y]
-   * [t_new]    [0,    0,   1][transform.t]
+   * [t_new]    [  0,    0, 1][transform.t]
+   * </pre>
    *
    * @param other The transform to transform the pose by.
    *
@@ -152,7 +153,7 @@ class WPILIB_DLLEXPORT Pose2d {
    * @param twist The change in pose in the robot's coordinate frame since the
    * previous pose update. For example, if a non-holonomic robot moves forward
    * 0.01 meters and changes angle by 0.5 degrees since the previous pose
-   * update, the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
+   * update, the twist would be Twist2d{0.01_m, 0_m, 0.5_deg}.
    *
    * @return The new pose of the robot.
    */

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

@@ -0,0 +1,180 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "Pose2d.h"
+#include "Transform3d.h"
+#include "Translation3d.h"
+#include "Twist3d.h"
+
+namespace frc {
+
+/**
+ * Represents a 3D pose containing translational and rotational elements.
+ */
+class WPILIB_DLLEXPORT Pose3d {
+ public:
+  /**
+   * Constructs a pose at the origin facing toward the positive X axis.
+   */
+  constexpr Pose3d() = default;
+
+  /**
+   * Constructs a pose with the specified translation and rotation.
+   *
+   * @param translation The translational component of the pose.
+   * @param rotation The rotational component of the pose.
+   */
+  Pose3d(Translation3d translation, Rotation3d rotation);
+
+  /**
+   * Constructs a pose with x, y, and z translations instead of a separate
+   * Translation3d.
+   *
+   * @param x The x component of the translational component of the pose.
+   * @param y The y component of the translational component of the pose.
+   * @param z The z component of the translational component of the pose.
+   * @param rotation The rotational component of the pose.
+   */
+  Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
+         Rotation3d rotation);
+
+  /**
+   * Transforms the pose by the given transformation and returns the new
+   * transformed pose.
+   *
+   * @param other The transform to transform the pose by.
+   *
+   * @return The transformed pose.
+   */
+  Pose3d operator+(const Transform3d& other) const;
+
+  /**
+   * Returns the Transform3d that maps the one pose to another.
+   *
+   * @param other The initial pose of the transformation.
+   * @return The transform that maps the other pose to the current pose.
+   */
+  Transform3d operator-(const Pose3d& other) const;
+
+  /**
+   * Checks equality between this Pose3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const Pose3d& other) const;
+
+  /**
+   * Checks inequality between this Pose3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const Pose3d& other) const;
+
+  /**
+   * Returns the underlying translation.
+   *
+   * @return Reference to the translational component of the pose.
+   */
+  const Translation3d& Translation() const { return m_translation; }
+
+  /**
+   * Returns the X component of the pose's translation.
+   *
+   * @return The x component of the pose's translation.
+   */
+  units::meter_t X() const { return m_translation.X(); }
+
+  /**
+   * Returns the Y component of the pose's translation.
+   *
+   * @return The y component of the pose's translation.
+   */
+  units::meter_t Y() const { return m_translation.Y(); }
+
+  /**
+   * Returns the Z component of the pose's translation.
+   *
+   * @return The z component of the pose's translation.
+   */
+  units::meter_t Z() const { return m_translation.Z(); }
+
+  /**
+   * Returns the underlying rotation.
+   *
+   * @return Reference to the rotational component of the pose.
+   */
+  const Rotation3d& Rotation() const { return m_rotation; }
+
+  /**
+   * Transforms the pose by the given transformation and returns the new pose.
+   * See + operator for the matrix multiplication performed.
+   *
+   * @param other The transform to transform the pose by.
+   *
+   * @return The transformed pose.
+   */
+  Pose3d TransformBy(const Transform3d& other) const;
+
+  /**
+   * Returns the other pose relative to the current pose.
+   *
+   * This function can often be used for trajectory tracking or pose
+   * stabilization algorithms to get the error between the reference and the
+   * current pose.
+   *
+   * @param other The pose that is the origin of the new coordinate frame that
+   * the current pose will be converted into.
+   *
+   * @return The current pose relative to the new origin pose.
+   */
+  Pose3d RelativeTo(const Pose3d& other) const;
+
+  /**
+   * Obtain a new Pose3d from a (constant curvature) velocity.
+   *
+   * The twist is a change in pose in the robot's coordinate frame since the
+   * previous pose update. When the user runs exp() on the previous known
+   * field-relative pose with the argument being the twist, the user will
+   * receive the new field-relative pose.
+   *
+   * "Exp" represents the pose exponential, which is solving a differential
+   * equation moving the pose forward in time.
+   *
+   * @param twist The change in pose in the robot's coordinate frame since the
+   * previous pose update. For example, if a non-holonomic robot moves forward
+   * 0.01 meters and changes angle by 0.5 degrees since the previous pose
+   * update, the twist would be Twist3d{0.01_m, 0_m, 0_m, Rotation3d{0.0, 0.0,
+   * 0.5_deg}}.
+   *
+   * @return The new pose of the robot.
+   */
+  Pose3d Exp(const Twist3d& twist) const;
+
+  /**
+   * Returns a Twist3d that maps this pose to the end pose. If c is the output
+   * of a.Log(b), then a.Exp(c) would yield b.
+   *
+   * @param end The end pose for the transformation.
+   *
+   * @return The twist that maps this to end.
+   */
+  Twist3d Log(const Pose3d& end) const;
+
+  /**
+   * Returns a Pose2d representing this Pose3d projected into the X-Y plane.
+   */
+  Pose2d ToPose2d() const;
+
+ private:
+  Translation3d m_translation;
+  Rotation3d m_rotation;
+};
+
+}  // namespace frc

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

@@ -0,0 +1,95 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/EigenCore.h"
+
+namespace frc {
+
+class WPILIB_DLLEXPORT Quaternion {
+ public:
+  /**
+   * Constructs a quaternion with a default angle of 0 degrees.
+   */
+  Quaternion() = default;
+
+  /**
+   * Constructs a quaternion with the given components.
+   *
+   * @param w W component of the quaternion.
+   * @param x X component of the quaternion.
+   * @param y Y component of the quaternion.
+   * @param z Z component of the quaternion.
+   */
+  Quaternion(double w, double x, double y, double z);
+
+  /**
+   * Multiply with another quaternion.
+   *
+   * @param other The other quaternion.
+   */
+  Quaternion operator*(const Quaternion& other) const;
+
+  /**
+   * Checks equality between this Quaternion and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const Quaternion& other) const;
+
+  /**
+   * Checks inequality between this Quaternion and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const Quaternion& other) const;
+
+  /**
+   * Returns the inverse of the quaternion.
+   */
+  Quaternion Inverse() const;
+
+  /**
+   * Normalizes the quaternion.
+   */
+  Quaternion Normalize() const;
+
+  /**
+   * Returns W component of the quaternion.
+   */
+  double W() const;
+
+  /**
+   * Returns X component of the quaternion.
+   */
+  double X() const;
+
+  /**
+   * Returns Y component of the quaternion.
+   */
+  double Y() const;
+
+  /**
+   * Returns Z component of the quaternion.
+   */
+  double Z() const;
+
+  /**
+   * Returns the rotation vector representation of this quaternion.
+   *
+   * This is also the log operator of SO(3).
+   */
+  Eigen::Vector3d ToRotationVector() const;
+
+ private:
+  double m_r = 1.0;
+  Eigen::Vector3d m_v{0.0, 0.0, 0.0};
+};
+
+}  // namespace frc

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

@@ -15,7 +15,7 @@ class json;
 namespace frc {
 
 /**
- * A rotation in a 2d coordinate frame represented by a point on the unit circle
+ * A rotation in a 2D coordinate frame represented by a point on the unit circle
  * (cosine and sine).
  */
 class WPILIB_DLLEXPORT Rotation2d {

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

@@ -0,0 +1,153 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "Quaternion.h"
+#include "Rotation2d.h"
+#include "frc/EigenCore.h"
+#include "units/angle.h"
+
+namespace frc {
+
+/**
+ * A rotation in a 3D coordinate frame.
+ */
+class WPILIB_DLLEXPORT Rotation3d {
+ public:
+  /**
+   * Constructs a Rotation3d with a default angle of 0 degrees.
+   */
+  Rotation3d() = default;
+
+  /**
+   * Constructs a Rotation3d from a quaternion.
+   *
+   * @param q The quaternion.
+   */
+  explicit Rotation3d(const Quaternion& q);
+
+  /**
+   * Constructs a Rotation3d from extrinsic roll, pitch, and yaw.
+   *
+   * Extrinsic rotations occur in that order around the axes in the fixed global
+   * frame rather than the body frame.
+   *
+   * @param roll The counterclockwise rotation angle around the X axis (roll).
+   * @param pitch The counterclockwise rotation angle around the Y axis (pitch).
+   * @param yaw The counterclockwise rotation angle around the Z axis (yaw).
+   */
+  Rotation3d(units::radian_t roll, units::radian_t pitch, units::radian_t yaw);
+
+  /**
+   * Constructs a Rotation3d with the given axis-angle representation. The axis
+   * doesn't have to be normalized.
+   *
+   * @param axis The rotation axis.
+   * @param angle The rotation around the axis.
+   */
+  Rotation3d(const Vectord<3>& axis, units::radian_t angle);
+
+  /**
+   * Adds two rotations together.
+   *
+   * @param other The rotation to add.
+   *
+   * @return The sum of the two rotations.
+   */
+  Rotation3d operator+(const Rotation3d& other) const;
+
+  /**
+   * Subtracts the new rotation from the current rotation and returns the new
+   * rotation.
+   *
+   * @param other The rotation to subtract.
+   *
+   * @return The difference between the two rotations.
+   */
+  Rotation3d operator-(const Rotation3d& other) const;
+
+  /**
+   * Takes the inverse of the current rotation.
+   *
+   * @return The inverse of the current rotation.
+   */
+  Rotation3d operator-() const;
+
+  /**
+   * Multiplies the current rotation by a scalar.
+   * @param scalar The scalar.
+   *
+   * @return The new scaled Rotation3d.
+   */
+  Rotation3d operator*(double scalar) const;
+
+  /**
+   * Checks equality between this Rotation3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const Rotation3d& other) const;
+
+  /**
+   * Checks inequality between this Rotation3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const Rotation3d& other) const;
+
+  /**
+   * Adds the new rotation to the current rotation.
+   *
+   * @param other The rotation to rotate by.
+   *
+   * @return The new rotated Rotation3d.
+   */
+  Rotation3d RotateBy(const Rotation3d& other) const;
+
+  /**
+   * Returns the quaternion representation of the Rotation3d.
+   */
+  const Quaternion& GetQuaternion() const;
+
+  /**
+   * Returns the counterclockwise rotation angle around the X axis (roll).
+   */
+  units::radian_t X() const;
+
+  /**
+   * Returns the counterclockwise rotation angle around the Y axis (pitch).
+   */
+  units::radian_t Y() const;
+
+  /**
+   * Returns the counterclockwise rotation angle around the Z axis (yaw).
+   */
+  units::radian_t Z() const;
+
+  /**
+   * Returns the axis in the axis-angle representation of this rotation.
+   */
+  Vectord<3> Axis() const;
+
+  /**
+   * Returns the angle in the axis-angle representation of this rotation.
+   */
+  units::radian_t Angle() const;
+
+  /**
+   * Returns a Rotation2d representing this Rotation3d projected into the X-Y
+   * plane.
+   */
+  Rotation2d ToRotation2d() const;
+
+ private:
+  Quaternion m_q;
+};
+
+}  // namespace frc

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

@@ -0,0 +1,121 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "Translation3d.h"
+
+namespace frc {
+
+class WPILIB_DLLEXPORT Pose3d;
+
+/**
+ * Represents a transformation for a Pose3d.
+ */
+class WPILIB_DLLEXPORT Transform3d {
+ public:
+  /**
+   * Constructs the transform that maps the initial pose to the final pose.
+   *
+   * @param initial The initial pose for the transformation.
+   * @param final The final pose for the transformation.
+   */
+  Transform3d(Pose3d initial, Pose3d final);
+
+  /**
+   * Constructs a transform with the given translation and rotation components.
+   *
+   * @param translation Translational component of the transform.
+   * @param rotation Rotational component of the transform.
+   */
+  Transform3d(Translation3d translation, Rotation3d rotation);
+
+  /**
+   * Constructs the identity transform -- maps an initial pose to itself.
+   */
+  constexpr Transform3d() = default;
+
+  /**
+   * Returns the translation component of the transformation.
+   *
+   * @return Reference to the translational component of the transform.
+   */
+  const Translation3d& Translation() const { return m_translation; }
+
+  /**
+   * Returns the X component of the transformation's translation.
+   *
+   * @return The x component of the transformation's translation.
+   */
+  units::meter_t X() const { return m_translation.X(); }
+
+  /**
+   * Returns the Y component of the transformation's translation.
+   *
+   * @return The y component of the transformation's translation.
+   */
+  units::meter_t Y() const { return m_translation.Y(); }
+
+  /**
+   * Returns the Z component of the transformation's translation.
+   *
+   * @return The z component of the transformation's translation.
+   */
+  units::meter_t Z() const { return m_translation.Z(); }
+
+  /**
+   * Returns the rotational component of the transformation.
+   *
+   * @return Reference to the rotational component of the transform.
+   */
+  const Rotation3d& Rotation() const { return m_rotation; }
+
+  /**
+   * Invert the transformation. This is useful for undoing a transformation.
+   *
+   * @return The inverted transformation.
+   */
+  Transform3d Inverse() const;
+
+  /**
+   * Scales the transform by the scalar.
+   *
+   * @param scalar The scalar.
+   * @return The scaled Transform3d.
+   */
+  Transform3d operator*(double scalar) const {
+    return Transform3d(m_translation * scalar, m_rotation * scalar);
+  }
+
+  /**
+   * Composes two transformations.
+   *
+   * @param other The transform to compose with this one.
+   * @return The composition of the two transformations.
+   */
+  Transform3d operator+(const Transform3d& other) const;
+
+  /**
+   * Checks equality between this Transform3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const Transform3d& other) const;
+
+  /**
+   * Checks inequality between this Transform3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const Transform3d& other) const;
+
+ private:
+  Translation3d m_translation;
+  Rotation3d m_rotation;
+};
+}  // namespace frc

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

@@ -16,12 +16,12 @@ class json;
 namespace frc {
 
 /**
- * Represents a translation in 2d space.
+ * Represents a translation in 2D space.
  * This object can be used to represent a point or a vector.
  *
  * This assumes that you are using conventional mathematical axes.
- * When the robot is placed on the origin, facing toward the X direction,
- * moving forward increases the X, whereas moving to the left increases the Y.
+ * When the robot is at the origin facing in the positive X direction, forward
+ * is positive X and left is positive Y.
  */
 class WPILIB_DLLEXPORT Translation2d {
  public:
@@ -49,10 +49,9 @@ class WPILIB_DLLEXPORT Translation2d {
   Translation2d(units::meter_t distance, const Rotation2d& angle);
 
   /**
-   * Calculates the distance between two translations in 2d space.
+   * Calculates the distance between two translations in 2D space.
    *
-   * This function uses the pythagorean theorem to calculate the distance.
-   * distance = std::sqrt((x2 - x1)^2 + (y2 - y1)^2)
+   * The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²).
    *
    * @param other The translation to compute the distance to.
    *
@@ -63,14 +62,14 @@ class WPILIB_DLLEXPORT Translation2d {
   /**
    * Returns the X component of the translation.
    *
-   * @return The x component of the translation.
+   * @return The X component of the translation.
    */
   units::meter_t X() const { return m_x; }
 
   /**
    * Returns the Y component of the translation.
    *
-   * @return The y component of the translation.
+   * @return The Y component of the translation.
    */
   units::meter_t Y() const { return m_y; }
 
@@ -82,16 +81,18 @@ class WPILIB_DLLEXPORT Translation2d {
   units::meter_t Norm() const;
 
   /**
-   * Applies a rotation to the translation in 2d space.
+   * Applies a rotation to the translation in 2D space.
    *
    * This multiplies the translation vector by a counterclockwise rotation
    * matrix of the given angle.
    *
+   * <pre>
    * [x_new]   [other.cos, -other.sin][x]
    * [y_new] = [other.sin,  other.cos][y]
+   * </pre>
    *
-   * For example, rotating a Translation2d of {2, 0} by 90 degrees will return a
-   * Translation2d of {0, 2}.
+   * For example, rotating a Translation2d of &lt;2, 0&gt; by 90 degrees will
+   * return a Translation2d of &lt;0, 2&gt;.
    *
    * @param other The rotation to rotate the translation by.
    *
@@ -100,11 +101,10 @@ class WPILIB_DLLEXPORT Translation2d {
   Translation2d RotateBy(const Rotation2d& other) const;
 
   /**
-   * Adds two translations in 2d space and returns the sum. This is similar to
-   * vector addition.
+   * Returns the sum of two translations in 2D space.
    *
-   * For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} =
-   * Translation2d{3.0, 8.0}
+   * For example, Translation3d{1.0, 2.5} + Translation3d{2.0, 5.5} =
+   * Translation3d{3.0, 8.0}.
    *
    * @param other The translation to add.
    *
@@ -113,11 +113,10 @@ class WPILIB_DLLEXPORT Translation2d {
   Translation2d operator+(const Translation2d& other) const;
 
   /**
-   * Subtracts the other translation from the other translation and returns the
-   * difference.
+   * Returns the difference between two translations.
    *
    * For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} =
-   * Translation2d{4.0, 2.0}
+   * Translation2d{4.0, 2.0}.
    *
    * @param other The translation to subtract.
    *
@@ -127,17 +126,17 @@ class WPILIB_DLLEXPORT Translation2d {
 
   /**
    * Returns the inverse of the current translation. This is equivalent to
-   * rotating by 180 degrees, flipping the point over both axes, or simply
-   * negating both components of the translation.
+   * rotating by 180 degrees, flipping the point over both axes, or negating all
+   * components of the translation.
    *
    * @return The inverse of the current translation.
    */
   Translation2d operator-() const;
 
   /**
-   * Multiplies the translation by a scalar and returns the new translation.
+   * Returns the translation multiplied by a scalar.
    *
-   * For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
+   * For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}.
    *
    * @param scalar The scalar to multiply by.
    *
@@ -146,9 +145,9 @@ class WPILIB_DLLEXPORT Translation2d {
   Translation2d operator*(double scalar) const;
 
   /**
-   * Divides the translation by a scalar and returns the new translation.
+   * Returns the translation divided by a scalar.
    *
-   * For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
+   * For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}.
    *
    * @param scalar The scalar to divide by.
    *

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

@@ -0,0 +1,185 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "Rotation3d.h"
+#include "Translation2d.h"
+#include "units/length.h"
+
+namespace frc {
+
+/**
+ * Represents a translation in 3D space.
+ * This object can be used to represent a point or a vector.
+ *
+ * This assumes that you are using conventional mathematical axes. When the
+ * robot is at the origin facing in the positive X direction, forward is
+ * positive X, left is positive Y, and up is positive Z.
+ */
+class WPILIB_DLLEXPORT Translation3d {
+ public:
+  /**
+   * Constructs a Translation3d with X, Y, and Z components equal to zero.
+   */
+  constexpr Translation3d() = default;
+
+  /**
+   * Constructs a Translation3d with the X, Y, and Z components equal to the
+   * provided values.
+   *
+   * @param x The x component of the translation.
+   * @param y The y component of the translation.
+   * @param z The z component of the translation.
+   */
+  Translation3d(units::meter_t x, units::meter_t y, units::meter_t z);
+
+  /**
+   * Constructs a Translation3d with the provided distance and angle. This is
+   * essentially converting from polar coordinates to Cartesian coordinates.
+   *
+   * @param distance The distance from the origin to the end of the translation.
+   * @param angle The angle between the x-axis and the translation vector.
+   */
+  Translation3d(units::meter_t distance, const Rotation3d& angle);
+
+  /**
+   * Calculates the distance between two translations in 3D space.
+   *
+   * The distance between translations is defined as
+   * √((x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)²).
+   *
+   * @param other The translation to compute the distance to.
+   *
+   * @return The distance between the two translations.
+   */
+  units::meter_t Distance(const Translation3d& other) const;
+
+  /**
+   * Returns the X component of the translation.
+   *
+   * @return The Z component of the translation.
+   */
+  units::meter_t X() const { return m_x; }
+
+  /**
+   * Returns the Y component of the translation.
+   *
+   * @return The Y component of the translation.
+   */
+  units::meter_t Y() const { return m_y; }
+
+  /**
+   * Returns the Z component of the translation.
+   *
+   * @return The Z component of the translation.
+   */
+  units::meter_t Z() const { return m_z; }
+
+  /**
+   * Returns the norm, or distance from the origin to the translation.
+   *
+   * @return The norm of the translation.
+   */
+  units::meter_t Norm() const;
+
+  /**
+   * Applies a rotation to the translation in 3D space.
+   *
+   * For example, rotating a Translation3d of &lt;2, 0, 0&gt; by 90 degrees
+   * around the Z axis will return a Translation3d of &lt;0, 2, 0&gt;.
+   *
+   * @param other The rotation to rotate the translation by.
+   *
+   * @return The new rotated translation.
+   */
+  Translation3d RotateBy(const Rotation3d& other) const;
+
+  /**
+   * Returns a Translation2d representing this Translation3d projected into the
+   * X-Y plane.
+   */
+  Translation2d ToTranslation2d() const;
+
+  /**
+   * Returns the sum of two translations in 3D space.
+   *
+   * For example, Translation3d{1.0, 2.5, 3.5} + Translation3d{2.0, 5.5, 7.5} =
+   * Translation3d{3.0, 8.0, 11.0}.
+   *
+   * @param other The translation to add.
+   *
+   * @return The sum of the translations.
+   */
+  Translation3d operator+(const Translation3d& other) const;
+
+  /**
+   * Returns the difference between two translations.
+   *
+   * For example, Translation3d{5.0, 4.0, 3.0} - Translation3d{1.0, 2.0, 3.0} =
+   * Translation3d{4.0, 2.0, 0.0}.
+   *
+   * @param other The translation to subtract.
+   *
+   * @return The difference between the two translations.
+   */
+  Translation3d operator-(const Translation3d& other) const;
+
+  /**
+   * Returns the inverse of the current translation. This is equivalent to
+   * negating all components of the translation.
+   *
+   * @return The inverse of the current translation.
+   */
+  Translation3d operator-() const;
+
+  /**
+   * Returns the translation multiplied by a scalar.
+   *
+   * For example, Translation3d{2.0, 2.5, 4.5} * 2 = Translation3d{4.0, 5.0,
+   * 9.0}.
+   *
+   * @param scalar The scalar to multiply by.
+   *
+   * @return The scaled translation.
+   */
+  Translation3d operator*(double scalar) const;
+
+  /**
+   * Returns the translation divided by a scalar.
+   *
+   * For example, Translation3d{2.0, 2.5, 4.5} / 2 = Translation3d{1.0, 1.25,
+   * 2.25}.
+   *
+   * @param scalar The scalar to divide by.
+   *
+   * @return The scaled translation.
+   */
+  Translation3d operator/(double scalar) const;
+
+  /**
+   * Checks equality between this Translation3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const Translation3d& other) const;
+
+  /**
+   * Checks inequality between this Translation3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const Translation3d& other) const;
+
+ private:
+  units::meter_t m_x = 0_m;
+  units::meter_t m_y = 0_m;
+  units::meter_t m_z = 0_m;
+};
+
+}  // namespace frc

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

@@ -12,9 +12,9 @@
 
 namespace frc {
 /**
- * A change in distance along arc since the last pose update. We can use ideas
- * from differential calculus to create new Pose2ds from a Twist2d and vise
- * versa.
+ * A change in distance along a 2D arc since the last pose update. We can use
+ * ideas from differential calculus to create new Pose2ds from a Twist2d and
+ * vise versa.
  *
  * A Twist can be used to represent a difference between two poses.
  */

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

@@ -0,0 +1,87 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Rotation3d.h"
+#include "units/angle.h"
+#include "units/length.h"
+#include "units/math.h"
+
+namespace frc {
+/**
+ * A change in distance along a 3D arc since the last pose update. We can use
+ * ideas from differential calculus to create new Pose3ds from a Twist3d and
+ * vise versa.
+ *
+ * A Twist can be used to represent a difference between two poses.
+ */
+struct WPILIB_DLLEXPORT Twist3d {
+  /**
+   * Linear "dx" component
+   */
+  units::meter_t dx = 0_m;
+
+  /**
+   * Linear "dy" component
+   */
+  units::meter_t dy = 0_m;
+
+  /**
+   * Linear "dz" component
+   */
+  units::meter_t dz = 0_m;
+
+  /**
+   * Rotation vector x component.
+   */
+  units::radian_t rx = 0_rad;
+
+  /**
+   * Rotation vector y component.
+   */
+  units::radian_t ry = 0_rad;
+
+  /**
+   * Rotation vector z component.
+   */
+  units::radian_t rz = 0_rad;
+
+  /**
+   * Checks equality between this Twist3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const Twist3d& other) const {
+    return units::math::abs(dx - other.dx) < 1E-9_m &&
+           units::math::abs(dy - other.dy) < 1E-9_m &&
+           units::math::abs(dz - other.dz) < 1E-9_m &&
+           units::math::abs(rx - other.rx) < 1E-9_rad &&
+           units::math::abs(ry - other.ry) < 1E-9_rad &&
+           units::math::abs(rz - other.rz) < 1E-9_rad;
+  }
+
+  /**
+   * Checks inequality between this Twist3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const Twist3d& other) const { return !operator==(other); }
+
+  /**
+   * Scale this by a given factor.
+   *
+   * @param factor The factor by which to scale.
+   * @return The scaled Twist3d.
+   */
+  Twist3d operator*(double factor) const {
+    return Twist3d{dx * factor, dy * factor, dz * factor,
+                   rx * factor, ry * factor, rz * factor};
+  }
+};
+}  // namespace frc