[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.
    *

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

@@ -13,6 +13,7 @@
 #include <wpi/json_fwd.h>
 
 #include "frc/geometry/Rotation2d.h"
+#include "units/area.h"
 #include "units/length.h"
 #include "units/math.h"
 
@@ -75,6 +76,23 @@ class WPILIB_DLLEXPORT Translation2d {
     return units::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 Distance(Translation2d), but
+   * avoids computing a square root.
+   *
+   * 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.
+   */
+  constexpr units::square_meter_t SquaredDistance(
+      const Translation2d& other) const {
+    return units::math::pow<2>(other.m_x - m_x) +
+           units::math::pow<2>(other.m_y - m_y);
+  }
+
   /**
    * Returns the X component of the translation.
    *
@@ -105,6 +123,17 @@ class WPILIB_DLLEXPORT Translation2d {
    */
   constexpr units::meter_t Norm() const { return units::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 Norm(), but
+   * avoids computing a square root.
+   *
+   * @return The squared norm of the translation.
+   */
+  constexpr units::square_meter_t SquaredNorm() const {
+    return units::math::pow<2>(m_x) + units::math::pow<2>(m_y);
+  }
+
   /**
    * Returns the angle this translation forms with the positive X axis.
    *
@@ -157,6 +186,32 @@ class WPILIB_DLLEXPORT Translation2d {
                 other.Y()};
   }
 
+  /**
+   * Computes the dot product between this translation and another translation
+   * in 2D space.
+   *
+   * 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.
+   */
+  constexpr units::square_meter_t Dot(const Translation2d& other) const {
+    return m_x * other.X() + m_y * other.Y();
+  }
+
+  /**
+   * Computes the cross product between this translation and another translation
+   * in 2D space.
+   *
+   * 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.
+   */
+  constexpr units::square_meter_t Cross(const Translation2d& other) const {
+    return m_x * other.Y() - m_y * other.X();
+  }
+
   /**
    * Returns the sum of two translations in 2D space.
    *

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

@@ -14,6 +14,7 @@
 
 #include "frc/geometry/Rotation3d.h"
 #include "frc/geometry/Translation2d.h"
+#include "units/area.h"
 #include "units/length.h"
 #include "units/math.h"
 
@@ -96,6 +97,24 @@ class WPILIB_DLLEXPORT Translation3d {
                              units::math::pow<2>(other.m_z - m_z));
   }
 
+  /**
+   * Calculates the squared distance between two translations in 3D space. This
+   * is equivalent to squaring the result of Distance(Translation3d), but avoids
+   * computing a square root.
+   *
+   * 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.
+   */
+  constexpr units::square_meter_t SquaredDistance(
+      const Translation3d& other) const {
+    return units::math::pow<2>(other.m_x - m_x) +
+           units::math::pow<2>(other.m_y - m_y) +
+           units::math::pow<2>(other.m_z - m_z);
+  }
+
   /**
    * Returns the X component of the translation.
    *
@@ -135,6 +154,17 @@ class WPILIB_DLLEXPORT Translation3d {
     return units::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 Norm(), but
+   * avoids computing a square root.
+   *
+   * @return The squared norm of the translation.
+   */
+  constexpr units::square_meter_t SquaredNorm() const {
+    return m_x * m_x + m_y * m_y + m_z * m_z;
+  }
+
   /**
    * Applies a rotation to the translation in 3D space.
    *
@@ -164,6 +194,38 @@ class WPILIB_DLLEXPORT Translation3d {
     return (*this - other).RotateBy(rot) + other;
   }
 
+  /**
+   * Computes the dot product between this translation and another translation
+   * in 3D space.
+   *
+   * 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.
+   */
+  constexpr units::square_meter_t Dot(const Translation3d& other) const {
+    return m_x * other.X() + m_y * other.Y() + m_z * other.Z();
+  }
+
+  /**
+   * Computes the cross product between this translation and another
+   * translation in 3D space. The resulting translation will be perpendicular to
+   * both translations.
+   *
+   * The 3D cross product between two translations is defined as <y₁z₂-y₂z₁,
+   * z₁x₂-z₂x₁, x₁y₂-x₂y₁>.
+   *
+   * @param other The translation to compute the cross product with.
+   * @return The cross product between the two translations.
+   */
+  constexpr Eigen::Vector<units::square_meter_t, 3> Cross(
+      const Translation3d& other) const {
+    return Eigen::Vector<units::square_meter_t, 3>{
+        {m_y * other.Z() - other.Y() * m_z},
+        {m_z * other.X() - other.Z() * m_x},
+        {m_x * other.Y() - other.X() * m_y}};
+  }
+
   /**
    * Returns a Translation2d representing this Translation3d projected into the
    * X-Y plane.

wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java

@@ -86,6 +86,12 @@ class Translation2dTest {
     assertEquals(Math.hypot(3.0, 5.0), one.getNorm(), kEpsilon);
   }
 
+  @Test
+  void testSquaredNorm() {
+    var one = new Translation2d(3.0, 5.0);
+    assertEquals(34.0, one.getSquaredNorm(), kEpsilon);
+  }
+
   @Test
   void testDistance() {
     var one = new Translation2d(1, 1);
@@ -93,6 +99,13 @@ class Translation2dTest {
     assertEquals(5.0 * Math.sqrt(2.0), one.getDistance(two), kEpsilon);
   }
 
+  @Test
+  void testSquaredDistance() {
+    var one = new Translation2d(1, 1);
+    var two = new Translation2d(6, 6);
+    assertEquals(50.0, one.getSquaredDistance(two), kEpsilon);
+  }
+
   @Test
   void testUnaryMinus() {
     var original = new Translation2d(-4.5, 7);
@@ -154,4 +167,18 @@ class Translation2dTest {
 
     assertEquals(vec, translation.toVector());
   }
+
+  @Test
+  void testDot() {
+    var one = new Translation2d(2.0, 3.0);
+    var two = new Translation2d(3.0, 4.0);
+    assertEquals(18.0, one.dot(two), kEpsilon);
+  }
+
+  @Test
+  void testCross() {
+    var one = new Translation2d(2.0, 3.0);
+    var two = new Translation2d(3.0, 4.0);
+    assertEquals(-1.0, one.cross(two), kEpsilon);
+  }
 }

wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java

@@ -149,6 +149,12 @@ class Translation3dTest {
     assertEquals(Math.sqrt(83.0), one.getNorm(), kEpsilon);
   }
 
+  @Test
+  void testSquaredNorm() {
+    var one = new Translation3d(3.0, 5.0, 7.0);
+    assertEquals(83.0, one.getSquaredNorm(), kEpsilon);
+  }
+
   @Test
   void testDistance() {
     var one = new Translation3d(1.0, 1.0, 1.0);
@@ -156,6 +162,13 @@ class Translation3dTest {
     assertEquals(5.0 * Math.sqrt(3.0), one.getDistance(two), kEpsilon);
   }
 
+  @Test
+  void testSquaredDistance() {
+    var one = new Translation3d(1.0, 1.0, 1.0);
+    var two = new Translation3d(6.0, 6.0, 6.0);
+    assertEquals(75.0, one.getSquaredDistance(two), kEpsilon);
+  }
+
   @Test
   void testUnaryMinus() {
     var original = new Translation3d(-4.5, 7.0, 9.0);
@@ -225,4 +238,23 @@ class Translation3dTest {
     assertEquals(translation1, origin.nearest(List.of(translation1, translation2, translation3)));
     assertEquals(translation2, origin.nearest(List.of(translation4, translation2, translation3)));
   }
+
+  @Test
+  void testDot() {
+    var one = new Translation3d(1.0, 2.0, 3.0);
+    var two = new Translation3d(4.0, 5.0, 6.0);
+    assertEquals(32.0, one.dot(two));
+  }
+
+  @Test
+  void testCross() {
+    var one = new Translation3d(1.0, 2.0, 3.0);
+    var two = new Translation3d(4.0, 5.0, 6.0);
+
+    var cross = one.cross(two);
+    assertAll(
+        () -> assertEquals(-3.0, cross.get(0, 0), kEpsilon),
+        () -> assertEquals(6.0, cross.get(1, 0), kEpsilon),
+        () -> assertEquals(-3.0, cross.get(2, 0), kEpsilon));
+  }
 }

wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp

@@ -68,12 +68,23 @@ TEST(Translation2dTest, Norm) {
   EXPECT_DOUBLE_EQ(std::hypot(3.0, 5.0), one.Norm().value());
 }
 
+TEST(Translation2dTest, SquaredNorm) {
+  const Translation2d one{3_m, 5_m};
+  EXPECT_DOUBLE_EQ(34.0, one.SquaredNorm().value());
+}
+
 TEST(Translation2dTest, Distance) {
   const Translation2d one{1_m, 1_m};
   const Translation2d two{6_m, 6_m};
   EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), one.Distance(two).value());
 }
 
+TEST(Translation2dTest, SquaredDistance) {
+  const Translation2d one{1_m, 1_m};
+  const Translation2d two{6_m, 6_m};
+  EXPECT_DOUBLE_EQ(50.0, one.SquaredDistance(two).value());
+}
+
 TEST(Translation2dTest, UnaryMinus) {
   const Translation2d original{-4.5_m, 7_m};
   const auto inverted = -original;
@@ -162,3 +173,15 @@ TEST(Translation2dTest, Constexpr) {
   static_assert(multiplied.X() == 2_m);
   static_assert(divided.Y() == 1_m);
 }
+
+TEST(Translation2dTest, Dot) {
+  const Translation2d one{2_m, 3_m};
+  const Translation2d two{3_m, 4_m};
+  EXPECT_DOUBLE_EQ(18.0, one.Dot(two).value());
+}
+
+TEST(Translation2dTest, Cross) {
+  const Translation2d one{2_m, 3_m};
+  const Translation2d two{3_m, 4_m};
+  EXPECT_DOUBLE_EQ(-1.0, one.Cross(two).value());
+}

wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp

@@ -114,12 +114,23 @@ TEST(Translation3dTest, Norm) {
   EXPECT_NEAR(one.Norm().value(), std::hypot(3, 5, 7), kEpsilon);
 }
 
+TEST(Translation3dTest, SquaredNorm) {
+  const Translation3d one{3_m, 5_m, 7_m};
+  EXPECT_NEAR(one.SquaredNorm().value(), 83.0, kEpsilon);
+}
+
 TEST(Translation3dTest, Distance) {
   const Translation3d one{1_m, 1_m, 1_m};
   const Translation3d two{6_m, 6_m, 6_m};
   EXPECT_NEAR(one.Distance(two).value(), 5 * std::sqrt(3), kEpsilon);
 }
 
+TEST(Translation3dTest, SquaredDistance) {
+  const Translation3d one{1_m, 1_m, 1_m};
+  const Translation3d two{6_m, 6_m, 6_m};
+  EXPECT_NEAR(one.SquaredDistance(two).value(), 75.0, kEpsilon);
+}
+
 TEST(Translation3dTest, UnaryMinus) {
   const Translation3d original{-4.5_m, 7_m, 9_m};
   const auto inverted = -original;
@@ -214,3 +225,19 @@ TEST(Translation3dTest, Nearest) {
   EXPECT_DOUBLE_EQ(nearest3.Y().value(), translation2.Y().value());
   EXPECT_DOUBLE_EQ(nearest3.Z().value(), translation2.Z().value());
 }
+
+TEST(Translation3dTest, Dot) {
+  const Translation3d one{1_m, 2_m, 3_m};
+  const Translation3d two{4_m, 5_m, 6_m};
+  EXPECT_NEAR(one.Dot(two).value(), 32.0, kEpsilon);
+}
+
+TEST(Translation3dTest, Cross) {
+  const Translation3d one{1_m, 2_m, 3_m};
+  const Translation3d two{4_m, 5_m, 6_m};
+
+  auto cross = one.Cross(two);
+  EXPECT_NEAR(cross[0].value(), -3.0, kEpsilon);
+  EXPECT_NEAR(cross[1].value(), 6.0, kEpsilon);
+  EXPECT_NEAR(cross[2].value(), -3.0, kEpsilon);
+}