From b3aeee18c8a2d07cdb03b0ef0d13935ab9c6da4b Mon Sep 17 00:00:00 2001 From: Rain Heuer <63077980+rmheuer@users.noreply.github.com> Date: Thu, 31 Jul 2025 23:05:39 -0500 Subject: [PATCH] [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 --- .../first/math/geometry/Translation2d.java | 50 +++++++++++++++ .../first/math/geometry/Translation3d.java | 61 +++++++++++++++++- .../include/frc/geometry/Translation2d.h | 55 ++++++++++++++++ .../include/frc/geometry/Translation3d.h | 62 +++++++++++++++++++ .../math/geometry/Translation2dTest.java | 27 ++++++++ .../math/geometry/Translation3dTest.java | 32 ++++++++++ .../native/cpp/geometry/Translation2dTest.cpp | 23 +++++++ .../native/cpp/geometry/Translation3dTest.cpp | 27 ++++++++ 8 files changed, 335 insertions(+), 2 deletions(-) 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); +}