[wpimath] Clean up Java Quaternion class (#4399)

Vector.norm() and Vector.dot() were added to make the implementation
simpler and match the C++ version more closely.

wpimath/src/main/java/edu/wpi/first/math/Vector.java

@@ -61,4 +61,35 @@ public class Vector<R extends Num> extends Matrix<R, N1> {
   public Vector<R> div(double value) {
     return new Vector<>(this.m_storage.divide(value));
   }
+
+  /**
+   * Returns the dot product of this vector with another.
+   *
+   * @param other The other vector.
+   * @return The dot product.
+   */
+  public double dot(Vector<R> other) {
+    double dot = 0.0;
+
+    for (int i = 0; i < getNumRows(); ++i) {
+      dot += get(i, 0) * other.get(i, 0);
+    }
+
+    return dot;
+  }
+
+  /**
+   * Returns the norm of this vector.
+   *
+   * @return The norm.
+   */
+  public double norm() {
+    double squaredNorm = 0.0;
+
+    for (int i = 0; i < getNumRows(); ++i) {
+      squaredNorm += get(i, 0) * get(i, 0);
+    }
+
+    return Math.sqrt(squaredNorm);
+  }
 }

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

@@ -45,20 +45,17 @@ public class Quaternion {
     final var r2 = other.m_r;
     final var v2 = other.m_v;
 
-    final var v1x = v1.get(0, 0);
-    final var v1y = v1.get(1, 0);
-    final var v1z = v1.get(2, 0);
-
-    final var v2x = v2.get(0, 0);
-    final var v2y = v2.get(1, 0);
-    final var v2z = v2.get(2, 0);
-
+    // v₁ x v₂
     var cross =
-        VecBuilder.fill(v1y * v2z - v2y * v1z, v2x * v1z - v1x * v2z, v1x * v2y - v2x * v1y);
-    double dot = v1x * v2x + v1y * v2y + v1z * v2z;
+        VecBuilder.fill(
+            v1.get(1, 0) * v2.get(2, 0) - v2.get(1, 0) * v1.get(2, 0),
+            v2.get(0, 0) * v1.get(2, 0) - v1.get(0, 0) * v2.get(2, 0),
+            v1.get(0, 0) * v2.get(1, 0) - v2.get(0, 0) * v1.get(1, 0));
 
+    // v = r₁v₂ + r₂v₁ + v₁ x v₂
     final var v = v2.times(r1).plus(v1.times(r2)).plus(cross);
-    return new Quaternion(r1 * r2 - dot, v.get(0, 0), v.get(1, 0), v.get(2, 0));
+
+    return new Quaternion(r1 * r2 - v1.dot(v2), v.get(0, 0), v.get(1, 0), v.get(2, 0));
   }
 
   @Override
@@ -78,20 +75,7 @@ public class Quaternion {
     if (obj instanceof Quaternion) {
       var other = (Quaternion) obj;
 
-      final var r1 = m_r;
-      final var v1 = m_v;
-      final var r2 = other.m_r;
-      final var v2 = other.m_v;
-
-      final var v1x = v1.get(0, 0);
-      final var v1y = v1.get(1, 0);
-      final var v1z = v1.get(2, 0);
-
-      final var v2x = v2.get(0, 0);
-      final var v2y = v2.get(1, 0);
-      final var v2z = v2.get(2, 0);
-
-      return Math.abs(r1 * r2 + v1x * v2x + v1y * v2y + v1z * v2z) > 1.0 - 1E-9;
+      return Math.abs(m_r * other.m_r + m_v.dot(other.m_v)) > 1.0 - 1E-9;
     }
     return false;
   }
@@ -172,7 +156,7 @@ public class Quaternion {
     // Sound State Representation through Encapsulation of Manifolds"
     //
     // https://arxiv.org/pdf/1107.1119.pdf
-    double norm = m_v.normF();
+    double norm = m_v.norm();
 
     if (norm < 1e-9) {
       return m_v.times(2.0 / getW() - 2.0 / 3.0 * norm * norm / (getW() * getW() * getW()));

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

@@ -64,7 +64,7 @@ public class Rotation3d implements Interpolatable<Rotation3d> {
    * @param angleRadians The rotation around the axis in radians.
    */
   public Rotation3d(Vector<N3> axis, double angleRadians) {
-    double norm = axis.normF();
+    double norm = axis.norm();
     if (norm == 0.0) {
       return;
     }

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

@@ -16,11 +16,14 @@ Quaternion Quaternion::operator*(const Quaternion& other) const {
   const auto& r2 = other.m_r;
   const auto& v2 = other.m_v;
 
+  // v₁ x v₂
   Eigen::Vector3d cross{v1(1) * v2(2) - v2(1) * v1(2),
                         v2(0) * v1(2) - v1(0) * v2(2),
                         v1(0) * v2(1) - v2(0) * v1(1)};
 
+  // v = r₁v₂ + r₂v₁ + v₁ x v₂
   Eigen::Vector3d v = r1 * v2 + r2 * v1 + cross;
+
   return Quaternion{r1 * r2 - v1.dot(v2), v(0), v(1), v(2)};
 }
 

wpimath/src/test/java/edu/wpi/first/math/VectorTest.java

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class VectorTest {
+  @Test
+  void testVectorDot() {
+    var vec1 = VecBuilder.fill(1.0, 2.0, 3.0);
+    var vec2 = VecBuilder.fill(4.0, 5.0, 6.0);
+
+    assertEquals(32.0, vec1.dot(vec2));
+
+    var vec3 = VecBuilder.fill(-1.0, 2.0, -3.0);
+    var vec4 = VecBuilder.fill(4.0, -5.0, 6.0);
+
+    assertEquals(-32.0, vec3.dot(vec4));
+  }
+
+  @Test
+  void testVectorNorm() {
+    assertEquals(Math.sqrt(14.0), VecBuilder.fill(1.0, 2.0, 3.0).norm());
+    assertEquals(Math.sqrt(14.0), VecBuilder.fill(1.0, -2.0, 3.0).norm());
+  }
+}