diff --git a/src/engine/engine_collision_box.c b/src/engine/engine_collision_box.c index fae00bc6..77b202d3 100644 --- a/src/engine/engine_collision_box.c +++ b/src/engine/engine_collision_box.c @@ -316,52 +316,52 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con, }; mjtNum c[2]; } d2; - d2 p, s, d /*, c, tmp1*/; - mjtNum u, v, w, e1, best /* ,e2 */, l /* , e3, e4 */; + d2 p, s, dd /*, c, tmp1*/; + mjtNum uu, vv, w, ee1, best /* ,e2 */, l /* , e3, e4 */; bestdist = bestdistmax; p.x = pos[0]; p.y = pos[1]; - d.x = halfaxis[0]; - d.y = halfaxis[1]; + dd.x = halfaxis[0]; + dd.y = halfaxis[1]; s.x = size2[0]; s.y = size2[1]; - l = sqrt(d.x * d.x + d.y * d.y); + l = sqrt(dd.x * dd.x + dd.y * dd.y); - u = d.x * s.y; - v = d.y * s.x; - w = d.x * p.y - d.y * p.x; + uu = dd.x * s.y; + vv = dd.y * s.x; + w = dd.x * p.y - dd.y * p.x; best = -1; - e1 = +u - v; - if ((e1 < 0) == (w < 0)) { - if (best < mju_abs(e1)) { - best = mju_abs(e1); + ee1 = +uu - vv; + if ((ee1 < 0) == (w < 0)) { + if (best < mju_abs(ee1)) { + best = mju_abs(ee1); c1 = 0; } } - e1 = -u - v; - if ((e1 < 0) == (w < 0)) { - if (best < mju_abs(e1)) { - best = mju_abs(e1); + ee1 = -uu - vv; + if ((ee1 < 0) == (w < 0)) { + if (best < mju_abs(ee1)) { + best = mju_abs(ee1); c1 = 1; } } - e1 = +u + v; - if ((e1 < 0) == (w < 0)) { - if (best < mju_abs(e1)) { - best = mju_abs(e1); + ee1 = +uu + vv; + if ((ee1 < 0) == (w < 0)) { + if (best < mju_abs(ee1)) { + best = mju_abs(ee1); c1 = 2; } } - e1 = -u + v; - if ((e1 < 0) == (w < 0)) { - if (best < mju_abs(e1)) { - best = mju_abs(e1); + ee1 = -uu + vv; + if ((ee1 < 0) == (w < 0)) { + if (best < mju_abs(ee1)) { + best = mju_abs(ee1); c1 = 3; } } @@ -369,21 +369,21 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con, // c.x = s.x * ((c1 / 2) ? -1 : 1); // c.y = s.y * ((c1 % 2) ? -1 : 1); - e1 = fabs(w) / l; + ee1 = fabs(w) / l; // e2 = best / l; // printf("%g %g %g %g %g %g\n",c.x,c.y,d.x,d.y,e1,e2); // tmp1.x = c.x - p.x; // tmp1.y = c.y - p.y; - e1 = d.x * d.x + d.y * d.y; + ee1 = dd.x * dd.x + dd.y * dd.y; // e2 = tmp1.x * d.x + tmp1.y * d.y; // e3 = e2 / e1; // printf("%g %g %g %g %g %g %g \n",c.x,c.y,d.x,d.y,e1,e2,e3); - e1 = p.x + (+s.y - p.y) / d.y * d.x; + ee1 = p.x + (+s.y - p.y) / dd.y * dd.x; // e2 = p.x + (-s.y - p.y) / d.y * d.x; // e3 = p.y + (+s.x - p.x) / d.x * d.y; // e4 = p.y + (-s.x - p.x) / d.x * d.y; diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 7d8eddd9..cad80992 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -632,7 +632,7 @@ static int can_collide(const mjModel* m, int b) { // broadphase collision detector int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) { - int i, j, b1, b2, toremove, cnt, npair = 0, nbody = m->nbody, ngeom = m->ngeom; + int b1, b2, toremove, cnt, npair = 0, nbody = m->nbody, ngeom = m->ngeom; mjtNum cov[9], cen[3], dif[3], eigval[3], frame[9], quat[4]; mjtBroadphase *sortbuf, *activebuf; mjtNum *aabb; @@ -727,13 +727,13 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) { } // allocate sort buffer - i = sizeof(mjtBroadphase)/sizeof(mjtNum); - j = sizeof(mjtBroadphase)%sizeof(mjtNum); - sortbuf = (mjtBroadphase*)mj_stackAlloc(d, 2*bufcnt*(i + (j ? 1 : 0))); - activebuf = (mjtBroadphase*)mj_stackAlloc(d, 2*bufcnt*(i + (j ? 1 : 0))); + int quot = sizeof(mjtBroadphase)/sizeof(mjtNum); + int rem = sizeof(mjtBroadphase)%sizeof(mjtNum); + sortbuf = (mjtBroadphase*)mj_stackAlloc(d, 2*bufcnt*(quot + (rem ? 1 : 0))); + activebuf = (mjtBroadphase*)mj_stackAlloc(d, 2*bufcnt*(quot + (rem ? 1 : 0))); // init sortbuf with axis0 - j = 0; + int k = 0; for (int i=1; igeom_type[g1] > m->geom_type[g2]) { - i = g1; + int i = g1; g1 = g2; g2 = i; } @@ -938,7 +938,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user, } // consolidate good - i = 0; + int i = 0; for (int j=0; jnefc > 0) { - int nnz = d->efc_J_rownnz[d->nefc - 1] + d->efc_J_rowadr[d->nefc - 1]; - if (d->nnzJ != nnz) { + int nnzJ = d->efc_J_rownnz[d->nefc - 1] + d->efc_J_rowadr[d->nefc - 1]; + if (d->nnzJ != nnzJ) { mju_error("constraint Jacobian mis-allocation: found nnzJ=%d but allocated %d", - nnz, d->nnzJ); + nnzJ, d->nnzJ); } } } else if (d->nefc > nefc_allocated) { diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 599d2e47..6c6a027a 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1562,28 +1562,28 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { // forward pass over bodies: compute cacc, cfrc_int mjtNum cacc[6], cfrc_body[6], cfrc_corr[6]; mju_zero(d->cfrc_int, 6); - for (int i=1; inbody; i++) { + for (int j=1; jnbody; j++) { // get body's first dof address - int bda = m->body_dofadr[i]; + int bda = m->body_dofadr[j]; // cacc = cacc_parent + cdofdot * qvel + cdof * qacc - mju_mulDofVec(cacc, d->cdof_dot+6*bda, d->qvel+bda, m->body_dofnum[i]); - mju_add(d->cacc+6*i, d->cacc+6*m->body_parentid[i], cacc, 6); - mju_mulDofVec(cacc, d->cdof+6*bda, d->qacc+bda, m->body_dofnum[i]); - mju_addTo(d->cacc+6*i, cacc, 6); + mju_mulDofVec(cacc, d->cdof_dot+6*bda, d->qvel+bda, m->body_dofnum[j]); + mju_add(d->cacc+6*j, d->cacc+6*m->body_parentid[j], cacc, 6); + mju_mulDofVec(cacc, d->cdof+6*bda, d->qacc+bda, m->body_dofnum[j]); + mju_addTo(d->cacc+6*j, cacc, 6); // cfrc_body = cinert * cacc + cvel x (cinert * cvel) - mju_mulInertVec(cfrc_body, d->cinert+10*i, d->cacc+6*i); - mju_mulInertVec(cfrc_corr, d->cinert+10*i, d->cvel+6*i); - mju_crossForce(cfrc, d->cvel+6*i, cfrc_corr); + mju_mulInertVec(cfrc_body, d->cinert+10*j, d->cacc+6*j); + mju_mulInertVec(cfrc_corr, d->cinert+10*j, d->cvel+6*j); + mju_crossForce(cfrc, d->cvel+6*j, cfrc_corr); mju_addTo(cfrc_body, cfrc, 6); // set cfrc_int = cfrc_body - cfrc_ext - mju_sub(d->cfrc_int+6*i, cfrc_body, d->cfrc_ext+6*i, 6); + mju_sub(d->cfrc_int+6*j, cfrc_body, d->cfrc_ext+6*j, 6); } // backward pass over bodies: accumulate cfrc_int from children - for (int i=m->nbody-1; i>0; i--) { - mju_addTo(d->cfrc_int+6*m->body_parentid[i], d->cfrc_int+6*i, 6); + for (int j=m->nbody-1; j>0; j--) { + mju_addTo(d->cfrc_int+6*m->body_parentid[j], d->cfrc_int+6*j, 6); } } diff --git a/src/engine/engine_plugin.cc b/src/engine/engine_plugin.cc index 046e5b1c..189a0eef 100644 --- a/src/engine/engine_plugin.cc +++ b/src/engine/engine_plugin.cc @@ -380,8 +380,8 @@ const mjpPlugin* mjp_getPluginUnsafe(const char* name, int* slot, int nslot) { return nullptr; } - Global& plugin = GetGlobal(); - PluginTable* table = &plugin.table(); + Global& plugins = GetGlobal(); + PluginTable* table = &plugins.table(); int found_slot = 0; while (table) { for (int i = 0; diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index be3c665c..ae882b26 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -909,14 +909,14 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, M_rowadr[i] = i*nv; // reverse order - for (int j=0; jdof_simplenum[i]) { - for (int adr=nv*i; adrgood[1] ? 0 : 1); + int i = (good[0]>good[1] ? 0 : 1); pnt[0] = sol[i][0][0]; pnt[1] = sol[i][0][1]; pnt[2] = sol[i][1][0]; diff --git a/src/engine/engine_vfs.c b/src/engine/engine_vfs.c index f0f91945..08053d6c 100644 --- a/src/engine/engine_vfs.c +++ b/src/engine/engine_vfs.c @@ -24,9 +24,10 @@ // strip path prefix from filename static void vfs_strippath(char* newname, const char* oldname) { - int i, sz = strlen(oldname); + int sz = strlen(oldname); // find last delimiter + int i; for (i=sz-1; i>=0; i--) { if (oldname[i]=='\\' || oldname[i]=='/') { break; @@ -45,9 +46,9 @@ static void vfs_strippath(char* newname, const char* oldname) { mju_strncpy(newname, oldname+i+1, mjMAXVFSNAME); // make lowercase - for (int i=strlen(newname)-1; i>=0; i--) { - if (newname[i]>='A' && newname[i]<='Z') { - newname[i] = (char)(((int)newname[i]) +'a' - 'A'); + for (int j=strlen(newname)-1; j>=0; j--) { + if (newname[j]>='A' && newname[j]<='Z') { + newname[j] = (char)(((int)newname[j]) +'a' - 'A'); } } } diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index e0ae56a0..b317eafd 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -464,12 +464,12 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, int objtype, category; mjtNum sz[3], mat[9], selpos[3]; mjtNum catenary[3*mjNCATENARY]; - mjtNum *cur, *nxt, *xpos, *xfrc; + mjtNum *cur, *nxt, *xfrc; mjtNum vec[3], end[3], axis[3], rod, len, det, tmp[9], quat[4]; mjtByte broken; mjvGeom* thisgeom; mjvPerturb localpert; - float scl = m->stat.meansize, rgba[4]; + float scl = m->stat.meansize; // make default pert if missing if (!pert) { @@ -612,12 +612,12 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjtNum density = mass / mju_max(mjMINVAL, volume); // scale = root3(density) - mjtNum scl = mju_pow(density*0.001, 1.0/3.0); + mjtNum scale = mju_pow(density*0.001, 1.0/3.0); // scale sizes, so that box/ellipsoid with density of 1000 has same mass - sz[0] *= scl; - sz[1] *= scl; - sz[2] *= scl; + sz[0] *= scale; + sz[1] *= scale; + sz[2] *= scale; } // construct geom @@ -659,6 +659,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, pert->refselpos[0], pert->refselpos[1], pert->refselpos[2]); // prepare color + float rgba[4]; mixcolor(rgba, m->vis.rgba.constraint, (pert->active & mjPERT_TRANSLATE)>0, (pert->active2 & mjPERT_TRANSLATE)>0); @@ -683,6 +684,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, START // prepare color, use inertia color + float rgba[4]; mixcolor(rgba, m->vis.rgba.inertia, (pert->active & mjPERT_ROTATE)>0, (pert->active2 & mjPERT_ROTATE)>0); @@ -1681,7 +1683,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, for (int i=1; inbody; i++) { if (!mju_isZero(d->xfrc_applied+6*i, 6) && (category & catmask)) { // point of application and force - xpos = d->xipos+3*i; + mjtNum *xpos = d->xipos+3*i; xfrc = d->xfrc_applied+6*i; // force perturbation