[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/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);
+}