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 7d13477495..5d3f479a97 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 @@ -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 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 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()); + public Vector toVector() { + return m_q.toRotationVector(); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java index c1ccd3c9a1..f79403ef92 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java @@ -125,7 +125,7 @@ public class Odometry3d { */ 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 = diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h index 1f377a8ce7..176f2c03ab 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h +++ b/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(); diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h index 47aa165b7c..f40be9b09c 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h +++ b/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. diff --git a/wpimath/src/main/native/include/frc/kinematics/Odometry3d.h b/wpimath/src/main/native/include/frc/kinematics/Odometry3d.h index ffdfb05bef..f24d6f86f6 100644 --- a/wpimath/src/main/native/include/frc/kinematics/Odometry3d.h +++ b/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 = - (angle - m_previousAngle).GetQuaternion().ToRotationVector(); + auto angle_difference = (angle - m_previousAngle).ToVector(); auto twist2d = m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions);