Add private function mju_fillInt

PiperOrigin-RevId: 845217293
Change-Id: I604372ab7ea7f4d47d9427d36dc8cd78e60d84ca

src/engine/engine_core_constraint.c

@@ -699,7 +699,6 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) {
                       1, mjCNSTR_FRICTION_DOF, i,
                       issparse ? 1 : 0,
                       issparse ? &i : NULL);
-
   }
 
   // find frictional tendons
@@ -1705,7 +1704,7 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
       flex_edgeadr = m->flex_edgeadr[id[0]];
       flex_edgenum = m->flex_edgenum[id[0]];
 
-      // init with all edges, subract rigid later
+      // init with all edges, subtract rigid later
       size = flex_edgenum;
 
       // process edges of this flex
@@ -2004,9 +2003,7 @@ void mj_makeConstraint(const mjModel* m, mjData* d) {
   }
 
   // clear tendon_efcadr
-  for (int i=0; i < m->ntendon; i++) {
-    d->tendon_efcadr[i] = -1;
-  }
+  mju_fillInt(d->tendon_efcadr, -1, m->ntendon);
 
   // reset nefc for the instantiation functions, instantiate all elements of Jacobian
   d->nefc = 0;
@@ -2108,9 +2105,7 @@ void mj_projectConstraint(const mjModel* m, mjData* d) {
 
     // markers for merged dofs, initialized to -1
     int* marker = mjSTACKALLOC(d, nv, int);
-    for (int i=0; i < nv; i++) {
-      marker[i] = -1;
-    }
+    mju_fillInt(marker, -1, nv);
 
     B_rowadr[0] = 0;
     for (int r=0; r < nefc; r++) {

src/engine/engine_io.c

@@ -944,7 +944,7 @@ void mj_makeDofDofMaps(int nv, int nM, int nC, int nD,
 
   // make mapM2M
   for (int i=0; i < nM; i++) M[i] = i;
-  for (int i=0; i < nC; i++) mapM2M[i] = -1;
+  mju_fillInt(mapM2M, -1, nC);
   copyM2Sparse(nv, dof_Madr, dof_simplenum, dof_parentid, M_rownnz,
                M_rowadr, M, mapM2M, /*reduced=*/1, /*upper=*/0, scratch);
 
@@ -1372,9 +1372,7 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
   static int kAwake = -(1+mjMINAWAKE);  // tree_asleep value for fully awake tree
 
   // set all trees to awake
-  for (int i=0; i < m->ntree; i++) {
-    d->tree_asleep[i] = kAwake;
-  }
+  mju_fillInt(d->tree_asleep, kAwake, m->ntree);
 
   // sleep enabled: handle static bodies and trees marked as mjSLEEP_INIT
   if (mjENABLED(mjENBL_SLEEP)) {

src/engine/engine_island.c

@@ -98,7 +98,7 @@ int mj_floodFill(int* island, int nr, const int* rownnz, const int* rowadr, cons
                  int* stack) {
   // initialize island count, set ids to -1
   int nisland = 0;
-  for (int i=0; i < nr; i++) island[i] = -1;
+  mju_fillInt(island, -1, nr);
 
   // iterate over vertices, discover islands
   for (int i=0; i < nr; i++) {

src/engine/engine_setconst.c

@@ -186,9 +186,9 @@ static void setFixed(mjModel* m, mjData* d) {
           m->tree_sleep_policy[treeid] = mjSLEEP_AUTO_NEVER;
         }
       }
-      continue; // next actuator
+      continue;  // next actuator
     case mjTRN_UNDEFINED:
-      continue; // next actuator
+      continue;  // next actuator
     }
 
     // wake tree containing bodyid, if any
@@ -501,12 +501,8 @@ static void set0(mjModel* m, mjData* d) {
       m->actuator_acc0[i] = mju_norm(tmp, nv);
     }
   } else {
-    for (int i=0; i < m->ntendon; i++) {
-      m->tendon_invweight0[i] = 0;
-    }
-    for (int i=0; i < m->nu; i++) {
-      m->actuator_acc0[i] = 0;
-    }
+    mju_zero(m->tendon_invweight0, m->ntendon);
+    mju_zero(m->actuator_acc0, m->nu);
   }
 
   // compute missing eq_data for body constraints

src/engine/engine_sleep.c

@@ -244,7 +244,7 @@ int mj_wake(const mjModel* m, mjData* d) {
   if (!mjENABLED(mjENBL_SLEEP)) {
     // sleep disabled but some trees still asleep: wake all
     if (d->ntree_awake < ntree) {
-      for (int i=0; i < ntree; i++) d->tree_asleep[i] = kAwake;
+      mju_fillInt(d->tree_asleep, kAwake, ntree);
     }
     return ntree - d->ntree_awake;
   }

src/engine/engine_solver.c

@@ -239,9 +239,7 @@ static int dualState(const mjModel* m, const mjData* d, int* state) {
   int nactive = ne + nf;
 
   // equality
-  for (int i=0; i < ne; i++) {
-    state[i] = mjCNSTRSTATE_QUADRATIC;
-  }
+  mju_fillInt(state, mjCNSTRSTATE_QUADRATIC, ne);
 
   // friction
   for (int i=ne; i < ne+nf; i++) {
@@ -301,9 +299,7 @@ static int dualState(const mjModel* m, const mjData* d, int* state) {
       }
 
       // replicate state in all cone dimensions
-      for (int j=0; j < dim; j++) {
-        state[i+j] = result;
-      }
+      mju_fillInt(state+i, result, dim);
 
       // advance
       i += (dim-1);

src/engine/engine_util_blas.c

@@ -678,16 +678,13 @@ void mju_addScl(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, mjtNum scl,
 
 // normalize vector, return length before normalization
 mjtNum mju_normalize(mjtNum* res, int n) {
-  mjtNum norm = (mjtNum)mju_sqrt(mju_dot(res, res, n));
-  mjtNum normInv;
+  mjtNum norm = mju_sqrt(mju_dot(res, res, n));
 
   if (norm < mjMINVAL) {
     res[0] = 1;
-    for (int i=1; i < n; i++) {
-      res[i] = 0;
-    }
+    mju_zero(res + 1, n - 1);
   } else {
-    normInv = 1/norm;
+    mjtNum normInv = 1 / norm;
     for (int i=0; i < n; i++) {
       res[i] *= normInv;
     }

src/engine/engine_util_misc.c

@@ -1394,6 +1394,12 @@ void mju_copyInt(int* res, const int* vec, int n) {
   memcpy(res, vec, n*sizeof(int));
 }
 
+// fill int vector with val
+void mju_fillInt(int* res, int val, int n) {
+  for (int i = 0; i < n; i++) {
+    res[i] = val;
+  }
+}
 
 // standard normal random number generator (optional second number)
 mjtNum mju_standardNormal(mjtNum* num2) {
@@ -1521,9 +1527,7 @@ void mju_lower2SymMap(int* map, int nr,
 
   // default all map entries to "no source"
   int nnz = res_rowadr[nr-1] + res_rownnz[nr-1];
-  for (int i = 0; i < nnz; i++) {
-    map[i] = -1;
-  }
+  mju_fillInt(map, -1, nnz);
 
   // initialize per-row cursor
   for (int i = 0; i < nr; i++) {

src/engine/engine_util_misc.h

@@ -151,6 +151,9 @@ MJAPI void mju_zeroInt(int* res, int n);
 // copy int vector vec into res
 MJAPI void mju_copyInt(int* res, const int* vec, int n);
 
+// fill int vector with val
+void mju_fillInt(int* res, int val, int n);
+
 // standard normal random number generator (optional second number)
 MJAPI mjtNum mju_standardNormal(mjtNum* num2);