Merge branch 'main' into 2027

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