Precompute rotated stiffness matrix before CG in implicit flex integrator.

This gives a 3x speedup in implicitfast.

Also cleanup old code that was used in the dense factorization of the stiffness matrix before we switched to CG.

PiperOrigin-RevId: 915900315
Change-Id: Id6973c4bfd7d371a43ec6db982703969b23a3550

test/engine/engine_derivative_test.cc

@@ -1485,7 +1485,7 @@ static void mulKD_dense(mjModel* m, mjData* d, mjtNum* H_dense,
     mju_zero(e_i.data(), nv);
     mju_zero(col.data(), nv);
     e_i[i] = 1.0;
-    mjd_flexInterp_mul(m, d, col.data(), e_i.data(), h * h, h);
+    mjd_flexInterp_mul(m, d, col.data(), e_i.data(), h * h, h, NULL);
     // col = +(h^2 + h*damp)*K*e_i, negate to match addH convention (H -= K)
     for (int j = 0; j < nv; j++) {
       H_dense[j * nv + i] = -col[j];

test/engine/engine_forward_test.cc

@@ -3147,7 +3147,7 @@ TEST_F(ForwardTest, FlexTrilinearInstability) {
   // using mulKD for legacy check consistency, but we know it applies h^2+h*d
   // scaling; actually, let's stick to the high-level property checks from
   // FlexStiffnessSign which used mulKD
-  mjd_flexInterp_mul(model, data, flex_Kv.data(), v.data(), h * h, h);
+  mjd_flexInterp_mul(model, data, flex_Kv.data(), v.data(), h * h, h, NULL);
 
   // compute v^T*M*v and v^T*scale*K*v
   mjtNum vMv = mju_dot(v.data(), Mv.data(), nv);