Add mju_gather and mju_scatter, private engine functions.

PiperOrigin-RevId: 750245294
Change-Id: I3a14ccdd55a324d3fe206e388fb2a513772f5f5e

test/benchmark/solveLD_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 {
@@ -50,11 +51,9 @@ static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) {
     vec[i] = 0.2 + 0.3*i;
   }
 
-  // make legacy matrix
+  // scatter into legacy matrix
   mjtNum* LDlegacy = mj_stackAllocNum(d, m->nM);
-  for (int i=0; i < m->nM; i++) {
-    LDlegacy[d->mapM2M[i]] = d->qLD[i];
-  }
+  mju_scatter(LDlegacy, d->qLD, d->mapM2M, m->nM);
 
   // benchmark
   while (state.KeepRunningBatch(kNumBenchmarkSteps)) {