From a9ee497e335303d4c8bd289dc15399032ba74aa8 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sun, 15 Oct 2023 07:39:20 -0700 Subject: [PATCH] Add spaces around comparison operators. PiperOrigin-RevId: 573620198 Change-Id: Icf295cc0dd381a4a7f0e2c94f2e12b499193e862 --- src/engine/engine_collision_convex.c | 38 +-- src/engine/engine_collision_driver.c | 344 +++++++++++------------ src/engine/engine_collision_primitive.c | 10 +- src/engine/engine_collision_sdf.c | 354 ++++++++++++------------ src/engine/engine_core_constraint.c | 68 ++--- src/engine/engine_core_smooth.c | 50 ++-- src/engine/engine_derivative.c | 4 +- src/engine/engine_forward.c | 4 +- src/engine/engine_io.c | 69 +++-- src/engine/engine_passive.c | 4 +- src/engine/engine_print.c | 4 +- src/engine/engine_ray.c | 50 ++-- src/engine/engine_sensor.c | 32 +-- src/engine/engine_setconst.c | 20 +- src/engine/engine_support.c | 96 +++---- src/engine/engine_util_container.c | 6 +- src/engine/engine_util_misc.c | 2 +- src/engine/engine_util_sparse.c | 54 ++-- src/engine/engine_vis_init.c | 16 +- src/engine/engine_vis_interact.c | 12 +- src/engine/engine_vis_visualize.c | 100 +++---- 21 files changed, 670 insertions(+), 667 deletions(-) diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 158d2eea..1f526af6 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -39,12 +39,12 @@ void mjccd_center(const void *obj, ccd_vec3_t *center) { int v = ccd->vert; // return geom position - if (g>=0) { + if (g >= 0) { mju_copy3(center->v, ccd->data->geom_xpos + 3*g); } // return flex element position - else if (e>=0) { + else if (e >= 0) { mju_copy3(center->v, ccd->data->flexelem_aabb + 6*(ccd->model->flex_elemadr[f]+e)); } @@ -64,14 +64,14 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { int g = ccd->geom; //-------------------------- flex element or vertex ----------------------------- - if (g<0) { + if (g < 0) { int f = ccd->flex; int dim = m->flex_dim[f]; mjtNum *res = vec->v; const mjtNum *dir = _dir->v; // flex element - if (ccd->elem>=0) { + if (ccd->elem >= 0) { int e = ccd->elem; const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1); const mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[f]; @@ -79,11 +79,11 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { // find element vertex with largest projection along dir mju_copy3(res, vert+3*edata[0]); mjtNum best = mju_dot3(res, dir); - for (int i=1; i<=dim; i++) { + for (int i=1; i <= dim; i++) { mjtNum dot = mju_dot3(vert+3*edata[i], dir); // better vertex found: assign - if (dot>best) { + if (dot > best) { best = dot; mju_copy3(res, vert+3*edata[i]); } @@ -285,7 +285,7 @@ static int mjc_MPRIteration(mjtCCD* obj1, mjtCCD* obj2, const ccd_t* ccd, mju_zero3(con->frame+3); // both geoms: fix contact frame normal - if (obj1->geom>=0 && obj2->geom>=0) { + if (obj1->geom >= 0 && obj2->geom >= 0) { mjc_fixNormal(m, d, con, obj1->geom, obj2->geom); } @@ -1130,7 +1130,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, int dim = m->flex_dim[f]; const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1); mjtNum* evert[4] = {NULL, NULL, NULL, NULL}; - for (int i=0; i<=dim; i++) { + for (int i=0; i <= dim; i++) { evert[i] = d->flexvert_xpos + 3*(m->flex_vertadr[f] + edata[i]); } mjtNum* ecenter = d->flexelem_aabb + 6*(m->flex_elemadr[f]+e); @@ -1145,7 +1145,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, // save elem vertices, transform to hfield frame mjtNum savevert[4][3]; - for (int i=0; i<=dim; i++) { + for (int i=0; i <= dim; i++) { mju_copy3(savevert[i], evert[i]); mju_sub3(vec, evert[i], hpos); mju_mulMatTVec(evert[i], hmat, vec, 3, 3); @@ -1161,7 +1161,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, xmin = xmax = evert[0][0]; ymin = ymax = evert[0][1]; zmin = zmax = evert[0][2]; - for (int i=1; i<=dim; i++) { + for (int i=1; i <= dim; i++) { xmin = mju_min(xmin, evert[i][0]); xmax = mju_max(xmax, evert[i][0]); ymin = mju_min(ymin, evert[i][1]); @@ -1175,7 +1175,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, (ymin-margin > hsize[1]) || (ymax+margin < -hsize[1]) || (zmin-margin > hsize[2]) || (zmax+margin < -hsize[3])) { // restore vertices and center - for (int i=0; i<=dim; i++) { + for (int i=0; i <= dim; i++) { mju_copy3(evert[i], savevert[i]); } mju_copy3(ecenter, savecenter); @@ -1217,23 +1217,23 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, // process all prisms in sub-grid cnt = 0; - for (int r=rmin; r2) { + if (nvert > 2) { // prism height test - if (prism.v[3][2]=mjMAXCONPAIR) { + if (cnt >= mjMAXCONPAIR) { r = rmax+1; c = cmax+1; k = 3; @@ -1258,7 +1258,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, } // restore elem vertices and center - for (int i=0; i<=dim; i++) { + for (int i=0; i <= dim; i++) { mju_copy3(evert[i], savevert[i]); } mju_copy3(ecenter, savecenter); diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index b6cf04af..5726e2b7 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -61,7 +61,7 @@ static inline void resetArena(mjData* d) { #ifdef ADDRESS_SANITIZER if (!d->threadpool) { ASAN_POISON_MEMORY_REGION( - (char*)d->arena + d->parena, d->narena - d->pstack - d->parena); + (char*)d->arena + d->parena, d->narena - d->pstack - d->parena); } #endif } @@ -145,19 +145,19 @@ static int filterSphere(const mjtNum pos1[3], const mjtNum pos2[3], mjtNum bound // filter contact based on bounding sphere test static int mj_filterSphere(const mjModel* m, mjData* d, int g1, int g2, mjtNum margin) { // neither geom is a plane - if (m->geom_rbound[g1]>0 && m->geom_rbound[g2]>0) { + if (m->geom_rbound[g1] > 0 && m->geom_rbound[g2] > 0) { return filterSphere(d->geom_xpos + 3*g1, d->geom_xpos + 3*g2, m->geom_rbound[g1] + m->geom_rbound[g2] + margin); } // one geom is a plane - if (m->geom_type[g1]==mjGEOM_PLANE && m->geom_rbound[g2]>0 + if (m->geom_type[g1] == mjGEOM_PLANE && m->geom_rbound[g2] > 0 && planeGeomDist(m, d, g1, g2) > margin + m->geom_rbound[g2]) { - return 1; + return 1; } - if (m->geom_type[g2]==mjGEOM_PLANE && m->geom_rbound[g1]>0 + if (m->geom_type[g2] == mjGEOM_PLANE && m->geom_rbound[g1] > 0 && planeGeomDist(m, d, g2, g1) > margin + m->geom_rbound[g1]) { - return 1; + return 1; } return 0; } @@ -186,7 +186,7 @@ static int filterBodyPair(int weldbody1, int weldparent1, int weldbody2, // return 1 if bodyflex can collide, 0 otherwise static int canCollide(const mjModel* m, int bf) { - if (bfnbody) { + if (bf < m->nbody) { return (m->body_contype[bf] || m->body_conaffinity[bf]); } else { int f = bf - m->nbody; @@ -199,10 +199,10 @@ static int canCollide(const mjModel* m, int bf) { // return 1 if two bodyflexes can collide, 0 otherwise static int canCollide2(const mjModel* m, int bf1, int bf2) { int nbody = m->nbody; - int contype1 = (bf1body_contype[bf1] : m->flex_contype[bf1-nbody]; - int conaffinity1 = (bf1body_conaffinity[bf1] : m->flex_conaffinity[bf1-nbody]; - int contype2 = (bf2body_contype[bf2] : m->flex_contype[bf2-nbody]; - int conaffinity2 = (bf2body_conaffinity[bf2] : m->flex_conaffinity[bf2-nbody]; + int contype1 = (bf1 < nbody) ? m->body_contype[bf1] : m->flex_contype[bf1-nbody]; + int conaffinity1 = (bf1 < nbody) ? m->body_conaffinity[bf1] : m->flex_conaffinity[bf1-nbody]; + int contype2 = (bf2 < nbody) ? m->body_contype[bf2] : m->flex_contype[bf2-nbody]; + int conaffinity2 = (bf2 < nbody) ? m->body_conaffinity[bf2] : m->flex_conaffinity[bf2-nbody]; // opposite of bitmask filter return (!filterBitmask(contype1, conaffinity1, contype2, conaffinity2)); @@ -212,7 +212,7 @@ static int canCollide2(const mjModel* m, int bf1, int bf2) { // return 1 if element is active, 0 otherwise int mj_isElemActive(const mjModel* m, int f, int e) { - if (m->flex_dim[f]<3) { + if (m->flex_dim[f] < 3) { return 1; } else { return (m->flex_elemlayer[m->flex_elemadr[f]+e] < m->flex_activelayers[f]); @@ -226,19 +226,19 @@ int mj_isElemActive(const mjModel* m, int f, int e) { // compare contact pairs by their geom/elem/vert IDs quicksortfunc(contactcompare, context, el1, el2) { const mjModel* m = (const mjModel*) context; - mjContact* con1 = (mjContact*)el1; - mjContact* con2 = (mjContact*)el2; + mjContact* c1 = (mjContact*)el1; + mjContact* c2 = (mjContact*)el2; // get colliding object ids - int con1_obj1 = con1->geom[0]>=0 ? con1->geom[0] : (con1->elem[0]>=0 ? con1->elem[0] : con1->vert[0]); - int con1_obj2 = con1->geom[1]>=0 ? con1->geom[1] : (con1->elem[1]>=0 ? con1->elem[1] : con1->vert[1]); - int con2_obj1 = con2->geom[0]>=0 ? con2->geom[0] : (con2->elem[0]>=0 ? con2->elem[0] : con2->vert[0]); - int con2_obj2 = con2->geom[1]>=0 ? con2->geom[1] : (con2->elem[1]>=0 ? con2->elem[1] : con2->vert[1]); + int con1_obj1 = c1->geom[0] >= 0 ? c1->geom[0] : (c1->elem[0] >= 0 ? c1->elem[0] : c1->vert[0]); + int con1_obj2 = c1->geom[1] >= 0 ? c1->geom[1] : (c1->elem[1] >= 0 ? c1->elem[1] : c1->vert[1]); + int con2_obj1 = c2->geom[0] >= 0 ? c2->geom[0] : (c2->elem[0] >= 0 ? c2->elem[0] : c2->vert[0]); + int con2_obj2 = c2->geom[1] >= 0 ? c2->geom[1] : (c2->elem[1] >= 0 ? c2->elem[1] : c2->vert[1]); // for geom:geom, reproduce the order of contacts without mj_collideTree // normally sorted by (g1, g2), but in mj_collideGeoms, g1 and g2 are swapped based on geom_type // here we undo this swapping for the purpose of sorting - needs to be done for each mjContact - if (con1->geom[0]>=0 && con1->geom[1] && con2->geom[0]>=0 && con2->geom[1]) { + if (c1->geom[0] >= 0 && c1->geom[1] && c2->geom[0] >= 0 && c2->geom[1]) { if (m->geom_type[con1_obj1] > m->geom_type[con1_obj2]) { int tmp = con1_obj1; con1_obj1 = con1_obj2; @@ -298,7 +298,7 @@ void mj_collision(const mjModel* m, mjData* d) { // process bodyflex pairs returned by broadphase, merge with predefined geom pairs int pairadr = 0; - for (int i=0; i>16) & 0xFFFF; int bf2 = broadphasepair[i] & 0xFFFF; @@ -317,8 +317,8 @@ void mj_collision(const mjModel* m, mjData* d) { int startadr = pairadr; if (npair) { // test all predefined pairs for which pair_signature<=signature - while (pairadrpair_signature[pairadr]<=signature) { - if (m->pair_signature[pairadr]==signature) { + while (pairadr < npair && m->pair_signature[pairadr] <= signature) { + if (m->pair_signature[pairadr] == signature) { merged = 1; } mj_collideGeoms(m, d, pairadr++, -1); @@ -334,30 +334,31 @@ void mj_collision(const mjModel* m, mjData* d) { int exadr = 0; if (nexclude) { // advance exadr while exclude_signature < signature - while (exadrexclude_signature[exadr]exclude_signature[exadr] < signature) { exadr++; } // skip this bodyflex pair if its signature is found in exclude array - if (exadrexclude_signature[exadr]==signature) { + if (exadr < nexclude && m->exclude_signature[exadr] == signature) { continue; } } // get bodyflex info - int isbody1 = (bf1body_bvhadr[bf1] : m->flex_bvhadr[bf1-nbody]); int bvh2 = (isbody2 ? m->body_bvhadr[bf2] : m->flex_bvhadr[bf2-nbody]); + int geomadr1 = (isbody1 ? m->body_geomadr[bf1] : -1); + int geomadr2 = (isbody2 ? m->body_geomadr[bf2] : -1); // process bodyflex pair: two single-geom bodies - if (isbody1 && isbody2 && m->body_geomnum[bf1]==1 && m->body_geomnum[bf2]==1) { - mj_collideGeomPair(m, d, m->body_geomadr[bf1], m->body_geomadr[bf2], - merged, startadr, pairadr); + if (isbody1 && isbody2 && m->body_geomnum[bf1] == 1 && m->body_geomnum[bf2] == 1) { + mj_collideGeomPair(m, d, geomadr1, geomadr2, merged, startadr, pairadr); } // process bodyflex pair: midphase - else if (!mjDISABLED(mjDSBL_MIDPHASE) && bvh1>=0 && bvh2>=0) { + else if (!mjDISABLED(mjDSBL_MIDPHASE) && bvh1 >= 0 && bvh2 >= 0) { int ncon_before = d->ncon; mj_collideTree(m, d, bf1, bf2, merged, startadr, pairadr); int ncon_after = d->ncon; @@ -369,13 +370,13 @@ void mj_collision(const mjModel* m, mjData* d) { // process bodyflex pair: all-to-all else { - int geomadr_end1 = m->body_geomadr[bf1] + m->body_geomnum[bf1]; - int geomadr_end2 = m->body_geomadr[bf2] + m->body_geomnum[bf2]; + int geomadr_end1 = geomadr1 + m->body_geomnum[bf1]; + int geomadr_end2 = geomadr2 + m->body_geomnum[bf2]; // body : body if (isbody1 && isbody2) { - for (int g1=m->body_geomadr[bf1]; g1body_geomadr[bf2]; g2body_geomadr[bf1]; gbody_geomadr[bf1]; g < geomadr_end1; g++) { // bitmask filtering at the geom-flex level if (filterBitmask(m->geom_contype[g], m->geom_conaffinity[g], m->flex_contype[f], m->flex_conaffinity[f])) { @@ -394,13 +395,14 @@ void mj_collision(const mjModel* m, mjData* d) { } // plane special processing - if (m->geom_type[g]==mjGEOM_PLANE) { + if (m->geom_type[g] == mjGEOM_PLANE) { mj_collidePlaneFlex(m, d, g, f); continue; } // collide geom with flex elements - for (int e=0; eflex_elemnum[f]; e++) { + int elemnum = m->flex_elemnum[f]; + for (int e=0; e < elemnum; e++) { mj_collideGeomElem(m, d, g, f, e); } } @@ -412,8 +414,8 @@ void mj_collision(const mjModel* m, mjData* d) { int f2 = bf2 - nbody; // collide elements of two flexes - for (int e1=0; e1flex_elemnum[f1]; e1++) { - for (int e2=0; e2flex_elemnum[f2]; e2++) { + for (int e1=0; e1 < m->flex_elemnum[f1]; e1++) { + for (int e2=0; e2 < m->flex_elemnum[f2]; e2++) { mj_collideElems(m, d, f1, e1, f2, e2); } } @@ -423,13 +425,13 @@ void mj_collision(const mjModel* m, mjData* d) { // finish merging predefined geom pairs if (npair) { - while (pairadrnflex; f++) { + for (int f=0; f < m->nflex; f++) { if (!m->flex_rigid[f] && (m->flex_contype[f] & m->flex_conaffinity[f])) { // internal collisions if (m->flex_internal[f]) { @@ -437,14 +439,14 @@ void mj_collision(const mjModel* m, mjData* d) { } // active element collisions - if (m->flex_selfcollide[f]!=mjFLEXSELF_NONE) { + if (m->flex_selfcollide[f] != mjFLEXSELF_NONE) { // element-element: midphase if (!mjDISABLED(mjDSBL_MIDPHASE) && - m->flex_selfcollide[f]!=mjFLEXSELF_NARROW && - m->flex_bvhadr[f]>=0) { + m->flex_selfcollide[f] != mjFLEXSELF_NARROW && + m->flex_bvhadr[f] >= 0) { // select midphase mode - if (m->flex_selfcollide[f]==mjFLEXSELF_BVH || - (m->flex_selfcollide[f]==mjFLEXSELF_AUTO && m->flex_dim[f]==3)) { + if (m->flex_selfcollide[f] == mjFLEXSELF_BVH || + (m->flex_selfcollide[f] == mjFLEXSELF_AUTO && m->flex_dim[f] == 3)) { mj_collideTree(m, d, nbody+f, nbody+f, 0, 0, 0); } else { mj_collideFlexSAP(m, d, f); @@ -454,9 +456,9 @@ void mj_collision(const mjModel* m, mjData* d) { // element-element: direct else { int flex_elemnum = m->flex_elemnum[f]; - for (int e1=0; e1body_weldid[bf1]==0) { - for (int i=m->body_geomadr[bf1]; ibody_geomadr[bf1]+m->body_geomnum[bf1]; i++) { - if (m->geom_type[i]==mjGEOM_PLANE) { + if (isbody1 && !isbody2 && m->body_weldid[bf1] == 0) { + for (int i=m->body_geomadr[bf1]; i < m->body_geomadr[bf1]+m->body_geomnum[bf1]; i++) { + if (m->geom_type[i] == mjGEOM_PLANE) { mj_collidePlaneFlex(m, d, i, f2); } } @@ -683,18 +685,18 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, nstack--; int node1 = stack[nstack].node1; int node2 = stack[nstack].node2; - mjtByte isleaf1 = (child1[2*node1]<0) && (child1[2*node1+1]<0); - mjtByte isleaf2 = (child2[2*node2]<0) && (child2[2*node2+1]<0); + mjtByte isleaf1 = (child1[2*node1] < 0) && (child1[2*node1+1] < 0); + mjtByte isleaf2 = (child2[2*node2] < 0) && (child2[2*node2+1] < 0); int nodeid1 = m->bvh_nodeid[bvhadr1 + node1]; int nodeid2 = m->bvh_nodeid[bvhadr2 + node2]; // SHOULD NOT OCCUR - if ((isleaf1 && nodeid1<0) || (isleaf2 && nodeid2<0)) { + if ((isleaf1 && nodeid1 < 0) || (isleaf2 && nodeid2 < 0)) { mju_error("BVH leaf has invalid node id"); } // self-collision: avoid repeated pairs - if (bf1==bf2 && node1>node2) { + if (bf1 == bf2 && node1 > node2) { continue; } @@ -702,8 +704,8 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, if (isbody1 && isbody2) { // both are leaves if (isleaf1 && isleaf2) { - mjtNum margin = mj_assignMargin(m, - mju_max(m->geom_margin[nodeid1], m->geom_margin[nodeid2])); + mjtNum maxmargin = mju_max(m->geom_margin[nodeid1], m->geom_margin[nodeid2]); + mjtNum margin = mj_assignMargin(m, maxmargin); if (!mj_filterSphere(m, d, nodeid1, nodeid2, margin)) { if (mj_collideOBB(m->geom_aabb + 6*nodeid1, m->geom_aabb + 6*nodeid2, @@ -719,8 +721,8 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, } // if no intersection at intermediate levels, stop - mjtNum margin = mj_assignMargin(m, - mju_max(m->body_margin[bf1], m->body_margin[bf2])); + mjtNum maxmargin = mju_max(m->body_margin[bf1], m->body_margin[bf2]); + mjtNum margin = mj_assignMargin(m, maxmargin); if (!mj_collideOBB(bvh1 + 6*node1, bvh2 + 6*node2, d->xipos + 3*bf1, d->ximat + 9*bf1, d->xipos + 3*bf2, d->ximat + 9*bf2, @@ -733,8 +735,8 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, else if (isbody1 && !isbody2) { // both are leaves if (isleaf1 && isleaf2) { - mjtNum margin = mj_assignMargin(m, - mju_max(m->geom_margin[nodeid1], m->flex_margin[f2])); + mjtNum maxmargin = mju_max(m->geom_margin[nodeid1], m->flex_margin[f2]); + mjtNum margin = mj_assignMargin(m, maxmargin); if (!filterBitmask(m->geom_contype[nodeid1], m->geom_conaffinity[nodeid1], m->flex_contype[f2], m->flex_conaffinity[f2]) && @@ -745,7 +747,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, NULL, NULL, margin, NULL, NULL, &initialize)) { // collide unless geom is plane (plane:flex handled separately) - if (m->geom_type[nodeid1]!=mjGEOM_PLANE) { + if (m->geom_type[nodeid1] != mjGEOM_PLANE) { mj_collideGeomElem(m, d, nodeid1, f2, nodeid2); } d->bvh_active[node1 + bvhadr1] = 1; @@ -756,12 +758,12 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, } // if no intersection at intermediate levels, stop - mjtNum margin = mj_assignMargin(m, - mju_max(m->body_margin[bf1], m->flex_margin[f2])); + mjtNum maxmargin = mju_max(m->body_margin[bf1], m->flex_margin[f2]); + mjtNum margin = mj_assignMargin(m, maxmargin); if (!mj_collideOBB(bvh1 + 6*node1, bvh2 + 6*node2, - d->xipos + 3*bf1, d->ximat + 9*bf1, - NULL, NULL, - margin, product, offset, &initialize)) { + d->xipos + 3*bf1, d->ximat + 9*bf1, + NULL, NULL, + margin, product, offset, &initialize)) { continue; } } @@ -783,8 +785,8 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, } // if no intersection at intermediate levels, stop - mjtNum margin = mj_assignMargin(m, - mju_max(m->flex_margin[f1], m->flex_margin[f2])); + mjtNum maxmargin = mju_max(m->flex_margin[f1], m->flex_margin[f2]); + mjtNum margin = mj_assignMargin(m, maxmargin); if (filterBox(bvh1 + 6*node1, bvh2 + 6*node2, margin)) { continue; } @@ -863,28 +865,28 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, // make AAMM (xmin[3], xmax[3]) for one bodyflex static void makeAAMM(const mjModel* m, mjData* d, mjtNum* aamm, int bf, const mjtNum* frame) { // body - if (bfnbody) { + if (bf < m->nbody) { int body = bf; int body_geomnum = m->body_geomnum[body]; // process all body geoms (body is collidable, should have geoms) - for (int i=0; ibody_geomadr[body]+i; mjtNum margin = mjENABLED(mjENBL_OVERRIDE) ? 0.5*m->opt.o_margin : m->geom_margin[geom]; mjtNum _aamm[6]; // set _aamm for this geom - for (int j=0; j<3; j++) { + for (int j=0; j < 3; j++) { mjtNum cen = mju_dot3(d->geom_xpos+3*geom, frame+3*j); _aamm[j] = cen - m->geom_rbound[geom] - margin; _aamm[j+3] = cen + m->geom_rbound[geom] + margin; } // update body aamm - if (i==0) { + if (i == 0) { mju_copy(aamm, _aamm, 6); } else { - for (int j=0; j<3; j++) { + for (int j=0; j < 3; j++) { aamm[j] = mju_min(aamm[j], _aamm[j]); aamm[j+3] = mju_max(aamm[j+3], _aamm[j+3]); } @@ -906,11 +908,11 @@ static void makeAAMM(const mjModel* m, mjData* d, mjtNum* aamm, int bf, const mj mju_mulMatVec(v, frame, vbase+3*i, 3, 3); // update aamm - if (i==0) { + if (i == 0) { mju_copy3(aamm, v); mju_copy3(aamm+3, v); } else { - for (int j=0; j<3; j++) { + for (int j=0; j < 3; j++) { aamm[j] = mju_min(aamm[j], v[j]); aamm[j+3] = mju_max(aamm[j+3], v[j]); } @@ -946,7 +948,7 @@ static void add_pair(const mjModel* m, int bf1, int bf2, int body_geomadr1 = m->body_geomadr[bf1]; int body_geomnum1 = m->body_geomnum[bf1]; contype1 = conaffinity1 = 0; - for (int i=body_geomadr1; igeom_contype[i]; conaffinity1 |= m->geom_conaffinity[i]; } @@ -960,7 +962,7 @@ static void add_pair(const mjModel* m, int bf1, int bf2, int body_geomadr2 = m->body_geomadr[bf2]; int body_geomnum2 = m->body_geomnum[bf2]; contype2 = conaffinity2 = 0; - for (int i=body_geomadr2; igeom_contype[i]; conaffinity2 |= m->geom_conaffinity[i]; } @@ -1021,7 +1023,7 @@ quicksortfunc(SAPcompare, context, el1, el2) { // using sweep-and-prune along specified axis (0-2). static int mj_SAP(mjData* d, const mjtNum* aamm, int n, int axis, int* pair, int maxpair) { // check inputs - if (n>=0x10000 || axis<0 || axis>2 || maxpair<1) { + if (n >= 0x10000 || axis < 0 || axis > 2 || maxpair < 1) { return -1; } @@ -1030,7 +1032,7 @@ static int mj_SAP(mjData* d, const mjtNum* aamm, int n, int axis, int* pair, int mjtSAP* activebuf = (mjtSAP*) mj_stackAllocByte(d, 2*n*sizeof(mjtSAP), _Alignof(mjtSAP)); // init sortbuf with specified axis - for (int i=0; i=maxpair) { + if (npair >= maxpair) { return maxpair; } } @@ -1088,9 +1090,9 @@ static int mj_SAP(mjData* d, const mjtNum* aamm, int n, int axis, int* pair, int // max value: remove corresponding min value from list else { int toremove = sortbuf[i].id_ismax & 0xFFFF; - for (int j=0; jbody_geomnum[b1] > 0) || - (m->body_weldid[b1]==0 && hasPlane(m, b1))) { + (m->body_weldid[b1] == 0 && hasPlane(m, b1))) { // add b1:body pairs that are not welded together - for (int b2=0; b2> 16]; int bf2 = bfid[sappair[i] & 0xFFFF]; // body pair: prune based on weld filter - if (bf1body_weldid[bf1]; int weld2 = m->body_weldid[bf2]; int parent_weld1 = m->body_weldid[m->body_parentid[weld1]]; @@ -1295,34 +1297,34 @@ static void mj_contactParam(const mjModel* m, int* condim, mjtNum* gap, mjtNum fri[3]; // get parameters from geom1 or flex1 - int priority1 = (f1<0) ? m->geom_priority[g1] : m->flex_priority[f1]; - int condim1 = (f1<0) ? m->geom_condim[g1] : m->flex_condim[f1]; - mjtNum gap1 = (f1<0) ? m->geom_gap[g1] : m->flex_gap[f1]; - mjtNum solmix1 = (f1<0) ? m->geom_solmix[g1] : m->flex_solmix[f1]; - const mjtNum* solref1 = (f1<0) ? m->geom_solref+g1*mjNREF : m->flex_solref+f1*mjNREF; - const mjtNum* solimp1 = (f1<0) ? m->geom_solimp+g1*mjNIMP : m->flex_solimp+f1*mjNIMP; - const mjtNum* friction1 = (f1<0) ? m->geom_friction+g1*3 : m->flex_friction+f1*3; + int priority1 = (f1 < 0) ? m->geom_priority[g1] : m->flex_priority[f1]; + int condim1 = (f1 < 0) ? m->geom_condim[g1] : m->flex_condim[f1]; + mjtNum gap1 = (f1 < 0) ? m->geom_gap[g1] : m->flex_gap[f1]; + mjtNum solmix1 = (f1 < 0) ? m->geom_solmix[g1] : m->flex_solmix[f1]; + const mjtNum* solref1 = (f1 < 0) ? m->geom_solref+g1*mjNREF : m->flex_solref+f1*mjNREF; + const mjtNum* solimp1 = (f1 < 0) ? m->geom_solimp+g1*mjNIMP : m->flex_solimp+f1*mjNIMP; + const mjtNum* friction1 = (f1 < 0) ? m->geom_friction+g1*3 : m->flex_friction+f1*3; // get parameters from geom2 or flex2 - int priority2 = (f2<0) ? m->geom_priority[g2] : m->flex_priority[f2]; - int condim2 = (f2<0) ? m->geom_condim[g2] : m->flex_condim[f2]; - mjtNum gap2 = (f2<0) ? m->geom_gap[g2] : m->flex_gap[f2]; - mjtNum solmix2 = (f2<0) ? m->geom_solmix[g2] : m->flex_solmix[f2]; - const mjtNum* solref2 = (f2<0) ? m->geom_solref+g2*mjNREF : m->flex_solref+f2*mjNREF; - const mjtNum* solimp2 = (f2<0) ? m->geom_solimp+g2*mjNIMP : m->flex_solimp+f2*mjNIMP; - const mjtNum* friction2 = (f2<0) ? m->geom_friction+g2*3 : m->flex_friction+f2*3; + int priority2 = (f2 < 0) ? m->geom_priority[g2] : m->flex_priority[f2]; + int condim2 = (f2 < 0) ? m->geom_condim[g2] : m->flex_condim[f2]; + mjtNum gap2 = (f2 < 0) ? m->geom_gap[g2] : m->flex_gap[f2]; + mjtNum solmix2 = (f2 < 0) ? m->geom_solmix[g2] : m->flex_solmix[f2]; + const mjtNum* solref2 = (f2 < 0) ? m->geom_solref+g2*mjNREF : m->flex_solref+f2*mjNREF; + const mjtNum* solimp2 = (f2 < 0) ? m->geom_solimp+g2*mjNIMP : m->flex_solimp+f2*mjNIMP; + const mjtNum* friction2 = (f2 < 0) ? m->geom_friction+g2*3 : m->flex_friction+f2*3; // gap: max *gap = mju_max(gap1, gap2); // different priority: copy from item with higher priority - if (priority1>priority2) { + if (priority1 > priority2) { *condim = condim1; mju_copy(solref, solref1, mjNREF); mju_copy(solimp, solimp1, mjNIMP); mju_copy(fri, friction1, 3); } - else if (priority1=mjMINVAL && solmix2>=mjMINVAL) { + if (solmix1 >= mjMINVAL && solmix2 >= mjMINVAL) { mix = solmix1 / (solmix1 + solmix2); - } else if (solmix10 && solref2[0]>0) { - for (int i=0; i 0 && solref2[0] > 0) { + for (int i=0; i < mjNREF; i++) { solref[i] = mix*solref1[i] + (1-mix)*solref2[i]; } } // reference direct: min else { - for (int i=0; i6 || *condim<1) { + if (*condim > 6 || *condim < 1) { mjERROR("Invalid condim value: %d", *condim); } } @@ -1501,7 +1503,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) { // allocate mjContact[mjMAXCONPAIR] on the arena mjContact* con = - (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact)); + (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact)); if (!con) { mj_warning(d, mjWARN_CONTACTFULL, d->ncon); return; @@ -1578,7 +1580,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) { } // add contacts returned by collision detector - for (int i=0; iflexvert_xpos + 3*(m->flex_vertadr[f]+i); // distance from plane to vertex @@ -1678,7 +1680,7 @@ static int planeVertex(mjContact* con, const mjtNum* pos, mjtNum rad, // project, check distance mjtNum dst = mju_dot3(ev, nrm); - if (dst<=-2*rad) { + if (dst <= -2*rad) { return 0; } @@ -1698,13 +1700,13 @@ void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) { int flex_evpairnum = m->flex_evpairnum[f]; // predefined element-vertex - for (int i=0; iflex_evpair + 2*m->flex_evpairadr[f] + 2*i; mj_collideElemVert(m, d, f, ev[0], ev[1]); } // within-element for tetrahedral only - if (m->flex_dim[f]!=3) { + if (m->flex_dim[f] != 3) { return; } @@ -1725,7 +1727,7 @@ void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) { // process all elements const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f]; - for (int e=0; eflex_elem + m->flex_elemdataadr[f] + e*4; con.elem[0] = e; @@ -1766,14 +1768,14 @@ void mj_collideFlexSAP(const mjModel* m, mjData* d, int f) { int* elid = mj_stackAllocInt(d, m->flex_elemnum[f]); int nactive = 0; int flex_elemnum = m->flex_elemnum[f]; - for (int i=0; iflexelem_aabb + 6*m->flex_elemadr[f]; - for (int i=0; ibvh_aabb_dyn + 6*(m->flex_bvhadr[f] - m->nbvhstatic); - int axis = (bvh[3]>bvh[4] && bvh[3]>bvh[5]) ? 0 : (bvh[4]>bvh[5] ? 1 : 2); + int axis = (bvh[3] > bvh[4] && bvh[3] > bvh[5]) ? 0 : (bvh[4] > bvh[5] ? 1 : 2); // call SAP; hard limit on number of pairs to avoid out-of-memory int maxsappair = mjMIN(nactive*(nactive-1)/2, 1000000); int* sappair = mj_stackAllocInt(d, maxsappair); int nsappair = mj_SAP(d, aamm, nactive, axis, sappair, maxsappair); - if (nsappair<0) { + if (nsappair < 0) { mjERROR("SAP failed"); } // send SAP pairs to nearphase - for (int i=0; i> 16]; int e2 = elid[sappair[i] & 0xFFFF]; mj_collideElems(m, d, f, e1, f, e2); @@ -1829,62 +1831,62 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { int b = m->geom_bodyid[g]; const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1); const int* bdata = m->flex_vertbodyid + m->flex_vertadr[f]; - for (int i=0; i<=dim; i++) { - if (b==bdata[edata[i]]) { + for (int i=0; i <= dim; i++) { + if (b == bdata[edata[i]]) { return; } } // allocate mjContact[mjMAXCONPAIR] on the arena mjContact* con = - (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact)); + (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact)); if (!con) { mj_warning(d, mjWARN_CONTACTFULL, d->ncon); return; } // sphere/capsule/box : capsule - if (dim==1 && (type==mjGEOM_SPHERE || type==mjGEOM_CAPSULE || type==mjGEOM_BOX)) { + if (dim == 1 && (type == mjGEOM_SPHERE || type == mjGEOM_CAPSULE || type == mjGEOM_BOX)) { // make capsule from vertices mjtNum pos[3], mat[9], size[2]; mj_makeCapsule(m, d, f, m->flex_elem + m->flex_elemdataadr[f] + e*2, pos, mat, size); // call raw primitive for corresponding geom type - if (type==mjGEOM_SPHERE) { + if (type == mjGEOM_SPHERE) { num = mjraw_SphereCapsule(con, margin, - d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g, - pos, mat, size); + d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g, + pos, mat, size); } - else if (type==mjGEOM_CAPSULE) { + else if (type == mjGEOM_CAPSULE) { num = mjraw_CapsuleCapsule(con, margin, - d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g, - pos, mat, size); + d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g, + pos, mat, size); } else { num = mjraw_CapsuleBox(con, margin, - pos, mat, size, - d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g); + pos, mat, size, + d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g); // reverse contact normals, since box geom is second - for (int i=0; iflexvert_xpos + 3*m->flex_vertadr[f]; num = mjraw_SphereTriangle(con, margin, - d->geom_xpos+3*g, m->geom_size[3*g], - vertxpos + 3*edata[0], vertxpos + 3*edata[1], - vertxpos + 3*edata[2], m->flex_radius[f]); + d->geom_xpos+3*g, m->geom_size[3*g], + vertxpos + 3*edata[0], vertxpos + 3*edata[1], + vertxpos + 3*edata[2], m->flex_radius[f]); } // general geom : elem @@ -1905,7 +1907,7 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { mj_contactParam(m, &condim, &gap, solref, solimp, friction, g, -1, -1, f); // add contacts - for (int i=0; iflex_elem + m->flex_elemdataadr[f2] + e2*(dim2+1); const int* bdata1 = m->flex_vertbodyid + m->flex_vertadr[f1]; const int* bdata2 = m->flex_vertbodyid + m->flex_vertadr[f2]; - for (int i1=0; i1<=dim1; i1++) { + for (int i1=0; i1 <= dim1; i1++) { int b1 = bdata1[edata1[i1]]; - for (int i2=0; i2<=dim2; i2++) { - if (b1==bdata2[edata2[i2]]) { + for (int i2=0; i2 <= dim2; i2++) { + if (b1 == bdata2[edata2[i2]]) { return; } } @@ -1963,14 +1965,14 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2 // allocate mjContact[mjMAXCONPAIR] on the arena mjContact* con = - (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact)); + (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact)); if (!con) { mj_warning(d, mjWARN_CONTACTFULL, d->ncon); return; } // capsule : capsule - if (dim1==1 && dim2==1) { + if (dim1 == 1 && dim2 == 1) { // make capsules from vertices mjtNum pos1[3], mat1[9], size1[2]; mjtNum pos2[3], mat2[9], size2[2]; @@ -2001,12 +2003,12 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2 mj_contactParam(m, &condim, &gap, solref, solimp, friction, -1, -1, f1, f2); // ignore gap in self collision, since margin is ignored - if (f1==f2) { + if (f1 == f2) { gap = 0; } // add contacts - for (int i=0; incon); return; } // sphere : capsule - if (dim==1) { + if (dim == 1) { mjtNum pos[3], mat[9], size[2]; mjtNum I[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; mj_makeCapsule(m, d, f, edata, pos, mat, size); @@ -2069,11 +2071,11 @@ void mj_collideElemVert(const mjModel* m, mjData* d, int f, int e, int v) { } // sphere : triangle - else if (dim==2) { + else if (dim == 2) { const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f]; num = mjraw_SphereTriangle(con, 0, vert, radius, - vertxpos + 3*edata[0], vertxpos + 3*edata[1], - vertxpos + 3*edata[2], radius); + vertxpos + 3*edata[0], vertxpos + 3*edata[1], + vertxpos + 3*edata[2], radius); } // sphere : tetrahdron @@ -2094,7 +2096,7 @@ void mj_collideElemVert(const mjModel* m, mjData* d, int f, int e, int v) { mj_contactParam(m, &condim, &gap, solref, solimp, friction, -1, -1, f, f); // add contacts - for (int i=0; i=1) { + } else if (a >= 1) { res[0] = v[0]; res[1] = v[1]; } else { @@ -582,7 +582,7 @@ int mjraw_SphereTriangle(mjContact* con, mjtNum margin, mjtNum sign3 = areaSign(p, b, o); // p is inside triangle - if (sign1==sign2 && sign2==sign3) { + if (sign1 == sign2 && sign2 == sign3) { // P is nearest point to S within triangle mju_copy3(X, P); } @@ -596,7 +596,7 @@ int mjraw_SphereTriangle(mjContact* con, mjtNum margin, dstx[2] = pointSegment(x[2], p, b, o); // select minimum - int best = (dstx[0]rbound) { + if (dst > rbound) { return 0; } diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c index f754c9e3..1e557e1a 100644 --- a/src/engine/engine_collision_sdf.c +++ b/src/engine/engine_collision_sdf.c @@ -43,44 +43,44 @@ static mjtNum geomDistance(const mjModel* m, const mjData* d, const mjpPlugin* p // see https://iquilezles.org/articles/distfunctions/ switch (type) { - case mjGEOM_PLANE: - return x[2]; - case mjGEOM_SPHERE: - return mju_norm3(x) - size[0]; - case mjGEOM_BOX: - a[0] = mju_abs(x[0]) - size[0]; - a[1] = mju_abs(x[1]) - size[1]; - a[2] = mju_abs(x[2]) - size[2]; - b[0] = mju_max(a[0], 0); - b[1] = mju_max(a[1], 0); - b[2] = mju_max(a[2], 0); - return mju_norm3(b) + mju_min(mju_max(a[0], mju_max(a[1], a[2])), 0); - case mjGEOM_CAPSULE: - a[0] = x[0]; - a[1] = x[1]; - a[2] = x[2] - mju_clip(x[2], -size[1], size[1]); - return mju_norm3(a) - size[0]; - case mjGEOM_ELLIPSOID: - a[0] = x[0] / size[0]; - a[1] = x[1] / size[1]; - a[2] = x[2] / size[2]; - b[0] = a[0] / size[0]; - b[1] = a[1] / size[1]; - b[2] = a[2] / size[2]; - mjtNum k0 = mju_norm3(a); - mjtNum k1 = mju_norm3(b); - return k0 * (k0 - 1.0) / k1; - case mjGEOM_CYLINDER: - a[0] = mju_sqrt(x[0]*x[0]+x[1]*x[1]) - size[0]; - a[1] = mju_abs(x[2]) - size[1]; - b[0] = mju_max(a[0], 0); - b[1] = mju_max(a[1], 0); - return mju_min(mju_max(a[0], a[1]), 0) + mju_norm(b, 2); - case mjGEOM_SDF: - return p->sdf_distance(x, d, i); - default: - mjERROR("sdf collisions not available for geom type %d", type); - return 0; + case mjGEOM_PLANE: + return x[2]; + case mjGEOM_SPHERE: + return mju_norm3(x) - size[0]; + case mjGEOM_BOX: + a[0] = mju_abs(x[0]) - size[0]; + a[1] = mju_abs(x[1]) - size[1]; + a[2] = mju_abs(x[2]) - size[2]; + b[0] = mju_max(a[0], 0); + b[1] = mju_max(a[1], 0); + b[2] = mju_max(a[2], 0); + return mju_norm3(b) + mju_min(mju_max(a[0], mju_max(a[1], a[2])), 0); + case mjGEOM_CAPSULE: + a[0] = x[0]; + a[1] = x[1]; + a[2] = x[2] - mju_clip(x[2], -size[1], size[1]); + return mju_norm3(a) - size[0]; + case mjGEOM_ELLIPSOID: + a[0] = x[0] / size[0]; + a[1] = x[1] / size[1]; + a[2] = x[2] / size[2]; + b[0] = a[0] / size[0]; + b[1] = a[1] / size[1]; + b[2] = a[2] / size[2]; + mjtNum k0 = mju_norm3(a); + mjtNum k1 = mju_norm3(b); + return k0 * (k0 - 1.0) / k1; + case mjGEOM_CYLINDER: + a[0] = mju_sqrt(x[0]*x[0]+x[1]*x[1]) - size[0]; + a[1] = mju_abs(x[2]) - size[1]; + b[0] = mju_max(a[0], 0); + b[1] = mju_max(a[1], 0); + return mju_min(mju_max(a[0], a[1]), 0) + mju_norm(b, 2); + case mjGEOM_SDF: + return p->sdf_distance(x, d, i); + default: + mjERROR("sdf collisions not available for geom type %d", type); + return 0; } } @@ -92,83 +92,83 @@ static void geomGradient(mjtNum gradient[3], const mjModel* m, const mjData* d, // see https://iquilezles.org/articles/distfunctions/ switch (type) { - case mjGEOM_PLANE: - mju_zero3(gradient); - gradient[2] = 1; - break; - case mjGEOM_SPHERE: - mju_copy3(gradient, x); - c = mju_norm3(x); - gradient[0] *= 1. / c; - gradient[1] *= 1. / c; - gradient[2] *= 1. / c; - break; - case mjGEOM_BOX: - mju_zero3(gradient); - a[0] = mju_abs(x[0]) - size[0]; - a[1] = mju_abs(x[1]) - size[1]; - a[2] = mju_abs(x[2]) - size[2]; - int k = a[0] > a[1] ? 0 : 1; - int l = a[2] > a[k] ? 2 : k; - if (a[l]<0) { - gradient[l] = x[l] / mju_abs(x[l]); - } else { - b[0] = mju_max(a[0], 0); - b[1] = mju_max(a[1], 0); - b[2] = mju_max(a[2], 0); - c = mju_norm3(b); - gradient[0] = a[0]>0 ? b[0] / c * x[0] / mju_abs(x[0]) : 0; - gradient[1] = a[1]>0 ? b[1] / c * x[1] / mju_abs(x[1]) : 0; - gradient[2] = a[2]>0 ? b[2] / c * x[2] / mju_abs(x[2]) : 0; - } - break; - case mjGEOM_CAPSULE: - a[0] = x[0]; - a[1] = x[1]; - a[2] = x[2] - mju_clip(x[2], -size[1], size[1]); - c = mju_norm3(a); - gradient[0] = a[0] / c; - gradient[1] = a[1] / c; - gradient[2] = a[2] / c; - break; - case mjGEOM_ELLIPSOID: - a[0] = x[0] / size[0]; - a[1] = x[1] / size[1]; - a[2] = x[2] / size[2]; - b[0] = a[0] / size[0]; - b[1] = a[1] / size[1]; - b[2] = a[2] / size[2]; - mjtNum k0 = mju_norm3(a); - mjtNum k1 = mju_norm3(b); - gradient[0] = a[0]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[0]/(k1*k1); - gradient[1] = a[1]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[1]/(k1*k1); - gradient[2] = a[2]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[2]/(k1*k1); - break; - case mjGEOM_CYLINDER: - c = mju_sqrt(x[0]*x[0]+x[1]*x[1]); - e = mju_abs(x[2]); - a[0] = c - size[0]; - a[1] = e - size[1]; - mjtNum grada[3] = {x[0] / c, x[1] / c, x[2] / e}; - int j = a[0] > a[1] ? 0 : 1; - if (a[j] < 0) { - gradient[0] = j==0 ? grada[0] : 0; - gradient[1] = j==0 ? grada[1] : 0; - gradient[2] = j==1 ? grada[2] : 0; - } else { - b[0] = mju_max(a[0], 0); - b[1] = mju_max(a[1], 0); - mjtNum bnorm = mju_norm(b, 2); - gradient[0] = grada[0] * b[0] / bnorm; - gradient[1] = grada[1] * b[0] / bnorm; - gradient[2] = grada[2] * b[1] / bnorm; - } - break; - case mjGEOM_SDF: - p->sdf_gradient(gradient, x, d, i); - break; - default: - mjERROR("sdf collisions not available for geom type %d", type); + case mjGEOM_PLANE: + mju_zero3(gradient); + gradient[2] = 1; + break; + case mjGEOM_SPHERE: + mju_copy3(gradient, x); + c = mju_norm3(x); + gradient[0] *= 1. / c; + gradient[1] *= 1. / c; + gradient[2] *= 1. / c; + break; + case mjGEOM_BOX: + mju_zero3(gradient); + a[0] = mju_abs(x[0]) - size[0]; + a[1] = mju_abs(x[1]) - size[1]; + a[2] = mju_abs(x[2]) - size[2]; + int k = a[0] > a[1] ? 0 : 1; + int l = a[2] > a[k] ? 2 : k; + if (a[l] < 0) { + gradient[l] = x[l] / mju_abs(x[l]); + } else { + b[0] = mju_max(a[0], 0); + b[1] = mju_max(a[1], 0); + b[2] = mju_max(a[2], 0); + c = mju_norm3(b); + gradient[0] = a[0] > 0 ? b[0] / c * x[0] / mju_abs(x[0]) : 0; + gradient[1] = a[1] > 0 ? b[1] / c * x[1] / mju_abs(x[1]) : 0; + gradient[2] = a[2] > 0 ? b[2] / c * x[2] / mju_abs(x[2]) : 0; + } + break; + case mjGEOM_CAPSULE: + a[0] = x[0]; + a[1] = x[1]; + a[2] = x[2] - mju_clip(x[2], -size[1], size[1]); + c = mju_norm3(a); + gradient[0] = a[0] / c; + gradient[1] = a[1] / c; + gradient[2] = a[2] / c; + break; + case mjGEOM_ELLIPSOID: + a[0] = x[0] / size[0]; + a[1] = x[1] / size[1]; + a[2] = x[2] / size[2]; + b[0] = a[0] / size[0]; + b[1] = a[1] / size[1]; + b[2] = a[2] / size[2]; + mjtNum k0 = mju_norm3(a); + mjtNum k1 = mju_norm3(b); + gradient[0] = a[0]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[0]/(k1*k1); + gradient[1] = a[1]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[1]/(k1*k1); + gradient[2] = a[2]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[2]/(k1*k1); + break; + case mjGEOM_CYLINDER: + c = mju_sqrt(x[0]*x[0]+x[1]*x[1]); + e = mju_abs(x[2]); + a[0] = c - size[0]; + a[1] = e - size[1]; + mjtNum grada[3] = {x[0] / c, x[1] / c, x[2] / e}; + int j = a[0] > a[1] ? 0 : 1; + if (a[j] < 0) { + gradient[0] = j == 0 ? grada[0] : 0; + gradient[1] = j == 0 ? grada[1] : 0; + gradient[2] = j == 1 ? grada[2] : 0; + } else { + b[0] = mju_max(a[0], 0); + b[1] = mju_max(a[1], 0); + mjtNum bnorm = mju_norm(b, 2); + gradient[0] = grada[0] * b[0] / bnorm; + gradient[1] = grada[1] * b[0] / bnorm; + gradient[2] = grada[2] * b[1] / bnorm; + } + break; + case mjGEOM_SDF: + p->sdf_gradient(gradient, x, d, i); + break; + default: + mjERROR("sdf collisions not available for geom type %d", type); } } @@ -179,16 +179,16 @@ mjtNum mjc_distance(const mjModel* m, const mjData* d, const mjSDF* s, const mjt mjtNum y[3]; switch (s->type) { - case mjSDFTYPE_SINGLE: - return geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]); - case mjSDFTYPE_INTERSECTION: - mju_rotVecMat(y, x, s->relmat); - mju_addTo3(y, s->relpos); - return mju_max(geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]), - geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1])); - default: - mjERROR("SDF type not available"); - return 0; + case mjSDFTYPE_SINGLE: + return geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]); + case mjSDFTYPE_INTERSECTION: + mju_rotVecMat(y, x, s->relmat); + mju_addTo3(y, s->relpos); + return mju_max(geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]), + geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1])); + default: + mjERROR("SDF type not available"); + return 0; } } @@ -199,33 +199,33 @@ void mjc_gradient(const mjModel* m, const mjData* d, const mjSDF* s, const mjtNum* point[2] = {x, y}; switch (s->type) { - case mjSDFTYPE_INTERSECTION: - mju_rotVecMat(y, x, s->relmat); - mju_addTo3(y, s->relpos); - int i = geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]) > - geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1]) ? 0 : 1; - geomGradient(gradient, m, d, s->plugin[i], s->id[i], point[i], s->geomtype[i]); - if (i==1) { - mju_rotVecMatT(gradient, gradient, s->relmat); - } - break; - case mjSDFTYPE_AVERAGE: - mju_rotVecMat(y, x, s->relmat); - mju_addTo3(y, s->relpos); - mjtNum grad1[3], grad2[3]; - geomGradient(grad1, m, d, s->plugin[0], s->id[0], x, s->geomtype[0]); - mju_normalize3(grad1); - geomGradient(grad2, m, d, s->plugin[1], s->id[1], y, s->geomtype[1]); - mju_rotVecMatT(grad2, grad2, s->relmat); - mju_normalize3(grad2); - mju_sub3(gradient, grad1, grad2); - mju_normalize3(gradient); - break; - case mjSDFTYPE_SINGLE: - geomGradient(gradient, m, d, s->plugin[0], s->id[0], point[0], s->geomtype[0]); - break; - default: - mjERROR("SDF type not available"); + case mjSDFTYPE_INTERSECTION: + mju_rotVecMat(y, x, s->relmat); + mju_addTo3(y, s->relpos); + int i = geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]) > + geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1]) ? 0 : 1; + geomGradient(gradient, m, d, s->plugin[i], s->id[i], point[i], s->geomtype[i]); + if (i == 1) { + mju_rotVecMatT(gradient, gradient, s->relmat); + } + break; + case mjSDFTYPE_AVERAGE: + mju_rotVecMat(y, x, s->relmat); + mju_addTo3(y, s->relpos); + mjtNum grad1[3], grad2[3]; + geomGradient(grad1, m, d, s->plugin[0], s->id[0], x, s->geomtype[0]); + mju_normalize3(grad1); + geomGradient(grad2, m, d, s->plugin[1], s->id[1], y, s->geomtype[1]); + mju_rotVecMatT(grad2, grad2, s->relmat); + mju_normalize3(grad2); + mju_sub3(gradient, grad1, grad2); + mju_normalize3(gradient); + break; + case mjSDFTYPE_SINGLE: + geomGradient(gradient, m, d, s->plugin[0], s->id[0], point[0], s->geomtype[0]); + break; + default: + mjERROR("SDF type not available"); } } @@ -257,7 +257,7 @@ static void undoTransformation(const mjModel* m, const mjData* d, int g, mjtNum sdf_xpos[3], mjtNum sdf_quat[4]) { mjtNum* xpos = d->geom_xpos + 3 * g; mjtNum* xmat = d->geom_xmat + 9 * g; - if (m->geom_type[g]==mjGEOM_MESH || m->geom_type[g]==mjGEOM_SDF) { + if (m->geom_type[g] == mjGEOM_MESH || m->geom_type[g] == mjGEOM_SDF) { mjtNum negpos[3], negquat[4], xquat[4]; mjtNum* pos = m->mesh_pos + 3 * m->geom_dataid[g]; mjtNum* quat = m->mesh_quat + 4 * m->geom_dataid[g]; @@ -328,19 +328,19 @@ static int addContact(mjtNum* points, mjContact* con, const mjtNum x[3], // finds minimum of Frank-Wolfe objective static mjtNum stepFrankWolfe(mjtNum x[3], const mjtNum* corners, int ncorners, const mjModel* m, const mjSDF* sdf, mjData* d) { - for (int step=0; stepopt.sdf_iterations; step++) { + for (int step=0; step < m->opt.sdf_iterations; step++) { mjtNum best = 1e10, fun, s[3], grad[3]; // evaluate gradient mjc_gradient(m, d, sdf, grad, x); // evaluate all corners - for (int i=0; iopt.sdf_iterations; step++) { + for (int step=0; step < m->opt.sdf_iterations; step++) { mjtNum grad[3]; // evaluate gradient mjc_gradient(m, d, s, grad, x); // sanity check - if (isnan(grad[0]) || grad[0]>mjMAXVAL || grad[0]<-mjMAXVAL || - isnan(grad[1]) || grad[1]>mjMAXVAL || grad[1]<-mjMAXVAL || - isnan(grad[2]) || grad[2]>mjMAXVAL || grad[2]<-mjMAXVAL) { + if (isnan(grad[0]) || grad[0] > mjMAXVAL || grad[0] < -mjMAXVAL || + isnan(grad[1]) || grad[1] > mjMAXVAL || grad[1] < -mjMAXVAL || + isnan(grad[2]) || grad[2] > mjMAXVAL || grad[2] < -mjMAXVAL) { return mjMAXVAL; } @@ -422,7 +422,7 @@ static int triangleIntersect(const mjtNum triangle[9], const mjModel* m, // coordinate change mju_addTo3(center, p); - return mjc_distance(m, d, sdf, center)= max_stack) mjERROR("BVH stack depth exceeded."); stack[nstack].node = child[2*node+i]; @@ -551,9 +551,9 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g collideBVH(m, (mjData*)d, g1, offset, rotation, faces, &npoints, &n0, &sdf); // Frank-Wolfe algorithm - for (int i=0; imesh_vert[3*(vertadr+m->mesh_face[3*face+v])+0], m->mesh_vert[3*(vertadr+m->mesh_face[3*face+v])+1], @@ -576,13 +576,13 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g x[2] = (corners[2]+corners[5]+corners[8])/3; // SHOULD NOT OCCUR - if (ncandidate==MAXMESHPNT) mjERROR("too many contact points"); + if (ncandidate == MAXMESHPNT)mjERROR("too many contact points"); // Frank-Wolfe depth = stepFrankWolfe(x, corners, 3, m, &sdf, (mjData*)d); // store candidate if there is penetration - if (depth<0) { + if (depth < 0) { mju_copy3(candidate + 3*ncandidate, x); index[ncandidate] = ncandidate; dist[ncandidate++] = depth; @@ -593,7 +593,7 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g mjQUICKSORT(index, ncandidate, sizeof(int), distcompare, dist); // add only the first mjMAXCONPAIR pairs - for (int i=0; iopt.sdf_initpoints) { + while (i < m->opt.sdf_initpoints) { x[0] = aabb[0] + (aabb[3]-aabb[0]) * mju_Halton(j, 2); x[1] = aabb[1] + (aabb[4]-aabb[1]) * mju_Halton(j, 3); x[2] = aabb[2] + (aabb[5]-aabb[2]) * mju_Halton(j, 5); @@ -723,7 +723,9 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m cnt = addContact(contacts, con, x, pos2true, squat2, dist, cnt, m, &sdf, (mjData*)d); // SHOULD NOT OCCUR - if (cnt>mjMAXCONPAIR) mjERROR("too many contact points"); + if (cnt > mjMAXCONPAIR) { + mjERROR("too many contact points"); + } } return cnt; diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index ba2bf539..d6f723f6 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -124,11 +124,11 @@ int mj_isDual(const mjModel* m) { // assign/clamp contact friction parameters void mj_assignFriction(const mjModel* m, mjtNum* target, const mjtNum* source) { if (mjENABLED(mjENBL_OVERRIDE)) { - for (int i=0; i<5; i++) { + for (int i=0; i < 5; i++) { target[i] = mju_max(mjMINMU, m->opt.o_friction[i]); } } else { - for (int i=0; i<5; i++) { + for (int i=0; i < 5; i++) { target[i] = mju_max(mjMINMU, source[i]); } } @@ -181,20 +181,20 @@ static int mj_elemBodyWeight(const mjModel* m, const mjData* d, int f, int e, in // compute inverse distances from contact point to element vertices // save body ids, find vertex v in element int vid = -1; - for (int i=0; i<=dim; i++) { + for (int i=0; i <= dim; i++) { mjtNum dist = mju_dist3(point, vert+3*edata[i]); weight[i] = 1.0/(mju_max(mjMINVAL, dist)); body[i] = m->flex_vertbodyid[m->flex_vertadr[f] + edata[i]]; // check if element vertex matches v - if (edata[i]==v) { + if (edata[i] == v) { vid = i; } } // v found in e: skip and shift remaining - if (vid>=0) { - while (vid= 0) { + while (vid < dim) { weight[vid] = weight[vid+1]; body[vid] = body[vid+1]; vid++; @@ -242,9 +242,9 @@ int mj_addContact(const mjModel* m, mjData* d, const mjContact* con) { // add #size rows to constraint Jacobian; set pos, margin, frictionloss, type, id static void mj_addConstraint(const mjModel* m, mjData* d, - const mjtNum* jac, const mjtNum* pos, - const mjtNum* margin, mjtNum frictionloss, - int size, int type, int id, int NV, const int* chain) { + const mjtNum* jac, const mjtNum* pos, + const mjtNum* margin, mjtNum frictionloss, + int size, int type, int id, int NV, const int* chain) { int empty, nv = m->nv, nefc = d->nefc; int *nnz = d->efc_J_rownnz, *adr = d->efc_J_rowadr, *ind = d->efc_J_colind; mjtNum *J = d->efc_J; @@ -665,7 +665,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { flex_edgenum = m->flex_edgenum[id[0]]; // add one constraint per edge - for (int e=flex_edgeadr; eflexedge_length[e] - m->flexedge_length0[e]; @@ -930,14 +930,14 @@ void mj_instantiateContact(const mjModel* m, mjData* d) { con->efc_address = d->nefc; // special case: single body on each side - if ((con->geom[0]>=0 || con->vert[0]>=0) && - (con->geom[1]>=0 || con->vert[1]>=0)) { + if ((con->geom[0] >= 0 || con->vert[0] >= 0) && + (con->geom[1] >= 0 || con->vert[1] >= 0)) { // get bodies int bid[2]; for (int side=0; side < 2; side++) { - bid[side] = (con->geom[side]>=0) ? - m->geom_bodyid[con->geom[side]] : - m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]]; + bid[side] = (con->geom[side] >= 0) ? + m->geom_bodyid[con->geom[side]] : + m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]]; } // compute Jacobian differences @@ -956,16 +956,16 @@ void mj_instantiateContact(const mjModel* m, mjData* d) { int nb = 0; int bid[8]; mjtNum bweight[8]; - for (int side=0; side<2; side++) { + for (int side=0; side < 2; side++) { // geom - if (con->geom[side]>=0) { + if (con->geom[side] >= 0) { bid[nb] = m->geom_bodyid[con->geom[side]]; bweight[nb] = side ? +1 : -1; nb++; } // flex vert - else if (con->vert[side]>=0) { + else if (con->vert[side] >= 0) { bid[nb] = m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]]; bweight[nb] = side ? +1 : -1; nb++; @@ -977,7 +977,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) { con->vert[1-side], con->pos, bid+nb, bweight+nb); // negative sign for first side of contact - if (side==0) { + if (side == 0) { mju_scl(bweight+nb, bweight+nb, -1, nw); } @@ -986,7 +986,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) { } // combine weighted Jacobians - NV = mj_jacSum(m, d, chain, nb, bid, bweight, con->pos, jacdif, dim>3); + NV = mj_jacSum(m, d, chain, nb, bid, bweight, con->pos, jacdif, dim > 3); } // skip contact if no DOFs affected @@ -1068,7 +1068,7 @@ void mj_diagApprox(const mjModel* m, mjData* d) { id = d->efc_id[i]; // clear edge counter - if (d->efc_type[i]!=mjEQ_FLEX) { + if (d->efc_type[i] != mjEQ_FLEX) { edgecnt = 0; } @@ -1139,31 +1139,33 @@ void mj_diagApprox(const mjModel* m, mjData* d) { // add the average translation and rotation components from both sides tran = rot = 0; - for (int side=0; side<2; side++) { + for (int side=0; side < 2; side++) { // get bodies and weights int nb, bid[4]; mjtNum bweight[4]; // geom - if (con->geom[side]>=0) { + if (con->geom[side] >= 0) { bid[0] = m->geom_bodyid[con->geom[side]]; bweight[0] = 1; nb = 1; + } // flex vert - } else if (con->vert[side]>=0) { + else if (con->vert[side] >= 0) { bid[0] = m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]]; bweight[0] = 1; nb = 1; + } // flex elem - } else { + else { nb = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side], con->vert[1-side], con->pos, bid, bweight); } // add weighted average over bodies - for (int k=0; kbody_invweight0[2*bid[k]] * bweight[k]; rot += m->body_invweight0[2*bid[k]+1] * bweight[k]; } @@ -1188,7 +1190,7 @@ void mj_diagApprox(const mjModel* m, mjData* d) { else { for (int j=0; j < dim-1; j++) { fri = con->friction[j]; - dA[i+2*j] = dA[i+2*j+1] = tran + fri*fri*(j<2 ? tran : rot); + dA[i+2*j] = dA[i+2*j+1] = tran + fri*fri*(j < 2 ? tran : rot); } // processed 2*dim-2 elements in one i-loop iteration; advance counter @@ -1401,7 +1403,7 @@ void mj_makeImpedance(const mjModel* m, mjData* d) { // elliptic contacts use solreffriction in non-normal directions, if non-zero int elliptic_friction = (tp == mjCNSTR_CONTACT_ELLIPTIC) && (j > 0); mjtNum* ref = elliptic_friction && (solreffriction[0] || solreffriction[1]) ? - solreffriction : solref; + solreffriction : solref; // friction: K = 0 if (tp == mjCNSTR_FRICTION_DOF || tp == mjCNSTR_FRICTION_TENDON || elliptic_friction) { @@ -1526,7 +1528,7 @@ static int mj_jacSumCount(const mjModel* m, mjData* d, int* chain, NV = mj_bodyChain(m, body[0], chain); // accumulate remaining - for (int i=1; iflex_edgenum[id[0]]; // process edges of this flex - for (int e=flex_edgeadr; eflex_vertbodyid[m->flex_vertadr[id[0]] + m->flex_edge[2*e]]; int b2 = m->flex_vertbodyid[m->flex_vertadr[id[0]] + m->flex_edge[2*e+1]]; @@ -1665,7 +1667,7 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) { // accumulate counts; flex NV already accumulated ne += mj_addConstraintCount(m, size, NV); - nnze += (m->eq_type[i]==mjEQ_FLEX) ? NV : size*NV; + nnze += (m->eq_type[i] == mjEQ_FLEX) ? NV : size*NV; } } @@ -1805,7 +1807,7 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) { int nb = 0, bid[8]; for (int side=0; side < 2; side++) { // geom - if (con->geom[side]>=0) { + if (con->geom[side] >= 0) { bid[nb++] = m->geom_bodyid[con->geom[side]]; } @@ -1819,7 +1821,7 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) { int f = con->flex[side]; int fdim = m->flex_dim[f]; const int* edata = m->flex_elem + m->flex_elemdataadr[f] + con->elem[side]*(fdim+1); - for (int k=0; k<=fdim; k++) { + for (int k=0; k <= fdim; k++) { bid[nb++] = m->flex_vertbodyid[m->flex_vertadr[f] + edata[k]]; } } diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index ba01e9dd..f083335e 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -284,7 +284,7 @@ void mj_camlight(const mjModel* m, mjData* d) { // adjust for mode switch ((mjtCamLight) m->cam_mode[i]) { case mjCAMLIGHT_FIXED: - break; + break; case mjCAMLIGHT_TRACK: case mjCAMLIGHT_TRACKCOM: // fixed global orientation @@ -393,15 +393,15 @@ void mj_updateDynamicBVH(const mjModel* m, mjData* d, int bvhadr, int bvhnum) { mju_zeroInt(modified, bvhnum); // mark leafs as modified - for (int i=0; ibvh_nodeid[bvhadr+i]>=0) { + for (int i=0; i < bvhnum; i++) { + if (m->bvh_nodeid[bvhadr+i] >= 0) { modified[i] = 1; } } // update non-leafs in backward pass (parents come before children) - for (int i=bvhnum-1; i>=0; i--) { - if (m->bvh_nodeid[bvhadr+i]<0) { + for (int i=bvhnum-1; i >= 0; i--) { + if (m->bvh_nodeid[bvhadr+i] < 0) { int child1 = m->bvh_child[2*(bvhadr+i)]; int child2 = m->bvh_child[2*(bvhadr+i)+1]; @@ -413,13 +413,13 @@ void mj_updateDynamicBVH(const mjModel* m, mjData* d, int bvhadr, int bvhnum) { // compute new (min, max) mjtNum xmin[3], xmax[3]; - for (int k=0; k<3; k++) { + for (int k=0; k < 3; k++) { xmin[k] = mju_min(aabb1[k] - aabb1[k+3], aabb2[k] - aabb2[k+3]); xmax[k] = mju_max(aabb1[k] + aabb1[k+3], aabb2[k] + aabb2[k+3]); } // convert to (center, size) - for (int k=0; k<3; k++) { + for (int k=0; k < 3; k++) { aabb[k] = 0.5*(xmax[k]+xmin[k]); aabb[k+3] = 0.5*(xmax[k]-xmin[k]); } @@ -446,20 +446,20 @@ void mj_flex(const mjModel* m, mjData* d) { } // compute Cartesian positions of flex vertices - for (int f=0; fnflex; f++) { + for (int f=0; f < m->nflex; f++) { int vstart = m->flex_vertadr[f]; int vend = m->flex_vertadr[f] + m->flex_vertnum[f]; // centered: copy body position if (m->flex_centered[f]) { - for (int i=vstart; iflexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]); } } // non-centered: map from local to global else { - for (int i=vstart; iflexvert_xpos+3*i, m->flex_vert+3*i, d->xmat+9*m->flex_vertbodyid[i]); mju_addTo3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]); } @@ -467,11 +467,11 @@ void mj_flex(const mjModel* m, mjData* d) { } // compute flex element aabb - for (int f=0; fnflex; f++) { + for (int f=0; f < m->nflex; f++) { int dim = m->flex_dim[f]; // process elements of this flex - for (int e=0; eflex_elemnum[f]; e++) { + for (int e=0; e < m->flex_elemnum[f]; e++) { const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1); const mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[f]; @@ -479,8 +479,8 @@ void mj_flex(const mjModel* m, mjData* d) { mjtNum xmin[3], xmax[3]; mju_copy3(xmin, vert+3*edata[0]); mju_copy3(xmax, vert+3*edata[0]); - for (int i=1; i<=dim; i++) { - for (int j=0; j<3; j++) { + for (int i=1; i <= dim; i++) { + for (int j=0; j < 3; j++) { mjtNum value = vert[3*edata[i]+j]; xmin[j] = mju_min(xmin[j], value); xmax[j] = mju_max(xmax[j], value); @@ -500,14 +500,14 @@ void mj_flex(const mjModel* m, mjData* d) { // update flex bhv_aabb_dyn if needed if (!mjDISABLED(mjDSBL_MIDPHASE)) { - for (int f=0; fnflex; f++) { - if (m->flex_bvhadr[f]>=0) { + for (int f=0; f < m->nflex; f++) { + if (m->flex_bvhadr[f] >= 0) { int flex_bvhadr = m->flex_bvhadr[f]; int flex_bvhnum = m->flex_bvhnum[f]; // copy element aabbs to bhv leaf aabbs - for (int i=flex_bvhadr; ibvh_nodeid[i]>=0) { + for (int i=flex_bvhadr; i < flex_bvhadr+flex_bvhnum; i++) { + if (m->bvh_nodeid[i] >= 0) { mju_copy(d->bvh_aabb_dyn + 6*(i - m->nbvhstatic), d->flexelem_aabb + 6*(m->flex_elemadr[f] + m->bvh_nodeid[i]), 6); } @@ -535,18 +535,18 @@ void mj_flex(const mjModel* m, mjData* d) { } // compute lengths and Jacobians of edges - for (int f=0; fnflex; f++) { + for (int f=0; f < m->nflex; f++) { // skip if edges cannot generate forces if (m->flex_rigid[f] || - (m->flex_edgeequality[f]==0 && - m->flex_edgestiffness[f]==0 && m->flex_edgedamping[f]==0)) { + (m->flex_edgeequality[f] == 0 && + m->flex_edgestiffness[f] == 0 && m->flex_edgedamping[f] == 0)) { continue; } // process edges of this flex int vbase = m->flex_vertadr[f]; int ebase = m->flex_edgeadr[f]; - for (int e=0; eflex_edgenum[f]; e++) { + for (int e=0; e < m->flex_edgenum[f]; e++) { int v1 = m->flex_edge[2*(ebase+e)]; int v2 = m->flex_edge[2*(ebase+e)+1]; int b1 = m->flex_vertbodyid[vbase+v1]; @@ -562,7 +562,7 @@ void mj_flex(const mjModel* m, mjData* d) { // sparse edge Jacobian if (issparse) { // set rowadr - if (ebase+e>0) { + if (ebase+e > 0) { rowadr[ebase+e] = rowadr[ebase+e-1] + rownnz[ebase+e-1]; } @@ -1110,7 +1110,7 @@ void mj_transmission(const mjModel* m, mjData* d) { const mjContact* con = d->contact+j; // contact involving flex, continue - if (con->geom[0]<0 || con->geom[1]<0) { + if (con->geom[0] < 0 || con->geom[1] < 0) { continue; } @@ -1772,7 +1772,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { con = d->contact+i; // skip contact involving flex - if (con->geom[0]<0 || con->geom[1]<0) { + if (con->geom[0] < 0 || con->geom[1] < 0) { continue; } diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 84c51d47..8414aec0 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -1471,12 +1471,12 @@ void mjd_passive_vel(const mjModel* m, mjData* d) { } // flex edge damping - for (int f=0; fnflex; f++) { + for (int f=0; f < m->nflex; f++) { if (!m->flex_rigid[f] && m->flex_edgedamping[f]) { mjtNum B = -m->flex_edgedamping[f]; // process edges of this flex - for (int e=m->flex_edgeadr[f]; eflex_edgeadr[f]+m->flex_edgenum[f]; e++) { + for (int e=m->flex_edgeadr[f]; e < m->flex_edgeadr[f]+m->flex_edgenum[f]; e++) { // add sparse or dense if (mj_isSparse(m)) { addJTBJSparse(m, d, d->flexedge_J, &B, 1, e, diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index c68ed24a..98fa76ed 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -215,7 +215,7 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) { // returns the next act given the current act_dot, after clamping static mjtNum nextActivation(const mjModel* m, const mjData* d, - int actuator_id, int act_adr, mjtNum act_dot) { + int actuator_id, int act_adr, mjtNum act_dot) { mjtNum act = d->act[act_adr]; if (m->actuator_dyntype[actuator_id] == mjDYN_FILTEREXACT) { @@ -569,7 +569,7 @@ void mj_solCG_island_multithreaded(const mjModel* m, mjData* d) { mj_markStack(d); // allocate array of arguments to be passed to threads mjSolIslandArgs* sol_cg_island_args = - mj_stackAllocByte(d, sizeof(mjSolIslandArgs) * d->nisland, _Alignof(mjSolIslandArgs)); + mj_stackAllocByte(d, sizeof(mjSolIslandArgs) * d->nisland, _Alignof(mjSolIslandArgs)); mjTask* tasks = mj_stackAllocByte(d, sizeof(mjTask) * d->nisland, _Alignof(mjTask)); for (int island = 0; island < d->nisland; ++island) { diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 3e2a0ba8..773a20f4 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -444,19 +444,16 @@ static int safeAddToBufferSize(intptr_t* offset, size_t* nbuffer, // allocate and initialize mjModel structure mjModel* mj_makeModel( - int nq, int nv, int nu, int na, int nbody, int nbvh, int nbvhstatic, - int nbvhdynamic, int njnt, int ngeom, int nsite, int ncam, int nlight, - int nflex, int nflexvert, int nflexedge, int nflexelem, int nflexelemdata, - int nflexshelldata, int nflexevpair, int nflextexcoord, int nmesh, - int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface, - int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface, - int nskinbone, int nskinbonevert, int nhfield, int nhfielddata, int ntex, - int ntexdata, int nmat, int npair, int nexclude, int neq, int ntendon, - int nwrap, int nsensor, int nnumeric, int nnumericdata, int ntext, - int ntextdata, int ntuple, int ntupledata, int nkey, int nmocap, - int nplugin, int npluginattr, int nuser_body, int nuser_jnt, int nuser_geom, - int nuser_site, int nuser_cam, int nuser_tendon, int nuser_actuator, - int nuser_sensor, int nnames, int npaths) { + int nq, int nv, int nu, int na, int nbody, int nbvh, int nbvhstatic, int nbvhdynamic, int njnt, + int ngeom, int nsite, int ncam, int nlight, int nflex, int nflexvert, int nflexedge, + int nflexelem, int nflexelemdata, int nflexshelldata, int nflexevpair, int nflextexcoord, + int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface, int nmeshgraph, + int nskin, int nskinvert, int nskintexvert, int nskinface, int nskinbone, int nskinbonevert, + int nhfield, int nhfielddata, int ntex, int ntexdata, int nmat, int npair, int nexclude, int neq, + int ntendon, int nwrap, int nsensor, int nnumeric, int nnumericdata, int ntext, int ntextdata, + int ntuple, int ntupledata, int nkey, int nmocap, int nplugin, int npluginattr, int nuser_body, + int nuser_jnt, int nuser_geom, int nuser_site, int nuser_cam, int nuser_tendon, + int nuser_actuator, int nuser_sensor, int nnames, int npaths) { intptr_t offset = 0; // allocate mjModel @@ -602,21 +599,21 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) { // allocate new model if needed if (!dest) { dest = mj_makeModel( - src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh, - src->nbvhstatic, src->nbvhdynamic, src->njnt, src->ngeom, src->nsite, - src->ncam, src->nlight, src->nflex, src->nflexvert, src->nflexedge, - src->nflexelem, src->nflexelemdata, src->nflexshelldata, - src->nflexevpair, src->nflextexcoord, src->nmesh, src->nmeshvert, - src->nmeshnormal, src->nmeshtexcoord, src->nmeshface, src->nmeshgraph, - src->nskin, src->nskinvert, src->nskintexvert, src->nskinface, - src->nskinbone, src->nskinbonevert, src->nhfield, src->nhfielddata, - src->ntex, src->ntexdata, src->nmat, src->npair, src->nexclude, - src->neq, src->ntendon, src->nwrap, src->nsensor, src->nnumeric, - src->nnumericdata, src->ntext, src->ntextdata, src->ntuple, - src->ntupledata, src->nkey, src->nmocap, src->nplugin, src->npluginattr, - src->nuser_body, src->nuser_jnt, src->nuser_geom, src->nuser_site, - src->nuser_cam, src->nuser_tendon, src->nuser_actuator, - src->nuser_sensor, src->nnames, src->npaths); + src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh, + src->nbvhstatic, src->nbvhdynamic, src->njnt, src->ngeom, src->nsite, + src->ncam, src->nlight, src->nflex, src->nflexvert, src->nflexedge, + src->nflexelem, src->nflexelemdata, src->nflexshelldata, + src->nflexevpair, src->nflextexcoord, src->nmesh, src->nmeshvert, + src->nmeshnormal, src->nmeshtexcoord, src->nmeshface, src->nmeshgraph, + src->nskin, src->nskinvert, src->nskintexvert, src->nskinface, + src->nskinbone, src->nskinbonevert, src->nhfield, src->nhfielddata, + src->ntex, src->ntexdata, src->nmat, src->npair, src->nexclude, + src->neq, src->ntendon, src->nwrap, src->nsensor, src->nnumeric, + src->nnumericdata, src->ntext, src->ntextdata, src->ntuple, + src->ntupledata, src->nkey, src->nmocap, src->nplugin, src->npluginattr, + src->nuser_body, src->nuser_jnt, src->nuser_geom, src->nuser_site, + src->nuser_cam, src->nuser_tendon, src->nuser_actuator, + src->nuser_sensor, src->nnames, src->npaths); } if (!dest) { mjERROR("failed to make mjModel. Invalid sizes."); @@ -1333,8 +1330,8 @@ static inline void* stackallocinternal(mjData* d, mjStackInfo* stack_info, size_ char* prev_pstack_ptr = (char*)(stack_info->top); size_t prev_misalign = (uintptr_t)prev_pstack_ptr % _Alignof(size_t); size_t* prev_usage_ptr = - (size_t*)(prev_pstack_ptr + - (prev_misalign ? _Alignof(size_t) - prev_misalign : 0)); + (size_t*)(prev_pstack_ptr + + (prev_misalign ? _Alignof(size_t) - prev_misalign : 0)); ASAN_UNPOISON_MEMORY_REGION(prev_usage_ptr, sizeof(size_t)); usage = current_alloc_usage + *prev_usage_ptr; ASAN_POISON_MEMORY_REGION(prev_usage_ptr, sizeof(size_t)); @@ -1343,7 +1340,7 @@ static inline void* stackallocinternal(mjData* d, mjStackInfo* stack_info, size_ // store new stack usage in the red zone size_t misalign = new_top_ptr % _Alignof(size_t); size_t* usage_ptr = - (size_t*)(new_top_ptr + (misalign ? _Alignof(size_t) - misalign : 0)); + (size_t*)(new_top_ptr + (misalign ? _Alignof(size_t) - misalign : 0)); ASAN_UNPOISON_MEMORY_REGION(usage_ptr, sizeof(size_t)); *usage_ptr = usage; ASAN_POISON_MEMORY_REGION(usage_ptr, sizeof(size_t)); @@ -1403,7 +1400,7 @@ __attribute__((always_inline)) static inline void markstackinternal(mjData* d, mjStackInfo* stack_info) { size_t top_old = stack_info->top; mjStackFrame* s = - (mjStackFrame*) stackallocinternal(d, stack_info, sizeof(mjStackFrame), _Alignof(mjStackFrame)); + (mjStackFrame*) stackallocinternal(d, stack_info, sizeof(mjStackFrame), _Alignof(mjStackFrame)); s->pbase = stack_info->stack_base; s->pstack = top_old; #ifdef ADDRESS_SANITIZER @@ -1447,8 +1444,8 @@ static inline void freestackinternal(mjStackInfo* stack_info) { #define mjSYMBOLIZELEN 256 char dbginfo[mjSYMBOLIZELEN]; __sanitizer_symbolize_pc( - s->pc, "mj_markStack %F at %S has no corresponding mj_freeStack", - dbginfo, sizeof(dbginfo)); + s->pc, "mj_markStack %F at %S has no corresponding mj_freeStack", + dbginfo, sizeof(dbginfo)); dbginfo[mjSYMBOLIZELEN - 1] = '\0'; mjERROR("%s", dbginfo); #undef mjSYMBOLIZELEN @@ -1584,7 +1581,7 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) { mju_zero(d->qvel, m->nv); mju_zero(d->act, m->na); mju_zero(d->ctrl, m->nu); - for (int i=0; ineq; i++) d->eq_active[i] = m->eq_active0[i]; + for (int i=0; i < m->neq; i++) d->eq_active[i] = m->eq_active0[i]; mju_zero(d->qfrc_applied, m->nv); mju_zero(d->xfrc_applied, 6*m->nbody); mju_zero(d->qacc, m->nv); @@ -1966,7 +1963,7 @@ const char* mj_validateReferences(const mjModel* m) { if (m->geom_dataid[i] >= m->nhfield || m->geom_dataid[i] < -1) { return "Invalid model: geom_dataid out of bounds."; } - } else if ((m->geom_type[i]==mjGEOM_MESH) || (m->geom_type[i]==mjGEOM_SDF)) { + } else if ((m->geom_type[i] == mjGEOM_MESH) || (m->geom_type[i] == mjGEOM_SDF)) { if (m->geom_dataid[i] >= m->nmesh || m->geom_dataid[i] < -1) { return "Invalid model: geom_dataid out of bounds."; } diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index a5088982..3da589fe 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -52,7 +52,7 @@ void mj_passive(const mjModel* m, mjData* d) { stiffness = m->jnt_stiffness[i]; // disabled : nothing to do - if (stiffness==0) { + if (stiffness == 0) { continue; } @@ -102,7 +102,7 @@ void mj_passive(const mjModel* m, mjData* d) { damping = m->flex_edgedamping[f]; // disabled or rigid: nothing to do - if (m->flex_rigid[f] || (stiffness==0 && damping==0)) { + if (m->flex_rigid[f] || (stiffness == 0 && damping == 0)) { continue; } diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 413f866a..0d1b15c1 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -362,7 +362,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* if (m->nbody) fprintf(fp, "\n"); // BVHs - for (int i=0; inbvh; i++) { + for (int i=0; i < m->nbvh; i++) { fprintf(fp, "\nBVH %d:\n", i); object_class = &m->nbvh; MJMODEL_POINTERS @@ -428,7 +428,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* if (m->nlight) fprintf(fp, "\n"); // flexes - for (int i=0; inflex; i++) { + for (int i=0; i < m->nflex; i++) { fprintf(fp, "\nFLEX %d:\n", i); fprintf(fp, " " NAME_FORMAT, "name"); fprintf(fp, " %s\n", m->names + m->name_flexadr[i]); diff --git a/src/engine/engine_ray.c b/src/engine/engine_ray.c index 2dc60250..4c08badc 100644 --- a/src/engine/engine_ray.c +++ b/src/engine/engine_ray.c @@ -844,15 +844,15 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl // compute bounding box mjtNum box[3][2] = {{0, 0}, {0, 0}, {0, 0}}; mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[flexid]; - for (int i=0; iflex_vertnum[flexid]; i++) { - for (int j=0; j<3; j++) { + for (int i=0; i < m->flex_vertnum[flexid]; i++) { + for (int j=0; j < 3; j++) { // update minimum along side j - if (box[j][0]>vert[3*i+j] || i==0) { + if (box[j][0] > vert[3*i+j] || i == 0) { box[j][0] = vert[3*i+j]; } // update maximum along side j - if (box[j][1]flex_radius[flexid]; - for (int j=0; j<3; j++) { + for (int j=0; j < 3; j++) { box[j][0] -= radius; box[j][1] += radius; } // construct box geom mjtNum pos[3], size[3], mat[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; - for (int j=0; j<3; j++) { + for (int j=0; j < 3; j++) { pos[j] = 0.5*(box[j][0]+box[j][1]); size[j] = 0.5*(box[j][1]-box[j][0]); } // apply bounding-box filter - if (ray_box(pos, mat, size, pnt, vec, NULL)<0) { + if (ray_box(pos, mat, size, pnt, vec, NULL) < 0) { return -1; } // construct basis vectors of normal plane mjtNum b0[3] = {1, 1, 1}, b1[3]; - if (mju_abs(vec[0])>=mju_abs(vec[1]) && mju_abs(vec[0])>=mju_abs(vec[2])) { + if (mju_abs(vec[0]) >= mju_abs(vec[1]) && mju_abs(vec[0]) >= mju_abs(vec[2])) { b0[0] = 0; - } else if (mju_abs(vec[1])>=mju_abs(vec[2])) { + } else if (mju_abs(vec[1]) >= mju_abs(vec[2])) { b0[1] = 0; } else { b0[2] = 0; @@ -895,9 +895,9 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl mjtNum x = -1; // check edges if rendered, or if skin - if (flg_edge || (dim>1 && flg_skin)) { - for (int e=m->flex_edgeadr[flexid]; - eflex_edgeadr[flexid]+m->flex_edgenum[flexid]; e++) { + if (flg_edge || (dim > 1 && flg_skin)) { + int edge_end = m->flex_edgeadr[flexid]+m->flex_edgenum[flexid]; + for (int e=m->flex_edgeadr[flexid]; e < edge_end; e++) { // get vertices for this edge mjtNum* v1 = d->flexvert_xpos + 3*(m->flex_vertadr[flexid]+m->flex_edge[2*e]); mjtNum* v2 = d->flexvert_xpos + 3*(m->flex_vertadr[flexid]+m->flex_edge[2*e+1]); @@ -916,7 +916,7 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl mjtNum sol = mju_rayGeom(pos, mat, size, pnt, vec, mjGEOM_CAPSULE); // update - if (sol>=0 && (x<0 || sol= 0 && (x < 0 || sol < x)) { x = sol; // construct intersection point @@ -935,8 +935,8 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl } // check vertices if rendered (and edges not checked) - else if (flg_vert && !(dim>1 && flg_skin)) { - for (int v=0; vflex_vertnum[flexid]; v++) { + else if (flg_vert && !(dim > 1 && flg_skin)) { + for (int v=0; v < m->flex_vertnum[flexid]; v++) { // get vertex mjtNum* vpos = d->flexvert_xpos + 3*(m->flex_vertadr[flexid] + v); @@ -947,7 +947,7 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl mjtNum sol = mju_rayGeom(vpos, NULL, size, pnt, vec, mjGEOM_SPHERE); // update - if (sol>=0 && (x<0 || sol= 0 && (x < 0 || sol < x)) { x = sol; *vertid = v; } @@ -955,11 +955,11 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl } // check faces if rendered - if (dim>1 && (flg_face || flg_skin)) { - for (int e=0; eflex_elemnum[flexid]; e++) { + if (dim > 1 && (flg_face || flg_skin)) { + for (int e=0; e < m->flex_elemnum[flexid]; e++) { // skip if 3D element is not visible int elayer = m->flex_elemlayer[m->flex_elemadr[flexid]+e]; - if (dim==3 && ((flg_skin && elayer>0) || (!flg_skin && elayer!=flex_layer))) { + if (dim == 3 && ((flg_skin && elayer > 0) || (!flg_skin && elayer != flex_layer))) { continue; } @@ -968,22 +968,22 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl mjtNum* v1 = d->flexvert_xpos + 3*(m->flex_vertadr[flexid] + edata[0]); mjtNum* v2 = d->flexvert_xpos + 3*(m->flex_vertadr[flexid] + edata[1]); mjtNum* v3 = d->flexvert_xpos + 3*(m->flex_vertadr[flexid] + edata[2]); - mjtNum* v4 = dim==2 ? NULL : d->flexvert_xpos + 3*(m->flex_vertadr[flexid] + edata[3]); + mjtNum* v4 = dim == 2 ? NULL : d->flexvert_xpos + 3*(m->flex_vertadr[flexid] + edata[3]); mjtNum* vptr[4][3] = {{v1, v2, v3}, {v1, v2, v4}, {v1, v3, v4}, {v2, v3, v4}}; int vid[4][3] = {{0, 1, 2}, {0, 1, 3}, {0, 2, 3}, {1, 2, 3}}; // process triangles of this element - for (int i=0; i<(dim==2?1:4); i++) { + for (int i=0; i < (dim == 2?1:4); i++) { // copy vertices into triangle representation mjtNum v[3][3]; - for (int j=0; j<3; j++) + for (int j=0; j < 3; j++) mju_copy3(v[j], vptr[i][j]); // intersect ray with triangle mjtNum sol = ray_triangle(v, pnt, vec, b0, b1); // update - if (sol>=0 && (x<0 || sol= 0 && (x < 0 || sol < x)) { x = sol; // construct intersection point @@ -996,9 +996,9 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl mju_dist3(v[1], intersect), mju_dist3(v[2], intersect) }; - if (dist[0]<=dist[1] && dist[0]<=dist[2]) { + if (dist[0] <= dist[1] && dist[0] <= dist[2]) { *vertid = edata[vid[i][0]]; - } else if (dist[1]<=dist[2]){ + } else if (dist[1] <= dist[2]){ *vertid = edata[vid[i][1]]; } else { *vertid = edata[vid[i][2]]; diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index e7e02bf9..f2fb29a6 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -208,8 +208,8 @@ static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3], rotation[1][1] = 1; rotation[2][2] = 1; rotation[3][3] = 1; - for (int i=0; i<3; i++) { - for (int j=0; j<3; j++) { + for (int i=0; i < 3; i++) { + for (int j=0; j < 3; j++) { rotation[i][j] = cam_xmat[j*3+i]; } } @@ -237,11 +237,11 @@ static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3], // projection matrix (3x4): product of all 4 matrices mjtNum proj[3][4] = {0}; - for (int i=0; i<3; i++) { - for (int j=0; j<3; j++) { - for (int k=0; k<4; k++) { - for (int l=0; l<4; l++) { - for (int n=0; n<4; n++) { + for (int i=0; i < 3; i++) { + for (int j=0; j < 3; j++) { + for (int k=0; k < 4; k++) { + for (int l=0; l < 4; l++) { + for (int n=0; n < 4; n++) { proj[i][n] += image[i][j] * focal[j][k] * rotation[k][l] * translation[l][n]; } } @@ -256,10 +256,10 @@ static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3], // project world coordinates into pixel space, see: // https://en.wikipedia.org/wiki/3D_projection#Mathematical_formula mjtNum pixel_coord_hom[3] = {0}; - for (int i=0; i<3; i++) { - for (int j=0; j<4; j++) { - pixel_coord_hom[i] += proj[i][j] * pos_hom[j]; - } + for (int i=0; i < 3; i++) { + for (int j=0; j < 4; j++) { + pixel_coord_hom[i] += proj[i][j] * pos_hom[j]; + } } // avoid dividing by tiny numbers @@ -716,8 +716,8 @@ void mj_sensorAcc(const mjModel* m, mjData* d) { // contact pointer, contacting bodies (-1 for flex) con = d->contact + j; int conbody[2]; - for (int k=0; k<2; k++) { - conbody[k] = (con->geom[k]>=0) ? m->geom_bodyid[con->geom[k]] : -1; + for (int k=0; k < 2; k++) { + conbody[k] = (con->geom[k] >= 0) ? m->geom_bodyid[con->geom[k]] : -1; } // select contacts involving sensorized body @@ -950,14 +950,14 @@ void mj_energyPos(const mjModel* m, mjData* d) { // add flex-level springs if (!mjDISABLED(mjDSBL_PASSIVE)) { - for (int i=0; inflex; i++) { + for (int i=0; i < m->nflex; i++) { stiffness = m->flex_edgestiffness[i]; - if (m->flex_rigid[i] || stiffness==0) { + if (m->flex_rigid[i] || stiffness == 0) { continue; } // process edges of this flex - for (int e=m->flex_edgeadr[i]; eflex_edgeadr[i]+m->flex_edgenum[i]; e++) { + for (int e=m->flex_edgeadr[i]; e < m->flex_edgeadr[i]+m->flex_edgenum[i]; e++) { mjtNum displacement = m->flexedge_length0[e] - d->flexedge_length[e]; d->energy[0] += 0.5*stiffness*displacement*displacement; } diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 438a9ec3..0e9b5b1f 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -115,14 +115,14 @@ static void set0(mjModel* m, mjData* d) { // compute body_invweight0 m->body_invweight0[0] = m->body_invweight0[1] = 0.0; - for (int i=1; inbody; i++) { + for (int i=1; i < m->nbody; i++) { // static bodies: zero invweight0 if (m->body_weldid[i] == 0) { m->body_invweight0[2*i] = m->body_invweight0[2*i+1] = 0; } // accelerate simple bodies with no rotations - else if (m->body_simple[i]==2) { + else if (m->body_simple[i] == 2) { mjtNum mass = m->body_mass[i]; if (!mass) { // SHOULD NOT OCCUR mjERROR("moving body %d has 0 mass", i); @@ -147,7 +147,7 @@ static void set0(mjModel* m, mjData* d) { } // compute dof_invweight0 - for (int i=0; injnt; i++) { + for (int i=0; i < m->njnt; i++) { // simple body with no rotations: no off-diagonal inertia if (m->body_simple[m->jnt_bodyid[i]] == 2) { int id = m->jnt_dofadr[i]; @@ -200,14 +200,14 @@ static void set0(mjModel* m, mjData* d) { // compute flexedge_invweight0, tendon_invweight0, actuator_acc0 if (nv) { // compute flexedge_invweight0 - for (int f=0; fnflex; f++) { - for (int i=m->flex_edgeadr[f]; iflex_edgeadr[f]+m->flex_edgenum[f]; i++) { + for (int f=0; f < m->nflex; f++) { + for (int i=m->flex_edgeadr[f]; i < m->flex_edgeadr[f]+m->flex_edgenum[f]; i++) { // bodies connected by edge int b1 = m->flex_vertbodyid[m->flex_vertadr[f] + m->flex_edge[2*i]]; int b2 = m->flex_vertbodyid[m->flex_vertadr[f] + m->flex_edge[2*i+1]]; // accelerate edges that connect simple bodies with no rotations - if (m->body_simple[b1]==2 && m->body_simple[b2]==2) { + if (m->body_simple[b1] == 2 && m->body_simple[b2] == 2) { m->flexedge_invweight0[i] = (1/m->body_mass[b1] + 1/m->body_mass[b2])/2; } @@ -217,7 +217,7 @@ static void set0(mjModel* m, mjData* d) { if (mj_isSparse(m)) { mju_zero(tmp, nv); int end = d->flexedge_J_rowadr[i] + d->flexedge_J_rownnz[i]; - for (int j=d->flexedge_J_rowadr[i]; jflexedge_J_rowadr[i]; j < end; j++) { tmp[d->flexedge_J_colind[j]] = d->flexedge_J[j]; } } else { @@ -232,7 +232,7 @@ static void set0(mjModel* m, mjData* d) { } // compute tendon_invweight0 - for (int i=0; intendon; i++) { + for (int i=0; i < m->ntendon; i++) { // make dense vector into tmp if (mj_isSparse(m)) { mju_zero(tmp, nv); @@ -422,8 +422,8 @@ static void setStat(mjModel* m, mjData* d) { } // adjust body size for flex edges involving body - for (int f=0; fnflex; f++) { - for (int e=m->flex_edgeadr[f]; eflex_edgeadr[f]+m->flex_edgenum[f]; e++) { + for (int f=0; f < m->nflex; f++) { + for (int e=m->flex_edgeadr[f]; e < m->flex_edgeadr[f]+m->flex_edgenum[f]; e++) { int b1 = m->flex_vertbodyid[m->flex_vertadr[f]+m->flex_edge[2*e]]; int b2 = m->flex_vertbodyid[m->flex_vertadr[f]+m->flex_edge[2*e+1]]; diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index fa19d68f..ceb3e2c3 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -100,22 +100,22 @@ const char* mjTIMERSTRING[mjNTIMER]= { // return size of a single state element static inline int mj_stateElemSize(const mjModel* m, mjtState spec) { switch (spec) { - case mjSTATE_TIME: return 1; - case mjSTATE_QPOS: return m->nq; - case mjSTATE_QVEL: return m->nv; - case mjSTATE_ACT: return m->na; - case mjSTATE_WARMSTART: return m->nv; - case mjSTATE_CTRL: return m->nu; - case mjSTATE_QFRC_APPLIED: return m->nv; - case mjSTATE_XFRC_APPLIED: return 6*m->nbody; - case mjSTATE_EQ_ACTIVE: return m->neq; // mjtByte, stored as mjtNum in state vector - case mjSTATE_MOCAP_POS: return 3*m->nmocap; - case mjSTATE_MOCAP_QUAT: return 4*m->nmocap; - case mjSTATE_USERDATA: return m->nuserdata; - case mjSTATE_PLUGIN: return m->npluginstate; - default: - mjERROR("invalid state element %u", spec); - return 0; + case mjSTATE_TIME: return 1; + case mjSTATE_QPOS: return m->nq; + case mjSTATE_QVEL: return m->nv; + case mjSTATE_ACT: return m->na; + case mjSTATE_WARMSTART: return m->nv; + case mjSTATE_CTRL: return m->nu; + case mjSTATE_QFRC_APPLIED: return m->nv; + case mjSTATE_XFRC_APPLIED: return 6*m->nbody; + case mjSTATE_EQ_ACTIVE: return m->neq; // mjtByte, stored as mjtNum in state vector + case mjSTATE_MOCAP_POS: return 3*m->nmocap; + case mjSTATE_MOCAP_QUAT: return 4*m->nmocap; + case mjSTATE_USERDATA: return m->nuserdata; + case mjSTATE_PLUGIN: return m->npluginstate; + default: + mjERROR("invalid state element %u", spec); + return 0; } } @@ -124,21 +124,21 @@ static inline int mj_stateElemSize(const mjModel* m, mjtState spec) { // return pointer to a single state element static inline mjtNum* mj_stateElemPtr(const mjModel* m, mjData* d, mjtState spec) { switch (spec) { - case mjSTATE_TIME: return &d->time; - case mjSTATE_QPOS: return d->qpos; - case mjSTATE_QVEL: return d->qvel; - case mjSTATE_ACT: return d->act; - case mjSTATE_WARMSTART: return d->qacc_warmstart; - case mjSTATE_CTRL: return d->ctrl; - case mjSTATE_QFRC_APPLIED: return d->qfrc_applied; - case mjSTATE_XFRC_APPLIED: return d->xfrc_applied; - case mjSTATE_MOCAP_POS: return d->mocap_pos; - case mjSTATE_MOCAP_QUAT: return d->mocap_quat; - case mjSTATE_USERDATA: return d->userdata; - case mjSTATE_PLUGIN: return d->plugin_state; - default: - mjERROR("invalid state element %u", spec); - return NULL; + case mjSTATE_TIME: return &d->time; + case mjSTATE_QPOS: return d->qpos; + case mjSTATE_QVEL: return d->qvel; + case mjSTATE_ACT: return d->act; + case mjSTATE_WARMSTART: return d->qacc_warmstart; + case mjSTATE_CTRL: return d->ctrl; + case mjSTATE_QFRC_APPLIED: return d->qfrc_applied; + case mjSTATE_XFRC_APPLIED: return d->xfrc_applied; + case mjSTATE_MOCAP_POS: return d->mocap_pos; + case mjSTATE_MOCAP_QUAT: return d->mocap_quat; + case mjSTATE_USERDATA: return d->userdata; + case mjSTATE_PLUGIN: return d->plugin_state; + default: + mjERROR("invalid state element %u", spec); + return NULL; } } @@ -248,7 +248,7 @@ int mj_mergeChain(const mjModel* m, int* chain, int b1, int b2) { } // neither body is movable: empty chain - if (b1==0 && b2==0) { + if (b1 == 0 && b2 == 0) { return 0; } @@ -257,19 +257,19 @@ int mj_mergeChain(const mjModel* m, int* chain, int b1, int b2) { da2 = m->body_dofadr[b2] + m->body_dofnum[b2] - 1; // merge chains - while (da1>=0 || da2>=0) { + while (da1 >= 0 || da2 >= 0) { chain[NV] = mjMAX(da1, da2); - if (da1==chain[NV]) { + if (da1 == chain[NV]) { da1 = m->dof_parentid[da1]; } - if (da2==chain[NV]) { + if (da2 == chain[NV]) { da2 = m->dof_parentid[da2]; } NV++; } // reverse order of chain: make it increasing - for (int i=0; ib2) { + if (b1 > b2) { int tmp = b1; b1 = b2; b2 = tmp; @@ -293,17 +293,17 @@ int mj_mergeChainSimple(const mjModel* m, int* chain, int b1, int b2) { int n1 = m->body_dofnum[b1], n2 = m->body_dofnum[b2]; // both fixed: nothing to do - if (n1==0 && n2==0) { + if (n1 == 0 && n2 == 0) { return 0; } // copy b1 dofs - for (int i=0; ibody_dofadr[b1] + i; } // copy b2 dofs - for (int i=0; ibody_dofadr[b2] + i; } @@ -317,7 +317,7 @@ int mj_bodyChain(const mjModel* m, int body, int* chain) { // simple body if (m->body_simple[body]) { int dofnum = m->body_dofnum[body]; - for (int i=0; ibody_dofadr[body] + i; } return dofnum; @@ -331,7 +331,7 @@ int mj_bodyChain(const mjModel* m, int body, int* chain) { } // not movable: empty chain - if (body==0) { + if (body == 0) { return 0; } @@ -340,13 +340,13 @@ int mj_bodyChain(const mjModel* m, int body, int* chain) { int NV = 0; // construct chain from child to parent - while (da>=0) { + while (da >= 0) { chain[NV++] = da; da = m->dof_parentid[da]; } // reverse order of chain: make it increasing - for (int i=0; id = d; array_list->element_size = element_size; @@ -35,7 +35,7 @@ mjArrayList* mju_arrayListCreate(mjData* d, size_t element_size, size_t initial_ // allocate array list buffer array_list->buffer = (void*) mj_stackAllocByte( - d, element_size * initial_capacity, _Alignof(mjtMaxAlign)); + d, element_size * initial_capacity, _Alignof(mjtMaxAlign)); return array_list; } @@ -63,7 +63,7 @@ void mju_arrayListAdd(mjArrayList* array_list, void* element) { if (cursor->next_segment == NULL) { // add a new segment with twice the capacity of the last segment cursor->next_segment = mju_arrayListCreate( - cursor->d, cursor->element_size, 2 * cursor->capacity); + cursor->d, cursor->element_size, 2 * cursor->capacity); } cursor = cursor->next_segment; } diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 3b1496b3..e1e126d7 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -630,7 +630,7 @@ static uint32_t _decode(char ch) { // returns number of chars written in buf: 4 * [(ndata + 2) / 3] + 1 size_t mju_encodeBase64(char* buf, const uint8_t* data, size_t ndata) { static const char *table = - "ABCDEFGHIJKLMNOPQRSTUBWXYZabcdefghijklmnopqrstuvwxyz0123456789+/"; + "ABCDEFGHIJKLMNOPQRSTUBWXYZabcdefghijklmnopqrstuvwxyz0123456789+/"; int i = 0, j = 0; diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index e86247bd..6a804ac2 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -379,19 +379,19 @@ void mju_addToSparseInc(mjtNum* dst, const mjtNum* src, int adrs = 0, adrd = 0, inds = indsrc[0], indd = inddst[0]; while (1) { // common non-zero index - if (inds==indd) { + if (inds == indd) { // add dst[adrd] += src[adrs]; // advance src - if (++adrsflex_dim[f]==0) { + if (m->flex_dim[f] == 0) { scn->flexfacenum[f] = 0; } // 2D: 2*fragments + 2*elements - else if (m->flex_dim[f]==2) { - scn->flexfacenum[f] = 2*m->flex_shellnum[f] + 2*m->flex_elemnum[f]; + else if (m->flex_dim[f] == 2) { + scn->flexfacenum[f] = 2*m->flex_shellnum[f] + 2*m->flex_elemnum[f]; } // 3D: max(fragments, 4*maxlayer) @@ -187,8 +187,8 @@ void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) { while (nlayer) { // count elements in this layer nlayer = 0; - for (int e=0; eflex_elemnum[f]; e++) { - if (m->flex_elemlayer[m->flex_elemadr[f]+e]==layer) { + for (int e=0; e < m->flex_elemnum[f]; e++) { + if (m->flex_elemlayer[m->flex_elemadr[f]+e] == layer) { nlayer++; } } @@ -234,8 +234,8 @@ void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) { memcpy(scn->flexedge, m->flex_edge, 2*m->nflexedge*sizeof(int)); // compute flexfaceadr - for (int f=0; fflexfaceadr[f] = f==0 ? 0 : scn->flexfaceadr[f-1]+scn->flexfacenum[f-1]; + for (int f=0; f < nflex; f++) { + scn->flexfaceadr[f] = f == 0 ? 0 : scn->flexfaceadr[f-1]+scn->flexfacenum[f-1]; } } diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index 0c4a14ef..6eacc958 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -546,7 +546,7 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur pert->localmass = 3 / mju_max(invmass, mjMINVAL); // scale localmass with flex average number of edges per vertex - if (pert->flexselect>=0 && !m->flex_rigid[pert->flexselect]) { + if (pert->flexselect >= 0 && !m->flex_rigid[pert->flexselect]) { pert->localmass *= (2.0*m->flex_edgenum[pert->flexselect]) / (mjtNum)m->flex_vertnum[pert->flexselect]; } @@ -773,7 +773,7 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt, *flexid = -1; if (vopt->flags[mjVIS_FLEXVERT] || vopt->flags[mjVIS_FLEXEDGE] || vopt->flags[mjVIS_FLEXFACE] || vopt->flags[mjVIS_FLEXSKIN]) { - for (int i=0; inflex; i++) { + for (int i=0; i < m->nflex; i++) { // process one flex int vertid; mjtNum newdist = mju_rayFlex(m, d, vopt->flex_layer, @@ -782,7 +782,7 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt, i, pos, ray, &vertid); // update if closer intersection found - if (newdist>=0 && (newdist= 0 && (newdist < flexdist || flexdist < 0)) { flexdist = newdist; flexbodyid = m->flex_vertbodyid[m->flex_vertadr[i] + vertid]; *flexid = i; @@ -842,9 +842,9 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt, // find smallest non-negative distance mjtNum raydist[3] = {geomdist, flexdist, skindist}; int best = -1; - for (int i=0; i<3; i++) { - if (raydist[i]>=0) { - if (best<0 || raydist[best]>raydist[i]) { + for (int i=0; i < 3; i++) { + if (raydist[i] >= 0) { + if (best < 0 || raydist[best] > raydist[i]) { best = i; } } diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 37325336..f86f92ed 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -148,9 +148,9 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags, // label contacting geom names or ids if (vopt->label == mjLABEL_CONTACTPOINT) { char contactlabel[2][48]; - for (int k=0; k<2; k++) { + for (int k=0; k < 2; k++) { // make geom label - if (con->geom[k]>=0) { + if (con->geom[k] >= 0) { const char* geomname = mj_id2name(m, mjOBJ_GEOM, con->geom[k]); if (geomname) { mjSNPRINTF(contactlabel[k], "%s", geomname); @@ -164,7 +164,7 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags, else { const char* flexname = mj_id2name(m, mjOBJ_FLEX, con->flex[k]); if (flexname) { - if (con->elem[k]>=0) { + if (con->elem[k] >= 0) { mjSNPRINTF(contactlabel[k], "%s.e%d", flexname, con->elem[k]); } else { @@ -172,7 +172,7 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags, } } else { - if (con->elem[k]>=0) { + if (con->elem[k] >= 0) { mjSNPRINTF(contactlabel[k], "f%d.e%d", con->flex[k], con->elem[k]); } else { @@ -265,9 +265,9 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags, // get bodyflex ids int bf[2]; - for (int k=0; k<2; k++) { - bf[k] = (con->geom[k]>=0) ? m->geom_bodyid[con->geom[k]] : - m->nbody + con->flex[k]; + for (int k=0; k < 2; k++) { + bf[k] = (con->geom[k] >= 0) ? m->geom_bodyid[con->geom[k]] : + m->nbody + con->flex[k]; } // make sure arrow points towards bodyflex with higher id @@ -599,7 +599,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, if ((vopt->flags[mjVIS_FLEXVERT] || vopt->flags[mjVIS_FLEXEDGE] || vopt->flags[mjVIS_FLEXFACE] || vopt->flags[mjVIS_FLEXSKIN]) && (category & catmask)) { - for (int i=0; inflex; i++) { + for (int i=0; i < m->nflex; i++) { if (vopt->flexgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->flex_group[i]))]) { START @@ -614,7 +614,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, setMaterial(m, thisgeom, m->flex_matid[i], m->flex_rgba+4*i, vopt->flags); // set texcoord - if (m->flex_texcoordadr[i]>=0) { + if (m->flex_texcoordadr[i] >= 0) { thisgeom->texcoord = 1; } else { @@ -622,17 +622,17 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, } // glow flex if selected - if (pert->flexselect==i) { + if (pert->flexselect == i) { markselected(&m->vis, thisgeom); } // skip if alpha is 0 - if (thisgeom->rgba[3]==0) { + if (thisgeom->rgba[3] == 0) { continue; } // vopt->label - if (vopt->label==mjLABEL_FLEX) { + if (vopt->label == mjLABEL_FLEX) { makeLabel(m, mjOBJ_FLEX, i, thisgeom->label); } @@ -723,11 +723,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, // flex BVH if (vopt->flags[mjVIS_FLEXBVH]) { float rgba[] = {1, 0, 0, 0.1}; - for (int f=0; fnflex; f++) { + for (int f=0; f < m->nflex; f++) { if (m->flex_bvhnum[f] && vopt->flexgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->flex_group[f]))]) { - for (int i=m->flex_bvhadr[f]; iflex_bvhadr[f]+m->flex_bvhnum[f]; i++) { - int isleaf = m->bvh_child[2*i]==-1 && m->bvh_child[2*i+1]==-1; + for (int i=m->flex_bvhadr[f]; i < m->flex_bvhadr[f]+m->flex_bvhnum[f]; i++) { + int isleaf = m->bvh_child[2*i] == -1 && m->bvh_child[2*i+1] == -1; if (scn->ngeom >= scn->maxgeom) break; if (m->bvh_depth[i] != vopt->bvh_depth) { if (!isleaf || m->bvh_depth[i] > vopt->bvh_depth) { @@ -743,7 +743,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, // b/304453879 : add LINEBOX geom for bounding box visualization START - mjv_initGeom(thisgeom, mjGEOM_BOX, aabb+3, aabb, NULL, rgba); + mjv_initGeom(thisgeom, mjGEOM_BOX, aabb+3, aabb, NULL, rgba); FINISH } } @@ -1351,7 +1351,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, if (m->geom_type[i] == mjGEOM_MESH || m->geom_type[i] == mjGEOM_SDF) { thisgeom->dataid *= 2; if (m->mesh_graphadr[m->geom_dataid[i]] >= 0 && vopt->flags[mjVIS_CONVEXHULL] && - (m->geom_contype[i] || m->geom_conaffinity[i])) { + (m->geom_contype[i] || m->geom_conaffinity[i])) { thisgeom->dataid += 1; } } @@ -1646,7 +1646,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mju_addToScl3(vfar[3], y, zver[1]); // triangulation and wireframe of the frustum - for (int e=0; e<4; e++) { + for (int e=0; e < 4; e++) { START mju_sub3(x, vfar[e], vnear[e]); mju_sub3(y, vnear[(e+1)%4], vnear[e]); @@ -1839,7 +1839,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, for (int j=0; j < npoints-1; j++) { START - sz[0] = m->tendon_width[i]; + sz[0] = m->tendon_width[i]; // construct geom mjv_connector(thisgeom, mjGEOM_CAPSULE, sz[0], catenary+3*j, catenary+3*j+3); @@ -2300,7 +2300,7 @@ static void makeSmooth(float* _face, float* _normal, mjtNum radius, mjtByte flg_ float* face = _face + 9*nface; float* normal = _normal + 9*nface; int ind[3] = {i0, i1, i2}; - int sign = radius>0 ? 1 : -1; + int sign = radius > 0 ? 1 : -1; // flat shading if (flg_flat) { @@ -2315,7 +2315,7 @@ static void makeSmooth(float* _face, float* _normal, mjtNum radius, mjtByte flg_ mju_normalize3(nrm); // set all vertex normals equal to face normal - for (int k=0; k<3; k++){ + for (int k=0; k < 3; k++){ normal[3*k+0] = (float) (sign*nrm[0]); normal[3*k+1] = (float) (sign*nrm[1]); normal[3*k+2] = (float) (sign*nrm[2]); @@ -2324,7 +2324,7 @@ static void makeSmooth(float* _face, float* _normal, mjtNum radius, mjtByte flg_ // smooth shading else { - for (int k=0; k<3; k++){ + for (int k=0; k < 3; k++){ normal[3*k+0] = (float) (sign*vertnorm[3*ind[k]+0]); normal[3*k+1] = (float) (sign*vertnorm[3*ind[k]+1]); normal[3*k+2] = (float) (sign*vertnorm[3*ind[k]+2]); @@ -2332,7 +2332,7 @@ static void makeSmooth(float* _face, float* _normal, mjtNum radius, mjtByte flg_ } // set positions: vertices offset by radius*normal - for (int k=0; k<3; k++){ + for (int k=0; k < 3; k++){ face[3*k+0] = (float) (vertxpos[3*ind[k]+0] + radius*vertnorm[3*ind[k]+0]); face[3*k+1] = (float) (vertxpos[3*ind[k]+1] + radius*vertnorm[3*ind[k]+1]); face[3*k+2] = (float) (vertxpos[3*ind[k]+2] + radius*vertnorm[3*ind[k]+2]); @@ -2354,13 +2354,13 @@ static void makeSide(float* _face, float* _normal, mjtNum radius, mjtNum v01[3] = {v1[0]-v0[0], v1[1]-v0[1], v1[2]-v0[2]}; mjtNum nrm[3]; mju_cross(nrm, v01, vertnorm+3*i1); - if (radius<0) { + if (radius < 0) { mju_scl3(nrm, nrm, -1); } mju_normalize3(nrm); // set normals - for (int k=0; k<3; k++){ + for (int k=0; k < 3; k++){ normal[3*k+0] = (float) nrm[0]; normal[3*k+1] = (float) nrm[1]; normal[3*k+2] = (float) nrm[2]; @@ -2368,8 +2368,8 @@ static void makeSide(float* _face, float* _normal, mjtNum radius, // set positions int ind[3] = {i0, i1, i1}; - for (int k=0; k<3; k++){ - mjtNum sign = (k==1 ? -1 : +1); + for (int k=0; k < 3; k++){ + mjtNum sign = (k == 1 ? -1 : +1); face[3*k+0] = (float) (vertxpos[3*ind[k]+0] + sign*radius*vertnorm[3*ind[k]+0]); face[3*k+1] = (float) (vertxpos[3*ind[k]+1] + sign*radius*vertnorm[3*ind[k]+1]); face[3*k+2] = (float) (vertxpos[3*ind[k]+2] + sign*radius*vertnorm[3*ind[k]+2]); @@ -2403,39 +2403,39 @@ void mjv_updateActiveFlex(const mjModel* m, mjData* d, mjvScene* scn, const mjvO scn->flexskinopt = opt->flags[mjVIS_FLEXSKIN]; // convert vertex positions from mjtNum to float - for (int v=0; v<3*m->nflexvert; v++) { + for (int v=0; v < 3*m->nflexvert; v++) { scn->flexvert[v] = (float) d->flexvert_xpos[v]; } // construct faces - for (int f=0; fnflex; f++) { + for (int f=0; f < m->nflex; f++) { int dim = m->flex_dim[f]; mjtNum radius = m->flex_radius[f]; mjtByte flg_flat = m->flex_flatskin[f]; const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f]; float* face = scn->flexface + 9*scn->flexfaceadr[f]; float* normal = scn->flexnormal + 9*scn->flexfaceadr[f]; - float* texdst = m->flex_texcoordadr[f]>=0 ? - scn->flextexcoord + 6*scn->flexfaceadr[f] : NULL; - const float* texsrc = m->flex_texcoordadr[f]>=0 ? - m->flex_texcoord + 2*m->flex_texcoordadr[f] : NULL; + float* texdst = m->flex_texcoordadr[f] >= 0 ? + scn->flextexcoord + 6*scn->flexfaceadr[f] : NULL; + const float* texsrc = m->flex_texcoordadr[f] >= 0 ? + m->flex_texcoord + 2*m->flex_texcoordadr[f] : NULL; // 1D, or face and skin disabled: no faces - if (dim==1 || (!opt->flags[mjVIS_FLEXFACE] && !opt->flags[mjVIS_FLEXSKIN])) { + if (dim == 1 || (!opt->flags[mjVIS_FLEXFACE] && !opt->flags[mjVIS_FLEXSKIN])) { scn->flexfaceused[f] = 0; } // 2D or 3D face: faces from elements, flat normals, texture else if (!opt->flags[mjVIS_FLEXSKIN]) { int nface = 0; - for (int e=0; eflex_elemnum[f]; e++) { + for (int e=0; e < m->flex_elemnum[f]; e++) { // in 3D, show only elements in selected layer - if (dim==2 || m->flex_elemlayer[m->flex_elemadr[f]+e]==opt->flex_layer) { + if (dim == 2 || m->flex_elemlayer[m->flex_elemadr[f]+e] == opt->flex_layer) { // get element data const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1); // triangles: two faces per element - if (dim==2) { + if (dim == 2) { makeFace(face, normal, radius, vertxpos, nface, edata[0], edata[1], edata[2]); copyTex(texdst, texsrc, nface, edata[0], edata[1], edata[2]); nface++; @@ -2482,27 +2482,27 @@ void mjv_updateActiveFlex(const mjModel* m, mjData* d, mjvScene* scn, const mjvO mju_zero(vertnorm, 3*m->flex_vertnum[f]); // add vertex normals: top element sides in 2D, shell fragments in 3D - if (dim==2) { - for (int e=0; eflex_elemnum[f]; e++) { + if (dim == 2) { + for (int e=0; e < m->flex_elemnum[f]; e++) { const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1); addNormal(vertnorm, vertxpos, edata[0], edata[1], edata[2]); } } else { - for (int s=0; sflex_shellnum[f]; s++) { + for (int s=0; s < m->flex_shellnum[f]; s++) { const int* sdata = m->flex_shell + m->flex_shelldataadr[f] + s*dim; addNormal(vertnorm, vertxpos, sdata[0], sdata[1], sdata[2]); } } // normalize vertex normals - for (int i=0; iflex_vertnum[f]; i++) { + for (int i=0; i < m->flex_vertnum[f]; i++) { mju_normalize3(vertnorm+3*i); } // create faces, offset along smoothed vertex normals, and texcoord int nface = 0; - if (dim==2) { - for (int e=0; eflex_elemnum[f]; e++) { + if (dim == 2) { + for (int e=0; e < m->flex_elemnum[f]; e++) { const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1); makeSmooth(face, normal, radius, flg_flat, vertnorm, vertxpos, nface, edata[0], edata[1], edata[2]); @@ -2514,7 +2514,7 @@ void mjv_updateActiveFlex(const mjModel* m, mjData* d, mjvScene* scn, const mjvO nface++; } } else { - for (int s=0; sflex_shellnum[f]; s++) { + for (int s=0; s < m->flex_shellnum[f]; s++) { const int* sdata = m->flex_shell + m->flex_shelldataadr[f] + s*dim; makeSmooth(face, normal, radius, flg_flat, vertnorm, vertxpos, nface, sdata[0], sdata[1], sdata[2]); @@ -2524,8 +2524,8 @@ void mjv_updateActiveFlex(const mjModel* m, mjData* d, mjvScene* scn, const mjvO } // 2D: close sides using shell fragments - if (dim==2) { - for (int s=0; sflex_shellnum[f]; s++) { + if (dim == 2) { + for (int s=0; s < m->flex_shellnum[f]; s++) { const int* sdata = m->flex_shell + m->flex_shelldataadr[f] + s*dim; makeSide(face, normal, radius, vertnorm, vertxpos, nface, sdata[0], sdata[1]); @@ -2665,10 +2665,10 @@ void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvO // normalize normals for (int k=vertadr; k < vertadr+vertnum; k++) { float s = sqrtf( - scn->skinnormal[3*k]*scn->skinnormal[3*k] + - scn->skinnormal[3*k+1]*scn->skinnormal[3*k+1] + - scn->skinnormal[3*k+2]*scn->skinnormal[3*k+2] - ); + scn->skinnormal[3*k+0]*scn->skinnormal[3*k+0] + + scn->skinnormal[3*k+1]*scn->skinnormal[3*k+1] + + scn->skinnormal[3*k+2]*scn->skinnormal[3*k+2] + ); float scl = 1/mjMAX(mjMINVAL, s); scn->skinnormal[3*k] *= scl;