[wpimath] Add Rotation3d rotation vector getter (#7564)

The code churn in Java is just making the function order consistent
between languages.

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

@@ -459,6 +459,32 @@ public class Rotation3d
     return Radians.of(getZ());
   }
 
+  /**
+   * Returns the axis in the axis-angle representation of this rotation.
+   *
+   * @return The axis in the axis-angle representation.
+   */
+  public Vector<N3> getAxis() {
+    double norm =
+        Math.sqrt(m_q.getX() * m_q.getX() + m_q.getY() * m_q.getY() + m_q.getZ() * m_q.getZ());
+    if (norm == 0.0) {
+      return VecBuilder.fill(0.0, 0.0, 0.0);
+    } else {
+      return VecBuilder.fill(m_q.getX() / norm, m_q.getY() / norm, m_q.getZ() / norm);
+    }
+  }
+
+  /**
+   * Returns the angle in radians in the axis-angle representation of this rotation.
+   *
+   * @return The angle in radians in the axis-angle representation of this rotation.
+   */
+  public double getAngle() {
+    double norm =
+        Math.sqrt(m_q.getX() * m_q.getX() + m_q.getY() * m_q.getY() + m_q.getZ() * m_q.getZ());
+    return 2.0 * Math.atan2(norm, m_q.getW());
+  }
+
   /**
    * Returns rotation matrix representation of this rotation.
    *
@@ -486,29 +512,12 @@ public class Rotation3d
   }
 
   /**
-   * Returns the axis in the axis-angle representation of this rotation.
+   * Returns rotation vector representation of this rotation.
    *
-   * @return The axis in the axis-angle representation.
+   * @return Rotation vector representation of this rotation.
    */
-  public Vector<N3> getAxis() {
+  public Vector<N3> toVector() {
-    double norm =
+    return m_q.toRotationVector();
-        Math.sqrt(m_q.getX() * m_q.getX() + m_q.getY() * m_q.getY() + m_q.getZ() * m_q.getZ());
-    if (norm == 0.0) {
-      return VecBuilder.fill(0.0, 0.0, 0.0);
-    } else {
-      return VecBuilder.fill(m_q.getX() / norm, m_q.getY() / norm, m_q.getZ() / norm);
-    }
-  }
-
-  /**
-   * Returns the angle in radians in the axis-angle representation of this rotation.
-   *
-   * @return The angle in radians in the axis-angle representation of this rotation.
-   */
-  public double getAngle() {
-    double norm =
-        Math.sqrt(m_q.getX() * m_q.getX() + m_q.getY() * m_q.getY() + m_q.getZ() * m_q.getZ());
-    return 2.0 * Math.atan2(norm, m_q.getW());
   }
 
   /**

wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java

@@ -125,7 +125,7 @@ public class Odometry3d<T> {
    */
   public Pose3d update(Rotation3d gyroAngle, T wheelPositions) {
     var angle = gyroAngle.plus(m_gyroOffset);
-    var angle_difference = angle.minus(m_previousAngle).getQuaternion().toRotationVector();
+    var angle_difference = angle.minus(m_previousAngle).toVector();
 
     var twist2d = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions);
     var twist =

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

@@ -404,7 +404,7 @@ constexpr Twist3d Pose3d::Log(const Pose3d& end) const {
 
     Vector3d u{
         {transform.X().value(), transform.Y().value(), transform.Z().value()}};
-    Vector3d rvec = transform.Rotation().GetQuaternion().ToRotationVector();
+    Vector3d rvec = transform.Rotation().ToVector();
     Matrix3d omega = detail::RotationVectorToMatrix(rvec);
     Matrix3d omegaSq = omega * omega;
     double theta = rvec.norm();

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

@@ -421,6 +421,13 @@ class WPILIB_DLLEXPORT Rotation3d {
                             1.0 - 2.0 * (x * x + y * y)}};
   }
 
+  /**
+   * Returns rotation vector representation of this rotation.
+   *
+   * @return Rotation vector representation of this rotation.
+   */
+  constexpr Eigen::Vector3d ToVector() const { return m_q.ToRotationVector(); }
+
   /**
    * Returns a Rotation2d representing this Rotation3d projected into the X-Y
    * plane.

wpimath/src/main/native/include/frc/kinematics/Odometry3d.h

@@ -120,8 +120,7 @@ class WPILIB_DLLEXPORT Odometry3d {
   const Pose3d& Update(const Rotation3d& gyroAngle,
                        const WheelPositions& wheelPositions) {
     auto angle = gyroAngle + m_gyroOffset;
-    auto angle_difference =
+    auto angle_difference = (angle - m_previousAngle).ToVector();
-        (angle - m_previousAngle).GetQuaternion().ToRotationVector();
 
     auto twist2d =
         m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions);