[wpimath] Expand Quaternion class with additional operators (#5600)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

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

@@ -52,6 +52,48 @@ public class Quaternion {
     m_z = z;
   }
 
+  /**
+   * Adds another quaternion to this quaternion entrywise.
+   *
+   * @param other The other quaternion.
+   * @return The quaternion sum.
+   */
+  public Quaternion plus(Quaternion other) {
+    return new Quaternion(
+        getW() + other.getW(), getX() + other.getX(), getY() + other.getY(), getZ() + other.getZ());
+  }
+
+  /**
+   * Subtracts another quaternion from this quaternion entrywise.
+   *
+   * @param other The other quaternion.
+   * @return The quaternion difference.
+   */
+  public Quaternion minus(Quaternion other) {
+    return new Quaternion(
+        getW() - other.getW(), getX() - other.getX(), getY() - other.getY(), getZ() - other.getZ());
+  }
+
+  /**
+   * Divides by a scalar.
+   *
+   * @param scalar The value to scale each component by.
+   * @return The scaled quaternion.
+   */
+  public Quaternion divide(double scalar) {
+    return new Quaternion(getW() / scalar, getX() / scalar, getY() / scalar, getZ() / scalar);
+  }
+
+  /**
+   * Multiplies with a scalar.
+   *
+   * @param scalar The value to scale each component by.
+   * @return The scaled quaternion.
+   */
+  public Quaternion times(double scalar) {
+    return new Quaternion(getW() * scalar, getX() * scalar, getY() * scalar, getZ() * scalar);
+  }
+
   /**
    * Multiply with another quaternion.
    *
@@ -96,12 +138,8 @@ public class Quaternion {
     if (obj instanceof Quaternion) {
       var other = (Quaternion) obj;
 
-      return Math.abs(
-              getW() * other.getW()
-                  + getX() * other.getX()
-                  + getY() * other.getY()
-                  + getZ() * other.getZ())
-          > 1.0 - 1E-9;
+      return Math.abs(dot(other) - norm() * other.norm()) < 1e-9
+          && Math.abs(norm() - other.norm()) < 1e-9;
     }
     return false;
   }
@@ -111,13 +149,45 @@ public class Quaternion {
     return Objects.hash(m_w, m_x, m_y, m_z);
   }
 
+  /**
+   * Returns the conjugate of the quaternion.
+   *
+   * @return The conjugate quaternion.
+   */
+  public Quaternion conjugate() {
+    return new Quaternion(getW(), -getX(), -getY(), -getZ());
+  }
+
+  /**
+   * Returns the elementwise product of two quaternions.
+   *
+   * @param other The other quaternion.
+   * @return The dot product of two quaternions.
+   */
+  public double dot(final Quaternion other) {
+    return getW() * other.getW()
+        + getX() * other.getX()
+        + getY() * other.getY()
+        + getZ() * other.getZ();
+  }
+
   /**
    * Returns the inverse of the quaternion.
    *
    * @return The inverse quaternion.
    */
   public Quaternion inverse() {
-    return new Quaternion(getW(), -getX(), -getY(), -getZ());
+    var norm = norm();
+    return conjugate().divide(norm * norm);
+  }
+
+  /**
+   * Calculates the L2 norm of the quaternion.
+   *
+   * @return The L2 norm.
+   */
+  public double norm() {
+    return Math.sqrt(dot(this));
   }
 
   /**
@@ -126,7 +196,7 @@ public class Quaternion {
    * @return The normalized quaternion.
    */
   public Quaternion normalize() {
-    double norm = Math.sqrt(getW() * getW() + getX() * getX() + getY() * getY() + getZ() * getZ());
+    double norm = norm();
     if (norm == 0.0) {
       return new Quaternion();
     } else {
@@ -134,6 +204,104 @@ public class Quaternion {
     }
   }
 
+  /**
+   * Rational power of a quaternion.
+   *
+   * @param t the power to raise this quaternion to.
+   * @return The quaternion power
+   */
+  public Quaternion pow(double t) {
+    // q^t = e^(ln(q^t)) = e^(t * ln(q))
+    return this.log().times(t).exp();
+  }
+
+  /**
+   * Matrix exponential of a quaternion.
+   *
+   * @param adjustment the "Twist" that will be applied to this quaternion.
+   * @return The quaternion product of exp(adjustment) * this
+   */
+  public Quaternion exp(Quaternion adjustment) {
+    return adjustment.exp().times(this);
+  }
+
+  /**
+   * Matrix exponential of a quaternion.
+   *
+   * <p>source: wpimath/algorithms.md
+   *
+   * <p>If this quaternion is in 𝖘𝖔(3) and you are looking for an element of SO(3), use {@link
+   * fromRotationVector}
+   *
+   * @return The Matrix exponential of this quaternion.
+   */
+  public Quaternion exp() {
+    var scalar = Math.exp(getW());
+
+    var axial_magnitude = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ());
+    var cosine = Math.cos(axial_magnitude);
+
+    double axial_scalar;
+
+    if (axial_magnitude < 1e-9) {
+      // Taylor series of sin(θ) / θ near θ = 0: 1 − θ²/6 + θ⁴/120 + O(n⁶)
+      var axial_magnitude_sq = axial_magnitude * axial_magnitude;
+      var axial_magnitude_sq_sq = axial_magnitude_sq * axial_magnitude_sq;
+      axial_scalar = 1.0 - axial_magnitude_sq / 6.0 + axial_magnitude_sq_sq / 120.0;
+    } else {
+      axial_scalar = Math.sin(axial_magnitude) / axial_magnitude;
+    }
+
+    return new Quaternion(
+        cosine * scalar,
+        getX() * axial_scalar * scalar,
+        getY() * axial_scalar * scalar,
+        getZ() * axial_scalar * scalar);
+  }
+
+  /**
+   * Log operator of a quaternion.
+   *
+   * @param end The quaternion to map this quaternion onto.
+   * @return The "Twist" that maps this quaternion to the argument.
+   */
+  public Quaternion log(Quaternion end) {
+    return end.times(this.inverse()).log();
+  }
+
+  /**
+   * The Log operator of a general quaternion.
+   *
+   * <p>source: wpimath/algorithms.md
+   *
+   * <p>If this quaternion is in SO(3) and you are looking for an element of 𝖘𝖔(3), use {@link
+   * toRotationVector}
+   *
+   * @return The logarithm of this quaternion.
+   */
+  public Quaternion log() {
+    var scalar = Math.log(norm());
+
+    var v_norm = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ());
+
+    var s_norm = getW() / norm();
+
+    if (Math.abs(s_norm + 1) < 1e-9) {
+      return new Quaternion(scalar, -Math.PI, 0, 0);
+    }
+
+    double v_scalar;
+
+    if (v_norm < 1e-9) {
+      // Taylor series expansion of atan2(y / x) / y around y = 0 => 1/x - y²/3*x³ + O(y⁴)
+      v_scalar = 1.0 / getW() - 1.0 / 3.0 * v_norm * v_norm / (getW() * getW() * getW());
+    } else {
+      v_scalar = Math.atan2(v_norm, getW()) / v_norm;
+    }
+
+    return new Quaternion(scalar, v_scalar * getX(), v_scalar * getY(), v_scalar * getZ());
+  }
+
   /**
    * Returns W component of the quaternion.
    *
@@ -174,6 +342,37 @@ public class Quaternion {
     return m_z;
   }
 
+  /**
+   * Returns the quaternion representation of this rotation vector.
+   *
+   * <p>This is also the exp operator of 𝖘𝖔(3).
+   *
+   * <p>source: wpimath/algorithms.md
+   *
+   * @param rvec The rotation vector.
+   * @return The quaternion representation of this rotation vector.
+   */
+  public static Quaternion fromRotationVector(Vector<N3> rvec) {
+    double theta = rvec.norm();
+
+    double cos = Math.cos(theta / 2);
+
+    double axial_scalar;
+
+    if (theta < 1e-9) {
+      // taylor series expansion of sin(θ/2) / θ = 1/2 - θ²/48 + O(θ⁴)
+      axial_scalar = 1.0 / 2.0 - theta * theta / 48.0;
+    } else {
+      axial_scalar = Math.sin(theta / 2) / theta;
+    }
+
+    return new Quaternion(
+        cos,
+        axial_scalar * rvec.get(0, 0),
+        axial_scalar * rvec.get(1, 0),
+        axial_scalar * rvec.get(2, 0));
+  }
+
   /**
    * Returns the rotation vector representation of this quaternion.
    *

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

@@ -420,7 +420,7 @@ public class Rotation3d implements Interpolatable<Rotation3d> {
   public boolean equals(Object obj) {
     if (obj instanceof Rotation3d) {
       var other = (Rotation3d) obj;
-      return m_q.equals(other.m_q);
+      return Math.abs(Math.abs(m_q.dot(other.m_q)) - m_q.norm() * other.m_q.norm()) < 1e-9;
     }
     return false;
   }

wpimath/src/main/native/cpp/geometry/Quaternion.cpp

@@ -4,6 +4,8 @@
 
 #include "frc/geometry/Quaternion.h"
 
+#include <numbers>
+
 #include <wpi/json.h>
 
 using namespace frc;
@@ -11,6 +13,42 @@ using namespace frc;
 Quaternion::Quaternion(double w, double x, double y, double z)
     : m_r{w}, m_v{x, y, z} {}
 
+Quaternion Quaternion::operator+(const Quaternion& other) const {
+  return Quaternion{
+      m_r + other.m_r,
+      m_v(0) + other.m_v(0),
+      m_v(1) + other.m_v(1),
+      m_v(2) + other.m_v(2),
+  };
+}
+
+Quaternion Quaternion::operator-(const Quaternion& other) const {
+  return Quaternion{
+      m_r - other.m_r,
+      m_v(0) - other.m_v(0),
+      m_v(1) - other.m_v(1),
+      m_v(2) - other.m_v(2),
+  };
+}
+
+Quaternion Quaternion::operator*(const double other) const {
+  return Quaternion{
+      m_r * other,
+      m_v(0) * other,
+      m_v(1) * other,
+      m_v(2) * other,
+  };
+}
+
+Quaternion Quaternion::operator/(const double other) const {
+  return Quaternion{
+      m_r / other,
+      m_v(0) / other,
+      m_v(1) / other,
+      m_v(2) / other,
+  };
+}
+
 Quaternion Quaternion::operator*(const Quaternion& other) const {
   // https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
   const auto& r1 = m_r;
@@ -33,22 +71,95 @@ Quaternion Quaternion::operator*(const Quaternion& other) const {
 }
 
 bool Quaternion::operator==(const Quaternion& other) const {
-  return std::abs(W() * other.W() + m_v.dot(other.m_v)) > 1.0 - 1E-9;
+  return std::abs(Dot(other) - Norm() * other.Norm()) < 1e-9 &&
+         std::abs(Norm() - other.Norm()) < 1e-9;
 }
 
-Quaternion Quaternion::Inverse() const {
+Quaternion Quaternion::Conjugate() const {
   return Quaternion{W(), -X(), -Y(), -Z()};
 }
 
+double Quaternion::Dot(const Quaternion& other) const {
+  return W() * other.W() + m_v.dot(other.m_v);
+}
+
+Quaternion Quaternion::Inverse() const {
+  double norm = Norm();
+  return Conjugate() / (norm * norm);
+}
+
+double Quaternion::Norm() const {
+  return std::sqrt(Dot(*this));
+}
+
 Quaternion Quaternion::Normalize() const {
-  double norm = std::sqrt(W() * W() + X() * X() + Y() * Y() + Z() * Z());
+  double norm = Norm();
   if (norm == 0.0) {
     return Quaternion{};
   } else {
-    return Quaternion{W() / norm, X() / norm, Y() / norm, Z() / norm};
+    return Quaternion{W(), X(), Y(), Z()} / norm;
   }
 }
 
+Quaternion Quaternion::Pow(const double other) const {
+  return (Log() * other).Exp();
+}
+
+Quaternion Quaternion::Exp(const Quaternion& other) const {
+  return other.Exp() * *this;
+}
+
+Quaternion Quaternion::Exp() const {
+  double scalar = std::exp(m_r);
+
+  double axial_magnitude = m_v.norm();
+  double cosine = std::cos(axial_magnitude);
+
+  double axial_scalar;
+
+  if (axial_magnitude < 1e-9) {
+    // Taylor series of sin(x)/x near x=0: 1 − x²/6 + x⁴/120 + O(n⁶)
+    double axial_magnitude_sq = axial_magnitude * axial_magnitude;
+    double axial_magnitude_sq_sq = axial_magnitude_sq * axial_magnitude_sq;
+    axial_scalar =
+        1.0 - axial_magnitude_sq / 6.0 + axial_magnitude_sq_sq / 120.0;
+  } else {
+    axial_scalar = std::sin(axial_magnitude) / axial_magnitude;
+  }
+
+  return Quaternion(cosine * scalar, X() * axial_scalar * scalar,
+                    Y() * axial_scalar * scalar, Z() * axial_scalar * scalar);
+}
+
+Quaternion Quaternion::Log(const Quaternion& other) const {
+  return (other * Inverse()).Log();
+}
+
+Quaternion Quaternion::Log() const {
+  double scalar = std::log(Norm());
+
+  double v_norm = m_v.norm();
+
+  double s_norm = W() / Norm();
+
+  if (std::abs(s_norm + 1) < 1e-9) {
+    return Quaternion{scalar, -std::numbers::pi, 0, 0};
+  }
+
+  double v_scalar;
+
+  if (v_norm < 1e-9) {
+    // Taylor series expansion of atan2(y / x) / y around y = 0 = 1/x -
+    // y^2/3*x^3 + O(y^4)
+    v_scalar = 1.0 / W() - 1.0 / 3.0 * v_norm * v_norm / (W() * W() * W());
+  } else {
+    v_scalar = std::atan2(v_norm, W()) / v_norm;
+  }
+
+  return Quaternion{scalar, v_scalar * m_v(0), v_scalar * m_v(1),
+                    v_scalar * m_v(2)};
+}
+
 double Quaternion::W() const {
   return m_r;
 }
@@ -83,6 +194,30 @@ Eigen::Vector3d Quaternion::ToRotationVector() const {
   }
 }
 
+Quaternion Quaternion::FromRotationVector(const Eigen::Vector3d& rvec) {
+  // 𝑣⃗ = θ * v̂
+  // v̂ = 𝑣⃗ / θ
+
+  // 𝑞 = std::cos(θ/2) + std::sin(θ/2) * v̂
+  // 𝑞 = std::cos(θ/2) + std::sin(θ/2) / θ * 𝑣⃗
+
+  double theta = rvec.norm();
+  double cos = std::cos(theta / 2);
+
+  double axial_scalar;
+
+  if (theta < 1e-9) {
+    // taylor series expansion of sin(θ/2) / θ around θ = 0 = 1/2 - θ²/48 +
+    // O(θ⁴)
+    axial_scalar = 1.0 / 2.0 - theta * theta / 48.0;
+  } else {
+    axial_scalar = std::sin(theta / 2) / theta;
+  }
+
+  return Quaternion{cos, axial_scalar * rvec(0), axial_scalar * rvec(1),
+                    axial_scalar * rvec(2)};
+}
+
 void frc::to_json(wpi::json& json, const Quaternion& quaternion) {
   json = wpi::json{{"W", quaternion.W()},
                    {"X", quaternion.X()},

wpimath/src/main/native/cpp/geometry/Rotation3d.cpp

@@ -174,6 +174,11 @@ Rotation3d Rotation3d::operator/(double scalar) const {
   return *this * (1.0 / scalar);
 }
 
+bool Rotation3d::operator==(const Rotation3d& other) const {
+  return std::abs(std::abs(m_q.Dot(other.m_q)) -
+                  m_q.Norm() * other.m_q.Norm()) < 1e-9;
+}
+
 Rotation3d Rotation3d::RotateBy(const Rotation3d& other) const {
   return Rotation3d{other.m_q * m_q};
 }

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

@@ -27,6 +27,34 @@ class WPILIB_DLLEXPORT Quaternion {
    */
   Quaternion(double w, double x, double y, double z);
 
+  /**
+   * Adds with another quaternion.
+   *
+   * @param other the other quaternion
+   */
+  Quaternion operator+(const Quaternion& other) const;
+
+  /**
+   * Subtracts another quaternion.
+   *
+   * @param other the other quaternion
+   */
+  Quaternion operator-(const Quaternion& other) const;
+
+  /**
+   * Multiples with a scalar value.
+   *
+   * @param other the scalar value
+   */
+  Quaternion operator*(const double other) const;
+
+  /**
+   * Divides by a scalar value.
+   *
+   * @param other the scalar value
+   */
+  Quaternion operator/(const double other) const;
+
   /**
    * Multiply with another quaternion.
    *
@@ -42,6 +70,16 @@ class WPILIB_DLLEXPORT Quaternion {
    */
   bool operator==(const Quaternion& other) const;
 
+  /**
+   * Returns the elementwise product of two quaternions.
+   */
+  double Dot(const Quaternion& other) const;
+
+  /**
+   * Returns the conjugate of the quaternion.
+   */
+  Quaternion Conjugate() const;
+
   /**
    * Returns the inverse of the quaternion.
    */
@@ -52,6 +90,52 @@ class WPILIB_DLLEXPORT Quaternion {
    */
   Quaternion Normalize() const;
 
+  /**
+   * Calculates the L2 norm of the quaternion.
+   */
+  double Norm() const;
+
+  /**
+   * Calculates this quaternion raised to a power.
+   *
+   * @param t the power to raise this quaternion to.
+   */
+  Quaternion Pow(const double t) const;
+
+  /**
+   * Matrix exponential of a quaternion.
+   *
+   * @param other the "Twist" that will be applied to this quaternion.
+   */
+  Quaternion Exp(const Quaternion& other) const;
+
+  /**
+   * Matrix exponential of a quaternion.
+   *
+   * source: wpimath/algorithms.md
+   *
+   *  If this quaternion is in 𝖘𝖔(3) and you are looking for an element of
+   * SO(3), use FromRotationVector
+   */
+  Quaternion Exp() const;
+
+  /**
+   * Log operator of a quaternion.
+   *
+   * @param other The quaternion to map this quaternion onto
+   */
+  Quaternion Log(const Quaternion& other) const;
+
+  /**
+   * Log operator of a quaternion.
+   *
+   * source:  wpimath/algorithms.md
+   *
+   * If this quaternion is in SO(3) and you are looking for an element of 𝖘𝖔(3),
+   * use ToRotationVector
+   */
+  Quaternion Log() const;
+
   /**
    * Returns W component of the quaternion.
    */
@@ -79,6 +163,15 @@ class WPILIB_DLLEXPORT Quaternion {
    */
   Eigen::Vector3d ToRotationVector() const;
 
+  /**
+   * Returns the quaternion representation of this rotation vector.
+   *
+   * This is also the exp operator of 𝖘𝖔(3).
+   *
+   * source: wpimath/algorithms.md
+   */
+  static Quaternion FromRotationVector(const Eigen::Vector3d& rvec);
+
  private:
   // Scalar r in versor form
   double m_r = 1.0;

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

@@ -132,7 +132,7 @@ class WPILIB_DLLEXPORT Rotation3d {
   /**
    * Checks equality between this Rotation3d and another object.
    */
-  bool operator==(const Rotation3d&) const = default;
+  bool operator==(const Rotation3d&) const;
 
   /**
    * Adds the new rotation to the current rotation. The other rotation is