[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.
    */