Merge branch 'main' into 2027

wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java

@@ -372,7 +372,11 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
     //
     // Kᵀ = Sᵀ.solve(CPᵀ)
     // K = (Sᵀ.solve(CPᵀ))ᵀ
-    final Matrix<States, Rows> K = S.transpose().solve(C.times(m_P.transpose())).transpose();
+    //
+    // Drop the transposes on symmetric matrices S and P.
+    //
+    // K = (S.solve(CP))ᵀ
+    final Matrix<States, Rows> K = S.solve(C.times(m_P)).transpose();
 
     // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
     m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, h.apply(m_xHat, u))));

wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java

@@ -230,7 +230,11 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
     //
     // Kᵀ = Sᵀ.solve(CPᵀ)
     // K = (Sᵀ.solve(CPᵀ))ᵀ
-    final Matrix<States, Outputs> K = S.transpose().solve(C.times(m_P.transpose())).transpose();
+    //
+    // Drop the transposes on symmetric matrices S and P.
+    //
+    // K = (S.solve(CP))ᵀ
+    final Matrix<States, Outputs> K = S.solve(C.times(m_P)).transpose();
 
     // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
     m_xHat = m_xHat.plus(K.times(y.minus(C.times(m_xHat).plus(D.times(u)))));

wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java

@@ -100,9 +100,11 @@ public class SteadyStateKalmanFilter<States extends Num, Inputs extends Num, Out
     //
     // Kᵀ = Sᵀ.solve(CPᵀ)
     // K = (Sᵀ.solve(CPᵀ))ᵀ
-    m_K =
-        new Matrix<>(
-            S.transpose().getStorage().solve(C.times(P.transpose()).getStorage()).transpose());
+    //
+    // Drop the transposes on symmetric matrices S and P.
+    //
+    // K = (S.solve(CP))ᵀ
+    m_K = new Matrix<>(S.getStorage().solve(C.times(P).getStorage()).transpose());
 
     reset();
   }

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.
+   *
+   * <p>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.
+   *
+   * <p>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.
+   *
+   * <p>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.
    *

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.
+   *
+   * <p>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.
+   *
+   * <p>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.
+   *
+   * <p>The 3D cross product between two translations is defined as &lt;y₁z₂-y₂z₁, z₁x₂-z₂x₁,
+   * x₁y₂-x₂y₁&gt;.
+   *
+   * @param other The translation to compute the cross product with.
+   * @return The cross product between the two translations.
+   */
+  public Vector<N3> 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.
    *