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

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

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