diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
index 1612a5e5fa..10c7c123bd 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
+++ b/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.
+ *
+ *
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.
+ *
+ *
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.
+ *
+ *
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.
*
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
index a929661ee9..ff7eb01de6 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
+++ b/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.
+ *
+ *
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.
+ *
+ *
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.
+ *
+ *
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.
+ */
+ public Vector 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.
*
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
index a859743988..f6775ff2eb 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
@@ -13,6 +13,7 @@
#include
#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.
*
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h
index b9876c05a3..166f871c59 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h
+++ b/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 .
+ *
+ * @param other The translation to compute the cross product with.
+ * @return The cross product between the two translations.
+ */
+ constexpr Eigen::Vector Cross(
+ const Translation3d& other) const {
+ return Eigen::Vector{
+ {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.
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
index 82ea7ab866..5514b90213 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
+++ b/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);
+ }
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java
index 95a970ba6f..a8efcde086 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java
+++ b/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));
+ }
}
diff --git a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
index 294c213a3d..e44e334aec 100644
--- a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
+++ b/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());
+}
diff --git a/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
index b297f1217c..cdc7a6ebb7 100644
--- a/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
+++ b/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);
+}