Add mju_gather and mju_scatter, private engine functions.

PiperOrigin-RevId: 750245294
Change-Id: I3a14ccdd55a324d3fe206e388fb2a513772f5f5e

test/benchmark/factorI_benchmark_test.cc

@@ -19,6 +19,7 @@
 #include <mujoco/mjdata.h>
 #include <mujoco/mujoco.h>
 #include "src/engine/engine_core_smooth.h"
+#include "src/engine/engine_util_misc.h"
 #include "test/fixture.h"
 
 namespace mujoco {
@@ -45,9 +46,7 @@ static void BM_factorI(benchmark::State& state, bool legacy, bool coil) {
 
   // M: mass matrix in CSR format
   mjtNum* M = mj_stackAllocNum(d, m->nM);
-  for (int i=0; i < m->nM; i++) {
-    M[i] = d->qM[d->mapM2M[i]];
-  }
+  mju_gather(M, d->qM, d->mapM2M, m->nM);
 
   // LDlegacy: legacy LD matrix (size nM)
   mjtNum* LDlegacy = mj_stackAllocNum(d, m->nM);