[wpimath] Add vector product and squared length operations to Translation2d/3d (#8133)

Adds methods to compute the dot and cross products between Translation2ds and Translation3ds, as well as methods to compute the square of Distance and Norm, which allows avoiding some calls to sqrt in many cases.

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

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

@@ -109,6 +109,22 @@ public class Translation2d
     return Math.hypot(other.m_x - m_x, other.m_y - m_y);
   }
 
+  /**
+   * Calculates the square of the distance between two translations in 2D space. This is equivalent
+   * to squaring the result of {@link #getDistance(Translation2d)}, but avoids computing a square
+   * root.
+   *
+   * <p>The square of the distance between translations is defined as (x₂−x₁)²+(y₂−y₁)².
+   *
+   * @param other The translation to compute the squared distance to.
+   * @return The square of the distance between the two translations, in square meters.
+   */
+  public double getSquaredDistance(Translation2d other) {
+    double dx = other.m_x - m_x;
+    double dy = other.m_y - m_y;
+    return dx * dx + dy * dy;
+  }
+
   /**
    * Returns the X component of the translation.
    *
@@ -165,6 +181,16 @@ public class Translation2d
     return Math.hypot(m_x, m_y);
   }
 
+  /**
+   * Returns the squared norm, or squared distance from the origin to the translation. This is
+   * equivalent to squaring the result of {@link #getNorm()}, but avoids computing a square root.
+   *
+   * @return The squared norm of the translation, in square meters.
+   */
+  public double getSquaredNorm() {
+    return m_x * m_x + m_y * m_y;
+  }
+
   /**
    * Returns the angle this translation forms with the positive X axis.
    *
@@ -214,6 +240,30 @@ public class Translation2d
         (m_x - other.getX()) * rot.getSin() + (m_y - other.getY()) * rot.getCos() + other.getY());
   }
 
+  /**
+   * Computes the dot product between this translation and another translation in 2D space.
+   *
+   * <p>The dot product between two translations is defined as x₁x₂+y₁y₂.
+   *
+   * @param other The translation to compute the dot product with.
+   * @return The dot product between the two translations, in square meters.
+   */
+  public double dot(Translation2d other) {
+    return m_x * other.m_x + m_y * other.m_y;
+  }
+
+  /**
+   * Computes the cross product between this translation and another translation in 2D space.
+   *
+   * <p>The 2D cross product between two translations is defined as x₁y₂-x₂y₁.
+   *
+   * @param other The translation to compute the cross product with.
+   * @return The cross product between the two translations, in square meters.
+   */
+  public double cross(Translation2d other) {
+    return m_x * other.m_y - m_y * other.m_x;
+  }
+
   /**
    * Returns the sum of two translations in 2D space.
    *

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

@@ -125,8 +125,26 @@ public class Translation3d
    * @return The distance between the two translations.
    */
   public double getDistance(Translation3d other) {
-    return Math.sqrt(
-        Math.pow(other.m_x - m_x, 2) + Math.pow(other.m_y - m_y, 2) + Math.pow(other.m_z - m_z, 2));
+    double dx = other.m_x - m_x;
+    double dy = other.m_y - m_y;
+    double dz = other.m_z - m_z;
+    return Math.sqrt(dx * dx + dy * dy + dz * dz);
+  }
+
+  /**
+   * Calculates the squared distance between two translations in 3D space. This is equivalent to
+   * squaring the result of {@link #getDistance(Translation3d)}, but avoids computing a square root.
+   *
+   * <p>The squared distance between translations is defined as (x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)².
+   *
+   * @param other The translation to compute the squared distance to.
+   * @return The squared distance between the two translations.
+   */
+  public double getSquaredDistance(Translation3d other) {
+    double dx = other.m_x - m_x;
+    double dy = other.m_y - m_y;
+    double dz = other.m_z - m_z;
+    return dx * dx + dy * dy + dz * dz;
   }
 
   /**
@@ -204,6 +222,16 @@ public class Translation3d
     return Math.sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
   }
 
+  /**
+   * Returns the squared norm, or squared distance from the origin to the translation. This is
+   * equivalent to squaring the result of {@link #getNorm()}, but avoids computing a square root.
+   *
+   * @return The squared norm of the translation.
+   */
+  public double getSquaredNorm() {
+    return m_x * m_x + m_y * m_y + m_z * m_z;
+  }
+
   /**
    * Applies a rotation to the translation in 3D space.
    *
@@ -230,6 +258,35 @@ public class Translation3d
     return this.minus(other).rotateBy(rot).plus(other);
   }
 
+  /**
+   * Computes the dot product between this translation and another translation in 3D space.
+   *
+   * <p>The dot product between two translations is defined as x₁x₂+y₁y₂+z₁z₂.
+   *
+   * @param other The translation to compute the dot product with.
+   * @return The dot product between the two translations, in square meters.
+   */
+  public double dot(Translation3d other) {
+    return m_x * other.m_x + m_y * other.m_y + m_z * other.m_z;
+  }
+
+  /**
+   * Computes the cross product between this translation and another translation in 3D space. The
+   * resulting translation will be perpendicular to both translations.
+   *
+   * <p>The 3D cross product between two translations is defined as &lt;y₁z₂-y₂z₁, z₁x₂-z₂x₁,
+   * x₁y₂-x₂y₁&gt;.
+   *
+   * @param other The translation to compute the cross product with.
+   * @return The cross product between the two translations.
+   */
+  public Vector<N3> cross(Translation3d other) {
+    return VecBuilder.fill(
+        m_y * other.m_z - other.m_y * m_z,
+        m_z * other.m_x - other.m_z * m_x,
+        m_x * other.m_y - other.m_x * m_y);
+  }
+
   /**
    * Returns a Translation2d representing this Translation3d projected into the X-Y plane.
    *