Merge branch 'main' into 2027

wpimath/src/main/native/include/frc/DARE.h

@@ -128,7 +128,11 @@ Eigen::Matrix<double, States, States> DARE(
     //
     // V₂ᵀ = W.solve(Gₖᵀ)
     // V₂ = W.solve(Gₖᵀ)ᵀ
-    StateMatrix V_2 = W_solver.solve(G_k.transpose()).transpose();
+    //
+    // Since W, Gₖ, and Hₖ are symmetric, drop the transposes on Gₖ and V₂.
+    //
+    // V₂ = W.solve(Gₖ)
+    StateMatrix V_2 = W_solver.solve(G_k);
 
     // Gₖ₊₁ = Gₖ + AₖV₂Aₖᵀ
     // Hₖ₊₁ = Hₖ + V₁ᵀHₖAₖ

wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h

@@ -397,8 +397,11 @@ class ExtendedKalmanFilter {
     //
     // Kᵀ = Sᵀ.solve(CPᵀ)
     // K = (Sᵀ.solve(CPᵀ))ᵀ
-    Matrixd<States, Rows> K =
-        S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
+    //
+    // Drop the transposes on symmetric matrices S and P.
+    //
+    // K = (S.solve(CP))ᵀ
+    Matrixd<States, Rows> K = S.ldlt().solve(C * m_P).transpose();
 
     // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + Kₖ₊₁(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
     m_xHat = addFuncX(m_xHat, K * residualFuncY(y, h(m_xHat, u)));

wpimath/src/main/native/include/frc/estimator/KalmanFilter.h

@@ -235,8 +235,11 @@ class KalmanFilter {
     //
     // Kᵀ = Sᵀ.solve(CPᵀ)
     // K = (Sᵀ.solve(CPᵀ))ᵀ
-    Matrixd<States, Outputs> K =
-        S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
+    //
+    // Drop the transposes on symmetric matrices S and P.
+    //
+    // K = (S.solve(CP))ᵀ
+    Matrixd<States, Outputs> K = S.ldlt().solve(C * m_P).transpose();
 
     // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
     m_xHat += K * (y - (C * m_xHat + D * u));

wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.h

@@ -114,7 +114,11 @@ class SteadyStateKalmanFilter {
       //
       // Kᵀ = Sᵀ.solve(CPᵀ)
       // K = (Sᵀ.solve(CPᵀ))ᵀ
-      m_K = S.transpose().ldlt().solve(C * P.value().transpose()).transpose();
+      //
+      // Drop the transposes on symmetric matrices S and P.
+      //
+      // K = (S.solve(CP))ᵀ
+      m_K = S.ldlt().solve(C * P.value()).transpose();
     } else if (P.error() == DAREError::QNotSymmetric ||
                P.error() == DAREError::QNotPositiveSemidefinite) {
       std::string msg =

wpimath/src/main/native/include/frc/geometry/Translation2d.h

@@ -13,6 +13,7 @@
 #include <wpi/json_fwd.h>
 
 #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.
    *

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 <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.
+   */
+  constexpr Eigen::Vector<units::square_meter_t, 3> Cross(
+      const Translation3d& other) const {
+    return Eigen::Vector<units::square_meter_t, 3>{
+        {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.