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