diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 092b0ecd..303375aa 100644 --- a/src/engine/engine_core_constraint.c +++ b/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++) { diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 32ea64f0..538e0afd 100644 --- a/src/engine/engine_io.c +++ b/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)) { diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index 0d9847d0..b2f62c4c 100644 --- a/src/engine/engine_island.c +++ b/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++) { diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 1eed2aef..af3664bb 100644 --- a/src/engine/engine_setconst.c +++ b/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 diff --git a/src/engine/engine_sleep.c b/src/engine/engine_sleep.c index 5c235901..f6d5892f 100644 --- a/src/engine/engine_sleep.c +++ b/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; } diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index f0b2d85f..1b01c06f 100644 --- a/src/engine/engine_solver.c +++ b/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); diff --git a/src/engine/engine_util_blas.c b/src/engine/engine_util_blas.c index a874d5f7..0041484f 100644 --- a/src/engine/engine_util_blas.c +++ b/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; } diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index da2521e1..3ceb521c 100644 --- a/src/engine/engine_util_misc.c +++ b/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++) { diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index e59aaed9..7457f460 100644 --- a/src/engine/engine_util_misc.h +++ b/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);