Add fixed-size 6x6 dense LU factorization with benchmark.

mju_factorLU6/mju_solveLU6: same algorithm as mju_factorLU/mju_solveLU
with compile-time size, allowing full unrolling. At n=6, factor+solve is
25% faster than the runtime-sized version (93 vs 124 ns), and fixed-size
LU factorization is faster than generic dense Cholesky (55 vs 61 ns):
at this size, runtime-n loop overhead outweighs Cholesky's 2x flop
advantage. See new lu_benchmark_test. Results agree with the generic
version to rounding, not bitwise: the compiler may fuse (FMA) the
unrolled version differently.

Also add two DenseLU tests: a pivoting-required matrix with zero
diagonal, and fixed-vs-generic agreement.

PiperOrigin-RevId: 947705056
Change-Id: I24c54c9510964aa376886e9dd721890eda9889d3

src/engine/engine_util_solve.c

@@ -834,6 +834,85 @@ void mju_solveLU(mjtNum* restrict x, const mjtNum* LU, const mjtNum* b, const in
 }
 
 
+// 6x6 specialization of mju_factorLU: same algorithm with compile-time size,
+// allowing full unrolling; produces identical results
+int mju_factorLU6(mjtNum A[36], int pivot[6]) {
+  for (int k=0; k < 6; k++) {
+    // initialize pivot
+    pivot[k] = k;
+
+    // find pivot: max absolute value in column k, rows k..n-1
+    mjtNum maxval = mju_abs(A[k*6+k]);
+    int maxrow = k;
+    for (int i=k+1; i < 6; i++) {
+      mjtNum val = mju_abs(A[i*6+k]);
+      if (val > maxval) {
+        maxval = val;
+        maxrow = i;
+      }
+    }
+
+    // check singularity
+    if (maxval < mjMINVAL) {
+      return 0;
+    }
+
+    // swap rows k and maxrow
+    if (maxrow != k) {
+      pivot[k] = maxrow;
+      for (int j=0; j < 6; j++) {
+        mjtNum tmp = A[k*6+j];
+        A[k*6+j] = A[maxrow*6+j];
+        A[maxrow*6+j] = tmp;
+      }
+    }
+
+    // compute multipliers and update trailing submatrix
+    mjtNum diaginv = 1.0 / A[k*6+k];
+    for (int i=k+1; i < 6; i++) {
+      A[i*6+k] *= diaginv;
+      mjtNum Aik = A[i*6+k];
+      for (int j=k+1; j < 6; j++) {
+        A[i*6+j] -= Aik * A[k*6+j];
+      }
+    }
+  }
+
+  return 1;
+}
+
+
+// solve A*x = b given 6x6 LU factorization from mju_factorLU6
+void mju_solveLU6(mjtNum x[6], const mjtNum LU[36], const mjtNum b[6], const int pivot[6]) {
+  for (int i=0; i < 6; i++) {
+    x[i] = b[i];
+  }
+
+  // apply row permutation and forward substitution: solve L*y = P*b
+  for (int i=0; i < 6; i++) {
+    // apply pivot swap
+    if (pivot[i] != i) {
+      mjtNum tmp = x[i];
+      x[i] = x[pivot[i]];
+      x[pivot[i]] = tmp;
+    }
+
+    // subtract known terms
+    for (int j=0; j < i; j++) {
+      x[i] -= LU[i*6+j] * x[j];
+    }
+  }
+
+  // back substitution: solve U*x = y
+  for (int i=6-1; i >= 0; i--) {
+    for (int j=i+1; j < 6; j++) {
+      x[i] -= LU[i*6+j] * x[j];
+    }
+    x[i] /= LU[i*6+i];
+  }
+}
+
+
 //------------------------------ sparse LU factorization -------------------------------------------
 
 // sparse reverse-order LU factorization, no fill-in (assuming tree topology)