From 455b1cd2e20fe6dbdf98e6fa73b270bffeea9f83 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sun, 28 May 2023 05:01:55 -0700 Subject: [PATCH] Add spaces around comparison operators in engine source files. PiperOrigin-RevId: 535989348 Change-Id: I883f7e82351299933c49b35a31842b5d8d6aea04 --- src/engine/engine_callback.c | 10 +- src/engine/engine_collision_convex.c | 116 ++--- src/engine/engine_collision_driver.c | 209 ++++----- src/engine/engine_collision_primitive.c | 8 +- src/engine/engine_core_constraint.c | 378 ++++++++--------- src/engine/engine_core_smooth.c | 208 ++++----- src/engine/engine_derivative.c | 194 ++++----- src/engine/engine_derivative_fd.c | 52 +-- src/engine/engine_forward.c | 84 ++-- src/engine/engine_inverse.c | 6 +- src/engine/engine_io.c | 304 +++++++------- src/engine/engine_passive.c | 54 +-- src/engine/engine_print.c | 152 +++---- src/engine/engine_ray.c | 218 +++++----- src/engine/engine_sensor.c | 148 +++---- src/engine/engine_setconst.c | 132 +++--- src/engine/engine_solver.c | 328 ++++++++------- src/engine/engine_support.c | 442 +++++++++---------- src/engine/engine_util_blas.c | 156 +++---- src/engine/engine_util_errmem.c | 2 +- src/engine/engine_util_misc.c | 156 +++---- src/engine/engine_util_solve.c | 248 +++++------ src/engine/engine_util_sparse.c | 136 +++--- src/engine/engine_util_spatial.c | 29 +- src/engine/engine_vfs.c | 38 +- src/engine/engine_vis_init.c | 24 +- src/engine/engine_vis_interact.c | 64 +-- src/engine/engine_vis_state.c | 10 +- src/engine/engine_vis_visualize.c | 537 ++++++++++++------------ 29 files changed, 2224 insertions(+), 2219 deletions(-) diff --git a/src/engine/engine_callback.c b/src/engine/engine_callback.c index 4c8f5a41..67249f2a 100644 --- a/src/engine/engine_callback.c +++ b/src/engine/engine_callback.c @@ -21,11 +21,11 @@ mjfGeneric mjcb_passive = 0; mjfGeneric mjcb_control = 0; mjfConFilt mjcb_contactfilter = 0; -mjfSensor mjcb_sensor = 0; -mjfTime mjcb_time = 0; -mjfAct mjcb_act_bias = 0; -mjfAct mjcb_act_gain = 0; -mjfAct mjcb_act_dyn = 0; +mjfSensor mjcb_sensor = 0; +mjfTime mjcb_time = 0; +mjfAct mjcb_act_bias = 0; +mjfAct mjcb_act_gain = 0; +mjfAct mjcb_act_dyn = 0; diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 147f98be..497a5b22 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -73,13 +73,13 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { case mjGEOM_ELLIPSOID: // find support point on unit sphere: scale dir by ellipsoid sizes and renormalize - for (int i=0; i<3; i++) { + for (int i=0; i < 3; i++) { res[i] = dir[i] * size[i]; } mju_normalize3(res); // transform to ellipsoid - for (int i=0; i<3; i++) { + for (int i=0; i < 3; i++) { res[i] *= size[i]; } break; @@ -87,7 +87,7 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { case mjGEOM_CYLINDER: // set result in XY plane: support on circle tmp = mju_sqrt(dir[0]*dir[0] + dir[1]*dir[1]); - if (tmp>mjMINVAL) { + if (tmp > mjMINVAL) { res[0] = dir[0]/tmp*size[0]; res[1] = dir[1]/tmp*size[0]; } else { @@ -99,7 +99,7 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { break; case mjGEOM_BOX: - for (int i=0; i<3; i++) { + for (int i=0; i < 3; i++) { res[i] = mju_sign(dir[i]) * size[i]; } break; @@ -111,16 +111,16 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { ibest = -1; // no graph data: exhaustive search - if (m->mesh_graphadr[m->geom_dataid[g]]<0) { + if (m->mesh_graphadr[m->geom_dataid[g]] < 0) { // search all vertices, find best - for (int i=0; imesh_vertnum[m->geom_dataid[g]]; i++) { + for (int i=0; i < m->mesh_vertnum[m->geom_dataid[g]]; i++) { // vdot = dot(vertex, dir) vdot = dir[0] * (mjtNum)vertdata[3*i] + dir[1] * (mjtNum)vertdata[3*i+1] + dir[2] * (mjtNum)vertdata[3*i+2]; // update best - if (vdot>tmp) { + if (vdot > tmp) { tmp = vdot; ibest = i; } @@ -158,7 +158,7 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { dir[2] * (mjtNum)vertdata[3*vert_globalid[locid]+2]; // update best - if (vdot>tmp) { + if (vdot > tmp) { tmp = vdot; ibest = locid; change = 1; @@ -177,14 +177,14 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { } // sanity check, SHOULD NOT OCCUR - if (ibest<0) { + if (ibest < 0) { mju_warning("mesh_support could not find support vertex"); mju_zero3(res); } // copy best vertex else { - for (int i=0; i<3; i++) { + for (int i=0; i < 3; i++) { res[i] = (mjtNum)vertdata[3*ibest + i]; } } @@ -195,7 +195,7 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { } // add dir*margin/2 to result - for (int i=0; i<3; i++) { + for (int i=0; i < 3; i++) { res[i] += dir[i] * ccd->margin/2; } @@ -214,7 +214,7 @@ static int mjc_MPRIteration(mjtCCD* obj1, mjtCCD* obj2, const ccd_t* ccd, mjContact* con, int g1, int g2, mjtNum margin) { ccd_vec3_t dir, pos; ccd_real_t depth; - if (ccdMPRPenetration(obj1, obj2, ccd, &depth, &dir, &pos)==0) { + if (ccdMPRPenetration(obj1, obj2, ccd, &depth, &dir, &pos) == 0) { // contact is found but normal is undefined if (ccdVec3Eq(&dir, ccd_vec3_origin)) { return 0; @@ -242,7 +242,7 @@ static int mjc_MPRIteration(mjtCCD* obj1, mjtCCD* obj2, const ccd_t* ccd, // compare new contact to previous contacts, return 1 if it is far from all of them static int mjc_isDistinctContact(mjContact* con, int ncon, mjtNum tolerance) { - for (int i=0; imargin) { + if (dist > margin) { return 0; } @@ -454,16 +454,16 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d, mjtNum threshold = mju_dot3(normal, dif) - margin; // no graph data: exhaustive search - if (m->mesh_graphadr[m->geom_dataid[g]]<0) { + if (m->mesh_graphadr[m->geom_dataid[g]] < 0) { // search all vertices, find best - for (int i=0; imesh_vertnum[m->geom_dataid[g]] && countmesh_vertnum[m->geom_dataid[g]] && count < maxplanemesh; i++) { // vdot = dot(vertex, dir) vdot = locdir[0] * (mjtNum)vertdata[3*i] + locdir[1] * (mjtNum)vertdata[3*i+1] + locdir[2] * (mjtNum)vertdata[3*i+2]; // detect contact, skip best - if (vdot>threshold && i!=obj.meshindex) { + if (vdot > threshold && i != obj.meshindex) { count += addplanemesh(con+count, vertdata+3*i, pos1, normal, pos2, mat2, con->pos, m->geom_rbound[g2]); @@ -472,7 +472,7 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d, } // use graph data - else if (obj.meshindex>=0) { + else if (obj.meshindex >= 0) { // get info graphadr = m->mesh_graphadr[m->geom_dataid[g]]; numvert = m->mesh_graph[graphadr]; @@ -482,14 +482,14 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d, // look for contacts in ibest neighborhood int i = vert_edgeadr[obj.meshindex]; - while ((locid=edge_localid[i])>=0 && count= 0 && count < maxplanemesh) { // vdot = dot(vertex, dir) vdot = locdir[0] * (mjtNum)vertdata[3*vert_globalid[locid]] + locdir[1] * (mjtNum)vertdata[3*vert_globalid[locid]+1] + locdir[2] * (mjtNum)vertdata[3*vert_globalid[locid]+2]; // detect contact - if (vdot>threshold) { + if (vdot > threshold) { count += addplanemesh(con+count, vertdata+3*vert_globalid[locid], pos1, normal, pos2, mat2, con->pos, m->geom_rbound[g2]); @@ -522,10 +522,10 @@ static void prism_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *ve const mjtPrism* p = (const mjtPrism*)obj; // find best vertex in halfspace determined by dir.z - istart = dir->v[2]<0 ? 0 : 3; + istart = dir->v[2] < 0 ? 0 : 3; ibest = istart; best = mju_dot3(p->v[istart], dir->v); - for (int i=istart+1; iv[i], dir->v)) > best) { ibest = i; best = tmp; @@ -543,7 +543,7 @@ static void prism_center(const void *obj, ccd_vec3_t *center) { // compute mean mju_zero3(center->v); - for (int i=0; i<6; i++) { + for (int i=0; i < 6; i++) { mju_addTo3(center->v, p->v[i]); } mju_scl3(center->v, center->v, 1.0/6.0); @@ -607,7 +607,7 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d, r2 = m->geom_rbound[g2]; // box-sphere test: horizontal plane - for (int i=0; i<2; i++) { + for (int i=0; i < 2; i++) { if ((size1[i] < pos[i]-r2-margin) || (-size1[i] > pos[i]+r2+margin)) { return 0; } @@ -710,23 +710,23 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d, // 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; i = 3; @@ -754,7 +754,7 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d, mju_copy3(pos2, savepos2); // fix contact normals - for (int i=0; i0) { + if (C > 0) { return 0; } @@ -783,19 +783,19 @@ static int mjc_ellipsoidInside(mjtNum nrm[3], const mjtNum pos[3], const mjtNum // main iteration int iter; - for (iter=0; iter0); + if (det < mjMINVAL || A < mjMINVAL) { + return (iter > 0); } // ray intersection with ellipse: pos + x*nrm, x>=0 mjtNum x = (-B + mju_sqrt(det))/A; - if (x<0) { - return (iter>0); + if (x < 0) { + return (iter > 0); } // new point on ellipsoid @@ -811,7 +811,7 @@ static int mjc_ellipsoidInside(mjtNum nrm[3], const mjtNum pos[3], const mjtNum mju_copy3(nrm, newnrm); // terminate if converged - if (change-mjMINVAL) { + if (deriv > -mjMINVAL) { break; } // delta mjtNum delta = -val/deriv; - if (deltageom_type[gid[i]]; // set to -1 if type cannot be processed - if (type[i]!=mjGEOM_SPHERE && - type[i]!=mjGEOM_CAPSULE && - type[i]!=mjGEOM_ELLIPSOID && - type[i]!=mjGEOM_CYLINDER) { + if (type[i] != mjGEOM_SPHERE && + type[i] != mjGEOM_CAPSULE && + type[i] != mjGEOM_ELLIPSOID && + type[i] != mjGEOM_CYLINDER) { type[i] = -1; } } // neither type can be processed: nothing to do - if (type[0]<0 && type[1]<0) { + if (type[0] < 0 && type[1] < 0) { return; } @@ -903,8 +903,8 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in // process geoms in type range int processed[2] = {0, 0}; - for (int i=0; i<2; i++) { - if (type[i]>=0) { + for (int i=0; i < 2; i++) { + if (type[i] >= 0) { // get geom mat and size mjtNum* mat = d->geom_xmat + 9*gid[i]; mjtNum* size = m->geom_size + 3*gid[i]; @@ -924,12 +924,12 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in case mjGEOM_CAPSULE: // Z: bottom cap - if (pos[2]<-size[1]) { + if (pos[2] < -size[1]) { nrm[2] = pos[2]+size[1]; } // Z: top cap - else if (pos[2]>size[1]) { + else if (pos[2] > size[1]) { nrm[2] = pos[2]-size[1]; } @@ -946,7 +946,7 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in case mjGEOM_ELLIPSOID: // guard against invalid ellipsoid size (just in case) - if (size[0]0.95*size[1]) { + if (mju_abs(pos[2]) > 0.95*size[1]) { break; } @@ -974,7 +974,7 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in dst2 = mju_abs(size[0]-mju_norm(pos, 2)); // require 4x closer to round than flat wall - if (dst1<0.25*dst2) { + if (dst1 < 0.25*dst2) { break; } diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 598d7d24..6ae2cd41 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -68,7 +68,7 @@ static inline mjtNum squaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]) { // bounding-sphere collision static int mj_collideSphere(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) { mjtNum bound = m->geom_rbound[g1] + m->geom_rbound[g2] + margin; if (squaredDist3(d->geom_xpos+3*g1, d->geom_xpos+3*g2) > bound*bound) { return 0; @@ -76,13 +76,13 @@ static int mj_collideSphere(const mjModel* m, mjData* d, int g1, int g2, mjtNum } // 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 && plane_geom(m, d, g1, g2) > margin+m->geom_rbound[g2]) { - return 0; + return 0; } - if (m->geom_type[g2]==mjGEOM_PLANE && m->geom_rbound[g1]>0 + if (m->geom_type[g2] == mjGEOM_PLANE && m->geom_rbound[g1] > 0 && plane_geom(m, d, g2, g1) > margin+m->geom_rbound[g1]) { - return 0; + return 0; } return 1; } @@ -97,9 +97,9 @@ void mj_collidePair(const mjModel* m, mjData* d, int g1, int g2, int merged, if (merged) { // find matching pair int found = 0; - for (int k=startadr; kpair_geom1[k]==g1 && m->pair_geom2[k]==g2) || - (m->pair_geom1[k]==g2 && m->pair_geom2[k]==g1)) { + for (int k=startadr; k < pairadr; k++) { + if ((m->pair_geom1[k] == g1 && m->pair_geom2[k] == g2) || + (m->pair_geom1[k] == g2 && m->pair_geom2[k] == g1)) { found = 1; break; } @@ -139,9 +139,9 @@ int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6], mjtByte infinite[2] = {inf1[0] || inf1[1] || inf1[2], inf2[0] || inf2[1] || inf2[2]}; // compute centers in local coordinates - if (product==NULL) { - for (int i=0; i<2; i++) { // bounding boxes - for (int j=0; j<3; j++) { // axes + if (product == NULL) { + for (int i=0; i < 2; i++) { // bounding boxes + for (int j=0; j < 3; j++) { // axes mju_rotVecMat(xcenter[i], aabb[i], xmat[i]); mju_addTo3(xcenter[i], xpos[i]); } @@ -149,9 +149,9 @@ int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6], } // compute normals in global coordinates - for (int i=0; i<2; i++) { // bounding boxes - for (int j=0; j<3; j++) { // faces - for (int k=0; k<3; k++) { // world axes + for (int i=0; i < 2; i++) { // bounding boxes + for (int j=0; j < 3; j++) { // faces + for (int k=0; k < 3; k++) { // world axes normal[i][j][k] = xmat[i][3*k+j]; } } @@ -159,10 +159,10 @@ int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6], // precompute dot products if (product && offset && *initialize) { - for (int i=0; i<2; i++) { // bodies - for (int j=0; j<2; j++) { // bodies - for (int k=0; k<3; k++) { // axes - for (int l=0; l<3; l++) { // axes + for (int i=0; i < 2; i++) { // bodies + for (int j=0; j < 2; j++) { // bodies + for (int k=0; k < 3; k++) { // axes + for (int l=0; l < 3; l++) { // axes product[18*i + 9*j + 3*k + l] = mju_dot3(normal[i][l], normal[j][k]); } offset[6*i + 3*j + k] = mju_dot3(xpos[i], normal[j][k]); @@ -173,13 +173,13 @@ int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6], } // check intersections - for (int j=0; j<2; j++) { // bounding boxes + for (int j=0; j < 2; j++) { // bounding boxes if (infinite[1-j]) { continue; // skip test against an infinite body } - for (int k=0; k<3; k++) { // face - for (int i=0; i<2; i++) { // bounding boxes - if (product==NULL) { + for (int k=0; k < 3; k++) { // face + for (int i=0; i < 2; i++) { // bounding boxes + if (product == NULL) { proj[i] = mju_dot3(xcenter[i], normal[j][k]); radius[i] = fabs(aabb[i][3]*mju_dot3(normal[i][0], normal[j][k])) + fabs(aabb[i][4]*mju_dot3(normal[i][1], normal[j][k])) + @@ -210,7 +210,7 @@ static mjCollisionTree* mj_stackAllocTree(mjData* d, int max_stack) { _Static_assert(sizeof(mjCollisionTree*) % sizeof(mjtNum) == 0, "mjCollisionTree has a different size from mjtNum"); return (mjCollisionTree*)mj_stackAlloc( - d, max_stack * sizeof(mjCollisionTree*) / sizeof(mjtNum)); + d, max_stack * sizeof(mjCollisionTree*) / sizeof(mjtNum)); } // binary search between two body trees @@ -245,7 +245,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int b1, int b2, int nodeid2 = m->bvh_geomid[bvhadr2 + node2]; // both are leaves - if (isleaf1 && isleaf2 && nodeid1!=-1 && nodeid2!=-1) { + if (isleaf1 && isleaf2 && nodeid1 != -1 && nodeid2 != -1) { if (mj_collideSphere(m, d, nodeid1, nodeid2, /*margin=*/ 0)) { if (mj_collideOBB(m->geom_aabb + 6*nodeid1, m->geom_aabb + 6*nodeid2, d->geom_xpos + 3*nodeid1, d->geom_xmat + 9*nodeid1, @@ -272,7 +272,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int b1, int b2, // keep traversing the tree if (!isleaf1 && isleaf2) { - for (int i=0; i<2; i++) { + for (int i=0; i < 2; i++) { if (child1[2*node1+i] != -1) { if (nstack >= max_stack) mju_error("BVH stack depth exceeded."); // SHOULD NOT OCCUR stack[nstack].node1 = child1[2*node1+i]; @@ -281,7 +281,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int b1, int b2, } } } else if (isleaf1 && !isleaf2) { - for (int i=0; i<2; i++) { + for (int i=0; i < 2; i++) { if (child2[2*node2+i] != -1) { if (nstack >= max_stack) mju_error("BVH stack depth exceeded."); // SHOULD NOT OCCUR stack[nstack].node1 = node1; @@ -380,14 +380,14 @@ void mj_collision(const mjModel* m, mjData* d) { // return if disabled if (mjDISABLED(mjDSBL_CONSTRAINT) || mjDISABLED(mjDSBL_CONTACT) - || m->nconmax==0 || m->nbody < 2) { + || m->nconmax == 0 || m->nbody < 2) { return; } // predefined only; ignore exclude - if (m->opt.collision==mjCOL_PAIR) { + if (m->opt.collision == mjCOL_PAIR) { d->nbodypair_broad = npair; - for (pairadr=0; pairadrngeompair_narrow; int ngeompair_mid_before = d->ngeompair_mid; mj_collideGeoms(m, d, pairadr, -1, 0, 0); @@ -405,7 +405,7 @@ void mj_collision(const mjModel* m, mjData* d) { unsigned int last_signature = -1; // loop over body pairs (broadphase or all) - for (int i=0; i>16) & 0xFFFF; b2 = broadphasepair[i] & 0xFFFF; @@ -422,10 +422,10 @@ void mj_collision(const mjModel* m, mjData* d) { // merge predefined pairs merged = 0; startadr = pairadr; - if (npair && m->opt.collision==mjCOL_ALL) { + if (npair && m->opt.collision == mjCOL_ALL) { // 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, 0, 0); @@ -435,12 +435,12 @@ void mj_collision(const mjModel* m, mjData* d) { // handle exclusion if (nexclude) { // advance exadr while exclude_signature < signature - while (exadrexclude_signature[exadr]exclude_signature[exadr] < signature) { exadr++; } // skip this body pair if its signature is found in exclude array - if (exadrexclude_signature[exadr]==signature) { + if (exadr < nexclude && m->exclude_signature[exadr] == signature) { continue; } } @@ -450,7 +450,7 @@ void mj_collision(const mjModel* m, mjData* d) { // test all geom pairs within this body pair if (m->body_geomnum[b1] && m->body_geomnum[b2]) { - if (!mjDISABLED(mjDSBL_MIDPHASE) && m->body_geomnum[b1]*m->body_geomnum[b2]>1) { + if (!mjDISABLED(mjDSBL_MIDPHASE) && m->body_geomnum[b1]*m->body_geomnum[b2] > 1) { int ncon_before = d->ncon; mj_collideTree(m, d, b1, b2, merged, startadr, pairadr); int ncon_after = d->ncon; @@ -458,8 +458,8 @@ void mj_collision(const mjModel* m, mjData* d) { mjQUICKSORT(d->contact + ncon_before, ncon_after - ncon_before, sizeof(mjContact), contactcompare, context); } else { - for (g1=m->body_geomadr[b1]; g1body_geomadr[b1]+m->body_geomnum[b1]; g1++) { - for (g2=m->body_geomadr[b2]; g2body_geomadr[b2]+m->body_geomnum[b2]; g2++) { + for (g1=m->body_geomadr[b1]; g1 < m->body_geomadr[b1]+m->body_geomnum[b1]; g1++) { + for (g2=m->body_geomadr[b2]; g2 < m->body_geomadr[b2]+m->body_geomnum[b2]; g2++) { mj_collidePair(m, d, g1, g2, merged, startadr, pairadr); } } @@ -470,8 +470,8 @@ void mj_collision(const mjModel* m, mjData* d) { } // finish merging predefined pairs - if (npair && m->opt.collision==mjCOL_ALL) { - while (pairadropt.collision == mjCOL_ALL) { + while (pairadr < npair) { mj_collideGeoms(m, d, pairadr++, -1, 0, 0); } } @@ -498,28 +498,28 @@ static void makeAABB(const mjModel* m, mjData* d, mjtNum* aabb, int body, const mjtNum _aabb[6], cen; // no geoms attached to body: set to 0 - if (m->body_geomnum[body]==0) { + if (m->body_geomnum[body] == 0) { mju_zero(aabb, 6); return; } // process all body geoms - for (int i=0; ibody_geomnum[body]; i++) { + for (int i=0; i < m->body_geomnum[body]; i++) { // get geom id geom = m->body_geomadr[body]+i; // set _aabb for this geom - for (int j=0; j<3; j++) { + for (int j=0; j < 3; j++) { cen = mju_dot3(d->geom_xpos+3*geom, frame+3*j); _aabb[2*j] = cen - m->geom_rbound[geom] - m->geom_margin[geom]; _aabb[2*j+1] = cen + m->geom_rbound[geom] + m->geom_margin[geom]; } // update body aabb - if (i==0) { + if (i == 0) { mju_copy(aabb, _aabb, 6); } else { - for (int j=0; j<3; j++) { + for (int j=0; j < 3; j++) { aabb[2*j] = mju_min(aabb[2*j], _aabb[2*j]); aabb[2*j+1] = mju_max(aabb[2*j+1], _aabb[2*j+1]); } @@ -536,8 +536,8 @@ static int has_plane_or_hfield(const mjModel* m, int body) { // scan geoms belonging to body int g; - for (g=start; ggeom_type[g]==mjGEOM_PLANE || m->geom_type[g]==mjGEOM_HFIELD) { + for (g=start; g < end; g++) { + if (m->geom_type[g] == mjGEOM_PLANE || m->geom_type[g] == mjGEOM_HFIELD) { return 1; } } @@ -549,7 +549,7 @@ static int has_plane_or_hfield(const mjModel* m, int body) { static int body_pair_filter(int weldbody1, int weldparent1, int weldbody2, int weldparent2, int dsbl_filterparent) { // same weldbody check - if (weldbody1==weldbody2) { + if (weldbody1 == weldbody2) { return 1; } @@ -566,9 +566,9 @@ static int body_pair_filter(int weldbody1, int weldparent1, int weldbody2, // add body pair in buffer static void add_pair(const mjModel* m, int b1, int b2, int* npair, int* pair, int maxpair) { // add pair if there is room in buffer - if ((*npair)body_geomnum[b1]==1 && m->body_geomnum[b2]==1) { + if (m && m->body_geomnum[b1] == 1 && m->body_geomnum[b2] == 1) { // get contypes and conaffinities int contype1 = m->geom_contype[m->body_geomadr[b1]]; int conaffinity1 = m->geom_conaffinity[m->body_geomadr[b1]]; @@ -582,7 +582,7 @@ static void add_pair(const mjModel* m, int b1, int b2, int* npair, int* pair, in } // add pair - if (b1valuevalue) { + if (b1->value < b2->value) { return -1; - } else if (b1->value==b2->value) { + } else if (b1->value == b2->value) { return 0; } else { return 1; @@ -617,9 +617,9 @@ quicksortfunc(paircompare, context, el1, el2) { int signature1 = *(int*)el1; int signature2 = *(int*)el2; - if (signature1body_geomnum[b]; g++) { + for (g=0; g < m->body_geomnum[b]; g++) { int ind = m->body_geomadr[b] + g; if (m->geom_contype[ind] || m->geom_conaffinity[ind]) { return 1; @@ -656,17 +656,18 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) { int dsbl_filterparent = mjDISABLED(mjDSBL_FILTERPARENT); // world with geoms, and body with plane or hfield, can collide all bodies - for (b1=0; b1body_geomnum[b1]>0) || (m->body_weldid[b1]==0 && has_plane_or_hfield(m, b1))) { + if ((b1 == 0 && m->body_geomnum[b1] > 0) || + (m->body_weldid[b1] == 0 && has_plane_or_hfield(m, b1))) { int weld1 = 0; int parent_weld1 = 0; - for (b2=0; b2body_weldid[b2]; int parent_weld2 = m->body_weldid[m->body_parentid[weld2]]; if (!body_pair_filter(weld1, parent_weld1, weld2, parent_weld2, @@ -680,23 +681,23 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) { // find center of non-world geoms; return if none cnt = 0; mju_zero3(cen); - for (int i=0; igeom_bodyid[i]) { mju_addTo3(cen, d->geom_xpos+3*i); cnt++; } } - if (cnt==0) { + if (cnt == 0) { return npair; } else { - for (int i=0; i<3; i++) { + for (int i=0; i < 3; i++) { cen[i] /= cnt; } } // compute covariance mju_zero(cov, 9); - for (int i=0; igeom_bodyid[i]) { mju_sub3(dif, d->geom_xpos+3*i, cen); mjtNum D00 = dif[0]*dif[0]; @@ -716,7 +717,7 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) { cov[8] += D22; } } - for (int i=0; i<9; i++) { + for (int i=0; i < 9; i++) { cov[i] /= cnt; } @@ -729,7 +730,7 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) { // construct body AABB for the aligned frame, count collidable int bufcnt = 0; - for (int i=1; i=0) { + if (ipair >= 0) { g1 = m->pair_geom1[ipair]; g2 = m->pair_geom2[ipair]; } @@ -872,7 +873,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user, } // apply filters if not predefined pair and not flg_user - if (ipair<0 && !flg_user) { + if (ipair < 0 && !flg_user) { // user filter if defined if (mjcb_contactfilter) { if (mjcb_contactfilter(m, d, g1, g2)) { @@ -888,14 +889,14 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user, } // set margin, gap, condim: dynamic - if (ipair<0) { + if (ipair < 0) { // margin and gap: max margin = mju_max(m->geom_margin[g1], m->geom_margin[g2]); gap = mju_max(m->geom_gap[g1], m->geom_gap[g2]); // condim: priority or max - if (m->geom_priority[g1]!=m->geom_priority[g2]) { - int gp = (m->geom_priority[g1]>m->geom_priority[g2] ? g1 : g2); + if (m->geom_priority[g1] != m->geom_priority[g2]) { + int gp = (m->geom_priority[g1] > m->geom_priority[g2] ? g1 : g2); condim = m->geom_condim[gp]; } else { condim = mjMAX(m->geom_condim[g1], m->geom_condim[g2]); @@ -936,23 +937,23 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user, d->ngeompair_narrow++; // check number of contacts, SHOULD NOT OCCUR - if (num>mjMAXCONPAIR) { + if (num > mjMAXCONPAIR) { mju_error("Too many contacts returned by collision function"); } // remove repeated contacts in box-box - if (type1==mjGEOM_BOX && type2==mjGEOM_BOX) { + if (type1 == mjGEOM_BOX && type2 == mjGEOM_BOX) { // use dim field to mark: -1: bad, 0: good - for (int i=0; igeom_priority[g1]!=m->geom_priority[g2]) { - int gp = (m->geom_priority[g1]>m->geom_priority[g2] ? g1 : g2); + if (m->geom_priority[g1] != m->geom_priority[g2]) { + int gp = (m->geom_priority[g1] > m->geom_priority[g2] ? g1 : g2); // friction - for (int i=0; i<3; i++) { + for (int i=0; i < 3; i++) { friction[2*i] = m->geom_friction[3*gp+i]; } @@ -998,31 +999,31 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user, // same priority else { // friction: max - for (int i=0; i<3; i++) { + for (int i=0; i < 3; i++) { friction[2*i] = mju_max(m->geom_friction[3*g1+i], m->geom_friction[3*g2+i]); } // solver mix factor - if (m->geom_solmix[g1]>=mjMINVAL && m->geom_solmix[g2]>=mjMINVAL) { + if (m->geom_solmix[g1] >= mjMINVAL && m->geom_solmix[g2] >= mjMINVAL) { mix = m->geom_solmix[g1] / (m->geom_solmix[g1] + m->geom_solmix[g2]); - } else if (m->geom_solmix[g1]geom_solmix[g2]geom_solmix[g1] < mjMINVAL && m->geom_solmix[g2] < mjMINVAL) { mix = 0.5; - } else if (m->geom_solmix[g1]geom_solmix[g1] < mjMINVAL) { mix = 0.0; } else { mix = 1.0; } // reference standard: mix - if (m->geom_solref[mjNREF*g1]>0 && m->geom_solref[mjNREF*g2]>0) { - for (int i=0; igeom_solref[mjNREF*g1] > 0 && m->geom_solref[mjNREF*g2] > 0) { + for (int i=0; i < mjNREF; i++) { solref[i] = mix*m->geom_solref[mjNREF*g1+i] + (1-mix)*m->geom_solref[mjNREF*g2+i]; } } // reference direct: min else { - for (int i=0; igeom_solref[mjNREF*g1+i], m->geom_solref[mjNREF*g2+i]); } } @@ -1040,7 +1041,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user, // set friction, solref, solimp: pair else { // friction - for (int i=0; i<5; i++) { + for (int i=0; i < 5; i++) { friction[i] = m->pair_friction[5*ipair+i]; } @@ -1052,12 +1053,12 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user, } // clamp friction to mjMINMU - for (int i=0; i<5; i++) { + for (int i=0; i < 5; i++) { friction[i] = mju_max(mjMINMU, friction[i]); } // add contact returned by collision detector - for (int i=0; i 6 || condim < 1) { // SHOULD NOT OCCUR mju_error("Invalid condim value: %d", i); @@ -1071,7 +1072,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user, mj_assignImp(m, con[i].solimp, solimp); // exclude in gap - if (con[i].dist=2) { + if (n1+n2 >= 2) { return n1+n2; } @@ -440,7 +440,7 @@ int mjc_CapsuleCapsule(const mjModel* m, const mjData* d, n3 = _SphereSphere(con+n1+n2, margin, vec1, mat1, size1, vec2, mat2, size2); // return if two contacts already found - if (n1+n2+n3>=2) { + if (n1+n2+n3 >= 2) { return n1+n2+n3; } diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 75eda11d..c0e9f52c 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -56,7 +56,7 @@ static inline void clearEfc(mjData* d) { // determine type of friction cone int mj_isPyramidal(const mjModel* m) { - if (m->opt.cone==mjCONE_PYRAMIDAL) { + if (m->opt.cone == mjCONE_PYRAMIDAL) { return 1; } else { return 0; @@ -67,8 +67,8 @@ int mj_isPyramidal(const mjModel* m) { // determine type of constraint Jacobian int mj_isSparse(const mjModel* m) { - if (m->opt.jacobian==mjJAC_SPARSE || - (m->opt.jacobian==mjJAC_AUTO && m->nv>=60)) { + if (m->opt.jacobian == mjJAC_SPARSE || + (m->opt.jacobian == mjJAC_AUTO && m->nv >= 60)) { return 1; } else { return 0; @@ -79,7 +79,7 @@ int mj_isSparse(const mjModel* m) { // determine type of solver int mj_isDual(const mjModel* m) { - if (m->opt.solver==mjSOL_PGS || m->opt.noslip_iterations>0) { + if (m->opt.solver == mjSOL_PGS || m->opt.noslip_iterations > 0) { return 1; } else { return 0; @@ -133,7 +133,7 @@ int mj_addContact(const mjModel* m, mjData* d, const mjContact* con) { d->parena = d->ncon * sizeof(mjContact); #ifdef ADDRESS_SANITIZER ASAN_POISON_MEMORY_REGION( - (char*)d->arena + d->parena, (d->nstack - d->pstack) * sizeof(mjtNum) - d->parena); + (char*)d->arena + d->parena, (d->nstack - d->pstack) * sizeof(mjtNum) - d->parena); #endif clearEfc(d); @@ -163,9 +163,9 @@ int mj_addConstraint(const mjModel* m, mjData* d, mjtNum *J = d->efc_J; // init empty guard for constraints other than contact - if (type==mjCNSTR_CONTACT_FRICTIONLESS || - type==mjCNSTR_CONTACT_PYRAMIDAL || - type==mjCNSTR_CONTACT_ELLIPTIC) { + if (type == mjCNSTR_CONTACT_FRICTIONLESS || + type == mjCNSTR_CONTACT_PYRAMIDAL || + type == mjCNSTR_CONTACT_ELLIPTIC) { empty = 0; } else { empty = 1; @@ -175,7 +175,7 @@ int mj_addConstraint(const mjModel* m, mjData* d, if (!mj_isSparse(m)) { // make sure jac is not empty if (empty) { - for (int i=0; iefc_pos[nefc+i] = (pos ? pos[i] : 0); d->efc_margin[nefc+i] = (margin ? margin[i] : 0); d->efc_frictionloss[nefc+i] = frictionloss; @@ -238,9 +238,9 @@ int mj_addConstraint(const mjModel* m, mjData* d, // increase counters d->nefc += size; - if (type==mjCNSTR_EQUALITY) { + if (type == mjCNSTR_EQUALITY) { d->ne += size; - } else if (type==mjCNSTR_FRICTION_DOF || type==mjCNSTR_FRICTION_TENDON) { + } else if (type == mjCNSTR_FRICTION_DOF || type == mjCNSTR_FRICTION_TENDON) { d->nf += size; } @@ -262,7 +262,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; } @@ -271,19 +271,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; @@ -307,17 +307,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; } @@ -380,7 +380,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { mjMARKSTACK; // disabled or no equality constraints: return - if (mjDISABLED(mjDSBL_EQUALITY) || m->nemax==0) { + if (mjDISABLED(mjDSBL_EQUALITY) || m->nemax == 0) { return; } @@ -396,7 +396,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { } // find active equality constraints - for (int i=0; ineq; i++) { + for (int i=0; i < m->neq; i++) { if (m->eq_active[i]) { // get constraint data data = m->eq_data + mjNEQDATA*i; @@ -410,7 +410,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { switch (m->eq_type[i]) { case mjEQ_CONNECT: // connect bodies with ball joint // find global points - for (int j=0; j<2; j++) { + for (int j=0; j < 2; j++) { mju_rotVecMat(pos[j], data + 3*j, d->xmat + 9*id[j]); mju_addTo3(pos[j], d->xpos + 3*id[j]); } @@ -430,7 +430,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { case mjEQ_WELD: // fix relative position and orientation // find global points - for (int j=0; j<2; j++) { + for (int j=0; j < 2; j++) { mjtNum* anchor = data + 3*(1-j); mju_rotVecMat(pos[j], anchor, d->xmat + 9*id[j]); mju_addTo3(pos[j], d->xpos + 3*id[j]); @@ -456,7 +456,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { mju_copy3(cpos+3, quat2+1); // copy axis components // correct rotation Jacobian: 0.5 * neg(q1) * (jac0-jac1) * q0 * relpose - for (int j=0; j=0); j++) - if (m->eq_type[i]==mjEQ_JOINT) { // joint object + for (int j=0; j < 1+(id[1] >= 0); j++) + if (m->eq_type[i] == mjEQ_JOINT) { // joint object pos[j][0] = d->qpos[m->jnt_qposadr[id[j]]]; ref[j] = m->qpos0[m->jnt_qposadr[id[j]]]; // make Jacobian: sparse or dense if (issparse) { // add first or second joint - if (j==0) { + if (j == 0) { NV = 1; chain[0] = m->jnt_dofadr[id[j]]; jac[j][0] = 1; @@ -510,7 +510,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { // copy Jacobian: sparse or dense if (issparse) { // add first or second chain - if (j==0) { + if (j == 0) { NV = d->ten_J_rownnz[id[j]]; memcpy(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV*sizeof(int)); mju_copy(jac[j], d->ten_J+d->ten_J_rowadr[id[j]], NV); @@ -525,7 +525,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { } // both objects defined - if (id[1]>=0) { + if (id[1] >= 0) { // compute position error dif = pos[1][0] - ref[1]; cpos[0] = pos[0][0] - ref[0] - data[0] - @@ -591,8 +591,8 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) { jac = mj_stackAlloc(d, nv); // find frictional dofs - for (int i=0; idof_frictionloss[i]>0) { + for (int i=0; i < nv; i++) { + if (m->dof_frictionloss[i] > 0) { // prepare Jacobian: sparse or dense if (issparse) { jac[0] = 1; @@ -612,8 +612,8 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) { } // find frictional tendons - for (int i=0; intendon; i++) { - if (m->tendon_frictionloss[i]>0) { + for (int i=0; i < m->ntendon; i++) { + if (m->tendon_frictionloss[i] > 0) { // add constraint if (mj_addConstraint(m, d, d->ten_J + (issparse ? d->ten_J_rowadr[i] : i*nv), 0, 0, m->tendon_frictionloss[i], @@ -646,23 +646,23 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) { jac = mj_stackAlloc(d, nv); // find joint limits - for (int i=0; injnt; i++) { + for (int i=0; i < m->njnt; i++) { if (m->jnt_limited[i]) { // get margin margin = m->jnt_margin[i]; // HINGE or SLIDE joint - if (m->jnt_type[i]==mjJNT_SLIDE || m->jnt_type[i]==mjJNT_HINGE) { + if (m->jnt_type[i] == mjJNT_SLIDE || m->jnt_type[i] == mjJNT_HINGE) { // get joint value value = d->qpos[m->jnt_qposadr[i]]; // process lower and upper limits - for (side=-1; side<=1; side+=2) { + for (side=-1; side <= 1; side+=2) { // compute distance (negative: penetration) dist = side * (m->jnt_range[2*i+(side+1)/2] - value); // detect joint limit - if (distjnt_type[i]==mjJNT_BALL) { + else if (m->jnt_type[i] == mjJNT_BALL) { // convert joint quaternion to axis-angle mju_quat2Vel(angleAxis, d->qpos+m->jnt_qposadr[i], 1); @@ -694,7 +694,7 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) { dist = mju_max(m->jnt_range[2*i], m->jnt_range[2*i+1]) - value; // detect joint limit - if (distntendon; i++) { + for (int i=0; i < m->ntendon; i++) { if (m->tendon_limited[i]) { // get value = lenth, margin value = d->ten_length[i]; margin = m->tendon_margin[i]; // process lower and upper limits - for (side=-1; side<=1; side+=2) { + for (side=-1; side <= 1; side+=2) { // compute distance (negative: penetration) dist = side * (m->tendon_range[2*i+(side+1)/2] - value); // detect tendon limit - if (distten_J+d->ten_J_rowadr[i], -side, d->ten_J_rownnz[i]); @@ -777,7 +777,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) { mjtNum cpos[6], cmargin[6], *jac, *jacdifp, *jacdifr, *jac1p, *jac2p, *jac1r, *jac2r; mjMARKSTACK; - if (mjDISABLED(mjDSBL_CONTACT) || ncon==0) { + if (mjDISABLED(mjDSBL_CONTACT) || ncon == 0) { return; } @@ -794,7 +794,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) { } // find contacts to be included - for (int i=0; icontact[i].exclude) { // get pointer to this contact, info con = d->contact + i; @@ -806,7 +806,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) { con->efc_address = d->nefc; // compute Jacobian differences - if (dim>3) { + if (dim > 3) { NV = mj_jacDifPair(m, d, chain, b1, b2, con->pos, con->pos, jac1p, jac2p, jacdifp, jac1r, jac2r, jacdifr); } else { @@ -815,20 +815,20 @@ void mj_instantiateContact(const mjModel* m, mjData* d) { } // skip contact if no DOFs affected - if (NV==0) { + if (NV == 0) { con->efc_address = -1; con->exclude = 3; continue; } // rotate Jacobian differences to contact frame - mju_mulMatMat(jac, con->frame, jacdifp, dim>1 ? 3 : 1, 3, NV); - if (dim>3) { + mju_mulMatMat(jac, con->frame, jacdifp, dim > 1 ? 3 : 1, 3, NV); + if (dim > 3) { mju_mulMatMat(jac + 3*NV, con->frame, jacdifr, dim-3, 3, NV); } // make frictionless contact - if (dim==1) { + if (dim == 1) { // add constraint (already checked space) mj_addConstraint(m, d, jac, &(con->dist), &(con->includemargin), 0, 1, mjCNSTR_CONTACT_FRICTIONLESS, i, @@ -843,7 +843,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) { cmargin[0] = cmargin[1] = con->includemargin; // one pair per friction dimension - for (int k=1; kdim; k++) { + for (int k=1; k < con->dim; k++) { // Jacobian for pair of opposing pyramid edges mju_addScl(jacdifp, jac, jac + k*NV, con->friction[k-1], NV); mju_addScl(jacdifp + NV, jac, jac + k*NV, -con->friction[k-1], NV); @@ -887,12 +887,12 @@ void mj_diagApprox(const mjModel* m, mjData* d) { mjtNum tran, rot, fri, *dA = d->efc_diagApprox; // loop over all constraints, compute approximate inverse inertia - for (int i=0; iefc_id[i]; // clear weld counter - if (d->efc_type[i]!=mjEQ_WELD) { + if (d->efc_type[i] != mjEQ_WELD) { weldcnt = 0; } @@ -912,21 +912,21 @@ void mj_diagApprox(const mjModel* m, mjData* d) { // body translation or rotation depending on weldcnt b1 = m->eq_obj1id[id]; b2 = m->eq_obj2id[id]; - dA[i] = m->body_invweight0[2*b1 + (weldcnt>2)] + - m->body_invweight0[2*b2 + (weldcnt>2)]; + dA[i] = m->body_invweight0[2*b1 + (weldcnt > 2)] + + m->body_invweight0[2*b2 + (weldcnt > 2)]; weldcnt++; break; case mjEQ_JOINT: case mjEQ_TENDON: // object 1 contribution - dA[i] = (m->eq_type[id]==mjEQ_JOINT ? + dA[i] = (m->eq_type[id] == mjEQ_JOINT ? m->dof_invweight0[m->jnt_dofadr[m->eq_obj1id[id]]] : m->tendon_invweight0[m->eq_obj1id[id]]); // add object 2 contribution if present - if (m->eq_obj2id[id]>=0) - dA[i] += (m->eq_type[id]==mjEQ_JOINT ? + if (m->eq_obj2id[id] >= 0) + dA[i] += (m->eq_type[id] == mjEQ_JOINT ? m->dof_invweight0[m->jnt_dofadr[m->eq_obj2id[id]]] : m->tendon_invweight0[m->eq_obj2id[id]]); break; @@ -962,14 +962,14 @@ void mj_diagApprox(const mjModel* m, mjData* d) { rot = m->body_invweight0[2*b1+1] + m->body_invweight0[2*b2+1]; // set frictionless - if (d->efc_type[i]==mjCNSTR_CONTACT_FRICTIONLESS) { + if (d->efc_type[i] == mjCNSTR_CONTACT_FRICTIONLESS) { dA[i] = tran; } // set elliptical - else if (d->efc_type[i]==mjCNSTR_CONTACT_ELLIPTIC) { - for (int j=0; jefc_type[i] == mjCNSTR_CONTACT_ELLIPTIC) { + for (int j=0; j < dim; j++) { + dA[i+j] = (j < 3 ? tran : rot); } // processed dim elements in one i-loop iteration; advance counter @@ -978,9 +978,9 @@ void mj_diagApprox(const mjModel* m, mjData* d) { // set pyramidal else { - for (int j=0; jcontact[id].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 @@ -1032,13 +1032,13 @@ static void getsolparam(const mjModel* m, const mjData* d, int i, mjtNum* solref } // check reference format: standard or direct, cannot be mixed - if ((solref[0]>0) ^ (solref[1]>0)) { + if ((solref[0] > 0) ^ (solref[1] > 0)) { mju_warning("mixed solref format, replacing with default"); mj_defaultSolRefImp(solref, NULL); } // integrator safety: impose ref[0]>=2*timestep for standard format - if (!mjDISABLED(mjDSBL_REFSAFE) && solref[0]>0) { + if (!mjDISABLED(mjDSBL_REFSAFE) && solref[0] > 0) { solref[0] = mju_max(solref[0], 2*m->opt.timestep); } @@ -1072,7 +1072,7 @@ static void getposdim(const mjModel* m, const mjData* d, int i, mjtNum* pos, int break; case mjCNSTR_EQUALITY: - if (m->eq_type[id]==mjEQ_WELD) { + if (m->eq_type[id] == mjEQ_WELD) { mjtNum rotlinratio = m->eq_data[mjNEQDATA*id+10]; mjtNum efc_pos[6]; @@ -1083,7 +1083,7 @@ static void getposdim(const mjModel* m, const mjData* d, int i, mjtNum* pos, int mju_scl3(efc_pos+3, d->efc_pos+i+3, rotlinratio); *dim = 6; *pos = mju_norm(efc_pos, 6); - } else if (m->eq_type[id]==mjEQ_CONNECT) { + } else if (m->eq_type[id] == mjEQ_CONNECT) { *dim = 3; *pos = mju_norm(d->efc_pos+i, 3); } @@ -1096,7 +1096,7 @@ static void getposdim(const mjModel* m, const mjData* d, int i, mjtNum* pos, int static void getimpedance(const mjtNum* solimp, mjtNum pos, mjtNum margin, mjtNum* imp, mjtNum* impP) { // flat function - if (solimp[0]==solimp[1] || solimp[2]<=mjMINVAL) { + if (solimp[0] == solimp[1] || solimp[2] <= mjMINVAL) { *imp = 0.5*(solimp[0] + solimp[1]); *impP = 0; return; @@ -1105,27 +1105,27 @@ static void getimpedance(const mjtNum* solimp, mjtNum pos, mjtNum margin, // x = abs((pos-margin) / width) mjtNum x = (pos-margin) / solimp[2]; mjtNum sgn = 1; - if (x<0) { + if (x < 0) { x = -x; sgn = -1; } // fully saturated - if (x>=1 || x<=0) { - *imp = (x>=1 ? solimp[1] : solimp[0]); + if (x >= 1 || x <= 0) { + *imp = (x >= 1 ? solimp[1] : solimp[0]); *impP = 0; return; } // linear mjtNum y, yP; - if (solimp[4]==1) { + if (solimp[4] == 1) { y = x; yP = 1; } // y(x) = a*x^p if x<=midpoint - else if (x<=solimp[3]) { + else if (x <= solimp[3]) { mjtNum a = 1/mju_pow(solimp[3], solimp[4]-1); y = a*mju_pow(x, solimp[4]); yP = solimp[4] * a*mju_pow(x, solimp[4]-1); @@ -1152,7 +1152,7 @@ void mj_makeImpedance(const mjModel* m, mjData* d) { mjtNum pos, imp, impP, Rpy, solref[mjNREF], solimp[mjNIMP]; // set efc_R, efc_KBIP - for (int i=0; iefc_margin[i], &imp, &impP); // set R and KBIP for all constraint dimensions - for (int j=0; jefc_diagApprox[i+j]/imp); // friction: K = 0 int tp = d->efc_type[i+j]; - if (tp==mjCNSTR_FRICTION_DOF || - tp==mjCNSTR_FRICTION_TENDON || - (tp==mjCNSTR_CONTACT_ELLIPTIC && j>0)) { + if (tp == mjCNSTR_FRICTION_DOF || + tp == mjCNSTR_FRICTION_TENDON || + (tp == mjCNSTR_CONTACT_ELLIPTIC && j > 0)) { KBIP[4*(i+j)] = 0; } // standard: K = 1 / (dmax^2 * timeconst^2 * dampratio^2) - else if (solref[0]>0) + else if (solref[0] > 0) KBIP[4*(i+j)] = 1 / mju_max(mjMINVAL, solimp[1]*solimp[1] * solref[0]*solref[0] * solref[1]*solref[1]); @@ -1186,7 +1186,7 @@ void mj_makeImpedance(const mjModel* m, mjData* d) { } // standard: B = 2 / (dmax*timeconst) - if (solref[1]>0) { + if (solref[1] > 0) { KBIP[4*(i+j)+1] = 2 / mju_max(mjMINVAL, solimp[1]*solref[0]); } @@ -1205,9 +1205,9 @@ void mj_makeImpedance(const mjModel* m, mjData* d) { } // frictional contacts: adjust R in friction dimensions, set contact master mu - for (int i=d->ne+d->nf; iefc_type[i]==mjCNSTR_CONTACT_PYRAMIDAL || - d->efc_type[i]==mjCNSTR_CONTACT_ELLIPTIC) { + for (int i=d->ne+d->nf; i < nefc; i++) { + if (d->efc_type[i] == mjCNSTR_CONTACT_PYRAMIDAL || + d->efc_type[i] == mjCNSTR_CONTACT_ELLIPTIC) { // extract id, dim, mu int id = d->efc_id[i]; dim = d->contact[id].dim; @@ -1220,9 +1220,9 @@ void mj_makeImpedance(const mjModel* m, mjData* d) { d->contact[id].mu = friction[0] * mju_sqrt(R[i+1]/R[i]); // elliptic - if (d->efc_type[i]==mjCNSTR_CONTACT_ELLIPTIC) { + if (d->efc_type[i] == mjCNSTR_CONTACT_ELLIPTIC) { // set remaining R's such that R[j]*mu[j]^2 = R[1]*mu[1]^2 - for (int j=1; jcontact[id].mu*d->contact[id].mu*R[i]; // assign Rpy to all pyramidal R - for (int j=0; j<2*(dim-1); j++) { + for (int j=0; j < 2*(dim-1); j++) { R[i+j] = Rpy; } @@ -1248,12 +1248,12 @@ void mj_makeImpedance(const mjModel* m, mjData* d) { } // set D = 1 / R - for (int i=0; iefc_D[i] = 1 / R[i]; } // adjust diagApprox so that R = (1-imp)/imp * diagApprox - for (int i=0; iefc_diagApprox[i] = R[i] * KBIP[4*i+2] / (1-KBIP[4*i+2]); } } @@ -1320,7 +1320,7 @@ static inline int mj_ne(const mjModel* m, mjData* d, int* nnz) { int id[2], size, NV, NV2, *chain = NULL, *chain2 = NULL; // disabled or no equality constraints: return - if (mjDISABLED(mjDSBL_EQUALITY) || m->nemax==0) { + if (mjDISABLED(mjDSBL_EQUALITY) || m->nemax == 0) { return 0; } @@ -1332,7 +1332,7 @@ static inline int mj_ne(const mjModel* m, mjData* d, int* nnz) { } // find active equality constraints - for (int i=0; ieq_active[i]) { id[0] = m->eq_obj1id[i]; id[1] = m->eq_obj2id[i]; @@ -1342,56 +1342,56 @@ static inline int mj_ne(const mjModel* m, mjData* d, int* nnz) { // process according to type switch (m->eq_type[i]) { - case mjEQ_CONNECT: - size = 3; - if (!nnz) { - break; - } - - NV = mj_jacDifPairCount(m, chain, id[1], id[0]); + case mjEQ_CONNECT: + size = 3; + if (!nnz) { break; + } - case mjEQ_WELD: - size = 6; - if (!nnz) { - break; - } + NV = mj_jacDifPairCount(m, chain, id[1], id[0]); + break; - NV = mj_jacDifPairCount(m, chain, id[1], id[0]); + case mjEQ_WELD: + size = 6; + if (!nnz) { break; + } - case mjEQ_JOINT: - case mjEQ_TENDON: - size = 1; - if (!nnz) { - break; - } + NV = mj_jacDifPairCount(m, chain, id[1], id[0]); + break; - for (int j=0; j<1+(id[1]>=0); j++) { - if (m->eq_type[i]==mjEQ_JOINT) { - if (!j) { - NV = 1; - chain[0] = m->jnt_dofadr[id[j]]; - } else { - NV2 = 1; - chain2[0] = m->jnt_dofadr[id[j]]; - } + case mjEQ_JOINT: + case mjEQ_TENDON: + size = 1; + if (!nnz) { + break; + } + + for (int j=0; j < 1+(id[1] >= 0); j++) { + if (m->eq_type[i] == mjEQ_JOINT) { + if (!j) { + NV = 1; + chain[0] = m->jnt_dofadr[id[j]]; } else { - if (!j) { - NV = d->ten_J_rownnz[id[j]]; - memcpy(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV*sizeof(int)); - } else { - NV2 = d->ten_J_rownnz[id[j]]; - memcpy(chain2, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV2*sizeof(int)); - } + NV2 = 1; + chain2[0] = m->jnt_dofadr[id[j]]; + } + } else { + if (!j) { + NV = d->ten_J_rownnz[id[j]]; + memcpy(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV*sizeof(int)); + } else { + NV2 = d->ten_J_rownnz[id[j]]; + memcpy(chain2, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV2*sizeof(int)); } } + } - if (id[1]>=0) { - NV = mju_combineSparseCount(NV, NV2, chain, chain2); - NV = 2; - } - break; + if (id[1] >= 0) { + NV = mju_combineSparseCount(NV, NV2, chain, chain2); + NV = 2; + } + break; } ne += mj_addConstraintCount(m, size, NV); nnze += size*NV; @@ -1417,15 +1417,15 @@ static inline int mj_nf(const mjModel* m, const mjData* d, int *nnz) { return 0; } - for (int i=0; idof_frictionloss[i]>0) { + for (int i=0; i < nv; i++) { + if (m->dof_frictionloss[i] > 0) { nf += mj_addConstraintCount(m, 1, 1); nnzf++; } } - for (int i=0; itendon_frictionloss[i]>0) { + for (int i=0; i < ntendon; i++) { + if (m->tendon_frictionloss[i] > 0) { nf += mj_addConstraintCount(m, 1, d->ten_J_rownnz[i]); nnzf += d->ten_J_rownnz[i]; } @@ -1453,7 +1453,7 @@ static inline int mj_nl(const mjModel* m, const mjData* d, int *nnz) { } - for (int i=0; injnt; i++) { + for (int i=0; i < m->njnt; i++) { if (!m->jnt_limited[i]) { continue; } @@ -1461,37 +1461,37 @@ static inline int mj_nl(const mjModel* m, const mjData* d, int *nnz) { margin = m->jnt_margin[i]; // slider and hinge joint limits can be bilateral, check both side - if (m->jnt_type[i]==mjJNT_SLIDE || m->jnt_type[i]==mjJNT_HINGE) { + if (m->jnt_type[i] == mjJNT_SLIDE || m->jnt_type[i] == mjJNT_HINGE) { value = d->qpos[m->jnt_qposadr[i]]; - for (side=-1; side<=1; side+=2) { + for (side=-1; side <= 1; side+=2) { dist = side * (m->jnt_range[2*i+(side+1)/2] - value); - if (distjnt_type[i]==mjJNT_BALL) { + else if (m->jnt_type[i] == mjJNT_BALL) { mjtNum angleAxis[3]; mju_quat2Vel(angleAxis, d->qpos+m->jnt_qposadr[i], 1); value = mju_normalize3(angleAxis); dist = mju_max(m->jnt_range[2*i], m->jnt_range[2*i+1]) - value; - if (disttendon_limited[i]) { value = d->ten_length[i]; margin = m->tendon_margin[i]; // tendon limits can be bilateral, check both sides - for (side=-1; side<=1; side+=2) { + for (side=-1; side <= 1; side+=2) { dist = side * (m->tendon_range[2*i+(side+1)/2] - value); - if (distten_J_rownnz[i]); nnzl += d->ten_J_rownnz[i]; } @@ -1519,7 +1519,7 @@ static inline int mj_nc(const mjModel* m, mjData* d, int* nnz) { mjMARKSTACK; int *chain = (int*)mj_stackAlloc(d, m->nv); - for (int i=0; icontact[i].exclude) { continue; } @@ -1533,7 +1533,7 @@ static inline int mj_nc(const mjModel* m, mjData* d, int* nnz) { continue; } - if (dim==1) { + if (dim == 1) { nc++; nnzc += NV; } else if (ispyramid) { @@ -1590,7 +1590,7 @@ void mj_makeConstraint(const mjModel* m, mjData* d) { d->parena = d->ncon * sizeof(mjContact); #ifdef ADDRESS_SANITIZER ASAN_POISON_MEMORY_REGION( - (char*)d->arena + d->parena, (d->nstack - d->pstack) * sizeof(mjtNum) - d->parena); + (char*)d->arena + d->parena, (d->nstack - d->pstack) * sizeof(mjtNum) - d->parena); #endif #define X(type, name, nr, nc) \ @@ -1699,7 +1699,7 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { mjMARKSTACK; // nothing to do - if (nefc==0 || !mj_isDual(m)) { + if (nefc == 0 || !mj_isDual(m)) { return; } @@ -1719,10 +1719,10 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { int* colindT = mj_stackAllocInt(d, nv*nefc); // construct JM2 = backsubM2(J')' by rows - for (int r=0; r0 ? rowadr[r-1]+rownnz[r-1] : 0); + int adr = (r > 0 ? rowadr[r-1]+rownnz[r-1] : 0); int remain = d->efc_J_rownnz[r]; // complete chain in reverse @@ -1732,16 +1732,16 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { rowadr[r] = adr; // get previous dof in src and dst - int prev_src = (remain>0 ? d->efc_J_colind[d->efc_J_rowadr[r]+remain-1] : -1); - int prev_dst = (nnz>0 ? m->dof_parentid[colind[adr+nnz-1]] : -1); + int prev_src = (remain > 0 ? d->efc_J_colind[d->efc_J_rowadr[r]+remain-1] : -1); + int prev_dst = (nnz > 0 ? m->dof_parentid[colind[adr+nnz-1]] : -1); // both finished: break - if (prev_src<0 && prev_dst<0) { + if (prev_src < 0 && prev_dst < 0) { break; } // add src - else if (prev_src>=prev_dst) { + else if (prev_src >= prev_dst) { colind[adr+nnz] = prev_src; JM2[adr+nnz] = d->efc_J[d->efc_J_rowadr[r]+remain-1]; remain--; @@ -1757,7 +1757,7 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { } // reverse order of chain: make it increasing - for (int i=0; i=0; i--) { + for (int i=nnz-1; i >= 0; i--) { // save x(i) and i-pointer mjtNum xi = JM2[adr+i]; int pi = i; @@ -1781,9 +1781,9 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { // x(j) -= L(i,j) * x(i) int Madr_ij = m->dof_Madr[colind[adr+i]]+1; int j = m->dof_parentid[colind[adr+i]]; - while (j>=0) { + while (j >= 0) { // match dof id in sparse vector - while (colind[adr+pi]>j) { + while (colind[adr+pi] > j) { pi--; } @@ -1815,9 +1815,9 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { rownnz, rowadr, colind, rowsuper, d); // add R to diagonal of AR - for (int i=0; iefc_AR_rownnz[i]; j++) { - if (i==d->efc_AR_colind[d->efc_AR_rowadr[i]+j]) { + for (int i=0; i < nefc; i++) { + for (int j=0; j < d->efc_AR_rownnz[i]; j++) { + if (i == d->efc_AR_colind[d->efc_AR_rowadr[i]+j]) { d->efc_AR[d->efc_AR_rowadr[i]+j] += d->efc_R[i]; break; } @@ -1837,7 +1837,7 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { mju_sqrMatTD(d->efc_AR, JM2T, NULL, nv, nefc); // add R to diagonal of AR - for (int r=0; refc_AR[r*(nefc+1)] += d->efc_R[r]; } } @@ -1856,7 +1856,7 @@ void mj_referenceConstraint(const mjModel* m, mjData* d) { mj_mulJacVec(m, d, d->efc_vel, d->qvel); // compute aref = -B*vel - K*I*(pos-margin) - for (int i=0; iefc_aref[i] = -KBIP[4*i+1]*d->efc_vel[i] -KBIP[4*i]*KBIP[4*i+2]*(d->efc_pos[i]-d->efc_margin[i]); } @@ -1885,12 +1885,12 @@ void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar, } // compute unconstrained efc_force - for (int i=0; i=R[i]*floss[i]) { + else if (jar[i] >= R[i]*floss[i]) { if (cost) { s += -0.5*R[i]*floss[i]*floss[i] + floss[i]*jar[i]; } @@ -1933,11 +1933,11 @@ void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar, } // contact - for (int i=ne+nf; iefc_type[i]!=mjCNSTR_CONTACT_ELLIPTIC) { + if (d->efc_type[i] != mjCNSTR_CONTACT_ELLIPTIC) { // constraint is satisfied: no cost - if (jar[i]>=0) { + if (jar[i] >= 0) { force[i] = 0; d->efc_state[i] = mjCNSTRSTATE_SATISFIED; @@ -1963,7 +1963,7 @@ void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar, // map to regular dual cone space mjtNum U[6]; U[0] = jar[i]*mu; - for (int j=1; j=mu*T || (T<=0 && N>=0)) { + if (N >= mu*T || (T <= 0 && N >= 0)) { mju_zero(force+i, dim); d->efc_state[i] = mjCNSTRSTATE_SATISFIED; } // bottom zone - else if (mu*N+T<=0 || (T<=0 && N<0)) { + else if (mu*N+T <= 0 || (T <= 0 && N < 0)) { if (cost) { - for (int j=0; jefc_state[i+j] = d->efc_state[i]; } diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index cedb6419..2297d609 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -50,14 +50,14 @@ void mj_kinematics(const mjModel* m, mjData* d) { mj_normalizeQuat(m, d->qpos); // normalize mocap quaternions - for (int i=0; inmocap; i++) { + for (int i=0; i < m->nmocap; i++) { mju_normalize4(d->mocap_quat+4*i); } // compute global cartesian positions and orientations of all bodies - for (int i=1; inbody; i++) { + for (int i=1; i < m->nbody; i++) { // free joint - if (m->body_jntnum[i]==1 && m->jnt_type[m->body_jntadr[i]]==mjJNT_FREE) { + if (m->body_jntnum[i] == 1 && m->jnt_type[m->body_jntadr[i]] == mjJNT_FREE) { // get addresses int jid = m->body_jntadr[i]; int qadr = m->jnt_qposadr[jid]; @@ -81,7 +81,7 @@ void mj_kinematics(const mjModel* m, mjData* d) { int pid = m->body_parentid[i]; // get body pos and quat: from model or mocap - if (m->body_mocapid[i]>=0) { + if (m->body_mocapid[i] >= 0) { bodypos = d->mocap_pos + 3*m->body_mocapid[i]; bodyquat = d->mocap_quat + 4*m->body_mocapid[i]; } else { @@ -95,7 +95,7 @@ void mj_kinematics(const mjModel* m, mjData* d) { mju_mulQuat(quat, d->xquat+4*pid, bodyquat); // accumulate joints, compute pos and quat for this body - for (int j=0; jbody_jntnum[i]; j++) { + for (int j=0; j < m->body_jntnum[i]; j++) { // get joint id, qpos address, joint type int jid = m->body_jntadr[i] + j; int qadr = m->jnt_qposadr[jid]; @@ -117,7 +117,7 @@ void mj_kinematics(const mjModel* m, mjData* d) { case mjJNT_BALL: case mjJNT_HINGE: // compute local quaternion rotation (qloc) - if (jtype==mjJNT_BALL) { + if (jtype == mjJNT_BALL) { mju_copy4(qloc, d->qpos+qadr); } else { mju_axisAngle2Quat(qloc, m->jnt_axis+3*jid, d->qpos[qadr] - m->qpos0[qadr]); @@ -152,21 +152,21 @@ void mj_kinematics(const mjModel* m, mjData* d) { } // compute/copy Cartesian positions and orientations of body inertial frames - for (int i=1; inbody; i++) { + for (int i=1; i < m->nbody; i++) { mj_local2Global(d, d->xipos+3*i, d->ximat+9*i, m->body_ipos+3*i, m->body_iquat+4*i, i, m->body_sameframe[i]); } // compute/copy Cartesian positions and orientations of geoms - for (int i=0; ingeom; i++) { + for (int i=0; i < m->ngeom; i++) { mj_local2Global(d, d->geom_xpos+3*i, d->geom_xmat+9*i, m->geom_pos+3*i, m->geom_quat+4*i, m->geom_bodyid[i], m->geom_sameframe[i]); } // compute/copy Cartesian positions and orientations of sites - for (int i=0; insite; i++) { + for (int i=0; i < m->nsite; i++) { mj_local2Global(d, d->site_xpos+3*i, d->site_xmat+9*i, m->site_pos+3*i, m->site_quat+4*i, m->site_bodyid[i], m->site_sameframe[i]); @@ -186,7 +186,7 @@ void mj_comPos(const mjModel* m, mjData* d) { mju_zero(d->subtree_com, m->nbody*3); // backwards pass over bodies: compute subtree_com and mass_subtree - for (int i=m->nbody-1; i>=0; i--) { + for (int i=m->nbody-1; i >= 0; i--) { // add local info mju_addToScl3(d->subtree_com+3*i, d->xipos+3*i, m->body_mass[i]); mass_subtree[i] += m->body_mass[i]; @@ -199,7 +199,7 @@ void mj_comPos(const mjModel* m, mjData* d) { } // compute local com - if (mass_subtree[i]subtree_com+3*i, d->xipos+3*i); } else { mju_scl3(d->subtree_com+3*i, d->subtree_com+3*i, @@ -208,14 +208,14 @@ void mj_comPos(const mjModel* m, mjData* d) { } // map inertias to frame centered at subtree_com - for (int i=1; inbody; i++) { + for (int i=1; i < m->nbody; i++) { mju_sub3(offset, d->xipos+3*i, d->subtree_com+3*m->body_rootid[i]); mju_inertCom(d->cinert+10*i, m->body_inertia+3*i, d->ximat+9*i, offset, m->body_mass[i]); } // map motion dofs to global frame centered at subtree_com - for (int j=0; jnjnt; j++) { + for (int j=0; j < m->njnt; j++) { // get dof address, body index int da = 6*m->jnt_dofadr[j]; int bi = m->jnt_bodyid[j]; @@ -229,7 +229,7 @@ void mj_comPos(const mjModel* m, mjData* d) { case mjJNT_FREE: // translation components: x, y, z in global frame mju_zero(d->cdof+da, 18); - for (int i=0; i<3; i++) { + for (int i=0; i < 3; i++) { d->cdof[da+3+7*i] = 1; } @@ -238,7 +238,7 @@ void mj_comPos(const mjModel* m, mjData* d) { mjFALLTHROUGH; case mjJNT_BALL: - for (int i=0; i<3; i++) { + for (int i=0; i < 3; i++) { // I_3 rotation in child frame (assume no subsequent rotations) axis[0] = d->xmat[9*bi+i+0]; axis[1] = d->xmat[9*bi+i+3]; @@ -268,7 +268,7 @@ void mj_camlight(const mjModel* m, mjData* d) { mjtNum pos[3], matT[9]; // compute Cartesian positions and orientations of cameras - for (int i=0; incam; i++) { + for (int i=0; i < m->ncam; i++) { // default processing for fixed mode mj_local2Global(d, d->cam_xpos+3*i, d->cam_xmat+9*i, m->cam_pos+3*i, m->cam_quat+4*i, m->cam_bodyid[i], 0); @@ -285,7 +285,7 @@ void mj_camlight(const mjModel* m, mjData* d) { mju_copy(d->cam_xmat+9*i, m->cam_mat0+9*i, 9); // position: track camera body - if (m->cam_mode[i]==mjCAMLIGHT_TRACK) { + if (m->cam_mode[i] == mjCAMLIGHT_TRACK) { mju_add3(d->cam_xpos+3*i, d->xpos+3*id, m->cam_pos0+3*i); } @@ -298,9 +298,9 @@ void mj_camlight(const mjModel* m, mjData* d) { case mjCAMLIGHT_TARGETBODY: case mjCAMLIGHT_TARGETBODYCOM: // only if target body is specified - if (id1>=0) { + if (id1 >= 0) { // get position to look at - if (m->cam_mode[i]==mjCAMLIGHT_TARGETBODY) { + if (m->cam_mode[i] == mjCAMLIGHT_TARGETBODY) { mju_copy3(pos, d->xpos+3*id1); } else { mju_copy3(pos, d->subtree_com+3*id1); @@ -328,7 +328,7 @@ void mj_camlight(const mjModel* m, mjData* d) { } // compute Cartesian positions and directions of lights - for (int i=0; inlight; i++) { + for (int i=0; i < m->nlight; i++) { // default processing for fixed mode mj_local2Global(d, d->light_xpos+3*i, 0, m->light_pos+3*i, 0, m->light_bodyid[i], 0); mju_rotVecQuat(d->light_xdir+3*i, m->light_dir+3*i, d->xquat+4*m->light_bodyid[i]); @@ -345,7 +345,7 @@ void mj_camlight(const mjModel* m, mjData* d) { mju_copy3(d->light_xdir+3*i, m->light_dir0+3*i); // position: track light body - if (m->light_mode[i]==mjCAMLIGHT_TRACK) { + if (m->light_mode[i] == mjCAMLIGHT_TRACK) { mju_add3(d->light_xpos+3*i, d->xpos+3*id, m->light_pos0+3*i); } @@ -358,9 +358,9 @@ void mj_camlight(const mjModel* m, mjData* d) { case mjCAMLIGHT_TARGETBODY: case mjCAMLIGHT_TARGETBODYCOM: // only if target body is specified - if (id1>=0) { + if (id1 >= 0) { // get position to look at - if (m->light_mode[i]==mjCAMLIGHT_TARGETBODY) { + if (m->light_mode[i] == mjCAMLIGHT_TARGETBODY) { mju_copy3(pos, d->xpos+3*id1); } else { mju_copy3(pos, d->subtree_com+3*id1); @@ -416,7 +416,7 @@ void mj_tendon(const mjModel* m, mjData* d) { } // loop over tendons - for (int i=0; itendon_adr[i]; d->ten_wrapadr[i] = wcnt; @@ -424,13 +424,13 @@ void mj_tendon(const mjModel* m, mjData* d) { // sparse Jacobian row init if (issparse) { - rowadr[i] = (i>0 ? rowadr[i-1] + rownnz[i-1] : 0); + rowadr[i] = (i > 0 ? rowadr[i-1] + rownnz[i-1] : 0); } // process joint tendon - if (m->wrap_type[adr]==mjWRAP_JOINT) { + if (m->wrap_type[adr] == mjWRAP_JOINT) { // process all defined joints - for (int j=0; jtendon_num[i]; j++) { + for (int j=0; j < m->tendon_num[i]; j++) { // get joint id int k = m->wrap_objid[adr+j]; @@ -455,11 +455,11 @@ void mj_tendon(const mjModel* m, mjData* d) { int x, *list = colind+rowadr[i]; mjtNum y, *listy = J+rowadr[i]; - for (int k=1; k=0 && list[j]>x) { + while (j >= 0 && list[j] > x) { list[j+1] = list[j]; listy[j+1] = listy[j]; j--; @@ -475,7 +475,7 @@ void mj_tendon(const mjModel* m, mjData* d) { // process spatial tendon divisor = 1; int j = 0; - while (jtendon_num[i]-1) { + while (j < m->tendon_num[i]-1) { // get 1st and 2nd object tp0 = m->wrap_type[adr+j]; id0 = m->wrap_objid[adr+j]; @@ -483,9 +483,9 @@ void mj_tendon(const mjModel* m, mjData* d) { id1 = m->wrap_objid[adr+j+1]; // pulley - if (tp0==mjWRAP_PULLEY || tp1==mjWRAP_PULLEY) { + if (tp0 == mjWRAP_PULLEY || tp1 == mjWRAP_PULLEY) { // get divisor, insert obj=-2 - if (tp0==mjWRAP_PULLEY) { + if (tp0 == mjWRAP_PULLEY) { divisor = m->wrap_prm[adr+j]; mju_zero3(d->wrap_xpos+wcnt*3); d->wrap_obj[wcnt] = -2; @@ -504,7 +504,7 @@ void mj_tendon(const mjModel* m, mjData* d) { wbody[0] = m->site_bodyid[id0]; // second object is geom: process site-geom-site - if (tp1==mjWRAP_SPHERE || tp1==mjWRAP_CYLINDER) { + if (tp1 == mjWRAP_SPHERE || tp1 == mjWRAP_CYLINDER) { // reassign, get 2nd site info tpw = tp1; idw = id1; @@ -513,19 +513,19 @@ void mj_tendon(const mjModel* m, mjData* d) { // do wrapping, possibly get 2 extra points (wlen>=0) sideid = mju_round(m->wrap_prm[adr+j+1]); - if (sideid<-1 || sideid>=m->nsite) { + if (sideid < -1 || sideid >= m->nsite) { mju_error("Invalid sideid %d in wrap_prm", sideid); // SHOULD NOT OCCUR } wlen = mju_wrap(wpnt+3, d->site_xpos+3*id0, d->site_xpos+3*id1, d->geom_xpos+3*idw, d->geom_xmat+9*idw, m->geom_size+3*idw, tpw, - (sideid>=0 ? d->site_xpos+3*sideid : 0)); + (sideid >= 0 ? d->site_xpos+3*sideid : 0)); } else { tpw = mjWRAP_NONE; } // complete sequence, accumulate lengths - if (wlen<0) { + if (wlen < 0) { mju_copy3(wpnt+3, d->site_xpos+3*id1); wbody[1] = m->site_bodyid[id1]; L[i] += mju_dist3(wpnt, wpnt+3)/divisor; @@ -537,8 +537,8 @@ void mj_tendon(const mjModel* m, mjData* d) { } // accumulate moments if consequtive points are in different bodies - for (int k=0; k<(wlen<0 ? 1 : 3); k++) { - if (wbody[k]!=wbody[k+1]) { + for (int k=0; k < (wlen < 0 ? 1 : 3); k++) { + if (wbody[k] != wbody[k+1]) { // get 3D position difference, normalize mju_sub3(dif, wpnt+3*k+3, wpnt+3*k); mju_normalize3(dif); @@ -581,19 +581,19 @@ void mj_tendon(const mjModel* m, mjData* d) { } // assign to wrap - mju_copy(d->wrap_xpos+wcnt*3, wpnt, (wlen<0 ? 3:9)); + mju_copy(d->wrap_xpos+wcnt*3, wpnt, (wlen < 0 ? 3:9)); d->wrap_obj[wcnt] = -1; - if (wlen>=0) { + if (wlen >= 0) { d->wrap_obj[wcnt+1] = d->wrap_obj[wcnt+2] = idw; } - d->ten_wrapnum[i] += (wlen<0 ? 1:3); - wcnt += (wlen<0 ? 1:3); + d->ten_wrapnum[i] += (wlen < 0 ? 1:3); + wcnt += (wlen < 0 ? 1:3); // advance - j += (tpw!=mjWRAP_NONE ? 2 : 1); + j += (tpw != mjWRAP_NONE ? 2 : 1); // assign last site before pulley or tendon end - if (j==m->tendon_num[i]-1 || m->wrap_type[adr+j+1]==mjWRAP_PULLEY) { + if (j == m->tendon_num[i]-1 || m->wrap_type[adr+j+1] == mjWRAP_PULLEY) { mju_copy3(d->wrap_xpos+wcnt*3, d->site_xpos+3*id1); d->wrap_obj[wcnt] = -1; d->ten_wrapnum[i]++; @@ -634,7 +634,7 @@ void mj_transmission(const mjModel* m, mjData* d) { int *chain; // compute lengths and moments - for (int i=0; iactuator_trnid[2*i]; idslider = m->actuator_trnid[2*i+1]; // for slider-crank only @@ -645,13 +645,13 @@ void mj_transmission(const mjModel* m, mjData* d) { case mjTRN_JOINT: // joint case mjTRN_JOINTINPARENT: // joint, force in parent frame // slide and hinge joint: scalar gear - if (m->jnt_type[id]==mjJNT_SLIDE || m->jnt_type[id]==mjJNT_HINGE) { + if (m->jnt_type[id] == mjJNT_SLIDE || m->jnt_type[id] == mjJNT_HINGE) { length[i] = d->qpos[m->jnt_qposadr[id]]*gear[0]; moment[i*nv + m->jnt_dofadr[id]] = gear[0]; } // ball joint: 3D wrench gear - else if (m->jnt_type[id]==mjJNT_BALL) { + else if (m->jnt_type[id] == mjJNT_BALL) { // j: qpos start address int j = m->jnt_qposadr[id]; @@ -659,7 +659,7 @@ void mj_transmission(const mjModel* m, mjData* d) { mju_quat2Vel(axis, d->qpos+j, 1); // gearAxis: rotate to parent frame if necessary - if (m->actuator_trntype[i]==mjTRN_JOINT) { + if (m->actuator_trntype[i] == mjTRN_JOINT) { mju_copy3(gearAxis, gear); } else { mju_negQuat(quat, d->qpos+j); @@ -691,7 +691,7 @@ void mj_transmission(const mjModel* m, mjData* d) { mju_quat2Vel(axis, d->qpos+j+3, 1); // gearAxis: rotate to world frame if necessary - if (m->actuator_trntype[i]==mjTRN_JOINT) { + if (m->actuator_trntype[i] == mjTRN_JOINT) { mju_copy3(gearAxis, gear+3); } else { mju_negQuat(quat, d->qpos+j+3); @@ -720,7 +720,7 @@ void mj_transmission(const mjModel* m, mjData* d) { av = mju_dot3(vec, axis); det = av*av + rod*rod - mju_dot3(vec, vec); ok = 1; - if (det<=0) { + if (det <= 0) { ok = 0; sdet = 0; length[i] = av; @@ -748,15 +748,15 @@ void mj_transmission(const mjModel* m, mjData* d) { mju_subFrom(jac, jacS, 3*nv); // apply chain rule - for (int j=0; jten_J_rowadr[id] + d->ten_J_rownnz[id]; - for (int j=d->ten_J_rowadr[id]; jten_J_rowadr[id]; j < end; j++) { moment[i*nv + d->ten_J_colind[j]] = d->ten_J[j] * gear[0]; } } else { @@ -878,7 +878,7 @@ void mj_transmission(const mjModel* m, mjData* d) { // count all relevant contacts, accumulate Jacobians int counter = 0; - for (int j=0; jncon; j++) { + for (int j=0; j < d->ncon; j++) { const mjContact* con = d->contact+j; int b1 = m->geom_bodyid[con->geom1]; int b2 = m->geom_bodyid[con->geom2]; @@ -893,14 +893,14 @@ void mj_transmission(const mjModel* m, mjData* d) { counter++; // condim 1 or elliptic cones: normal is in the first row - if (con->dim == 1 || m->opt.cone==mjCONE_ELLIPTIC) { + if (con->dim == 1 || m->opt.cone == mjCONE_ELLIPTIC) { efc_force[con->efc_address] = 1; } // pyramidal cones: average all pyramid directions else { int npyramid = con->dim-1; // number of frictional directions - for (int k=0; k<2*npyramid; k++) { + for (int k=0; k < 2*npyramid; k++) { efc_force[con->efc_address+k] = 0.5/npyramid; } } @@ -919,7 +919,7 @@ void mj_transmission(const mjModel* m, mjData* d) { // accumulate in moment_exclude if (issparse) { - for (int k=0; kcinert, 10*m->nbody); // backward pass over bodies, accumulate composite inertias - for (int i=m->nbody-1; i>0; i--) { - if (m->body_parentid[i]>0) { + for (int i=m->nbody-1; i > 0; i--) { + if (m->body_parentid[i] > 0) { mju_addTo(crb+10*m->body_parentid[i], crb+10*i, 10); } } @@ -973,7 +973,7 @@ void mj_crbSkip(const mjModel* m, mjData* d, int skipsimple) { mju_zero(d->qM, m->nM); // dense backward pass over dofs - for (int i=m->nv-1; i>=0; i--) { + for (int i=m->nv-1; i >= 0; i--) { // copy if (skipsimple && m->dof_simplenum[i]) { d->qM[m->dof_Madr[i]] = m->dof_M0[i]; @@ -990,7 +990,7 @@ void mj_crbSkip(const mjModel* m, mjData* d, int skipsimple) { // sparse backward pass over ancestors int j = i; - while (j>=0) { + while (j >= 0) { // M(i,j) += cdof_j * crb_body(i) * cdof_i = cdof_j * tmp d->qM[Madr_ij] += mju_dot(d->cdof+6*j, tmp, 6); @@ -1027,12 +1027,12 @@ void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNu mju_copy(qLD, M, m->nM); // dense backward loop over dofs (regular only, simple diagonal already copied) - for (int k=nv-1; k>=0; k--) { + for (int k=nv-1; k >= 0; k--) { // get address of M(k,k) Madr_kk = dof_Madr[k]; // check for small/negative numbers on diagonal - if (qLD[Madr_kk]=0) { + while (i >= 0) { tmp = qLD[Madr_ki] / qLD[Madr_kk]; // tmp = M(k,i) / M(k,k) // get number of ancestors of i (including i) - if (inM - dof_Madr[i+1]; @@ -1067,7 +1067,7 @@ void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNu } // compute 1/diag(D), 1/sqrt(diag(D)) - for (int i=0; inv; // single vector - if (n==1) { + if (n == 1) { // x <- inv(L') * x; skip simple, exploit sparsity of input vector - for (int i=nv-1; i>=0; i--) { + for (int i=nv-1; i >= 0; i--) { if (!m->dof_simplenum[i] && x[i]) { // init int Madr_ij = dof_Madr[i]+1; @@ -1106,7 +1106,7 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n, // traverse ancestors backwards // read directly from x[i] since i cannot be a parent of itself - while (j>=0) { + while (j >= 0) { x[j] -= qLD[Madr_ij++]*x[i]; // x(j) -= L(i,j) * x(i) // advance to parent @@ -1116,12 +1116,12 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n, } // x <- inv(D) * x - for (int i=0; idof_simplenum[i]) { // init int Madr_ij = dof_Madr[i]+1; @@ -1129,7 +1129,7 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n, // traverse ancestors backwards // write directly in x[i] since i cannot be a parent of itself - while (j>=0) { + while (j >= 0) { x[i] -= qLD[Madr_ij++]*x[j]; // x(i) -= L(i,j) * x(j) // advance to parent @@ -1145,16 +1145,16 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n, mjtNum tmp; // x <- inv(L') * x; skip simple - for (int i=nv-1; i>=0; i--) { + for (int i=nv-1; i >= 0; i--) { if (!m->dof_simplenum[i]) { // init int Madr_ij = dof_Madr[i]+1; int j = dof_parentid[i]; // traverse ancestors backwards - while (j>=0) { + while (j >= 0) { // process all vectors, exploit sparsity - for (offset=0; offsetdof_simplenum[i]) { // init int Madr_ij = dof_Madr[i]+1; @@ -1182,9 +1182,9 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n, // traverse ancestors backwards tmp = x[i+offset]; - while (j>=0) { + while (j >= 0) { // process all vectors - for (offset=0; offset=0; i--) { + for (int i=nv-1; i >= 0; i--) { mjtNum tmp; if (!m->dof_simplenum[i] && (tmp = x[i+offset])) { // init @@ -1234,7 +1234,7 @@ void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n) int j = dof_parentid[i]; // traverse ancestors backwards - while (j>=0) { + while (j >= 0) { x[j+offset] -= qLD[Madr_ij++] * tmp; // x(j) -= L(i,j) * x(i) // advance to parent @@ -1244,7 +1244,7 @@ void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n) } // x <- sqrt(inv(D)) * x - for (int i=0; icvel, 6); // forward pass over bodies - for (int i=1; inbody; i++) { + for (int i=1; i < m->nbody; i++) { // get body's first dof address int bda = m->body_dofadr[i]; @@ -1270,7 +1270,7 @@ void mj_comVel(const mjModel* m, mjData* d) { mju_copy(cvel, d->cvel+6*m->body_parentid[i], 6); // cvel = cvel_parent + cdof * qvel, cdofdot = cvel x cdof - for (int j=0; jbody_dofnum[i]; j++) { + for (int j=0; j < m->body_dofnum[i]; j++) { // compute cvel and cdofdot switch (m->jnt_type[m->dof_jntid[bda+j]]) { case mjJNT_FREE: @@ -1287,7 +1287,7 @@ void mj_comVel(const mjModel* m, mjData* d) { case mjJNT_BALL: // compute all 3 cdofdots using parent velocity - for (int k=0; k<3; k++) { + for (int k=0; k < 3; k++) { mju_crossMotion(cdofdot+6*(j+k), cvel, d->cdof+6*(bda+j+k)); } @@ -1326,7 +1326,7 @@ void mj_subtreeVel(const mjModel* m, mjData* d) { mjtNum* body_vel = mj_stackAlloc(d, 6*m->nbody); // bodywise quantities - for (int i=0; inbody; i++) { + for (int i=0; i < m->nbody; i++) { // compute and save body velocity mj_objectVelocity(m, d, mjOBJ_BODY, i, body_vel+6*i, 0); @@ -1342,7 +1342,7 @@ void mj_subtreeVel(const mjModel* m, mjData* d) { } // subtree linvel - for (int i=m->nbody-1; i>=0; i--) { + for (int i=m->nbody-1; i >= 0; i--) { // non-world: add linear momentum to parent if (i) { mju_addTo3(d->subtree_linvel+3*m->body_parentid[i], d->subtree_linvel+3*i); @@ -1354,7 +1354,7 @@ void mj_subtreeVel(const mjModel* m, mjData* d) { } // subtree angmom - for (int i=m->nbody-1; i>0; i--) { + for (int i=m->nbody-1; i > 0; i--) { int parent = m->body_parentid[i]; // momentum wrt body i @@ -1399,7 +1399,7 @@ void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) { } // forward pass over bodies: accumulate cacc, set cfrc_body - for (int i=1; inbody; i++) { + for (int i=1; i < m->nbody; i++) { // get body's first dof address int bda = m->body_dofadr[i]; @@ -1424,13 +1424,13 @@ void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) { mju_zero(loc_cfrc_body, 6); // backward pass over bodies: accumulate cfrc_body from children - for (int i=m->nbody-1; i>0; i--) + for (int i=m->nbody-1; i > 0; i--) if (m->body_parentid[i]) { mju_addTo(loc_cfrc_body+6*m->body_parentid[i], loc_cfrc_body+6*i, 6); } // result = cdof * cfrc_body - for (int i=0; inv; i++) { + for (int i=0; i < m->nv; i++) { result[i] = mju_dot(d->cdof+6*i, loc_cfrc_body+6*m->dof_bodyid[i], 6); } @@ -1453,7 +1453,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { // cfrc_ext = perturb mju_zero(d->cfrc_ext, 6*nbody); - for (int i=1; ixfrc_applied+6*i, 6)) { // rearrange as torque:force mju_copy3(cfrc, d->xfrc_applied+6*i+3); @@ -1467,8 +1467,8 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { } // cfrc_ext += contacts - for (int i=0; incon; i++) - if (d->contact[i].efc_address>=0) { + for (int i=0; i < d->ncon; i++) + if (d->contact[i].efc_address >= 0) { // get contact pointer con = d->contact+i; @@ -1502,7 +1502,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { // cfrc_ext += connect and weld constraints int i = 0; while (i < d->ne) { - if (d->efc_type[i]!=mjCNSTR_EQUALITY) + if (d->efc_type[i] != mjCNSTR_EQUALITY) mju_error("Row %d of efc is not an equality constraint", i); // SHOULD NOT OCCUR int id = d->efc_id[i]; @@ -1514,7 +1514,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { case mjEQ_WELD: // cfrc = world-oriented torque:force vector mju_copy3(cfrc + 3, d->efc_force + i); - if (m->eq_type[id]==mjEQ_WELD) { + if (m->eq_type[id] == mjEQ_WELD) { mju_copy3(cfrc, d->efc_force + i + 3); } else { mju_zero3(cfrc); // no torque from connect @@ -1523,7 +1523,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { // body 1 if ((k = m->eq_obj1id[id])) { // transform point on body1: local -> global - mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id]==mjEQ_WELD), 0, k, 0); + mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id] == mjEQ_WELD), 0, k, 0); // tmp = subtree CoM-based torque_force vector mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], pos, 0); @@ -1535,7 +1535,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { // body 2 if ((k = m->eq_obj2id[id])) { // transform point on body2: local -> global - mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id]==mjEQ_CONNECT), 0, k, 0); + mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id] == mjEQ_CONNECT), 0, k, 0); // tmp = subtree CoM-based torque_force vector mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], pos, 0); @@ -1545,7 +1545,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { } // increment rows - i += m->eq_type[id]==mjEQ_WELD ? 6 : 3; + i += m->eq_type[id] == mjEQ_WELD ? 6 : 3; break; case mjEQ_JOINT: @@ -1562,7 +1562,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { // forward pass over bodies: compute cacc, cfrc_int mjtNum cacc[6], cfrc_body[6], cfrc_corr[6]; mju_zero(d->cfrc_int, 6); - for (int j=1; jnbody; j++) { + for (int j=1; j < m->nbody; j++) { // get body's first dof address int bda = m->body_dofadr[j]; @@ -1583,7 +1583,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { } // backward pass over bodies: accumulate cfrc_int from children - for (int j=m->nbody-1; j>0; j--) { + for (int j=m->nbody-1; j > 0; j--) { mju_addTo(d->cfrc_int+6*m->body_parentid[j], d->cfrc_int+6*j, 6); } } diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 86633b55..da1771c9 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -280,19 +280,19 @@ static void mjd_comVel_vel_dense(const mjModel* m, mjData* d, mjtNum* Dcvel, mjt mju_zero(Dcvel, nbody*6*nv); // forward pass over bodies: accumulate Dcvel, set Dcdofdot - for (int i=1; inbody; i++) { + for (int i=1; i < m->nbody; i++) { // Dcvel = Dcvel_parent mju_copy(Dcvel+i*6*nv, Dcvel+m->body_parentid[i]*6*nv, 6*nv); // Dcvel += D(cdof * qvel), Dcdofdot = D(cvel x cdof) - for (int j=m->body_dofadr[i]; jbody_dofadr[i]+m->body_dofnum[i]; j++) { + for (int j=m->body_dofadr[i]; j < m->body_dofadr[i]+m->body_dofnum[i]; j++) { switch (m->jnt_type[m->dof_jntid[j]]) { case mjJNT_FREE: // Dcdofdot = 0 mju_zero(Dcdofdot+j*6*nv, 18*nv); // Dcvel += cdof * (D qvel) - for (int k=0; k<6; k++) { + for (int k=0; k < 6; k++) { Dcvel[i*6*nv + k*nv + j+0] += d->cdof[(j+0)*6 + k]; Dcvel[i*6*nv + k*nv + j+1] += d->cdof[(j+1)*6 + k]; Dcvel[i*6*nv + k*nv + j+2] += d->cdof[(j+2)*6 + k]; @@ -304,13 +304,13 @@ static void mjd_comVel_vel_dense(const mjModel* m, mjData* d, mjtNum* Dcvel, mjt case mjJNT_BALL: // Dcdofdot = D crossMotion(cvel, cdof) - for (int k=0; k<3; k++) { + for (int k=0; k < 3; k++) { mjd_crossMotion_vel(mat, d->cdof+6*(j+k)); mju_mulMatMat(Dcdofdot+(j+k)*6*nv, mat, Dcvel+i*6*nv, 6, 6, nv); } // Dcvel += cdof * (D qvel) - for (int k=0; k<6; k++) { + for (int k=0; k < 6; k++) { Dcvel[i*6*nv + k*nv + j+0] += d->cdof[(j+0)*6 + k]; Dcvel[i*6*nv + k*nv + j+1] += d->cdof[(j+1)*6 + k]; Dcvel[i*6*nv + k*nv + j+2] += d->cdof[(j+2)*6 + k]; @@ -326,7 +326,7 @@ static void mjd_comVel_vel_dense(const mjModel* m, mjData* d, mjtNum* Dcvel, mjt mju_mulMatMat(Dcdofdot+j*6*nv, mat, Dcvel+i*6*nv, 6, 6, nv); // Dcvel += cdof * (D qvel) - for (int k=0; k<6; k++) { + for (int k=0; k < 6; k++) { Dcvel[i*6*nv + k*nv + j] += d->cdof[j*6 + k]; } } @@ -355,14 +355,14 @@ void mjd_rne_vel_dense(const mjModel* m, mjData* d) { mju_zero(Dcacc, nbody*6*nv); // forward pass over bodies: accumulate Dcacc, set Dcfrcbody - for (int i=1; ibody_parentid[i]*6*nv, 6*nv); // Dcacc += D(cdofdot * qvel) - for (int j=m->body_dofadr[i]; jbody_dofadr[i]+m->body_dofnum[i]; j++) { + for (int j=m->body_dofadr[i]; j < m->body_dofadr[i]+m->body_dofnum[i]; j++) { // Dcacc += cdofdot * (D qvel) - for (int k=0; k<6; k++) { + for (int k=0; k < 6; k++) { Dcacc[i*6*nv + k*nv + j] += d->cdof_dot[j*6 + k]; } @@ -392,21 +392,21 @@ void mjd_rne_vel_dense(const mjModel* m, mjData* d) { mju_zero(Dcfrcbody, 6*nv); // backward pass over bodies: accumulate Dcfrcbody - for (int i=m->nbody-1; i>0; i--) { + for (int i=m->nbody-1; i > 0; i--) { if (m->body_parentid[i]) { mju_addTo(Dcfrcbody+m->body_parentid[i]*6*nv, Dcfrcbody+i*6*nv, 6*nv); } } // qDeriv -= D(cdof * cfrc_body) - for (int i=0; idof_bodyid[i]*6+k)*nv, d->cdof[i*6+k], nv); // dense to sparse: qDeriv -= row int end = d->D_rowadr[i] + d->D_rownnz[i]; - for (int adr=d->D_rowadr[i]; adrD_rowadr[i]; adr < end; adr++) { d->qDeriv[adr] -= row[d->D_colind[adr]]; } } @@ -430,7 +430,7 @@ static void copyFromParent(const mjModel* m, mjData* d, mjtNum* mat, int n) { // count dofs in ancestors int ndof = 0; int np = m->body_weldid[m->body_parentid[n]]; - while (np>0) { + while (np > 0) { // add self dofs ndof += m->body_dofnum[np]; @@ -454,7 +454,7 @@ static void addToParent(const mjModel* m, mjData* d, mjtNum* mat, int n) { // find matching nonzeros int np = m->body_parentid[n]; int i = 0, ip = 0; - while (iB_rownnz[n] && ipB_rownnz[np]) { + while (i < d->B_rownnz[n] && ip < d->B_rownnz[np]) { // columns match if (d->B_colind[d->B_rowadr[n] + i] == d->B_colind[d->B_rowadr[np] + ip]) { mju_addTo(mat + 6*(d->B_rowadr[np] + ip), mat + 6*(d->B_rowadr[n] + i), 6); @@ -485,15 +485,15 @@ static void mjd_comVel_vel(const mjModel* m, mjData* d, mjtNum* Dcvel, mjtNum* D mjtNum mat[36], matT[36]; // 6x6 matrices // forward pass over bodies: accumulate Dcvel, set Dcdofdot - for (int i = 1; ibody_dofadr[i] + m->body_dofnum[i]; - for (int j = m->body_dofadr[i]; jbody_dofadr[i]; j < doflast; j++) { // number of dof ancestors of dof j - int Jadr = (jdof_Madr[j + 1] : m->nM) - (m->dof_Madr[j] + 1); + int Jadr = (j < nv - 1 ? m->dof_Madr[j + 1] : m->nM) - (m->dof_Madr[j] + 1); // Dcvel += D(cdof * qvel), Dcdofdot = D(cvel x cdof) switch (m->jnt_type[m->dof_jntid[j]]) { @@ -512,7 +512,7 @@ static void mjd_comVel_vel(const mjModel* m, mjData* d, mjtNum* Dcvel, mjtNum* D case mjJNT_BALL: // Dcdofdot = Dcvel * D crossMotion(cvel, cdof) - for (int dj=0; dj<3; dj++) { + for (int dj=0; dj < 3; dj++) { mjd_crossMotion_vel(mat, d->cdof + 6 * (j + dj)); mju_transpose(matT, mat, 6, 6); mju_mulMatMat(Dcdofdot + 6*Dadr[j + dj], Dcvel + 6*Badr[i], matT, Jadr + dj, 6, 6); @@ -573,13 +573,13 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) { mjd_comVel_vel(m, d, Dcvel, Dcdofdot); // forward pass over bodies: accumulate Dcacc, set Dcfrcbody - for (int i=1; ibody_dofadr[i] + m->body_dofnum[i]; - for (int j=m->body_dofadr[i]; jbody_dofadr[i]; j < doflast; j++) { // number of dof ancestors of dof j int Jadr = (j < nv - 1 ? m->dof_Madr[j + 1] : m->nM) - (m->dof_Madr[j] + 1); @@ -615,12 +615,12 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) { mju_zero(Dcfrcbody, 6*Bnnz[0]); // backward pass over bodies: accumulate Dcfrcbody - for (int i=m->nbody-1; i>0; i--) { + for (int i=m->nbody-1; i > 0; i--) { addToParent(m, d, Dcfrcbody, i); } // process all dofs, update qDeriv - for (int j=0; jdof_bodyid[j]; @@ -639,19 +639,19 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) { // construct sparse Jacobian structure of body; return nnz static int bodyJacSparse(const mjModel* m, int body, int* ind) { // skip fixed bodies - while (body>0 && m->body_dofnum[body]==0) { + while (body > 0 && m->body_dofnum[body] == 0) { body = m->body_parentid[body]; } // body is not movable: empty chain - if (body==0) { + if (body == 0) { return 0; } // count dofs int nnz = 0; int dof = m->body_dofadr[body] + m->body_dofnum[body] - 1; - while (dof>=0) { + while (dof >= 0) { nnz++; dof = m->dof_parentid[dof]; } @@ -659,7 +659,7 @@ static int bodyJacSparse(const mjModel* m, int body, int* ind) { // fill array in reverse (increasing dof) int cnt = 0; dof = m->body_dofadr[body] + m->body_dofnum[body] - 1; - while (dof>=0) { + while (dof >= 0) { ind[nnz-cnt-1] = dof; cnt++; dof = m->dof_parentid[dof]; @@ -680,20 +680,20 @@ static void addJTBJ(const mjModel* m, mjData* d, const mjtNum* J, const mjtNum* mjtNum* row = mj_stackAlloc(d, nv); // process non-zero elements of B - for (int i=0; iD_rownnz[k]; - for (int s=0; sD_rowadr[k] + s; d->qDeriv[adr] += row[d->D_colind[adr]]; } @@ -709,9 +709,9 @@ static void addJTBJ(const mjModel* m, mjData* d, const mjtNum* J, const mjtNum* // add J'*B*J to qDeriv, sparse version static void addJTBJSparse( - const mjModel* m, mjData* d, const mjtNum* J, - const mjtNum* B, int n, int offset, - const int* J_rownnz, const int* J_rowadr, const int* J_colind) { + const mjModel* m, mjData* d, const mjtNum* J, + const mjtNum* B, int n, int offset, + const int* J_rownnz, const int* J_rowadr, const int* J_colind) { int nv = m->nv; // allocate row @@ -770,7 +770,7 @@ static mjtNum mjd_muscleGain_vel(mjtNum len, mjtNum vel, const mjtNum lengthrang mjtNum fvmax = prm[8]; // scale force if negative - if (force<0) { + if (force < 0) { force = scale / mjMAX(mjMINVAL, acc0); } @@ -788,16 +788,16 @@ static mjtNum mjd_muscleGain_vel(mjtNum len, mjtNum vel, const mjtNum lengthrang // length curve mjtNum FL = 0; - if (L>=lmin && L<=a) { + if (L >= lmin && L <= a) { x = (L-lmin) / mjMAX(mjMINVAL, a-lmin); FL = 0.5*x*x; - } else if (L<=1) { + } else if (L <= 1) { x = (1-L) / mjMAX(mjMINVAL, 1-a); FL = 1 - 0.5*x*x; - } else if (L<=b) { + } else if (L <= b) { x = (L-1) / mjMAX(mjMINVAL, b-1); FL = 1 - 0.5*x*x; - } else if (L<=lmax) { + } else if (L <= lmax) { x = (lmax-L) / mjMAX(mjMINVAL, lmax-b); FL = 0.5*x*x; } @@ -805,13 +805,13 @@ static mjtNum mjd_muscleGain_vel(mjtNum len, mjtNum vel, const mjtNum lengthrang // velocity curve mjtNum dFV; mjtNum y = fvmax-1; - if (V<=-1) { + if (V <= -1) { // FV = 0 dFV = 0; - } else if (V<=0) { + } else if (V <= 0) { // FV = (V+1)*(V+1) dFV = 2*V + 2; - } else if (V<=y) { + } else if (V <= y) { // FV = fvmax - (y-V)*(y-V) / mjMAX(mjMINVAL, y) dFV = (-2*V + 2*y) / mjMAX(mjMINVAL, y); } else { @@ -835,23 +835,23 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) { } // process actuators - for (int i=0; inu; i++) { + for (int i=0; i < m->nu; i++) { mjtNum bias_vel = 0, gain_vel = 0; // affine bias - if (m->actuator_biastype[i]==mjBIAS_AFFINE) { + if (m->actuator_biastype[i] == mjBIAS_AFFINE) { // extract bias info: prm = [const, kp, kv] bias_vel = (m->actuator_biasprm + mjNBIAS*i)[2]; } // affine gain - if (m->actuator_gaintype[i]==mjGAIN_AFFINE) { + if (m->actuator_gaintype[i] == mjGAIN_AFFINE) { // extract bias info: prm = [const, kp, kv] gain_vel = (m->actuator_gainprm + mjNGAIN*i)[2]; } // muscle gain - else if (m->actuator_gaintype[i]==mjGAIN_MUSCLE) { + else if (m->actuator_gaintype[i] == mjGAIN_MUSCLE) { gain_vel = mjd_muscleGain_vel(d->actuator_length[i], d->actuator_velocity[i], m->actuator_lengthrange+2*i, @@ -860,8 +860,8 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) { } // force = gain .* [ctrl/act] - if (gain_vel!=0) { - if (m->actuator_dyntype[i]==mjDYN_NONE) { + if (gain_vel != 0) { + if (m->actuator_dyntype[i] == mjDYN_NONE) { bias_vel += gain_vel * d->ctrl[i]; } else { bias_vel += gain_vel * d->act[i-(m->nu - m->na)]; @@ -869,7 +869,7 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) { } // add - if (bias_vel!=0) { + if (bias_vel != 0) { addJTBJ(m, d, d->actuator_moment+i*nv, &bias_vel, 1); } } @@ -915,8 +915,8 @@ static void addToQuadrant(mjtNum* restrict B, const mjtNum D[9], int col_quad, i // forces due to fluid mass moving with the body, B is 6x6 static void mjd_addedMassForces( - mjtNum* restrict B, const mjtNum local_vels[6], const mjtNum fluid_density, - const mjtNum virtual_mass[3], const mjtNum virtual_inertia[3]) { + mjtNum* restrict B, const mjtNum local_vels[6], const mjtNum fluid_density, + const mjtNum virtual_mass[3], const mjtNum virtual_inertia[3]) { const mjtNum lin_vel[3] = {local_vels[3], local_vels[4], local_vels[5]}; const mjtNum ang_vel[3] = {local_vels[0], local_vels[1], local_vels[2]}; const mjtNum virtual_lin_mom[3] = { @@ -935,7 +935,7 @@ static void mjd_addedMassForces( // force[:3] += cross(virtual_ang_mom, ang_vel) mjd_cross(virtual_ang_mom, ang_vel, Da, Db); addToQuadrant(B, Db, 0, 0); - for (int i=0; i<9; ++i) { + for (int i=0; i < 9; ++i) { Da[i] *= fluid_density * virtual_inertia[i % 3]; } addToQuadrant(B, Da, 0, 0); @@ -943,7 +943,7 @@ static void mjd_addedMassForces( // force[:3] += cross(virtual_lin_mom, lin_vel) mjd_cross(virtual_lin_mom, lin_vel, Da, Db); addToQuadrant(B, Db, 0, 1); - for (int i=0; i<9; ++i) { + for (int i=0; i < 9; ++i) { Da[i] *= fluid_density * virtual_mass[i % 3]; } addToQuadrant(B, Da, 0, 1); @@ -951,7 +951,7 @@ static void mjd_addedMassForces( // force[3:] += cross(virtual_lin_mom, ang_vel) mjd_cross(virtual_lin_mom, ang_vel, Da, Db); addToQuadrant(B, Db, 1, 0); - for (int i=0; i<9; ++i) { + for (int i=0; i < 9; ++i) { Da[i] *= fluid_density * virtual_mass[i % 3]; } addToQuadrant(B, Da, 1, 1); @@ -961,9 +961,9 @@ static void mjd_addedMassForces( // torque due to motion in the fluid, D is 3x3 static inline void mjd_viscous_torque( - mjtNum* restrict D, const mjtNum lvel[6], const mjtNum fluid_density, - const mjtNum fluid_viscosity, const mjtNum size[3], - const mjtNum slender_drag_coef, const mjtNum ang_drag_coef) + mjtNum* restrict D, const mjtNum lvel[6], const mjtNum fluid_density, + const mjtNum fluid_viscosity, const mjtNum size[3], + const mjtNum slender_drag_coef, const mjtNum ang_drag_coef) { const mjtNum d_max = mju_max(mju_max(size[0], size[1]), size[2]); const mjtNum d_min = mju_min(mju_min(size[0], size[1]), size[2]); @@ -1016,9 +1016,9 @@ static inline void mjd_viscous_torque( // drag due to motion in the fluid, D is 3x3 static inline void mjd_viscous_drag( - mjtNum* restrict D, const mjtNum lvel[6], const mjtNum fluid_density, - const mjtNum fluid_viscosity, const mjtNum size[3], - const mjtNum blunt_drag_coef, const mjtNum slender_drag_coef) { + mjtNum* restrict D, const mjtNum lvel[6], const mjtNum fluid_density, + const mjtNum fluid_viscosity, const mjtNum size[3], + const mjtNum blunt_drag_coef, const mjtNum slender_drag_coef) { const mjtNum d_max = mju_max(mju_max(size[0], size[1]), size[2]); const mjtNum d_min = mju_min(mju_min(size[0], size[1]), size[2]); const mjtNum d_mid = size[0] + size[1] + size[2] - d_max - d_min; @@ -1046,7 +1046,7 @@ static inline void mjd_viscous_drag( const mjtNum lin_coef = fluid_viscosity * 3.0 * mjPI * eq_sphere_D; const mjtNum quad_coef = fluid_density * ( - A_proj*blunt_drag_coef + slender_drag_coef*(A_max - A_proj)); + A_proj*blunt_drag_coef + slender_drag_coef*(A_max - A_proj)); const mjtNum Aproj_coef = fluid_density * norm * (blunt_drag_coef - slender_drag_coef); const mjtNum dAproj_dv[3] = { @@ -1084,8 +1084,8 @@ static inline void mjd_viscous_drag( // Kutta lift due to motion in the fluid, D is 3x3 static inline void mjd_kutta_lift( - mjtNum* restrict D, const mjtNum lvel[6], const mjtNum fluid_density, - const mjtNum size[3], const mjtNum kutta_lift_coef) { + mjtNum* restrict D, const mjtNum lvel[6], const mjtNum fluid_density, + const mjtNum size[3], const mjtNum kutta_lift_coef) { const mjtNum a = pow2(size[1] * size[2]); const mjtNum b = pow2(size[2] * size[0]); const mjtNum c = pow2(size[0] * size[1]); @@ -1097,7 +1097,7 @@ static inline void mjd_kutta_lift( const mjtNum proj_num = a * xx + b * yy + c * zz; const mjtNum norm2 = xx + yy + zz; const mjtNum df_denom = mjPI * kutta_lift_coef * fluid_density / mju_max( - mjMINVAL, mju_sqrt(proj_denom * proj_num * norm2)); + mjMINVAL, mju_sqrt(proj_denom * proj_num * norm2)); const mjtNum dfx_coef = yy * (a - b) + zz * (a - c); const mjtNum dfy_coef = xx * (b - a) + zz * (b - c); @@ -1138,8 +1138,8 @@ static inline void mjd_kutta_lift( // Magnus force due to motion in the fluid, B is 6x6 static inline void mjd_magnus_force( - mjtNum* restrict B, const mjtNum lvel[6], const mjtNum fluid_density, - const mjtNum size[3], const mjtNum magnus_lift_coef) { + mjtNum* restrict B, const mjtNum lvel[6], const mjtNum fluid_density, + const mjtNum size[3], const mjtNum magnus_lift_coef) { const mjtNum volume = 4.0/3.0 * mjPI * size[0] * size[1] * size[2]; // magnus_coef = magnus_lift_coef * fluid_density * volume @@ -1149,9 +1149,11 @@ static inline void mjd_magnus_force( // premultiply by magnus_coef const mjtNum lin_vel[3] = { - magnus_coef * lvel[3], magnus_coef * lvel[4], magnus_coef * lvel[5]}; + magnus_coef * lvel[3], magnus_coef * lvel[4], magnus_coef * lvel[5] + }; const mjtNum ang_vel[3] = { - magnus_coef * lvel[0], magnus_coef * lvel[1], magnus_coef * lvel[2]}; + magnus_coef * lvel[0], magnus_coef * lvel[1], magnus_coef * lvel[2] + }; // force[3:] += magnus_coef * cross(ang_vel, lin_vel) mjd_cross(ang_vel, lin_vel, D_ang, D_lin); @@ -1186,25 +1188,25 @@ void mjd_ellipsoidFluid(const mjModel* m, mjData* d, int bodyid) { nnz = bodyJacSparse(m, bodyid, colind); // prepare rownnz, rowadr, colind for all 6 rows - for (int i=0; i<6; i++) { + for (int i=0; i < 6; i++) { rownnz[i] = nnz; rowadr[i] = i == 0 ? 0 : rowadr[i-1] + nnz; - for (int k=0; kbody_geomnum[bodyid]; j++) { + for (int j=0; j < m->body_geomnum[bodyid]; j++) { const int geomid = m->body_geomadr[bodyid] + j; mju_geomSemiAxes(m, geomid, semiaxes); readFluidGeomInteraction( - m->geom_fluid + mjNFLUID*geomid, &geom_interaction_coef, - &blunt_drag_coef, &slender_drag_coef, &ang_drag_coef, - &kutta_lift_coef, &magnus_lift_coef, - virtual_mass, virtual_inertia); + m->geom_fluid + mjNFLUID*geomid, &geom_interaction_coef, + &blunt_drag_coef, &slender_drag_coef, &ang_drag_coef, + &kutta_lift_coef, &magnus_lift_coef, + virtual_mass, virtual_inertia); // scales all forces, read from MJCF as boolean (0.0 or 1.0) if (geom_interaction_coef == 0.0) { @@ -1228,8 +1230,8 @@ void mjd_ellipsoidFluid(const mjModel* m, mjData* d, int bodyid) { // compress geom Jacobian in-place if (mj_isSparse(m)) { - for (int i=0; i<6; i++) { - for (int k=0; kbody_mass[i] * 6.0); + (inertia[1] + inertia[2] - inertia[0])) / m->body_mass[i] * 6.0); box[1] = mju_sqrt(mju_max(mjMINVAL, - (inertia[0] + inertia[2] - inertia[1])) / m->body_mass[i] * 6.0); + (inertia[0] + inertia[2] - inertia[1])) / m->body_mass[i] * 6.0); box[2] = mju_sqrt(mju_max(mjMINVAL, - (inertia[0] + inertia[1] - inertia[2])) / m->body_mass[i] * 6.0); + (inertia[0] + inertia[1] - inertia[2])) / m->body_mass[i] * 6.0); // map from CoM-centered to local body-centered 6D velocity mj_objectVelocity(m, d, mjOBJ_BODY, i, lvel, 1); @@ -1321,8 +1323,8 @@ void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i) nnz = bodyJacSparse(m, i, colind); // compress body Jacobian in-place - for (int j=0; j<6; j++) { - for (int k=0; kopt.viscosity>0) { + if (m->opt.viscosity > 0) { // diameter of sphere approximation mjtNum diam = (box[0] + box[1] + box[2])/3.0; // mju_scl3(lfrc, lvel, -mjPI*diam*diam*diam*m->opt.viscosity) B = -mjPI*diam*diam*diam*m->opt.viscosity; - for (int j=0; j<3; j++) { + for (int j=0; j < 3; j++) { if (mj_isSparse(m)) { addJTBJSparse(m, d, J, &B, 1, j, rownnz, rowadr, colind); } else { @@ -1362,7 +1364,7 @@ void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i) // mju_scl3(lfrc+3, lvel+3, -3.0*mjPI*diam*m->opt.viscosity); B = -3.0*mjPI*diam*m->opt.viscosity; - for (int j=0; j<3; j++) { + for (int j=0; j < 3; j++) { if (mj_isSparse(m)) { addJTBJSparse(m, d, J, &B, 1, 3+j, rownnz, rowadr, colind); } else { @@ -1372,7 +1374,7 @@ void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i) } // add lift and drag force and torque - if (m->opt.density>0) { + if (m->opt.density > 0) { // lfrc[0] -= m->opt.density*box[0]*(box[1]*box[1]*box[1]*box[1]+box[2]*box[2]*box[2]*box[2])* // mju_abs(lvel[0])*lvel[0]/64.0; B = -m->opt.density*box[0]*(box[1]*box[1]*box[1]*box[1]+box[2]*box[2]*box[2]*box[2])* @@ -1445,9 +1447,9 @@ void mjd_passive_vel(const mjModel* m, mjData* d) { } // dof damping - for (int i=0; iD_rownnz[i]; - for (int j=0; jD_rowadr[i] + j; // identify diagonal element @@ -1459,8 +1461,8 @@ void mjd_passive_vel(const mjModel* m, mjData* d) { } // tendon damping - for (int i=0; intendon; i++) { - if (m->tendon_damping[i]>0) { + for (int i=0; i < m->ntendon; i++) { + if (m->tendon_damping[i] > 0) { mjtNum B = -m->tendon_damping[i]; // add sparse or dense @@ -1474,15 +1476,15 @@ void mjd_passive_vel(const mjModel* m, mjData* d) { } // fluid drag model, either body-level (inertia box) or geom-level (ellipsoid) - if (m->opt.viscosity>0 || m->opt.density>0) { - for (int i=1; ibody_mass[i]opt.viscosity > 0 || m->opt.density > 0) { + for (int i=1; i < nbody; i++) { + if (m->body_mass[i] < mjMINVAL) { continue; } int use_ellipsoid_model = 0; // if any child geom uses the ellipsoid model, inertia-box model is disabled for parent body - for (int j=0; jbody_geomnum[i] && use_ellipsoid_model==0; j++) { + for (int j=0; j < m->body_geomnum[i] && use_ellipsoid_model == 0; j++) { const int geomid = m->body_geomadr[i] + j; use_ellipsoid_model += (m->geom_fluid[mjNFLUID*geomid] > 0); } diff --git a/src/engine/engine_derivative_fd.c b/src/engine/engine_derivative_fd.c index fb6bcaa5..141e0d26 100644 --- a/src/engine/engine_derivative_fd.c +++ b/src/engine/engine_derivative_fd.c @@ -68,7 +68,7 @@ static void setState(const mjModel* m, mjData* d, mjtNum time, const mjtNum* sta // dx = (x2 - x1) / h static void diff(mjtNum* restrict dx, const mjtNum* x1, const mjtNum* x2, mjtNum h, int n) { mjtNum inv_h = 1/h; - for (int i=0; iopt.integrator) { - case mjINT_EULER: - mj_EulerSkip(m, d, skipstage >= mjSTAGE_POS); - break; + case mjINT_EULER: + mj_EulerSkip(m, d, skipstage >= mjSTAGE_POS); + break; - case mjINT_RK4: - // ignore skipstage - mj_RungeKutta(m, d, 4); - break; + case mjINT_RK4: + // ignore skipstage + mj_RungeKutta(m, d, 4); + break; - case mjINT_IMPLICIT: - case mjINT_IMPLICITFAST: - mj_implicitSkip(m, d, skipstage >= mjSTAGE_VEL); - break; + case mjINT_IMPLICIT: + case mjINT_IMPLICITFAST: + mj_implicitSkip(m, d, skipstage >= mjSTAGE_VEL); + break; - default: - mju_error("Invalid integrator"); + default: + mju_error("Invalid integrator"); } TM_END(mjTIMER_STEP); @@ -208,7 +208,7 @@ void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) { mju_copy(qfrc_passive, d->qfrc_passive, nv); // loop over dofs - for (int i=0; iqvel[i]; @@ -224,9 +224,9 @@ void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) { mju_scl(fd, fd, 1/eps, nv); // copy to i-th column of qDeriv - for (int j=0; jD_rowadr[j] + cnt[j]; - if (cnt[j]D_rownnz[j] && d->D_colind[adr] == i) { + if (cnt[j] < d->D_rownnz[j] && d->D_colind[adr] == i) { d->qDeriv[adr] = fd[j]; cnt[j]++; } @@ -257,7 +257,7 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) { memset(cnt, 0, nv*sizeof(int)); // loop over dofs - for (int i=0; iqvel[i]; @@ -283,8 +283,8 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) { mju_scl(fd, fd, 0.5/eps, nv); // copy to sparse qDeriv - for (int j=0; jD_rownnz[j] && d->D_colind[d->D_rowadr[j]+cnt[j]]==i) { + for (int j=0; j < nv; j++) { + if (cnt[j] < d->D_rownnz[j] && d->D_colind[d->D_rowadr[j]+cnt[j]] == i) { d->qDeriv[d->D_rowadr[j]+cnt[j]] = fd[j]; cnt[j]++; } @@ -292,8 +292,8 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) { } // make sure final row counters equal rownnz - for (int i=0; iD_rownnz[i]) { + for (int i=0; i < nv; i++) { + if (cnt[i] != d->D_rownnz[i]) { mju_error("error in constructing FD sparse derivative"); } } @@ -368,7 +368,7 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered, // finite-difference controls: skip=mjSTAGE_VEL, handle ctrl at range limits if (DyDu || DsDu) { - for (int i=0; iactuator_ctrllimited[i]; // nudge forward, if possible given ctrlrange int nudge_fwd = !limited || inRange(ctrl[i], ctrl[i]+eps, m->actuator_ctrlrange+2*i); @@ -415,7 +415,7 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered, // finite-difference activations: skip=mjSTAGE_VEL if (DyDa || DsDa) { - for (int i=0; iact[i] += eps; @@ -462,7 +462,7 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered, // finite-difference velocities: skip=mjSTAGE_POS if (DyDv || DsDv) { - for (int i=0; iqvel[i] += eps; @@ -509,7 +509,7 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered, // finite-difference positions: skip=mjSTAGE_NONE if (DyDq || DsDq) { mjtNum *dpos = mj_stackAlloc(d, nv); // allocate position perturbation - for (int i=0; inq; i++) { + for (int i=0; i < m->nq; i++) { if (mju_isBad(d->qpos[i])) { mj_warning(d, mjWARN_BADQPOS, i); mj_resetData(m, d); @@ -61,7 +61,7 @@ void mj_checkPos(const mjModel* m, mjData* d) { // check velocities, reset if bad void mj_checkVel(const mjModel* m, mjData* d) { - for (int i=0; inv; i++) { + for (int i=0; i < m->nv; i++) { if (mju_isBad(d->qvel[i])) { mj_warning(d, mjWARN_BADQVEL, i); mj_resetData(m, d); @@ -76,7 +76,7 @@ void mj_checkVel(const mjModel* m, mjData* d) { // check accelerations, reset if bad void mj_checkAcc(const mjModel* m, mjData* d) { - for (int i=0; inv; i++) { + for (int i=0; i < m->nv; i++) { if (mju_isBad(d->qacc[i])) { mj_warning(d, mjWARN_BADQACC, i); mj_resetData(m, d); @@ -166,7 +166,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { mju_zero(d->actuator_force, nu); // disabled or no actuation: return - if (nu==0 || mjDISABLED(mjDSBL_ACTUATION)) { + if (nu == 0 || mjDISABLED(mjDSBL_ACTUATION)) { return; } @@ -176,7 +176,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { if (mjDISABLED(mjDSBL_CLAMPCTRL)) { mju_copy(ctrl, d->ctrl, nu); } else { - for (int i=0; iactuator_ctrllimited[i]) { mjtNum *ctrlrange = m->actuator_ctrlrange + 2*i; @@ -188,7 +188,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } // check controls, set all to 0 if any are bad - for (int i=0; iactuator_plugin[i] >= 0) { continue; @@ -275,7 +275,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { // handle actuator plugins if (m->nplugin) { const int nslot = mjp_pluginCount(); - for (int i=0; inplugin; i++) { + for (int i=0; i < m->nplugin; i++) { const int slot = m->plugin[i]; const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot); if (!plugin) { @@ -291,7 +291,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } // clamp actuator_force - for (int i=0; iactuator_forcelimited[i]) { mjtNum *forcerange = m->actuator_forcerange + 2*i; force[i] = mju_clip(force[i], forcerange[0], forcerange[1]); @@ -302,7 +302,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { mju_mulMatTVec(d->qfrc_actuator, moment, force, nu, nv); // act_dot for stateful actuators - for (int i=0; iactuator_plugin[i] >= 0) { continue; } @@ -392,7 +392,7 @@ static void warmstart(const mjModel* m, mjData* d) { mj_constraintUpdate(m, d, jar, &cost_warmstart, 0); // PGS - if (m->opt.solver==mjSOL_PGS) { + if (m->opt.solver == mjSOL_PGS) { // cost(force_warmstart) mjtNum PGS_warmstart = mju_dot(d->efc_force, d->efc_b, nefc); mjtNum* ARf = mj_stackAlloc(d, nefc); @@ -406,7 +406,7 @@ static void warmstart(const mjModel* m, mjData* d) { PGS_warmstart += 0.5*mju_dot(d->efc_force, ARf, nefc); // use zero if better - if (PGS_warmstart>0) { + if (PGS_warmstart > 0) { mju_zero(d->efc_force, nefc); mju_zero(d->qfrc_constraint, nv); } @@ -417,7 +417,7 @@ static void warmstart(const mjModel* m, mjData* d) { // add Gauss to cost(qacc_warmstart) mjtNum* Ma = mj_stackAlloc(d, nv); mj_mulM(m, d, Ma, d->qacc_warmstart); - for (int i=0; iqfrc_smooth[i])*(d->qacc_warmstart[i]-d->qacc_smooth[i]); } @@ -426,7 +426,7 @@ static void warmstart(const mjModel* m, mjData* d) { mj_constraintUpdate(m, d, d->efc_b, &cost_smooth, 0); // use qacc_smooth if better - if (cost_warmstart>cost_smooth) { + if (cost_warmstart > cost_smooth) { mju_copy(d->qacc, d->qacc_smooth, nv); } } @@ -488,7 +488,7 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) { mju_copy(d->qacc_warmstart, d->qacc, nv); // run noslip solver if enabled - if (m->opt.noslip_iterations>0) { + if (m->opt.noslip_iterations > 0) { mj_solNoSlip(m, d, m->opt.noslip_iterations); } @@ -507,11 +507,11 @@ static void mj_advance(const mjModel* m, mjData* d, mju_addToScl(d->act, act_dot, m->opt.timestep, m->na); // clamp activations - for (int i=0; inu; i++) { + for (int i=0; i < m->nu; i++) { int j = m->actuator_actadr[i]; if (j > -1 && m->actuator_actlimited[i]) { mjtNum* actrange = m->actuator_actrange + 2*i; - for (int k=0; kactuator_actnum[i]; k++) { + for (int k=0; k < m->actuator_actnum[i]; k++) { d->act[j+k] = mju_clip(d->act[j+k], actrange[0], actrange[1]); } } @@ -552,8 +552,8 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) { // check for dof damping int dof_damping = 0; - for (int i=0; idof_damping[i]>0) { + for (int i=0; i < nv; i++) { + if (m->dof_damping[i] > 0) { dof_damping = 1; break; } @@ -571,7 +571,7 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) { // MhB = M + h*diag(B) mju_copy(MhB, d->qM, m->nM); - for (int i=0; idof_Madr[i]] += m->opt.timestep * m->dof_damping[i]; } @@ -618,8 +618,8 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) { int nv = m->nv, nq = m->nq, na = m->na; mjtNum h = m->opt.timestep, time = d->time; mjtNum C[9], T[9], *X[10], *F[10], *dX; - const mjtNum* A = (N==4 ? RK4_A : 0); - const mjtNum* B = (N==4 ? RK4_B : 0); + const mjtNum* A = (N == 4 ? RK4_A : 0); + const mjtNum* B = (N == 4 ? RK4_B : 0); mjMARKSTACK; // check order @@ -629,16 +629,16 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) { // allocate space for intermediate solutions dX = mj_stackAlloc(d, 2*nv+na); - for (int i=0; iopt.integrator) { - case mjINT_EULER: - mj_Euler(m, d); - break; + case mjINT_EULER: + mj_Euler(m, d); + break; - case mjINT_RK4: - mj_RungeKutta(m, d, 4); - break; + case mjINT_RK4: + mj_RungeKutta(m, d, 4); + break; - case mjINT_IMPLICIT: - case mjINT_IMPLICITFAST: - mj_implicit(m, d); - break; + case mjINT_IMPLICIT: + case mjINT_IMPLICITFAST: + mj_implicit(m, d); + break; - default: - mju_error("Invalid integrator"); + default: + mju_error("Invalid integrator"); } TM_END(mjTIMER_STEP); diff --git a/src/engine/engine_inverse.c b/src/engine/engine_inverse.c index 04140701..9db983e4 100644 --- a/src/engine/engine_inverse.c +++ b/src/engine/engine_inverse.c @@ -123,7 +123,7 @@ void mj_inverseSkip(const mjModel* m, mjData* d, int nv = m->nv; // position-dependent - if (skipstageqfrc_inverse[i] += m->dof_armature[i]*d->qacc[i] - d->qfrc_passive[i] - d->qfrc_constraint[i]; } diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index de908918..4fb089cd 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -285,7 +285,7 @@ static void bufwrite(const void* src, int num, int szbuf, void* buf, int* ptrbuf } // check size - if (*ptrbuf+num>szbuf) { + if (*ptrbuf+num > szbuf) { mju_error("Attempting to write outside model buffer"); } @@ -304,7 +304,7 @@ static void bufread(void* dest, int num, int szbuf, const void* buf, int* ptrbuf } // check size - if (*ptrbuf+num>szbuf) { + if (*ptrbuf+num > szbuf) { mju_error("Attempting to read outside model buffer"); } @@ -362,7 +362,7 @@ static int safeAddToBufferSize(intptr_t* offset, int* nbuffer, size_t type_size, return 0; } #if (__has_builtin(__builtin_add_overflow) && __has_builtin(__builtin_mul_overflow)) \ - || (defined(__GNUC__) && __GNUC__ >= 5) + || (defined(__GNUC__) && __GNUC__ >= 5) // supported by GCC and Clang int to_add = 0; if (__builtin_mul_overflow(nc, nr, &to_add)) return 0; @@ -654,8 +654,8 @@ static mjModel* _mj_loadModel(const char* filename, int vfs_provider) { bufread(header, 4*sizeof(int), buffer_sz, buffer, &ptrbuf); // check header - for (int i=0; i<4; i++) { - if (header[i]!=expected_header[i]) { + for (int i=0; i < 4; i++) { + if (header[i] != expected_header[i]) { switch (i) { case 0: mju_warning("Model missing header ID"); @@ -692,7 +692,7 @@ static mjModel* _mj_loadModel(const char* filename, int vfs_provider) { info[35], info[36], info[37], info[38], info[39], info[40], info[41], info[42], info[43], info[44], info[45], info[46], info[47], info[48], info[49], info[50], info[51], info[52]); - if (!m || m->nbuffer!=info[getnint()-1]) { + if (!m || m->nbuffer != info[getnint()-1]) { mju_closeResource(r); mju_warning("Corrupted model, wrong size parameters"); mj_deleteModel(m); @@ -775,7 +775,7 @@ int mj_sizeModel(const mjModel* m) { + sizeof(mjVisual) + sizeof(mjStatistic)); -MJMODEL_POINTERS_PREAMBLE(m) + MJMODEL_POINTERS_PREAMBLE(m) #define X(type, name, nr, nc) \ size += sizeof(type)*(m->nr)*(nc); MJMODEL_POINTERS @@ -1367,16 +1367,16 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) { // set mocap_pos/quat = body_pos/quat for mocap bodies if (m->body_mocapid) { - for (int i=0; inbody; i++) { + for (int i=0; i < m->nbody; i++) { int id = m->body_mocapid[i]; - if (id>=0) { + if (id >= 0) { mju_copy3(d->mocap_pos+3*id, m->body_pos+3*i); mju_copy4(d->mocap_quat+4*id, m->body_quat+4*i); } } } else { // set the mocap_quats to {1, 0, 0, 0} - for (int i=0; inmocap; i++) { + for (int i=0; i < m->nmocap; i++) { d->mocap_quat[4*i] = 1.0; } } @@ -1426,7 +1426,7 @@ void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key) { _resetData(m, d, 0); // copy keyframe data if key is valid - if (key>=0 && keynkey) { + if (key >= 0 && key < m->nkey) { d->time = m->key_time[key]; mju_copy(d->qpos, m->key_qpos+key*m->nq, m->nq); mju_copy(d->qvel, m->key_qvel+key*m->nv, m->nv); @@ -1510,7 +1510,7 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) { case mjSENS_PLUGIN: return -1; - // don't use a 'default' case, so compiler warns about missing values + // don't use a 'default' case, so compiler warns about missing values } return -1; } @@ -1520,55 +1520,55 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) { // -2: invalid objtype static int numObjects(const mjModel* m, mjtObj objtype) { switch (objtype) { - case mjOBJ_UNKNOWN: - return -1; - case mjOBJ_BODY: - case mjOBJ_XBODY: - return m->nbody; - case mjOBJ_JOINT: - return m->njnt; - case mjOBJ_DOF: - return m->nv; - case mjOBJ_GEOM: - return m->ngeom; - case mjOBJ_SITE: - return m->nsite; - case mjOBJ_CAMERA: - return m->ncam; - case mjOBJ_LIGHT: - return m->nlight; - case mjOBJ_MESH: - return m->nmesh; - case mjOBJ_SKIN: - return m->nskin; - case mjOBJ_HFIELD: - return m->nhfield; - case mjOBJ_TEXTURE: - return m->ntex; - case mjOBJ_MATERIAL: - return m->nmat; - case mjOBJ_PAIR: - return m->npair; - case mjOBJ_EXCLUDE: - return m->nexclude; - case mjOBJ_EQUALITY: - return m->neq; - case mjOBJ_TENDON: - return m->ntendon; - case mjOBJ_ACTUATOR: - return m->nu; - case mjOBJ_SENSOR: - return m->nsensor; - case mjOBJ_NUMERIC: - return m->nnumeric; - case mjOBJ_TEXT: - return m->ntext; - case mjOBJ_TUPLE: - return m->ntuple; - case mjOBJ_KEY: - return m->nkey; - case mjOBJ_PLUGIN: - return m->nplugin; + case mjOBJ_UNKNOWN: + return -1; + case mjOBJ_BODY: + case mjOBJ_XBODY: + return m->nbody; + case mjOBJ_JOINT: + return m->njnt; + case mjOBJ_DOF: + return m->nv; + case mjOBJ_GEOM: + return m->ngeom; + case mjOBJ_SITE: + return m->nsite; + case mjOBJ_CAMERA: + return m->ncam; + case mjOBJ_LIGHT: + return m->nlight; + case mjOBJ_MESH: + return m->nmesh; + case mjOBJ_SKIN: + return m->nskin; + case mjOBJ_HFIELD: + return m->nhfield; + case mjOBJ_TEXTURE: + return m->ntex; + case mjOBJ_MATERIAL: + return m->nmat; + case mjOBJ_PAIR: + return m->npair; + case mjOBJ_EXCLUDE: + return m->nexclude; + case mjOBJ_EQUALITY: + return m->neq; + case mjOBJ_TENDON: + return m->ntendon; + case mjOBJ_ACTUATOR: + return m->nu; + case mjOBJ_SENSOR: + return m->nsensor; + case mjOBJ_NUMERIC: + return m->nnumeric; + case mjOBJ_TEXT: + return m->ntext; + case mjOBJ_TUPLE: + return m->ntuple; + case mjOBJ_KEY: + return m->nkey; + case mjOBJ_PLUGIN: + return m->nplugin; } return -2; } @@ -1675,7 +1675,7 @@ const char* mj_validateReferences(const mjModel* m) { #undef MJMODEL_REFERENCES // special logic that doesn't fit in the macro: - for (int i=0; inbody; i++) { + for (int i=0; i < m->nbody; i++) { if (i > 0 && m->body_parentid[i] >= i) { return "Invalid model: bad body_parentid."; } @@ -1686,7 +1686,7 @@ const char* mj_validateReferences(const mjModel* m) { return "Invalid model: bad body_weldid."; } } - for (int i=0; injnt; i++) { + for (int i=0; i < m->njnt; i++) { if (m->jnt_type[i] >= 4 || m->jnt_type[i] < 0) { return "Invalid model: jnt_type out of bounds."; } @@ -1699,12 +1699,12 @@ const char* mj_validateReferences(const mjModel* m) { return "Invalid model: jnt_dofadr out of bounds."; } } - for (int i=0; inv; i++) { + for (int i=0; i < m->nv; i++) { if (m->dof_parentid[i] >= i) { return "Invalid model: bad dof_parentid."; } } - for (int i=0; ingeom; i++) { + for (int i=0; i < m->ngeom; i++) { if (m->geom_condim[i] > 6 || m->geom_condim[i] < 0) { return "Invalid model: geom_condim out of bounds."; } @@ -1718,19 +1718,19 @@ const char* mj_validateReferences(const mjModel* m) { } } } - for (int i=0; inhfield; i++) { + for (int i=0; i < m->nhfield; i++) { int hfield_adr = m->hfield_adr[i] + m->hfield_nrow[i]*m->hfield_ncol[i]; if (hfield_adr > m->nhfielddata || m->hfield_adr[i] < 0) { return "Invalid model: hfield_adr out of bounds."; } } - for (int i=0; intex; i++) { + for (int i=0; i < m->ntex; i++) { int tex_adr = m->tex_adr[i] + 3*m->tex_height[i]*m->tex_width[i]; if (tex_adr > m->ntexdata || m->tex_adr[i] < 0) { return "Invalid model: tex_adr out of bounds."; } } - for (int i=0; inpair; i++) { + for (int i=0; i < m->npair; i++) { int pair_body1 = (m->pair_signature[i] & 0xFFFF) - 1; if (pair_body1 >= m->nbody || pair_body1 < 0) { return "Invalid model: pair_body1 out of bounds."; @@ -1740,104 +1740,104 @@ const char* mj_validateReferences(const mjModel* m) { return "Invalid model: pair_body2 out of bounds."; } } - for (int i=0; ineq; i++) { + for (int i=0; i < m->neq; i++) { int obj1id = m->eq_obj1id[i]; int obj2id = m->eq_obj2id[i]; switch (m->eq_type[i]) { - case mjEQ_JOINT: - if (obj1id >= m->njnt || obj1id < 0) { - return "Invalid model: eq_obj1id out of bounds."; - } - // -1 is the value used if second object is omitted. - if (obj2id >= m->njnt || obj2id < -1) { - return "Invalid model: eq_obj2id out of bounds."; - } - break; + case mjEQ_JOINT: + if (obj1id >= m->njnt || obj1id < 0) { + return "Invalid model: eq_obj1id out of bounds."; + } + // -1 is the value used if second object is omitted. + if (obj2id >= m->njnt || obj2id < -1) { + return "Invalid model: eq_obj2id out of bounds."; + } + break; - case mjEQ_TENDON: - if (obj1id >= m->ntendon || obj1id < 0) { - return "Invalid model: eq_obj1id out of bounds."; - } - // -1 is the value used if second object is omitted. - if (obj2id >= m->ntendon || obj2id < -1) { - return "Invalid model: eq_obj2id out of bounds."; - } - break; + case mjEQ_TENDON: + if (obj1id >= m->ntendon || obj1id < 0) { + return "Invalid model: eq_obj1id out of bounds."; + } + // -1 is the value used if second object is omitted. + if (obj2id >= m->ntendon || obj2id < -1) { + return "Invalid model: eq_obj2id out of bounds."; + } + break; - case mjEQ_WELD: - case mjEQ_CONNECT: - if (obj1id >= m->nbody || obj1id < 0) { - return "Invalid model: eq_obj1id out of bounds."; - } - if (obj2id >= m->nbody || obj2id < 0) { - return "Invalid model: eq_obj2id out of bounds."; - } - break; + case mjEQ_WELD: + case mjEQ_CONNECT: + if (obj1id >= m->nbody || obj1id < 0) { + return "Invalid model: eq_obj1id out of bounds."; + } + if (obj2id >= m->nbody || obj2id < 0) { + return "Invalid model: eq_obj2id out of bounds."; + } + break; - default: - mju_error("mj_validateReferences: unknown equality constraint type."); + default: + mju_error("mj_validateReferences: unknown equality constraint type."); } } - for (int i=0; inwrap; i++) { + for (int i=0; i < m->nwrap; i++) { int wrap_objid = m->wrap_objid[i]; switch (m->wrap_type[i]) { - case mjWRAP_NONE: - case mjWRAP_PULLEY: - // wrap_objid not used. - break; - case mjWRAP_JOINT: - if (wrap_objid >= m->njnt || wrap_objid < 0) { - return "Invalid model: wrap_objid out of bounds."; - } - break; - case mjWRAP_SITE: - if (wrap_objid >= m->nsite || wrap_objid < 0) { - return "Invalid model: wrap_objid out of bounds."; - } - break; - case mjWRAP_SPHERE: - case mjWRAP_CYLINDER: - if (wrap_objid >= m->ngeom || wrap_objid < 0) { - return "Invalid model: wrap_objid out of bounds."; - } - break; + case mjWRAP_NONE: + case mjWRAP_PULLEY: + // wrap_objid not used. + break; + case mjWRAP_JOINT: + if (wrap_objid >= m->njnt || wrap_objid < 0) { + return "Invalid model: wrap_objid out of bounds."; + } + break; + case mjWRAP_SITE: + if (wrap_objid >= m->nsite || wrap_objid < 0) { + return "Invalid model: wrap_objid out of bounds."; + } + break; + case mjWRAP_SPHERE: + case mjWRAP_CYLINDER: + if (wrap_objid >= m->ngeom || wrap_objid < 0) { + return "Invalid model: wrap_objid out of bounds."; + } + break; } } - for (int i=0; inu; i++) { + for (int i=0; i < m->nu; i++) { int actuator_trntype = m->actuator_trntype[i]; int id = m->actuator_trnid[2*i]; int idslider = m->actuator_trnid[2*i+1]; switch (actuator_trntype) { - case mjTRN_JOINT: - case mjTRN_JOINTINPARENT: - if (id < 0 || id >= m->njnt) { - return "Invalid model: actuator_trnid out of bounds."; - } - break; - case mjTRN_TENDON: - if (id < 0 || id >= m->ntendon) { - return "Invalid model: actuator_trnid out of bounds."; - } - break; - case mjTRN_SITE: - if (id < 0 || id >= m->nsite) { - return "Invalid model: actuator_trnid out of bounds."; - } - break; - case mjTRN_SLIDERCRANK: - if (id < 0 || id >= m->nsite) { - return "Invalid model: actuator_trnid out of bounds."; - } - if (idslider < 0 || idslider >= m->nsite) { - return "Invalid model: actuator_trnid out of bounds."; - } - break; - case mjTRN_UNDEFINED: - // actuator_trnid not used. - break; + case mjTRN_JOINT: + case mjTRN_JOINTINPARENT: + if (id < 0 || id >= m->njnt) { + return "Invalid model: actuator_trnid out of bounds."; + } + break; + case mjTRN_TENDON: + if (id < 0 || id >= m->ntendon) { + return "Invalid model: actuator_trnid out of bounds."; + } + break; + case mjTRN_SITE: + if (id < 0 || id >= m->nsite) { + return "Invalid model: actuator_trnid out of bounds."; + } + break; + case mjTRN_SLIDERCRANK: + if (id < 0 || id >= m->nsite) { + return "Invalid model: actuator_trnid out of bounds."; + } + if (idslider < 0 || idslider >= m->nsite) { + return "Invalid model: actuator_trnid out of bounds."; + } + break; + case mjTRN_UNDEFINED: + // actuator_trnid not used. + break; } } - for (int i=0; insensor; i++) { + for (int i=0; i < m->nsensor; i++) { mjtSensor sensor_type = m->sensor_type[i]; int sensor_size; if (sensor_type == mjSENS_PLUGIN) { @@ -1851,7 +1851,7 @@ const char* mj_validateReferences(const mjModel* m) { sensor_size = sensorSize(sensor_type, m->sensor_dim[i]); } if (sensor_size < 0) { - return "Invalid model: Bad sensor_type."; + return "Invalid model: Bad sensor_type."; } int sensor_adr = m->sensor_adr[i]; if (sensor_adr < 0 || sensor_adr + sensor_size > m->nsensordata) { @@ -1872,7 +1872,7 @@ const char* mj_validateReferences(const mjModel* m) { return "Invalid model: invalid sensor_refid"; } } - for (int i=0; inexclude; i++) { + for (int i=0; i < m->nexclude; i++) { int exclude_body1 = (m->exclude_signature[i] & 0xFFFF) - 1; if (exclude_body1 >= m->nbody || exclude_body1 < 0) { return "Invalid model: exclude_body1 out of bounds."; @@ -1882,8 +1882,8 @@ const char* mj_validateReferences(const mjModel* m) { return "Invalid model: exclude_body2 out of bounds."; } } - for (int i=0; intuple; i++) { - for (int j=0; jtuple_size[i]; j++) { + for (int i=0; i < m->ntuple; i++) { + for (int j=0; j < m->tuple_size[i]; j++) { int adr = m->tuple_adr[i] + j; int nobj = numObjects(m, m->tuple_objtype[adr]); if (nobj == -2) { diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index 0c250558..597cdb0a 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -48,7 +48,7 @@ void mj_passive(const mjModel* m, mjData* d) { } // joint-level springs - for (int i=0; injnt; i++) { + for (int i=0; i < m->njnt; i++) { stiffness = m->jnt_stiffness[i]; int padr = m->jnt_qposadr[i]; @@ -85,13 +85,13 @@ void mj_passive(const mjModel* m, mjData* d) { } // dof-level dampers - for (int i=0; inv; i++) { + for (int i=0; i < m->nv; i++) { damping = m->dof_damping[i]; d->qfrc_passive[i] -= damping*d->qvel[i]; } // tendon-level spring-dampers - for (int i=0; intendon; i++) { + for (int i=0; i < m->ntendon; i++) { stiffness = m->tendon_stiffness[i]; damping = m->tendon_damping[i]; @@ -113,7 +113,7 @@ void mj_passive(const mjModel* m, mjData* d) { // transform to joint torque, add to qfrc_passive: dense or sparse if (issparse) { int end = d->ten_J_rowadr[i] + d->ten_J_rownnz[i]; - for (int j=d->ten_J_rowadr[i]; jten_J_rowadr[i]; j < end; j++) { d->qfrc_passive[d->ten_J_colind[j]] += d->ten_J[j] * frc; } } else { @@ -126,7 +126,7 @@ void mj_passive(const mjModel* m, mjData* d) { mjtNum force[3], torque[3]={0}; // apply per-body gravity compensation - for (int i=1; inbody; i++) { + for (int i=1; i < m->nbody; i++) { if (m->body_gravcomp[i]) { mju_scl3(force, m->opt.gravity, -(m->body_mass[i]*m->body_gravcomp[i])); mj_applyFT(m, d, force, torque, d->xipos+3*i, i, d->qfrc_passive); @@ -135,15 +135,15 @@ void mj_passive(const mjModel* m, mjData* d) { } // body-level viscosity, lift and drag - if (m->opt.viscosity>0 || m->opt.density>0) { - for (int i=1; inbody; i++) { - if (m->body_mass[i]opt.viscosity > 0 || m->opt.density > 0) { + for (int i=1; i < m->nbody; i++) { + if (m->body_mass[i] < mjMINVAL) { continue; } int use_ellipsoid_model = 0; // if any child geom uses the ellipsoid model, inertia-box model is disabled for parent body - for (int j=0; jbody_geomnum[i] && use_ellipsoid_model==0; j++) { + for (int j=0; j < m->body_geomnum[i] && use_ellipsoid_model == 0; j++) { const int geomid = m->body_geomadr[i] + j; use_ellipsoid_model += (m->geom_fluid[mjNFLUID*geomid] > 0); } @@ -164,7 +164,7 @@ void mj_passive(const mjModel* m, mjData* d) { if (m->nplugin) { const int nslot = mjp_pluginCount(); // iterate over plugins, call compute if type is mjPLUGIN_PASSIVE - for (int i=0; inplugin; i++) { + for (int i=0; i < m->nplugin; i++) { const int slot = m->plugin[i]; const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot); if (!plugin) { @@ -209,7 +209,7 @@ void mj_inertiaBoxFluidModel(const mjModel* m, mjData* d, int i) { mju_zero(lfrc, 6); // set viscous force and torque - if (m->opt.viscosity>0) { + if (m->opt.viscosity > 0) { // diameter of sphere approximation diam = (box[0] + box[1] + box[2])/3.0; @@ -221,7 +221,7 @@ void mj_inertiaBoxFluidModel(const mjModel* m, mjData* d, int i) { } // add lift and drag force and torque - if (m->opt.density>0) { + if (m->opt.density > 0) { // force lfrc[3] -= 0.5*m->opt.density*box[1]*box[2]*mju_abs(lvel[3])*lvel[3]; lfrc[4] -= 0.5*m->opt.density*box[0]*box[2]*mju_abs(lvel[4])*lvel[4]; @@ -252,16 +252,16 @@ void mj_ellipsoidFluidModel(const mjModel* m, mjData* d, int bodyid) { mjtNum semiaxes[3], virtual_mass[3], virtual_inertia[3]; mjtNum blunt_drag_coef, slender_drag_coef, ang_drag_coef; - for (int j=0; jbody_geomnum[bodyid]; j++) { + for (int j=0; j < m->body_geomnum[bodyid]; j++) { const int geomid = m->body_geomadr[bodyid] + j; mju_geomSemiAxes(m, geomid, semiaxes); readFluidGeomInteraction( - m->geom_fluid + mjNFLUID*geomid, &geom_interaction_coef, - &blunt_drag_coef, &slender_drag_coef, &ang_drag_coef, - &kutta_lift_coef, &magnus_lift_coef, - virtual_mass, virtual_inertia); + m->geom_fluid + mjNFLUID*geomid, &geom_interaction_coef, + &blunt_drag_coef, &slender_drag_coef, &ang_drag_coef, + &kutta_lift_coef, &magnus_lift_coef, + virtual_mass, virtual_inertia); // scales all forces, read from MJCF as boolean (0.0 or 1.0) if (geom_interaction_coef == 0.0) { @@ -364,11 +364,11 @@ static inline mjtNum mji_ellipsoid_max_moment(const mjtNum size[3], const int di // lift and drag forces due to motion in the fluid void mj_viscousForces( - const mjtNum local_vels[6], const mjtNum fluid_density, - const mjtNum fluid_viscosity, const mjtNum size[3], - const mjtNum magnus_lift_coef, const mjtNum kutta_lift_coef, - const mjtNum blunt_drag_coef, const mjtNum slender_drag_coef, - const mjtNum ang_drag_coef, mjtNum local_force[6]) + const mjtNum local_vels[6], const mjtNum fluid_density, + const mjtNum fluid_viscosity, const mjtNum size[3], + const mjtNum magnus_lift_coef, const mjtNum kutta_lift_coef, + const mjtNum blunt_drag_coef, const mjtNum slender_drag_coef, + const mjtNum ang_drag_coef, mjtNum local_force[6]) { const mjtNum lin_vel[3] = {local_vels[3], local_vels[4], local_vels[5]}; const mjtNum ang_vel[3] = {local_vels[0], local_vels[1], local_vels[2]}; @@ -406,7 +406,7 @@ void mj_viscousForces( // cosine between velocity and normal to the surface // divided by proj_denom instead of sqrt(proj_denom) to account for skipped normalization in norm const mjtNum cos_alpha = proj_num / mju_max( - mjMINVAL, mju_norm3(lin_vel) * proj_denom); + mjMINVAL, mju_norm3(lin_vel) * proj_denom); mjtNum kutta_circ[3]; mju_cross(kutta_circ, norm, lin_vel); kutta_circ[0] *= kutta_lift_coef * fluid_density * cos_alpha * A_proj; @@ -434,11 +434,11 @@ void mj_viscousForces( }; const mjtNum drag_lin_coef = // linear plus quadratic - fluid_viscosity*lin_visc_force_coef + fluid_density*mju_norm3(lin_vel)*( - A_proj*blunt_drag_coef + slender_drag_coef*(A_max - A_proj)); + fluid_viscosity*lin_visc_force_coef + fluid_density*mju_norm3(lin_vel)*( + A_proj*blunt_drag_coef + slender_drag_coef*(A_max - A_proj)); const mjtNum drag_ang_coef = // linear plus quadratic - fluid_viscosity * lin_visc_torq_coef + - fluid_density * mju_norm3(mom_visc); + fluid_viscosity * lin_visc_torq_coef + + fluid_density * mju_norm3(mom_visc); local_force[0] -= drag_ang_coef * ang_vel[0]; local_force[1] -= drag_ang_coef * ang_vel[1]; diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 1616327d..0de90613 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -52,9 +52,9 @@ static void printArray(const char* str, int nr, int nc, const mjtNum* data, FILE } if (nr && nc) { fprintf(fp, "%s\n", str); - for (int r=0; rnbody; i++) { + for (int i=0; i < m->nbody; i++) { totalmass += m->body_mass[i]; } @@ -305,7 +305,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* // qpos0 fprintf(fp, NAME_FORMAT, "qpos0"); - for (int i=0; inq; i++) { + for (int i=0; i < m->nq; i++) { fprintf(fp, float_format, m->qpos0[i]); fprintf(fp, " "); } @@ -313,7 +313,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* // qpos_spring fprintf(fp, NAME_FORMAT, "qpos_spring"); - for (int i=0; inq; i++) { + for (int i=0; i < m->nq; i++) { fprintf(fp, float_format, m->qpos_spring[i]); fprintf(fp, " "); } @@ -347,7 +347,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* } // bodies - for (int i=0; inbody; i++) { + for (int i=0; i < m->nbody; i++) { fprintf(fp, "\nBODY %d:\n", i); fprintf(fp, " " NAME_FORMAT, "name"); fprintf(fp, " %s\n", m->names + m->name_bodyadr[i]); @@ -357,7 +357,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* if (m->nbody) fprintf(fp, "\n"); // joints - for (int i=0; injnt; i++) { + for (int i=0; i < m->njnt; i++) { fprintf(fp, "\nJOINT %d:\n", i); fprintf(fp, " " NAME_FORMAT, "name"); fprintf(fp, " %s\n", m->names + m->name_jntadr[i]); @@ -367,7 +367,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* if (m->njnt) fprintf(fp, "\n"); // dofs - for (int i=0; inv; i++) { + for (int i=0; i < m->nv; i++) { fprintf(fp, "\nDOF %d:\n", i); object_class = &m->nv; MJMODEL_POINTERS @@ -375,7 +375,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* if (m->nv) fprintf(fp, "\n"); // geoms - for (int i=0; ingeom; i++) { + for (int i=0; i < m->ngeom; i++) { fprintf(fp, "\nGEOM %d:\n", i); fprintf(fp, " " NAME_FORMAT, "name"); fprintf(fp, " %s\n", m->names + m->name_geomadr[i]); @@ -385,7 +385,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* if (m->ngeom) fprintf(fp, "\n"); // sites - for (int i=0; insite; i++) { + for (int i=0; i < m->nsite; i++) { fprintf(fp, "\nSITE %d:\n", i); fprintf(fp, " " NAME_FORMAT, "name"); fprintf(fp, " %s\n", m->names + m->name_siteadr[i]); @@ -395,7 +395,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* if (m->nsite) fprintf(fp, "\n"); // cameras - for (int i=0; incam; i++) { + for (int i=0; i < m->ncam; i++) { fprintf(fp, "\nCAMERA %d:\n", i); fprintf(fp, " " NAME_FORMAT, "name"); fprintf(fp, " %s\n", m->names + m->name_camadr[i]); @@ -405,7 +405,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* if (m->ncam) fprintf(fp, "\n"); // lights - for (int i=0; inlight; i++) { + for (int i=0; i < m->nlight; i++) { fprintf(fp, "\nLIGHT %d:\n", i); fprintf(fp, " " NAME_FORMAT, "name"); fprintf(fp, " %s\n", m->names + m->name_lightadr[i]); @@ -415,13 +415,13 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* if (m->nlight) fprintf(fp, "\n"); // meshes - for (int i=0; inmesh; i++) { + for (int i=0; i < m->nmesh; i++) { fprintf(fp, "\nMESH %d:\n", i); fprintf(fp, " " NAME_FORMAT, "name"); fprintf(fp, " %s\n", m->names + m->name_meshadr[i]); object_class = &m->nmesh; MJMODEL_POINTERS - if (m->mesh_graphadr[i]>=0) { + if (m->mesh_graphadr[i] >= 0) { fprintf(fp, " " NAME_FORMAT, "qhull face"); fprintf(fp, " %d\n", m->mesh_graph[m->mesh_graphadr[i]+1]); fprintf(fp, " " NAME_FORMAT, "qhull vert"); @@ -431,7 +431,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* if (m->nmesh) fprintf(fp, "\n"); // skins - for (int i=0; inskin; i++) { + for (int i=0; i < m->nskin; i++) { fprintf(fp, "\nSKIN %d:\n", i); fprintf(fp, " " NAME_FORMAT, "name"); fprintf(fp, " %s\n", m->names + m->name_skinadr[i]); @@ -441,7 +441,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* if (m->nskin) fprintf(fp, "\n"); // hfields - for (int i=0; inhfield; i++) { + for (int i=0; i < m->nhfield; i++) { fprintf(fp, "\nHEIGHTFIELD %d:\n", i); fprintf(fp, " " NAME_FORMAT, "name"); fprintf(fp, " %s\n", m->names + m->name_hfieldadr[i]); @@ -451,7 +451,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* if (m->nhfield) fprintf(fp, "\n"); // textures - for (int i=0; intex; i++) { + for (int i=0; i < m->ntex; i++) { fprintf(fp, "\nTEXTURE %d:\n", i); fprintf(fp, " " NAME_FORMAT, "name"); fprintf(fp, " %s\n", m->names + m->name_texadr[i]); @@ -461,7 +461,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* if (m->ntex) fprintf(fp, "\n"); // materials - for (int i=0; inmat; i++) { + for (int i=0; i < m->nmat; i++) { fprintf(fp, "\nMATERIAL %d:\n", i); fprintf(fp, " " NAME_FORMAT, "name"); fprintf(fp, " %s\n", m->names + m->name_matadr[i]); @@ -471,7 +471,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* if (m->nmat) fprintf(fp, "\n"); // pairs - for (int i=0; inpair; i++) { + for (int i=0; i < m->npair; i++) { fprintf(fp, "\nPAIR %d:\n", i); fprintf(fp, " " NAME_FORMAT, "name"); fprintf(fp, " %s\n", m->names + m->name_pairadr[i]); @@ -481,7 +481,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* if (m->npair) fprintf(fp, "\n"); // excludes - for (int i=0; inexclude; i++) { + for (int i=0; i < m->nexclude; i++) { fprintf(fp, "\nEXCLUDE %d:\n", i); fprintf(fp, " " NAME_FORMAT, "name"); fprintf(fp, " %s\n", m->names + m->name_excludeadr[i]); @@ -491,7 +491,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* if (m->nexclude) fprintf(fp, "\n"); // equality constraints - for (int i=0; ineq; i++) { + for (int i=0; i < m->neq; i++) { fprintf(fp, "\nEQUALITY %d:\n", i); fprintf(fp, " " NAME_FORMAT, "name"); fprintf(fp, " %s\n", m->names + m->name_eqadr[i]); @@ -501,14 +501,14 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* if (m->neq) fprintf(fp, "\n"); // tendons - for (int i=0; intendon; i++) { + for (int i=0; i < m->ntendon; i++) { fprintf(fp, "\nTENDON %d:\n", i); fprintf(fp, " " NAME_FORMAT, "name"); fprintf(fp, " %s\n", m->names + m->name_tendonadr[i]); object_class = &m->ntendon; MJMODEL_POINTERS fprintf(fp, " path \n"); - for (int j=0; jtendon_num[i]; j++) { + for (int j=0; j < m->tendon_num[i]; j++) { int k = m->tendon_adr[i]+j; fprintf(fp, " %d %d ", m->wrap_type[k], m->wrap_objid[k]); fprintf(fp, float_format, m->wrap_prm[k]); @@ -519,7 +519,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* if (m->ntendon) fprintf(fp, "\n"); // actuators - for (int i=0; inu; i++) { + for (int i=0; i < m->nu; i++) { fprintf(fp, "\nACTUATOR %d:\n", i); fprintf(fp, " " NAME_FORMAT, "name"); fprintf(fp, " %s\n", m->names + m->name_actuatoradr[i]); @@ -529,7 +529,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* if (m->nu) fprintf(fp, "\n"); // sensors - for (int i=0; insensor; i++) { + for (int i=0; i < m->nsensor; i++) { fprintf(fp, "\nSENSOR %d:\n", i); fprintf(fp, " " NAME_FORMAT, "name"); fprintf(fp, " %s\n", m->names + m->name_sensoradr[i]); @@ -539,12 +539,12 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* if (m->nsensor) fprintf(fp, "\n"); // custom numeric parameters - for (int i=0; innumeric; i++) { + for (int i=0; i < m->nnumeric; i++) { fprintf(fp, "\nNUMERIC %d:\n", i); fprintf(fp, " name %s\n", m->names + m->name_numericadr[i]); fprintf(fp, " size %d\n", m->numeric_size[i]); fprintf(fp, " value "); - for (int j=0; jnumeric_size[i]; j++) { + for (int j=0; j < m->numeric_size[i]; j++) { fprintf(fp, float_format, m->numeric_data[m->numeric_adr[i]+j]); } fprintf(fp, "\n"); @@ -552,7 +552,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* if (m->nnumeric) fprintf(fp, "\n"); // custom text parameters - for (int i=0; intext; i++) { + for (int i=0; i < m->ntext; i++) { fprintf(fp, "\nTEXT %d:\n", i); fprintf(fp, " name %s\n", m->names + m->name_textadr[i]); fprintf(fp, " size %d\n", m->text_size[i]); @@ -561,12 +561,12 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* if (m->ntext) fprintf(fp, "\n"); // custom tuple parameters - for (int i=0; intuple; i++) { + for (int i=0; i < m->ntuple; i++) { fprintf(fp, "\nTUPLE %d:\n", i); fprintf(fp, " name %s\n", m->names + m->name_tupleadr[i]); fprintf(fp, " size %d\n", m->tuple_size[i]); fprintf(fp, " elements\n"); - for (int j=m->tuple_adr[i]; jtuple_adr[i]+m->tuple_size[i]; j++) { + for (int j=m->tuple_adr[i]; j < m->tuple_adr[i]+m->tuple_size[i]; j++) { fprintf(fp, " %s %d, prm = ", mju_type2Str(m->tuple_objtype[j]), m->tuple_objid[j]); fprintf(fp, float_format, m->tuple_objprm[j]); @@ -576,58 +576,58 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* if (m->ntuple) fprintf(fp, "\n"); // keyframes (only if different from default) - for (int i=0; inkey; i++) { + for (int i=0; i < m->nkey; i++) { // print name if (m->names[m->name_keyadr[i]]) { fprintf(fp, "key_name%d %s\n", i, m->names + m->name_keyadr[i]); } // print time if non-0 - if (m->key_time[i]!=0) { + if (m->key_time[i] != 0) { fprintf(fp, "key_time%d %.4f\n", i, m->key_time[i]); } // check qpos for difference int k = 0; - for (int j=0; jnq; j++) + for (int j=0; j < m->nq; j++) if (m->qpos0[j] != m->key_qpos[i*m->nq + j]) { k = 1; } // print if different - if (k==1) { + if (k == 1) { fprintf(fp, "key_qpos%d ", i); - for (int j=0; jnq; j++) { + for (int j=0; j < m->nq; j++) { fprintf(fp, float_format, m->key_qpos[i*m->nq + j]); } fprintf(fp, "\n"); } // check qvel for nonzero - for (int j=0; jnv; j++) + for (int j=0; j < m->nv; j++) if (m->key_qvel[i*m->nv + j]) { k = 2; } // print if nonzero - if (k==2) { + if (k == 2) { fprintf(fp, "key_qvel%d ", i); - for (int j=0; jnv; j++) { + for (int j=0; j < m->nv; j++) { fprintf(fp, float_format, m->key_qvel[i*m->nv + j]); } fprintf(fp, "\n"); } // check act for nonzero - for (int j=0; jna; j++) + for (int j=0; j < m->na; j++) if (m->key_act[i*m->na + j]) { k = 3; } // print if nonzero - if (k==3) { + if (k == 3) { fprintf(fp, "key_act%d ", i); - for (int j=0; jna; j++) { + for (int j=0; j < m->na; j++) { fprintf(fp, float_format, m->key_act[i*m->na + j]); } fprintf(fp, "\n"); @@ -635,10 +635,10 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* // check mpos for difference if (m->nmocap) { - for (int j=0; jnbody; j++) { - if (m->body_mocapid[j]>=0) { + for (int j=0; j < m->nbody; j++) { + if (m->body_mocapid[j] >= 0) { int id = m->body_mocapid[j]; - if (m->body_pos[3*j] != m->key_mpos[i*3*m->nmocap + 3*id] || + if (m->body_pos[3*j] != m->key_mpos[i*3*m->nmocap + 3*id] || m->body_pos[3*j+1] != m->key_mpos[i*3*m->nmocap + 3*id+1] || m->body_pos[3*j+2] != m->key_mpos[i*3*m->nmocap + 3*id+2]) { k = 4; @@ -649,9 +649,9 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* } // print if nonzero - if (k==4) { + if (k == 4) { fprintf(fp, "key_mpos%d ", i); - for (int j=0; j<3*m->nmocap; j++) { + for (int j=0; j < 3*m->nmocap; j++) { fprintf(fp, float_format, m->key_mpos[i*3*m->nmocap + j]); } fprintf(fp, "\n"); @@ -659,10 +659,10 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* // check mquat for difference if (m->nmocap) { - for (int j=0; jnbody; j++) { - if (m->body_mocapid[j]>=0) { + for (int j=0; j < m->nbody; j++) { + if (m->body_mocapid[j] >= 0) { int id = m->body_mocapid[j]; - if (m->body_quat[4*j] != m->key_mquat[i*4*m->nmocap + 4*id] || + if (m->body_quat[4*j] != m->key_mquat[i*4*m->nmocap + 4*id] || m->body_quat[4*j+1] != m->key_mquat[i*4*m->nmocap + 4*id+1] || m->body_quat[4*j+2] != m->key_mquat[i*4*m->nmocap + 4*id+2] || m->body_quat[4*j+3] != m->key_mquat[i*4*m->nmocap + 4*id+3]) { @@ -674,16 +674,16 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* } // print if nonzero - if (k==5) { + if (k == 5) { fprintf(fp, "key_mquat%d ", i); - for (int j=0; j<4*m->nmocap; j++) { + for (int j=0; j < 4*m->nmocap; j++) { fprintf(fp, float_format, m->key_mquat[i*4*m->nmocap + j]); } fprintf(fp, "\n"); } // check ctrl for nonzero - for (int j=0; jnu; j++) { + for (int j=0; j < m->nu; j++) { if (m->key_ctrl[i*m->nu + j]) { k = 6; break; @@ -691,9 +691,9 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* } // print if nonzero - if (k==6) { + if (k == 6) { fprintf(fp, "key_ctrl%d ", i); - for (int j=0; jnu; j++) { + for (int j=0; j < m->nu; j++) { fprintf(fp, float_format, m->key_ctrl[i*m->nu + j]); } fprintf(fp, "\n"); @@ -790,12 +790,12 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, // WARNING int active_warnings = 0; - for (int i=0; iwarning[i].number; } if (active_warnings) { fprintf(fp, "WARNING\n"); - for (int i=0; iwarning[i].number) fprintf(fp, " %d: lastinfo = %d number = %d\n", i, d->warning[i].lastinfo, d->warning[i].number); @@ -804,12 +804,12 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, // TIMER mjtNum active_timers = 0; - for (int i=0; itimer[i].duration; } if (active_timers) { fprintf(fp, "TIMER\n"); - for (int i=0; itimer[i].number); @@ -822,7 +822,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, fprintf(fp, "SOLVER STAT\n"); fprintf(fp, " solver_iter = %d\n", d->solver_iter); fprintf(fp, " solver_nnz = %d\n", d->solver_nnz); - for (int i=0; isolver_iter); i++) { + for (int i=0; i < mjMIN(mjNSOLVER, d->solver_iter); i++) { fprintf(fp, " %d: improvement = ", i); fprintf(fp, float_format, d->solver[i].improvement); fprintf(fp, " gradient = "); @@ -886,11 +886,11 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, printArrayInt("TEN_J_ROWNNZ", m->ntendon, 1, d->ten_J_rownnz, fp); printArrayInt("TEN_J_ROWADR", m->ntendon, 1, d->ten_J_rowadr, fp); printSparse("TEN_J", d->ten_J, m->ntendon, d->ten_J_rownnz, - d->ten_J_rowadr, d->ten_J_colind, fp, float_format); + d->ten_J_rowadr, d->ten_J_colind, fp, float_format); } - for (int i=0; intendon; i++) { + for (int i=0; i < m->ntendon; i++) { fprintf(fp, "TENDON %d: %d wrap points\n", i, d->ten_wrapnum[i]); - for (int j=0; jten_wrapnum[i]; j++) { + for (int j=0; j < d->ten_wrapnum[i]; j++) { fprintf(fp, " %d: ", d->wrap_obj[d->ten_wrapadr[i]+j]); printVector("", d->wrap_xpos+3*(d->ten_wrapadr[i]+j), 3, fp, float_format); } @@ -965,7 +965,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, // contact fprintf(fp, "CONTACT\n"); - for (int i=0; incon; i++) { + for (int i=0; i < d->ncon; i++) { fprintf(fp, " %d:\n dim %d\n geom ", i, d->contact[i].dim); const char* geom1 = mj_id2name(m, mjOBJ_GEOM, d->contact[i].geom1); if (geom1) { @@ -1066,9 +1066,9 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, } -#ifdef __clang__ -#pragma clang diagnostic pop -#endif + #ifdef __clang__ + #pragma clang diagnostic pop + #endif // print mjData to text file diff --git a/src/engine/engine_ray.c b/src/engine/engine_ray.c index 773372fb..83893350 100644 --- a/src/engine/engine_ray.c +++ b/src/engine/engine_ray.c @@ -67,22 +67,22 @@ static mjtNum latitude(const mjtNum vec[3]) { static int ray_eliminate(const mjModel* m, const mjData* d, int geomid, const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude) { // body exclusion - if (m->geom_bodyid[geomid]==bodyexclude) { + if (m->geom_bodyid[geomid] == bodyexclude) { return 1; } // invisible geom exclusion - if (m->geom_matid[geomid]<0 && m->geom_rgba[4*geomid+3]==0) { + if (m->geom_matid[geomid] < 0 && m->geom_rgba[4*geomid+3] == 0) { return 1; } // invisible material exclusion - if (m->geom_matid[geomid]>=0 && m->mat_rgba[4*m->geom_matid[geomid]+3]==0) { + if (m->geom_matid[geomid] >= 0 && m->mat_rgba[4*m->geom_matid[geomid]+3] == 0) { return 1; } // static exclusion - if (!flg_static && m->body_weldid[m->geom_bodyid[geomid]]==0) { + if (!flg_static && m->body_weldid[m->geom_bodyid[geomid]] == 0) { return 1; } @@ -94,7 +94,7 @@ static int ray_eliminate(const mjModel* m, const mjData* d, int geomid, // group inclusion/exclusion int groupid = mjMIN(mjNGROUP-1, mjMAX(0, m->geom_group[geomid])); - return (geomgroup[groupid]==0); + return (geomgroup[groupid] == 0); } @@ -103,7 +103,7 @@ static int ray_eliminate(const mjModel* m, const mjData* d, int geomid, static mjtNum ray_quad(mjtNum a, mjtNum b, mjtNum c, mjtNum* x) { // compute determinant and check mjtNum det = b*b - a*c; - if (det=0) { + if (x[0] >= 0) { return x[0]; - } else if (x[1]>=0) { + } else if (x[1] >= 0) { return x[1]; } else { return -1; @@ -131,24 +131,24 @@ mjtNum ray_triangle(mjtNum v[][3], const mjtNum* lpnt, const mjtNum* lvec, const mjtNum* b0, const mjtNum* b1) { // dif = v[i] - lpnt mjtNum dif[3][3]; - 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++) { dif[i][j] = v[i][j] - lpnt[j]; } } // project difference vectors in normal plane mjtNum planar[3][2]; - for (int i=0; i<3; i++) { + for (int i=0; i < 3; i++) { planar[i][0] = mju_dot3(b0, dif[i]); planar[i][1] = mju_dot3(b1, dif[i]); } // reject if on the same side of any coordinate axis - if ((planar[0][0]>0 && planar[1][0]>0 && planar[2][0]>0) || - (planar[0][0]<0 && planar[1][0]<0 && planar[2][0]<0) || - (planar[0][1]>0 && planar[1][1]>0 && planar[2][1]>0) || - (planar[0][1]<0 && planar[1][1]<0 && planar[2][1]<0)) { + if ((planar[0][0] > 0 && planar[1][0] > 0 && planar[2][0] > 0) || + (planar[0][0] < 0 && planar[1][0] < 0 && planar[2][0] < 0) || + (planar[0][1] > 0 && planar[1][1] > 0 && planar[2][1] > 0) || + (planar[0][1] < 0 && planar[1][1] < 0 && planar[2][1] < 0)) { return -1; } @@ -158,14 +158,14 @@ mjtNum ray_triangle(mjtNum v[][3], const mjtNum* lpnt, const mjtNum* lvec, planar[0][1]-planar[2][1], planar[1][1]-planar[2][1]}; mjtNum b[2] = {-planar[2][0], -planar[2][1]}; mjtNum det = A[0]*A[3] - A[1]*A[2]; - if (mju_abs(det)1) { + if (t0 < 0 || t1 < 0|| t0+t1 > 1) { return -1; } @@ -176,7 +176,7 @@ mjtNum ray_triangle(mjtNum v[][3], const mjtNum* lpnt, const mjtNum* lvec, mjtNum nrm[3]; mju_cross(nrm, dif[0], dif[1]); // normal to triangle plane mjtNum denom = mju_dot3(lvec, nrm); - if (mju_abs(denom)-mjMINVAL) { + if (lvec[2] > -mjMINVAL) { return -1; } // intersection with plane const mjtNum x = -lpnt[2]/lvec[2]; - if (x<0) { + if (x < 0) { return -1; } mjtNum p0 = lpnt[0] + x*lvec[0]; mjtNum p1 = lpnt[1] + x*lvec[1]; // accept only within rendered rectangle - if ((size[0]<=0 || mju_abs(p0)<=size[0]) && - (size[1]<=0 || mju_abs(p1)<=size[1])) { + if ((size[0] <= 0 || mju_abs(p0) <= size[0]) && + (size[1] <= 0 || mju_abs(p1) <= size[1])) { return x; } else { return -1; @@ -237,7 +237,7 @@ static mjtNum ray_capsule(const mjtNum* pos, const mjtNum* mat, const mjtNum* si const mjtNum* pnt, const mjtNum* vec) { // bounding sphere test mjtNum ssz = size[0] + size[1]; - if (ray_sphere(pos, NULL, ssz*ssz, pnt, vec)<0) { + if (ray_sphere(pos, NULL, ssz*ssz, pnt, vec) < 0) { return -1; } @@ -257,8 +257,8 @@ static mjtNum ray_capsule(const mjtNum* pos, const mjtNum* mat, const mjtNum* si sol = ray_quad(a, b, c, xx); // make sure round solution is between flat sides - if (sol>=0 && mju_abs(lpnt[2]+sol*lvec[2])<=size[1]) { - if (x<0 || sol= 0 && mju_abs(lpnt[2]+sol*lvec[2]) <= size[1]) { + if (x < 0 || sol < x) { x = sol; } } @@ -271,9 +271,9 @@ static mjtNum ray_capsule(const mjtNum* pos, const mjtNum* mat, const mjtNum* si ray_quad(a, b, c, xx); // accept only top half of sphere - for (int i=0; i<2; i++) { - if (xx[i]>=0 && lpnt[2]+xx[i]*lvec[2]>=size[1]) { - if (x<0 || xx[i]= 0 && lpnt[2]+xx[i]*lvec[2] >= size[1]) { + if (x < 0 || xx[i] < x) { x = xx[i]; } } @@ -286,9 +286,9 @@ static mjtNum ray_capsule(const mjtNum* pos, const mjtNum* mat, const mjtNum* si ray_quad(a, b, c, xx); // accept only bottom half of sphere - for (int i=0; i<2; i++) { - if (xx[i]>=0 && lpnt[2]+xx[i]*lvec[2]<=-size[1]) { - if (x<0 || xx[i]= 0 && lpnt[2]+xx[i]*lvec[2] <= -size[1]) { + if (x < 0 || xx[i] < x) { x = xx[i]; } } @@ -326,7 +326,7 @@ static mjtNum ray_cylinder(const mjtNum* pos, const mjtNum* mat, const mjtNum* s const mjtNum* pnt, const mjtNum* vec) { // bounding sphere test mjtNum ssz = size[0]*size[0] + size[1]*size[1]; - if (ray_sphere(pos, NULL, ssz, pnt, vec)<0) { + if (ray_sphere(pos, NULL, ssz, pnt, vec) < 0) { return -1; } @@ -339,20 +339,20 @@ static mjtNum ray_cylinder(const mjtNum* pos, const mjtNum* mat, const mjtNum* s // flat sides int side; - if (mju_abs(lvec[2])>mjMINVAL) { - for (side=-1; side<=1; side+=2) { + if (mju_abs(lvec[2]) > mjMINVAL) { + for (side=-1; side <= 1; side+=2) { // soludion of: lpnt[2] + x*lvec[2] = side*height_size sol = (side*size[1]-lpnt[2])/lvec[2]; // process if non-negative - if (sol>=0) { + if (sol >= 0) { // intersection with horizontal face mjtNum p0 = lpnt[0] + sol*lvec[0]; mjtNum p1 = lpnt[1] + sol*lvec[1]; // accept within radius if (p0*p0 + p1*p1 <= size[0]*size[0]) { - if (x<0 || sol=0 && mju_abs(lpnt[2]+sol*lvec[2])<=size[1]) { - if (x<0 || sol= 0 && mju_abs(lpnt[2]+sol*lvec[2]) <= size[1]) { + if (x < 0 || sol < x) { x = sol; } } @@ -386,14 +386,14 @@ static mjtNum ray_box(const mjtNum* pos, const mjtNum* mat, const mjtNum* size, const mjtNum* pnt, const mjtNum* vec, mjtNum* all) { // clear all if (all) { - for (int i=0; i<6; i++) { + for (int i=0; i < 6; i++) { all[i] = -1; } } // bounding sphere test mjtNum ssz = size[0]*size[0] + size[1]*size[1] + size[2]*size[2]; - if (ray_sphere(pos, NULL, ssz, pnt, vec)<0) { + if (ray_sphere(pos, NULL, ssz, pnt, vec) < 0) { return -1; } @@ -412,23 +412,23 @@ static mjtNum ray_box(const mjtNum* pos, const mjtNum* mat, const mjtNum* size, mjtNum x = -1, sol; // loop over axes with non-zero vec - for (int i=0; i<3; i++) { - if (mju_abs(lvec[i])>mjMINVAL) { - for (int side=-1; side<=1; side+=2) { + for (int i=0; i < 3; i++) { + if (mju_abs(lvec[i]) > mjMINVAL) { + for (int side=-1; side <= 1; side+=2) { // soludion of: lpnt[i] + x*lvec[i] = side*size[i] sol = (side*size[i]-lpnt[i])/lvec[i]; // process if non-negative - if (sol>=0) { + if (sol >= 0) { // intersection with face mjtNum p0 = lpnt[iface[i][0]] + sol*lvec[iface[i][0]]; mjtNum p1 = lpnt[iface[i][1]] + sol*lvec[iface[i][1]]; // accept within rectangle - if (mju_abs(p0)<=size[iface[i][0]] && - mju_abs(p1)<=size[iface[i][1]]) { + if (mju_abs(p0) <= size[iface[i][0]] && + mju_abs(p1) <= size[iface[i][1]]) { // update - if (x<0 || solgeom_type[id]!=mjGEOM_HFIELD) { + if (m->geom_type[id] != mjGEOM_HFIELD) { mju_error("mj_rayHfield: geom with hfield type expected"); } @@ -484,7 +484,7 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id, // check top box: done if no intersection mjtNum all[6]; mjtNum top_intersect = ray_box(top_pos, d->geom_xmat+9*id, top_size, pnt, vec, all); - if (top_intersect<0) { + if (top_intersect < 0) { return x; } @@ -494,9 +494,9 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id, // construct basis vectors of normal plane mjtNum b0[3] = {1, 1, 1}, b1[3]; - if (mju_abs(lvec[0])>=mju_abs(lvec[1]) && mju_abs(lvec[0])>=mju_abs(lvec[2])) { + if (mju_abs(lvec[0]) >= mju_abs(lvec[1]) && mju_abs(lvec[0]) >= mju_abs(lvec[2])) { b0[0] = 0; - } else if (mju_abs(lvec[1])>=mju_abs(lvec[2])) { + } else if (mju_abs(lvec[1]) >= mju_abs(lvec[2])) { b0[1] = 0; } else { b0[2] = 0; @@ -508,8 +508,8 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id, // find ray segment intersecting top box mjtNum seg[2] = {0, top_intersect}; - for (int i=0; i<6; i++) { - if (all[i]>seg[1]) { + for (int i=0; i < 6; i++) { + if (all[i] > seg[1]) { seg[0] = top_intersect; seg[1] = all[i]; } @@ -519,7 +519,7 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id, mjtNum dx = (2.0*size[0]) / (ncol-1); mjtNum dy = (2.0*size[1]) / (nrow-1); mjtNum SX[2], SY[2]; - for (int i=0; i<2; i++) { + for (int i=0; i < 2; i++) { SX[i] = (lpnt[0] + seg[i]*lvec[0] + size[0]) / dx; SY[i] = (lpnt[1] + seg[i]*lvec[1] + size[1]) / dy; } @@ -531,8 +531,8 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id, int rmax = mjMIN(nrow-1, (int)mju_ceil(mjMAX(SY[0], SY[1]))+1); // check triangles within bounds - for (int r=rmin; r=0 && (x<0 || sol= 0 && (x < 0 || sol < x)) { x = sol; } @@ -551,15 +551,15 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id, {dx*c-size[0], dy*(r+1)-size[1], data[(r+1)*ncol+c]*size[2]} }; sol = ray_triangle(vb, lpnt, lvec, b0, b1); - if (sol>=0 && (x<0 || sol= 0 && (x < 0 || sol < x)) { x = sol; } } } // check viable sides of top box - for (int i=0; i<4; i++) { - if (all[i]>=0 && (all[i]= 0 && (all[i] < x || x < 0)) { // normalized height of intersection point mjtNum z = (lpnt[2] + all[i]*lvec[2]) / size[2]; @@ -567,19 +567,19 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id, mjtNum y, y0, z0, z1; // side normal to x-axis - if (i<2) { + if (i < 2) { y = (lpnt[1] + all[i]*lvec[1] + size[1]) / dy; y0 = mjMAX(0, mjMIN(nrow-2, mju_floor(y))); - z0 = (mjtNum)data[mju_round(y0)*nrow + (i==1 ? ncol-1 : 0)]; - z1 = (mjtNum)data[mju_round(y0+1)*nrow + (i==1 ? ncol-1 : 0)]; + z0 = (mjtNum)data[mju_round(y0)*nrow + (i == 1 ? ncol-1 : 0)]; + z1 = (mjtNum)data[mju_round(y0+1)*nrow + (i == 1 ? ncol-1 : 0)]; } // side normal to y-axis else { y = (lpnt[0] + all[i]*lvec[0] + size[0]) / dx; y0 = mjMAX(0, mjMIN(ncol-2, mju_floor(y))); - z0 = (mjtNum)data[mju_round(y0) + (i==3 ? (nrow-1)*ncol : 0)]; - z1 = (mjtNum)data[mju_round(y0+1) + (i==3 ? (nrow-1)*ncol : 0)]; + z0 = (mjtNum)data[mju_round(y0) + (i == 3 ? (nrow-1)*ncol : 0)]; + z1 = (mjtNum)data[mju_round(y0+1) + (i == 3 ? (nrow-1)*ncol : 0)]; } // check if point is below line segment @@ -631,7 +631,7 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt, const mjtNum* bvh = m->bvh_aabb + 6*bvhadr; const int* child = m->bvh_child + 2*bvhadr; - if (meshid==-1) { + if (meshid == -1) { mju_error("mju_rayTree: mesh id of geom %d is -1", meshid); // SHOULD NOT OCCUR } @@ -647,9 +647,9 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt, // construct basis vectors of normal plane mjtNum b0[3] = {1, 1, 1}, b1[3]; - if (mju_abs(lvec[0])>=mju_abs(lvec[1]) && mju_abs(lvec[0])>=mju_abs(lvec[2])) { + if (mju_abs(lvec[0]) >= mju_abs(lvec[1]) && mju_abs(lvec[0]) >= mju_abs(lvec[2])) { b0[0] = 0; - } else if (mju_abs(lvec[1])>=mju_abs(lvec[2])) { + } else if (mju_abs(lvec[1]) >= mju_abs(lvec[2])) { b0[1] = 0; } else { b0[2] = 0; @@ -687,8 +687,8 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt, // convert to mjtNum mjtNum v[3][3]; - 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++) { v[i][j] = (mjtNum)vf[i][j]; } } @@ -697,7 +697,7 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt, sol = ray_triangle(v, lpnt, lvec, b0, b1); // update - if (sol>=0 && (x<0 || sol= 0 && (x < 0 || sol < x)) { x = sol; } continue; @@ -707,7 +707,7 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt, d->bvh_active[node + bvhadr] = 1; // add children to the stack - for (int i=0; i<2; i++) { + for (int i=0; i < 2; i++) { if (child[2*node+i] != -1) { if (nstack >= mjMAXTREEDEPTH) mju_error("BVH stack depth exceeded in geom %d.", id); stack[nstack] = child[2*node+i]; @@ -723,12 +723,12 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt, mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int id, const mjtNum* pnt, const mjtNum* vec) { // check geom type - if (m->geom_type[id]!=mjGEOM_MESH) { + if (m->geom_type[id] != mjGEOM_MESH) { mju_error("mj_rayMesh: geom with mesh type expected"); } // bounding box test - if (ray_box(d->geom_xpos+3*id, d->geom_xmat+9*id, m->geom_size+3*id, pnt, vec, NULL)<0) { + if (ray_box(d->geom_xpos+3*id, d->geom_xmat+9*id, m->geom_size+3*id, pnt, vec, NULL) < 0) { return -1; } @@ -772,15 +772,15 @@ mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert, const mjtNum* pnt, const mjtNum* vec, int vertid[1]) { // compute bounding box mjtNum box[3][2] = {{0, 0}, {0, 0}, {0, 0}}; - for (int i=0; ivert[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]=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; @@ -816,7 +816,7 @@ mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert, mjtNum x = -1, sol; // process all faces - for (int i=0; i=0 && (x<0 || sol= 0 && (x < 0 || sol < x)) { x = sol; // construct intersection point @@ -845,9 +845,9 @@ mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert, // find nearest vertex mjtNum dist = mju_dist3(intersect, v[0]); *vertid = face[3*i]; - for (int j=1; j<3; j++) { + for (int j=1; j < 3; j++) { mjtNum newdist = mju_dist3(intersect, v[j]); - if (newdist aabb[3+j]) { return 0; } @@ -893,7 +893,7 @@ mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum mjtNum dist, newdist; // check vector length - if (mju_norm3(vec)ngeom; i++) { + for (int i=0; i < m->ngeom; i++) { if (!ray_eliminate(m, d, i, geomgroup, flg_static, bodyexclude)) { // handle mesh and hfield separately - if (m->geom_type[i]==mjGEOM_MESH) { + if (m->geom_type[i] == mjGEOM_MESH) { newdist = mj_rayMesh(m, d, i, pnt, vec); - } else if (m->geom_type[i]==mjGEOM_HFIELD) { + } else if (m->geom_type[i] == mjGEOM_HFIELD) { newdist = mj_rayHfield(m, d, i, pnt, vec); } @@ -918,7 +918,7 @@ mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum } // update if closer intersection found - if (newdist>=0 && (newdist= 0 && (newdist < dist || dist < 0)) { dist = newdist; *geomid = i; } @@ -938,17 +938,17 @@ void mju_multiRayPrepare(const mjModel* m, const mjData* d, const mjtNum pnt[3], } // compute eliminate flag for all geoms - for (int geomid=0; geomidngeom; geomid++) + for (int geomid=0; geomid < m->ngeom; geomid++) geom_eliminate[geomid] = ray_eliminate(m, d, geomid, geomgroup, flg_static, bodyexclude); - for (int b=0; bnbody; b++) { + for (int b=0; b < m->nbody; b++) { // skip precomputation if no bounding volume is available if (m->body_bvhadr[b] == -1) { continue; } // loop over child geoms, compute bounding angles - for (int i=0; ibody_geomnum[b]; i++) { + for (int i=0; i < m->body_geomnum[b]; i++) { int g = i + m->body_geomadr[b]; mjtNum AABB[4] = {mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL}; mjtNum* aabb = m->geom_aabb + 6*g; @@ -961,7 +961,7 @@ void mju_multiRayPrepare(const mjModel* m, const mjData* d, const mjtNum pnt[3], } // add to geom_eliminate if distance of bounding sphere is above cutoff - if (mju_dist3(d->geom_xpos+3*g, pnt)>cutoff+m->geom_rbound[g]) { + if (mju_dist3(d->geom_xpos+3*g, pnt) > cutoff+m->geom_rbound[g]) { geom_eliminate[g] = 1; continue; } @@ -975,7 +975,7 @@ void mju_multiRayPrepare(const mjModel* m, const mjData* d, const mjtNum pnt[3], } // loop over box vertices, compute spherical aperture - for (int v=0; v<8; v++) { + for (int v=0; v < 8; v++) { mjtNum vert[3], box[3]; vert[0] = (v&1 ? aabb[0]+aabb[3] : aabb[0]-aabb[3]); vert[1] = (v&2 ? aabb[1]+aabb[4] : aabb[1]-aabb[4]); @@ -1020,7 +1020,7 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co mjtNum dist, newdist; // check vector length - if (mju_norm3(vec)nbody; b++) { + for (int b=0; b < m->nbody; b++) { // exclude body using bounding sphere test if (m->body_bvhadr[b] != -1) { mjtNum* pos = m->bvh_aabb + 6*m->body_bvhadr[b]; @@ -1041,13 +1041,13 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co mjtNum* size = pos + 3; mjtNum ssz = size[0]*size[0] + size[1]*size[1] + size[2]*size[2]; mju_add3(center, pos, d->xipos+3*b); - if (ray_sphere(center, NULL, ssz, pnt, vec)<0) { + if (ray_sphere(center, NULL, ssz, pnt, vec) < 0) { continue; } } // loop over geoms if bounding sphere test fails - for (int g=0; gbody_geomnum[b]; g++) { + for (int g=0; g < m->body_geomnum[b]; g++) { int i = m->body_geomadr[b] + g; if (ray_eliminate[i]) { continue; @@ -1055,16 +1055,16 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co // exclude geom using bounding angles if (m->body_bvhadr[b] != -1) { - if (azimuth<(geom_ba+4*i)[0] || elevation<(geom_ba+4*i)[1] || - azimuth>(geom_ba+4*i)[2] || elevation>(geom_ba+4*i)[3]) { + if (azimuth < (geom_ba+4*i)[0] || elevation < (geom_ba+4*i)[1] || + azimuth > (geom_ba+4*i)[2] || elevation > (geom_ba+4*i)[3]) { continue; } } // handle mesh and hfield separately - if (m->geom_type[i]==mjGEOM_MESH) { + if (m->geom_type[i] == mjGEOM_MESH) { newdist = mj_rayMesh(m, d, i, pnt, vec); - } else if (m->geom_type[i]==mjGEOM_HFIELD) { + } else if (m->geom_type[i] == mjGEOM_HFIELD) { newdist = mj_rayHfield(m, d, i, pnt, vec); } @@ -1075,7 +1075,7 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co } // update if closer intersection found - if (newdist>=0 && (newdist= 0 && (newdist < dist || dist < 0)) { dist = newdist; *geomid = i; } @@ -1101,7 +1101,7 @@ void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* cutoff, geom_ba, geom_eliminate); // loop over rays - for (int i=0; insensor; i++) { - if (m->sensor_needstage[i]==stage && m->sensor_noise[i]>0) { + for (int i=0; i < m->nsensor; i++) { + if (m->sensor_needstage[i] == stage && m->sensor_noise[i] > 0) { // get sensor info adr = m->sensor_adr[i]; dim = m->sensor_dim[i]; noise = m->sensor_noise[i]; // real or positive: add noise directly, with clamp for positive - if (m->sensor_datatype[i]==mjDATATYPE_REAL || - m->sensor_datatype[i]==mjDATATYPE_POSITIVE) { - for (int j=0; jsensor_datatype[i] == mjDATATYPE_REAL || + m->sensor_datatype[i] == mjDATATYPE_POSITIVE) { + for (int j=0; j < dim; j++) { // get random numbers; use only the first one rnd[0] = mju_standardNormal(rnd+1); // positive - if (m->sensor_datatype[i]==mjDATATYPE_POSITIVE) { + if (m->sensor_datatype[i] == mjDATATYPE_POSITIVE) { // add noise only if positive, keep it positive - if (d->sensordata[adr+j]>0) { + if (d->sensordata[adr+j] > 0) { d->sensordata[adr+j] = mjMAX(0, d->sensordata[adr+j]+rnd[0]*noise); } } @@ -82,14 +82,14 @@ static void add_noise(const mjModel* m, mjData* d, mjtStage stage) { mju_axisAngle2Quat(quat, rnd+1, rnd[0]); // axis - if (m->sensor_datatype[i]==mjDATATYPE_AXIS) { + if (m->sensor_datatype[i] == mjDATATYPE_AXIS) { // apply quaternion rotation to axis, assign mju_rotVecQuat(res, d->sensordata+adr, quat); mju_copy3(d->sensordata+adr, res); } // quaternion - else if (m->sensor_datatype[i]==mjDATATYPE_QUATERNION) { + else if (m->sensor_datatype[i] == mjDATATYPE_QUATERNION) { // apply quaternion rotation to quaternion, assign mju_mulQuat(d->sensordata+adr, d->sensordata+adr, quat); } @@ -108,22 +108,22 @@ static void add_noise(const mjModel* m, mjData* d, mjtStage stage) { // apply cutoff after each stage static void apply_cutoff(const mjModel* m, mjData* d, mjtStage stage) { // process sensors matching stage and having positive cutoff - for (int i=0; insensor; i++) { - if (m->sensor_needstage[i]==stage && m->sensor_cutoff[i]>0) { + for (int i=0; i < m->nsensor; i++) { + if (m->sensor_needstage[i] == stage && m->sensor_cutoff[i] > 0) { // get sensor info int adr = m->sensor_adr[i]; int dim = m->sensor_dim[i]; mjtNum cutoff = m->sensor_cutoff[i]; // process all dimensions - for (int j=0; jsensor_datatype[i]==mjDATATYPE_REAL) { + if (m->sensor_datatype[i] == mjDATATYPE_REAL) { d->sensordata[adr+j] = mju_clip(d->sensordata[adr+j], -cutoff, cutoff); } // positive: apply on positive side only - else if (m->sensor_datatype[i]==mjDATATYPE_POSITIVE) { + else if (m->sensor_datatype[i] == mjDATATYPE_POSITIVE) { d->sensordata[adr+j] = mju_min(cutoff, d->sensordata[adr+j]); } } @@ -201,13 +201,13 @@ void mj_sensorPos(const mjModel* m, mjData* d) { } // process sensors matching stage - for (int i=0; insensor; i++) { + for (int i=0; i < m->nsensor; i++) { // skip sensor plugins -- these are handled after builtin sensor types if (m->sensor_type[i] == mjSENS_PLUGIN) { continue; } - if (m->sensor_needstage[i]==mjSTAGE_POS) { + if (m->sensor_needstage[i] == mjSTAGE_POS) { // get sensor info objtype = m->sensor_objtype[i]; objid = m->sensor_objid[i]; @@ -247,8 +247,8 @@ void mj_sensorPos(const mjModel* m, mjData* d) { case mjSENS_JOINTLIMITPOS: // jointlimitpos d->sensordata[adr] = 0; - for (int j=ne+nf; jefc_type[j]==mjCNSTR_LIMIT_JOINT && d->efc_id[j]==objid) { + for (int j=ne+nf; j < nefc; j++) { + if (d->efc_type[j] == mjCNSTR_LIMIT_JOINT && d->efc_id[j] == objid) { d->sensordata[adr] = d->efc_pos[j] - d->efc_margin[j]; break; } @@ -257,8 +257,8 @@ void mj_sensorPos(const mjModel* m, mjData* d) { case mjSENS_TENDONLIMITPOS: // tendonlimitpos d->sensordata[adr] = 0; - for (int j=ne+nf; jefc_type[j]==mjCNSTR_LIMIT_TENDON && d->efc_id[j]==objid) { + for (int j=ne+nf; j < nefc; j++) { + if (d->efc_type[j] == mjCNSTR_LIMIT_TENDON && d->efc_id[j] == objid) { d->sensordata[adr] = d->efc_pos[j] - d->efc_margin[j]; break; } @@ -274,7 +274,7 @@ void mj_sensorPos(const mjModel* m, mjData* d) { // reference frame unspecified: global frame if (refid == -1) { - if (m->sensor_type[i]==mjSENS_FRAMEPOS) { + if (m->sensor_type[i] == mjSENS_FRAMEPOS) { mju_copy3(d->sensordata+adr, xpos); } else { // offset = (0 or 1 or 2) for (x or y or z)-axis sensors, respectively @@ -288,7 +288,7 @@ void mj_sensorPos(const mjModel* m, mjData* d) { // reference frame specified else { get_xpos_xmat(d, reftype, refid, i, &xpos_ref, &xmat_ref); - if (m->sensor_type[i]==mjSENS_FRAMEPOS) { + if (m->sensor_type[i] == mjSENS_FRAMEPOS) { mju_sub3(rvec, xpos, xpos_ref); mju_rotVecMatT(d->sensordata+adr, rvec, xmat_ref); } else { @@ -301,25 +301,25 @@ void mj_sensorPos(const mjModel* m, mjData* d) { break; case mjSENS_FRAMEQUAT: // framequat - { - // get global object quaternion - mjtNum objquat[4]; - get_xquat(m, d, objtype, objid, i, objquat); + { + // get global object quaternion + mjtNum objquat[4]; + get_xquat(m, d, objtype, objid, i, objquat); - // reference frame unspecified: copy object quaternion - if (refid == -1) { - mju_copy4(d->sensordata+adr, objquat); - } else { - // reference frame specified, get global reference quaternion - mjtNum refquat[4]; - get_xquat(m, d, reftype, refid, i, refquat); + // reference frame unspecified: copy object quaternion + if (refid == -1) { + mju_copy4(d->sensordata+adr, objquat); + } else { + // reference frame specified, get global reference quaternion + mjtNum refquat[4]; + get_xquat(m, d, reftype, refid, i, refquat); - // relative quaternion - mju_negQuat(refquat, refquat); - mju_mulQuat(d->sensordata+adr, refquat, objquat); - } + // relative quaternion + mju_negQuat(refquat, refquat); + mju_mulQuat(d->sensordata+adr, refquat, objquat); } - break; + } + break; case mjSENS_SUBTREECOM: // subtreecom mju_copy3(d->sensordata+adr, d->subtree_com+3*objid); @@ -352,14 +352,14 @@ void mj_sensorPos(const mjModel* m, mjData* d) { // compute plugin sensor values if (m->nplugin) { const int nslot = mjp_pluginCount(); - for (int i=0; inplugin; i++) { + for (int i=0; i < m->nplugin; i++) { const int slot = m->plugin[i]; const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot); if (!plugin) { mju_error("invalid plugin slot: %d", slot); } if ((plugin->capabilityflags & mjPLUGIN_SENSOR) && - (plugin->needstage==mjSTAGE_POS || plugin->needstage==mjSTAGE_NONE)) { + (plugin->needstage == mjSTAGE_POS || plugin->needstage == mjSTAGE_NONE)) { if (!plugin->compute) { mju_error("`compute` is a null function pointer for plugin at slot %d", slot); } @@ -387,13 +387,13 @@ void mj_sensorVel(const mjModel* m, mjData* d) { // process sensors matching stage int subtreeVel = 0; - for (int i=0; insensor; i++) { + for (int i=0; i < m->nsensor; i++) { // skip sensor plugins -- these are handled after builtin sensor types if (m->sensor_type[i] == mjSENS_PLUGIN) { continue; } - if (m->sensor_needstage[i]==mjSTAGE_VEL) { + if (m->sensor_needstage[i] == mjSTAGE_VEL) { // get sensor info type = m->sensor_type[i]; objtype = m->sensor_objtype[i]; @@ -403,10 +403,10 @@ void mj_sensorVel(const mjModel* m, mjData* d) { adr = m->sensor_adr[i]; // call mj_subtreeVel when first relevant sensor is encountered - if (subtreeVel==0 && - (type==mjSENS_SUBTREELINVEL || - type==mjSENS_SUBTREEANGMOM || - type==mjSENS_USER)) { + if (subtreeVel == 0 && + (type == mjSENS_SUBTREELINVEL || + type == mjSENS_SUBTREEANGMOM || + type == mjSENS_USER)) { // compute subtree_linvel, subtree_angmom mj_subtreeVel(m, d); @@ -450,8 +450,8 @@ void mj_sensorVel(const mjModel* m, mjData* d) { case mjSENS_JOINTLIMITVEL: // jointlimitvel d->sensordata[adr] = 0; - for (int j=ne+nf; jefc_type[j]==mjCNSTR_LIMIT_JOINT && d->efc_id[j]==objid) { + for (int j=ne+nf; j < nefc; j++) { + if (d->efc_type[j] == mjCNSTR_LIMIT_JOINT && d->efc_id[j] == objid) { d->sensordata[adr] = d->efc_vel[j]; break; } @@ -460,8 +460,8 @@ void mj_sensorVel(const mjModel* m, mjData* d) { case mjSENS_TENDONLIMITVEL: // tendonlimitvel d->sensordata[adr] = 0; - for (int j=ne+nf; jefc_type[j]==mjCNSTR_LIMIT_TENDON && d->efc_id[j]==objid) { + for (int j=ne+nf; j < nefc; j++) { + if (d->efc_type[j] == mjCNSTR_LIMIT_TENDON && d->efc_id[j] == objid) { d->sensordata[adr] = d->efc_vel[j]; break; } @@ -495,7 +495,7 @@ void mj_sensorVel(const mjModel* m, mjData* d) { } // copy linear or angular component - if (m->sensor_type[i]==mjSENS_FRAMELINVEL) { + if (m->sensor_type[i] == mjSENS_FRAMELINVEL) { mju_copy3(d->sensordata+adr, xvel+3); } else { mju_copy3(d->sensordata+adr, xvel); @@ -533,13 +533,13 @@ void mj_sensorVel(const mjModel* m, mjData* d) { // trigger computation of plugins if (m->nplugin) { const int nslot = mjp_pluginCount(); - for (int i=0; inplugin; i++) { + for (int i=0; i < m->nplugin; i++) { const int slot = m->plugin[i]; const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot); if (!plugin) { mju_error("invalid plugin slot: %d", slot); } - if ((plugin->capabilityflags & mjPLUGIN_SENSOR) && plugin->needstage==mjSTAGE_VEL) { + if ((plugin->capabilityflags & mjPLUGIN_SENSOR) && plugin->needstage == mjSTAGE_VEL) { if (!plugin->compute) { mju_error("`compute` is null for plugin at slot %d", slot); } @@ -576,13 +576,13 @@ void mj_sensorAcc(const mjModel* m, mjData* d) { // process sensors matching stage int rnePost = 0; - for (int i=0; insensor; i++) { + for (int i=0; i < m->nsensor; i++) { // skip sensor plugins -- these are handled after builtin sensor types if (m->sensor_type[i] == mjSENS_PLUGIN) { continue; } - if (m->sensor_needstage[i]==mjSTAGE_ACC) { + if (m->sensor_needstage[i] == mjSTAGE_ACC) { // get sensor info type = m->sensor_type[i]; objtype = m->sensor_objtype[i]; @@ -590,11 +590,11 @@ void mj_sensorAcc(const mjModel* m, mjData* d) { adr = m->sensor_adr[i]; // call mj_rnePostConstraint when first relevant sensor is encountered - if (rnePost==0 && - type!=mjSENS_TOUCH && - type!=mjSENS_ACTUATORFRC && - type!=mjSENS_JOINTLIMITFRC && - type!=mjSENS_TENDONLIMITFRC) { + if (rnePost == 0 && + type != mjSENS_TOUCH && + type != mjSENS_ACTUATORFRC && + type != mjSENS_JOINTLIMITFRC && + type != mjSENS_TENDONLIMITFRC) { // compute cacc, cfrc_int, cfrc_ext mj_rnePostConstraint(m, d); @@ -613,19 +613,19 @@ void mj_sensorAcc(const mjModel* m, mjData* d) { d->sensordata[adr] = 0; // find contacts in sensor zone, add normal forces - for (int j=0; jncon; j++) { + for (int j=0; j < d->ncon; j++) { // contact pointer, contacting bodies con = d->contact + j; body1 = m->geom_bodyid[con->geom1]; body2 = m->geom_bodyid[con->geom2]; // select contacts involving sensorized body - if (con->efc_address>=0 && (bodyid==body1 || bodyid==body2)) { + if (con->efc_address >= 0 && (bodyid == body1 || bodyid == body2)) { // get contact force:torque in contact frame mj_contactForce(m, d, j, conforce); // nothing to do if normal is zero - if (conforce[0]<=0) { + if (conforce[0] <= 0) { continue; } @@ -634,7 +634,7 @@ void mj_sensorAcc(const mjModel* m, mjData* d) { mju_normalize3(conray); // flip ray direction if sensor is on body2 - if (bodyid==body2) { + if (bodyid == body2) { mju_scl3(conray, conray, -1); } @@ -688,8 +688,8 @@ void mj_sensorAcc(const mjModel* m, mjData* d) { case mjSENS_JOINTLIMITFRC: // jointlimitfrc d->sensordata[adr] = 0; - for (int j=ne+nf; jefc_type[j]==mjCNSTR_LIMIT_JOINT && d->efc_id[j]==objid) { + for (int j=ne+nf; j < nefc; j++) { + if (d->efc_type[j] == mjCNSTR_LIMIT_JOINT && d->efc_id[j] == objid) { d->sensordata[adr] = d->efc_force[j]; break; } @@ -698,8 +698,8 @@ void mj_sensorAcc(const mjModel* m, mjData* d) { case mjSENS_TENDONLIMITFRC: // tendonlimitfrc d->sensordata[adr] = 0; - for (int j=ne+nf; jefc_type[j]==mjCNSTR_LIMIT_TENDON && d->efc_id[j]==objid) { + for (int j=ne+nf; j < nefc; j++) { + if (d->efc_type[j] == mjCNSTR_LIMIT_TENDON && d->efc_id[j] == objid) { d->sensordata[adr] = d->efc_force[j]; break; } @@ -712,7 +712,7 @@ void mj_sensorAcc(const mjModel* m, mjData* d) { mj_objectAcceleration(m, d, objtype, objid, tmp, 0); // copy linear or angular component - if (m->sensor_type[i]==mjSENS_FRAMELINACC) { + if (m->sensor_type[i] == mjSENS_FRAMELINACC) { mju_copy3(d->sensordata+adr, tmp+3); } else { mju_copy3(d->sensordata+adr, tmp); @@ -742,13 +742,13 @@ void mj_sensorAcc(const mjModel* m, mjData* d) { // trigger computation of plugins if (m->nplugin) { const int nslot = mjp_pluginCount(); - for (int i=0; inplugin; i++) { + for (int i=0; i < m->nplugin; i++) { const int slot = m->plugin[i]; const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot); if (!plugin) { mju_error("invalid plugin slot: %d", slot); } - if ((plugin->capabilityflags & mjPLUGIN_SENSOR) && plugin->needstage==mjSTAGE_ACC) { + if ((plugin->capabilityflags & mjPLUGIN_SENSOR) && plugin->needstage == mjSTAGE_ACC) { if (!plugin->compute) { mju_error("`compute` is null for plugin at slot %d", slot); } @@ -787,14 +787,14 @@ void mj_energyPos(const mjModel* m, mjData* d) { // init potential energy: -sum_i body(i).mass * mju_dot(body(i).pos, gravity) d->energy[0] = 0; if (!mjDISABLED(mjDSBL_GRAVITY)) { - for (int i=1; inbody; i++) { + for (int i=1; i < m->nbody; i++) { d->energy[0] -= m->body_mass[i] * mju_dot3(m->opt.gravity, d->xipos+3*i); } } // add joint-level springs if (!mjDISABLED(mjDSBL_PASSIVE)) { - for (int i=0; injnt; i++) { + for (int i=0; i < m->njnt; i++) { stiffness = m->jnt_stiffness[i]; padr = m->jnt_qposadr[i]; @@ -825,7 +825,7 @@ void mj_energyPos(const mjModel* m, mjData* d) { // add tendon-level springs if (!mjDISABLED(mjDSBL_PASSIVE)) { - for (int i=0; intendon; i++) { + for (int i=0; i < m->ntendon; i++) { stiffness = m->tendon_stiffness[i]; mjtNum length = d->ten_length[i]; mjtNum displacement = 0; diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index a6a754a1..1fbd1172 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -42,14 +42,14 @@ static void set0(mjModel* m, mjData* d) { // save camera and light mode, set to fixed if (m->ncam) { cammode = (int*) mj_stackAlloc(d, m->ncam); - for (int i=0; incam; i++) { + for (int i=0; i < m->ncam; i++) { cammode[i] = m->cam_mode[i]; m->cam_mode[i] = mjCAMLIGHT_FIXED; } } if (m->nlight) { lightmode = (int*) mj_stackAlloc(d, m->nlight); - for (int i=0; inlight; i++) { + for (int i=0; i < m->nlight; i++) { lightmode[i] = m->light_mode[i]; m->light_mode[i] = mjCAMLIGHT_FIXED; } @@ -63,7 +63,7 @@ static void set0(mjModel* m, mjData* d) { mj_crbSkip(m, d, 0); // save dof_M0 - for (int i=0; idof_M0[i] = d->qM[m->dof_Madr[i]]; } @@ -73,10 +73,10 @@ static void set0(mjModel* m, mjData* d) { mj_transmission(m, d); // restore camera and light mode - for (int i=0; incam; i++) { + for (int i=0; i < m->ncam; i++) { m->cam_mode[i] = cammode[i]; } - for (int i=0; inlight; i++) { + for (int i=0; i < m->nlight; i++) { m->light_mode[i] = lightmode[i]; } @@ -86,7 +86,7 @@ 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++) { if (nv) { // inverse spatial inertia: A = J*inv(M)*J' mj_jacBodyCom(m, d, jac, jac+3*nv, i); @@ -100,13 +100,13 @@ static void set0(mjModel* m, mjData* d) { } // compute dof_invweight0 - for (int i=0; injnt; i++) { + for (int i=0; i < m->njnt; i++) { id = m->jnt_dofadr[i]; // get number of components - if (m->jnt_type[i]==mjJNT_FREE) { + if (m->jnt_type[i] == mjJNT_FREE) { dnum = 6; - } else if (m->jnt_type[i]==mjJNT_BALL) { + } else if (m->jnt_type[i] == mjJNT_BALL) { dnum = 3; } else { dnum = 1; @@ -115,7 +115,7 @@ static void set0(mjModel* m, mjData* d) { // inverse joint inertia: A = J*inv(M)*J' if (nv) { mju_zero(jac, dnum*nv); - for (int j=0; jdof_invweight0[id] = m->dof_invweight0[id+1] = m->dof_invweight0[id+2] = - (A[0] + A[7] + A[14])/3; + (A[0] + A[7] + A[14])/3; m->dof_invweight0[id+3] = m->dof_invweight0[id+4] = m->dof_invweight0[id+5] = - (A[21] + A[28] + A[35])/3; - } else if (dnum==3) + (A[21] + A[28] + A[35])/3; + } else if (dnum == 3) m->dof_invweight0[id] = m->dof_invweight0[id+1] = m->dof_invweight0[id+2] = - (A[0] + A[4] + A[8])/3; + (A[0] + A[4] + A[8])/3; else { m->dof_invweight0[id] = A[0]; } @@ -138,12 +138,12 @@ static void set0(mjModel* m, mjData* d) { // compute tendon_invweight0 if (nv) { - 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); int end = d->ten_J_rowadr[i] + d->ten_J_rownnz[i]; - for (int j=d->ten_J_rowadr[i]; jten_J_rowadr[i]; j < end; j++) { tmp[d->ten_J_colind[j]] = d->ten_J[j]; } } else { @@ -156,24 +156,24 @@ static void set0(mjModel* m, mjData* d) { } // compute actuator_acc0 - for (int i=0; inu; i++) { + for (int i=0; i < m->nu; i++) { mj_solveM(m, d, tmp, d->actuator_moment+i*nv, 1); m->actuator_acc0[i] = mju_norm(tmp, nv); } } else { - for (int i=0; inu; i++) { + for (int i=0; i < m->nu; i++) { m->actuator_acc0[i] = 0; } } // compute missing eq_data for body constraints - for (int i=0; ineq; i++) { + for (int i=0; i < m->neq; i++) { // get ids id1 = m->eq_obj1id[i]; id2 = m->eq_obj2id[i]; // connect constraint - if (m->eq_type[i]==mjEQ_CONNECT) { + if (m->eq_type[i] == mjEQ_CONNECT) { // pos = anchor position in global frame mj_local2Global(d, pos, 0, m->eq_data+mjNEQDATA*i, 0, id1, 0); @@ -183,7 +183,7 @@ static void set0(mjModel* m, mjData* d) { } // weld constraint - else if (m->eq_type[i]==mjEQ_WELD) { + else if (m->eq_type[i] == mjEQ_WELD) { // skip if user has set any quaternion data if (m->eq_data[mjNEQDATA*i+6] || m->eq_data[mjNEQDATA*i+7] || @@ -208,28 +208,28 @@ static void set0(mjModel* m, mjData* d) { } // camera compos0, pos0, mat0 - for (int i=0; incam; i++) { + for (int i=0; i < m->ncam; i++) { // get body ids id = m->cam_bodyid[i]; // camera body id1 = m->cam_targetbodyid[i]; // target body // compute positional offsets mju_sub3(m->cam_pos0+3*i, d->cam_xpos+3*i, d->xpos+3*id); - mju_sub3(m->cam_poscom0+3*i, d->cam_xpos+3*i, d->subtree_com+ (id1>=0 ? 3*id1 : 3*id)); + mju_sub3(m->cam_poscom0+3*i, d->cam_xpos+3*i, d->subtree_com+ (id1 >= 0 ? 3*id1 : 3*id)); // copy mat mju_copy(m->cam_mat0+9*i, d->cam_xmat+9*i, 9); } // light compos0, pos0, dir0 - for (int i=0; inlight; i++) { + for (int i=0; i < m->nlight; i++) { // get body ids id = m->light_bodyid[i]; // light body id1 = m->light_targetbodyid[i]; // target body // compute positional offsets mju_sub3(m->light_pos0+3*i, d->light_xpos+3*i, d->xpos+3*id); - mju_sub3(m->light_poscom0+3*i, d->light_xpos+3*i, d->subtree_com+ (id1>=0 ? 3*id1 : 3*id)); + mju_sub3(m->light_poscom0+3*i, d->light_xpos+3*i, d->subtree_com+ (id1 >= 0 ? 3*id1 : 3*id)); // copy dir mju_copy3(m->light_dir0+3*i, d->light_xdir+3*i); @@ -242,7 +242,7 @@ static void set0(mjModel* m, mjData* d) { // accumulate bounding box static void updateBox(mjtNum* xmin, mjtNum* xmax, mjtNum* pos, mjtNum radius) { - for (int i=0; i<3; i++) { + for (int i=0; i < 3; i++) { xmin[i] = mjMIN(xmin[i], pos[i] - radius); xmax[i] = mjMAX(xmax[i], pos[i] + radius); } @@ -258,22 +258,22 @@ static void setStat(mjModel* m, mjData* d) { mjtNum* body = mj_stackAlloc(d, m->nbody); // compute bounding box of bodies, joint centers, geoms and sites - for (int i=1; inbody; i++) { + for (int i=1; i < m->nbody; i++) { updateBox(xmin, xmax, d->xpos+3*i, 0); updateBox(xmin, xmax, d->xipos+3*i, 0); } - for (int i=0; injnt; i++) { + for (int i=0; i < m->njnt; i++) { updateBox(xmin, xmax, d->xanchor+3*i, 0); } - for (int i=0; insite; i++) { + for (int i=0; i < m->nsite; i++) { updateBox(xmin, xmax, d->site_xpos+3*i, 0); } - for (int i=0; ingeom; i++) { + for (int i=0; i < m->ngeom; i++) { // set rbound: regular geom rbound, or 0.1 of plane or hfield max size rbound = 0; if (m->geom_rbound[i] > 0) { rbound = m->geom_rbound[i]; - } else if (m->geom_type[i]==mjGEOM_PLANE) { + } else if (m->geom_type[i] == mjGEOM_PLANE) { // finite in at least one direction if (m->geom_size[3*i] || m->geom_size[3*i+1]) { rbound = mjMAX(m->geom_size[3*i], m->geom_size[3*i+1]) * 0.1; @@ -283,7 +283,7 @@ static void setStat(mjModel* m, mjData* d) { else { rbound = 1; } - } else if (m->geom_type[i]==mjGEOM_HFIELD) { + } else if (m->geom_type[i] == mjGEOM_HFIELD) { int j = m->geom_dataid[i]; rbound = mjMAX(m->hfield_size[4*j], mjMAX(m->hfield_size[4*j+1], @@ -298,13 +298,13 @@ static void setStat(mjModel* m, mjData* d) { mju_scl3(m->stat.center, m->stat.center, 0.5); // compute bounding box size - if (xmax[0]>xmin[0]) + if (xmax[0] > xmin[0]) m->stat.extent = mju_max(1E-5, mju_max(xmax[0]-xmin[0], mju_max(xmax[1]-xmin[1], xmax[2]-xmin[2]))); // set body size to max com-joint distance mju_zero(body, m->nbody); - for (int i=0; injnt; i++) { + for (int i=0; i < m->njnt; i++) { // handle this body int id = m->jnt_bodyid[i]; body[id] = mju_max(body[id], mju_dist3(d->xipos+3*id, d->xanchor+3*i)); @@ -316,18 +316,18 @@ static void setStat(mjModel* m, mjData* d) { body[0] = 0; // set body size to max of old value, and geom rbound + com-geom dist - for (int i=1; inbody; i++) { - for (int id=m->body_geomadr[i]; idbody_geomadr[i]+m->body_geomnum[i]; id++) { - if (m->geom_rbound[id]>0) { + for (int i=1; i < m->nbody; i++) { + for (int id=m->body_geomadr[i]; id < m->body_geomadr[i]+m->body_geomnum[i]; id++) { + if (m->geom_rbound[id] > 0) { body[i] = mju_max(body[i], m->geom_rbound[id] + mju_dist3(d->xipos+3*i, d->geom_xpos+3*id)); } } } // compute meansize, make sure all sizes are above min - if (m->nbody>1) { + if (m->nbody > 1) { m->stat.meansize = 0; - for (int i=1; inbody; i++) { + for (int i=1; i < m->nbody; i++) { body[i] = mju_max(body[i], 1E-5); m->stat.meansize += body[i]/(m->nbody-1); } @@ -337,9 +337,9 @@ static void setStat(mjModel* m, mjData* d) { m->stat.extent = mju_max(m->stat.extent, 2 * m->stat.meansize); // compute meanmass - if (m->nbody>1) { + if (m->nbody > 1) { m->stat.meanmass = 0; - for (int i=1; inbody; i++) { + for (int i=1; i < m->nbody; i++) { m->stat.meanmass += m->body_mass[i]; } m->stat.meanmass /= (m->nbody-1); @@ -348,7 +348,7 @@ static void setStat(mjModel* m, mjData* d) { // compute meaninertia if (m->nv) { m->stat.meaninertia = 0; - for (int i=0; inv; i++) { + for (int i=0; i < m->nv; i++) { m->stat.meaninertia += d->qM[m->dof_Madr[i]]; } m->stat.meaninertia /= m->nv; @@ -369,7 +369,7 @@ static void setSpring(mjModel* m, mjData* d) { mj_transmission(m, d); // copy if model spring length is -1 - for (int i=0; intendon; i++) { + for (int i=0; i < m->ntendon; i++) { if (m->tendon_lengthspring[2*i] == -1 && m->tendon_lengthspring[2*i+1] == -1) { // explicit springlength unused, set equal to ten_length m->tendon_lengthspring[2*i] = m->tendon_lengthspring[2*i+1] = d->ten_length[i]; @@ -382,10 +382,10 @@ static void setSpring(mjModel* m, mjData* d) { // entry point: set all constant fields of mjModel, except for lengthrange void mj_setConst(mjModel* m, mjData* d) { // compute subtreemass - for (int i=0; inbody; i++) { + for (int i=0; i < m->nbody; i++) { m->body_subtreemass[i] = m->body_mass[i]; } - for (int i=m->nbody-1; i>0; i--) { + for (int i=m->nbody-1; i > 0; i--) { m->body_subtreemass[m->body_parentid[i]] += m->body_subtreemass[i]; } @@ -418,7 +418,7 @@ static mjtNum evalAct(const mjModel* m, mjData* d, int index, int side, // impose maxforce nrm = mju_norm(d->qfrc_applied, nv); - if (opt->maxforce>0 && nrm>opt->maxforce) { + if (opt->maxforce > 0 && nrm > opt->maxforce) { mju_scl(d->qfrc_applied, d->qfrc_applied, opt->maxforce/mjMAX(mjMINVAL, nrm), nv); } @@ -435,18 +435,18 @@ static mjtNum evalAct(const mjModel* m, mjData* d, int index, int side, int mj_setLengthRange(mjModel* m, mjData* d, int index, const mjLROpt* opt, char* error, int error_sz) { // check index - if (index<0 || index>=m->nu) { + if (index < 0 || index >= m->nu) { mju_error("Invalid actuator index in mj_setLengthRange"); } // skip depending on mode and type - int ismuscle = (m->actuator_gaintype[index]==mjGAIN_MUSCLE || - m->actuator_biastype[index]==mjBIAS_MUSCLE); - int isuser = (m->actuator_gaintype[index]==mjGAIN_USER || - m->actuator_biastype[index]==mjBIAS_USER); - if ((opt->mode==mjLRMODE_NONE) || - (opt->mode==mjLRMODE_MUSCLE && !ismuscle) || - (opt->mode==mjLRMODE_MUSCLEUSER && !ismuscle && !isuser)) { + int ismuscle = (m->actuator_gaintype[index] == mjGAIN_MUSCLE || + m->actuator_biastype[index] == mjBIAS_MUSCLE); + int isuser = (m->actuator_gaintype[index] == mjGAIN_USER || + m->actuator_biastype[index] == mjBIAS_USER); + if ((opt->mode == mjLRMODE_NONE) || + (opt->mode == mjLRMODE_MUSCLE && !ismuscle) || + (opt->mode == mjLRMODE_MUSCLEUSER && !ismuscle && !isuser)) { return 1; } @@ -461,8 +461,8 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index, // use joint and tendon limits if available if (opt->uselimit) { // joint or jointinparent - if (m->actuator_trntype[index]==mjTRN_JOINT || - m->actuator_trntype[index]==mjTRN_JOINTINPARENT) { + if (m->actuator_trntype[index] == mjTRN_JOINT || + m->actuator_trntype[index] == mjTRN_JOINTINPARENT) { // make sure joint is limited if (m->jnt_limited[threadid]) { // copy range @@ -475,7 +475,7 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index, } // tendon - if (m->actuator_trntype[index]==mjTRN_TENDON) { + if (m->actuator_trntype[index] == mjTRN_TENDON) { // make sure tendon is limited if (m->tendon_limited[threadid]) { // copy range @@ -491,7 +491,7 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index, // optimize in both directions mjtNum lmin[2] = {0, 0}, lmax[2] = {0, 0}; int side; - for (side=0; side<2; side++) { + for (side=0; side < 2; side++) { // init at qpos0 mj_resetData(m, d); @@ -502,17 +502,17 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index, mjtNum len = evalAct(m, d, index, side, opt); // reset: cannot proceed - if (d->time==0) { + if (d->time == 0) { snprintf(error, error_sz, "Unstable lengthrange simulation in actuator %d", index); return 0; } // update limits if (d->time > opt->inttotal-opt->interval) { - if (lenlmax[side] || !updated) { + if (len > lmax[side] || !updated) { lmax[side] = len; } @@ -521,12 +521,12 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index, } // assign - m->actuator_lengthrange[2*index+side] = (side==0 ? lmin[side] : lmax[side]); + m->actuator_lengthrange[2*index+side] = (side == 0 ? lmin[side] : lmax[side]); } // check range mjtNum dif = m->actuator_lengthrange[2*index+1] - m->actuator_lengthrange[2*index]; - if (dif<=0) { + if (dif <= 0) { snprintf(error, error_sz, "Invalid lengthrange (%g, %g) in actuator %d", m->actuator_lengthrange[2*index], @@ -535,7 +535,7 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index, } // check convergence, side 0 - if (lmax[0]-lmin[0]>opt->tolrange*dif) { + if (lmax[0]-lmin[0] > opt->tolrange*dif) { snprintf(error, error_sz, "Lengthrange computation did not converge in actuator %d:\n" " eval (%g, %g)\n range (%g, %g)", @@ -546,7 +546,7 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index, } // check convergence, side 1 - if (lmax[1]-lmin[1]>opt->tolrange*dif) { + if (lmax[1]-lmin[1] > opt->tolrange*dif) { snprintf(error, error_sz, "Lengthrange computation did not converge in actuator %d:\n" " eval (%g, %g)\n range (%g, %g)", diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index 975fc242..affe394a 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -48,7 +48,7 @@ static void saveStats(const mjModel* m, mjData* d, int* piter, (*piter)++; // save if within range - if (isolver[i].improvement = improvement; d->solver[i].gradient = gradient; d->solver[i].lineslope = lineslope; @@ -80,9 +80,9 @@ static void ARdiaginv(const mjModel* m, mjData* d, mjtNum* res, int flg_subR) { // sparse if (mj_isSparse(m)) { - for (int i=0; iefc_AR_rownnz[i]; j++) { - if (i==d->efc_AR_colind[rowadr[i]+j]) { + for (int i=0; i < nefc; i++) { + for (int j=0; j < d->efc_AR_rownnz[i]; j++) { + if (i == d->efc_AR_colind[rowadr[i]+j]) { res[i] = 1/(flg_subR ? mju_max(mjMINVAL, d->efc_AR[rowadr[i]+j]-d->efc_R[i]) : d->efc_AR[rowadr[i]+j]); break; @@ -93,7 +93,7 @@ static void ARdiaginv(const mjModel* m, mjData* d, mjtNum* res, int flg_subR) { // dense else { - for (int i=0; iefc_AR[i*(nefc+1)]-d->efc_R[i]) : d->efc_AR[i*(nefc+1)]); } @@ -121,36 +121,36 @@ static void extractBlock(const mjModel* m, mjData* d, mjtNum* Ac, if( col>=start && col=rownnz[start]) { + if (k >= rownnz[start]) { mju_error("Internal error in extractComponent"); } // copy rows - for (int j=0; jefc_R[start+j]; Ac[j*(n+1)] = mjMAX(1e-10, Ac[j*(n+1)]); } @@ -165,7 +165,7 @@ static void residual(const mjModel* m, mjData* d, mjtNum* res, int i, int dim, i // sparse if (mj_isSparse(m)) { - for (int j=0; jefc_b[i+j] + mju_dotSparse(d->efc_AR + d->efc_AR_rowadr[i+j], d->efc_force, d->efc_AR_rownnz[i+j], d->efc_AR_colind + d->efc_AR_rowadr[i+j]); @@ -174,13 +174,13 @@ static void residual(const mjModel* m, mjData* d, mjtNum* res, int i, int dim, i // dense else { - for (int j=0; jefc_b[i+j] + mju_dot(d->efc_AR+(i+j)*nefc, d->efc_force, nefc); } } if (flg_subR) { - for (int j=0; jefc_R[i+j]*d->efc_force[i+j]; } } @@ -194,7 +194,7 @@ static mjtNum costChange(const mjtNum* A, mjtNum* force, const mjtNum* oldforce, mjtNum delta[6], change; // compute change - if (dim==1) { + if (dim == 1) { delta[0] = force[0] - oldforce[0]; change = 0.5*delta[0]*delta[0]*A[0] + delta[0]*res[0]; } else { @@ -203,7 +203,7 @@ static mjtNum costChange(const mjtNum* A, mjtNum* force, const mjtNum* oldforce, } // positive change: restore - if (change>1e-10) { + if (change > 1e-10) { mju_copy(force, oldforce, dim); change = 0; } @@ -223,15 +223,15 @@ static int dualState(const mjModel* m, mjData* d) { nactive = ne + nf; // equality - for (int i=0; i=floss[i]) { + } else if (force[i] >= floss[i]) { state[i] = mjCNSTRSTATE_LINEARNEG; } else { state[i] = mjCNSTRSTATE_QUADRATIC; @@ -239,10 +239,10 @@ static int dualState(const mjModel* m, mjData* d) { } // limit and contact - for (int i=ne+nf; iefc_type[i]!=mjCNSTR_CONTACT_ELLIPTIC) { - if (force[i]<=0) { + if (d->efc_type[i] != mjCNSTR_CONTACT_ELLIPTIC) { + if (force[i] <= 0) { state[i] = mjCNSTRSTATE_SATISFIED; } else { state[i] = mjCNSTRSTATE_QUADRATIC; @@ -259,7 +259,7 @@ static int dualState(const mjModel* m, mjData* d) { // f = map force to regular-cone space f[0] = force[i]/mu; - for (int j=1; jfriction[j-1]; } @@ -268,12 +268,12 @@ static int dualState(const mjModel* m, mjData* d) { mjtNum T = mju_norm(f+1, dim-1); // top zone - if (mu*N>=T) { + if (mu*N >= T) { result = mjCNSTRSTATE_SATISFIED; } // bottom zone - else if (N+mu*T<=0) { + else if (N+mu*T <= 0) { result = mjCNSTRSTATE_QUADRATIC; nactive += dim; } @@ -285,7 +285,7 @@ static int dualState(const mjModel* m, mjData* d) { } // replicate state in all cone dimensions - for (int j=0; jefc_type[i]==mjCNSTR_CONTACT_ELLIPTIC) { + if (d->efc_type[i] == mjCNSTR_CONTACT_ELLIPTIC) { dim = d->contact[d->efc_id[i]].dim; } else { dim = 1; @@ -337,19 +337,19 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) { mju_copy(oldforce, force+i, dim); // simple constraint - if (d->efc_type[i]!=mjCNSTR_CONTACT_ELLIPTIC) { + if (d->efc_type[i] != mjCNSTR_CONTACT_ELLIPTIC) { // unconstrained minimum force[i] -= res[0]*ARinv[i]; // impose interval and inequality constraints - if (i>=ne && i= ne && i < ne+nf) { + if (force[i] < -floss[i]) { force[i] = -floss[i]; - } else if (force[i]>floss[i]) { + } else if (force[i] > floss[i]) { force[i] = floss[i]; } - } else if (i>=ne+nf) { - if (force[i]<0) { + } else if (i >= ne+nf) { + if (force[i] < 0) { force[i] = 0; } } @@ -368,12 +368,12 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) { extractBlock(m, d, Athis, i, dim, 0); // normal force too small: normal update - if (force[i]=mjMINVAL) { + if (denom >= mjMINVAL) { // x = v' * res / denom x = -mju_dot(v, res, dim) / denom; // make sure normal is non-negative - if (force[i]+x*v[0]<0) { + if (force[i]+x*v[0] < 0) { x = -v[0]/force[i]; } // add x*v to f - for (int j=0; jefc_state, nefc*sizeof(int)); int nactive = dualState(m, d); int nchange = 0; - for (int i=0; iefc_state[i]); + for (int i=0; i < nefc; i++) { + nchange += (oldstate[i] != d->efc_state[i]); } // scale improvement, save stats, count @@ -475,7 +475,7 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) { saveStats(m, d, &iter, improvement, 0, 0, nactive, nchange, 0, 0); // terminate - if (improvementopt.tolerance) { + if (improvement < m->opt.tolerance) { break; } } @@ -486,7 +486,7 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) { // set nnz if (mj_isSparse(m)) { d->solver_nnz = 0; - for (int i=0; isolver_nnz += d->efc_AR_rownnz[i]; } } else { @@ -521,19 +521,19 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) { dualState(m, d); // main iteration - while (iterefc_R[i]; } } // perform one sweep: dry friction - for (int i=ne; ifloss[i]) { + } else if (force[i] > floss[i]) { force[i] = floss[i]; } @@ -554,16 +554,16 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) { } // perform one sweep: contact friction - for (int i=ne+nf; iefc_type[i]==mjCNSTR_CONTACT_PYRAMIDAL) { + if (d->efc_type[i] == mjCNSTR_CONTACT_PYRAMIDAL) { // get contact info con = d->contact + d->efc_id[i]; dim = con->dim; mu = con->friction; // loop over pairs of opposing pyramid edges - for (int j=i; jmid) { + } else if (y > mid) { force[j] = 2*mid; force[j+1] = 0; } else { @@ -617,7 +617,7 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) { } // elliptic contact - else if (d->efc_type[i]==mjCNSTR_CONTACT_ELLIPTIC) { + else if (d->efc_type[i] == mjCNSTR_CONTACT_ELLIPTIC) { // get contact info con = d->contact + d->efc_id[i]; dim = con->dim; @@ -632,12 +632,12 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) { // bc = b-subvector + Ac,rest * f_rest mju_copy(bc, res, dim-1); - for (int j=0; jefc_state, nefc*sizeof(int)); int nactive = dualState(m, d); int nchange = 0; - for (int i=0; iefc_state[i]); + for (int i=0; i < nefc; i++) { + nchange += (oldstate[i] != d->efc_state[i]); } // scale improvement, save stats, count @@ -691,7 +691,7 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) { saveStats(m, d, &iter, improvement, 0, 0, nactive, nchange, 0, 0); // terminate - if (improvementopt.noslip_tolerance) { + if (improvement < m->opt.noslip_tolerance) { break; } } @@ -722,8 +722,8 @@ struct _mjCGContext { mjtNum* quad; // quadratic polynomials for constraint costs (nefc x 3) // Hessian (Newton only) - int flg_Newton; // 1: Newton, 0: CG (const) - int nnz; // total number of non-zeros + int flg_Newton; // 1: Newton, 0: CG (const) + int nnz; // total number of non-zeros mjtNum* H; // Cholesky factorization of Hessian (nv x nv) mjtNum* Hcone; // with cone contributions if present (nv x nv) int* rownnz; // non-zeros in row (nv X 1) @@ -731,16 +731,16 @@ struct _mjCGContext { int* colind; // column indices (nv x nv) // globals - mjtNum cost; // constraint + Gauss cost - mjtNum quadGauss[3]; // quadratic polynomial for Gauss cost - int nactive; // number of active constraints - int ncone; // number of contacts in cone state - int nupdate; // number of Cholesky updates + mjtNum cost; // constraint + Gauss cost + mjtNum quadGauss[3]; // quadratic polynomial for Gauss cost + int nactive; // number of active constraints + int ncone; // number of contacts in cone state + int nupdate; // number of Cholesky updates // linesearch diagnostics - int LSiter; // number of linesearch iterations - int LSresult; // linesearch result - mjtNum LSslope; // linesearch slope at solution + int LSiter; // number of linesearch iterations + int LSresult; // linesearch result + mjtNum LSslope; // linesearch slope at solution }; typedef struct _mjCGContext mjCGContext; @@ -787,14 +787,14 @@ static void CGupdateConstraint(const mjModel* m, mjData* d, mjCGContext* ctx) { // count active and cone ctx->nactive = 0; ctx->ncone = 0; - for (int i=0; inactive += (d->efc_state[i]!=mjCNSTRSTATE_SATISFIED); - ctx->ncone += (d->efc_state[i]==mjCNSTRSTATE_CONE); + for (int i=0; i < nefc; i++) { + ctx->nactive += (d->efc_state[i] != mjCNSTRSTATE_SATISFIED); + ctx->ncone += (d->efc_state[i] == mjCNSTRSTATE_CONE); } // add Gauss cost, set in quadratic[0] mjtNum Gauss = 0; - for (int i=0; iMa[i]-d->qfrc_smooth[i])*(d->qacc[i]-d->qacc_smooth[i]); } ctx->quadGauss[0] = Gauss; @@ -808,7 +808,7 @@ static void CGupdateGradient(const mjModel* m, mjData* d, mjCGContext* ctx) { int nv = m->nv; // grad = M*qacc - qfrc_smooth - qfrc_constraint - for (int i=0; igrad[i] = ctx->Ma[i] - d->qfrc_smooth[i] - d->qfrc_constraint[i]; } @@ -841,7 +841,7 @@ static void CGprepare(const mjModel* m, const mjData* d, mjCGContext* ctx) { ctx->quadGauss[2] = 0.5*mju_dot(v, ctx->Mv, nv); // process constraints - for (int i=0; iJv + i; mjtNum* Jaref = ctx->Jaref + i; @@ -857,7 +857,7 @@ static void CGprepare(const mjModel* m, const mjData* d, mjCGContext* ctx) { quad[2] = Jv[0]*D[0]*Jv[0]; // elliptic cone: extra processing - if (d->efc_type[i]==mjCNSTR_CONTACT_ELLIPTIC) { + if (d->efc_type[i] == mjCNSTR_CONTACT_ELLIPTIC) { // extract contact info mjContact* con = d->contact + d->efc_id[i]; int dim = con->dim; @@ -865,7 +865,7 @@ static void CGprepare(const mjModel* m, const mjData* d, mjCGContext* ctx) { mjtNum* friction = con->friction; // complete vector quadratic (for bottom zone) - for (int j=1; jquadGauss); // equality - for (int i=0; iquad+3*i); } // friction - for (int i=ne; iJaref[i], dir = ctx->Jv[i]; mjtNum x = start + alpha*dir; @@ -943,12 +943,12 @@ static void CGeval(const mjModel* m, mjData* d, mjCGContext* ctx, mjCGPnt* p) { mjtNum Rf = d->efc_R[i]*f; // -bound < x < bound : quadratic - if (-Rfquad+3*i); } // x < -bound : linear negative - else if (x<=-Rf) { + else if (x <= -Rf) { mjtNum qf[3] = {f*(-0.5*Rf-start), -f*dir, 0}; mju_addTo3(quadTotal, qf); } @@ -961,8 +961,8 @@ static void CGeval(const mjModel* m, mjData* d, mjCGContext* ctx, mjCGPnt* p) { } // limit and contact - for (int i=ne+nf; iefc_type[i]==mjCNSTR_CONTACT_ELLIPTIC) { // elliptic cone + for (int i=ne+nf; i < nefc; i++) { + if (d->efc_type[i] == mjCNSTR_CONTACT_ELLIPTIC) { // elliptic cone // extract contact info mjContact* con = d->contact + d->efc_id[i]; mjtNum* quad = ctx->quad + 3*i; @@ -982,9 +982,9 @@ static void CGeval(const mjModel* m, mjData* d, mjCGContext* ctx, mjCGPnt* p) { mjtNum Tsqr = UU + alpha*(2*UV + alpha*VV); // no tangential force : top or bottom zone - if (Tsqr<=0) { + if (Tsqr <= 0) { // bottom zone: quadratic cost - if (N<0) { + if (N < 0) { mju_addTo3(quadTotal, quad); } @@ -997,12 +997,12 @@ static void CGeval(const mjModel* m, mjData* d, mjCGContext* ctx, mjCGPnt* p) { mjtNum T = mju_sqrt(Tsqr); // N>=mu*T : top zone - if (N>=mu*T) { + if (N >= mu*T) { // nothing to do } // mu*N+T<=0 : bottom zone - else if (mu*N+T<=0) { + else if (mu*N+T <= 0) { mju_addTo3(quadTotal, quad); } @@ -1027,7 +1027,7 @@ static void CGeval(const mjModel* m, mjData* d, mjCGContext* ctx, mjCGPnt* p) { mjtNum x = ctx->Jaref[i] + alpha*ctx->Jv[i]; // active - if (x<0) { + if (x < 0) { mju_addTo3(quadTotal, ctx->quad+3*i); } } @@ -1039,7 +1039,7 @@ static void CGeval(const mjModel* m, mjData* d, mjCGContext* ctx, mjCGPnt* p) { deriv[1] += 2*quadTotal[2]; // check for convexity; SHOULD NOT OCCUR - if (deriv[1]<=0) { + if (deriv[1] <= 0) { mju_warning("Linesearch objective is not convex"); deriv[1] = mjMINVAL; } @@ -1057,15 +1057,17 @@ static void CGeval(const mjModel* m, mjData* d, mjCGContext* ctx, mjCGPnt* p) { static int updateBracket(const mjModel* m, mjData* d, mjCGContext* ctx, mjCGPnt* p, mjCGPnt candidates[3], mjCGPnt* pnext) { int flag = 0; - for (int i=0; i<3; i++) { + for (int i=0; i < 3; i++) { // negative deriv - if (p->deriv[0]<0 && candidates[i].deriv[0]<0 && p->deriv[0]deriv[0] < 0 && candidates[i].deriv[0] < 0 && p->deriv[0] < candidates[i].deriv[0]) { *p = candidates[i]; flag = 1; } // positive deriv - else if (p->deriv[0]>0 && candidates[i].deriv[0]>0 && p->deriv[0]>candidates[i].deriv[0]) { + else if (p->deriv[0] > 0 && + candidates[i].deriv[0] > 0 && + p->deriv[0] > candidates[i].deriv[0]) { *p = candidates[i]; flag = 2; } @@ -1096,7 +1098,7 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) { // save search vector length, check mjtNum snorm = mju_norm(ctx->search, m->nv); - if (snormLSresult = 1; // search vector too small return 0; } @@ -1119,13 +1121,13 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) { // always attempt one Newton step p1.alpha = p0.alpha - p0.deriv[0]/p0.deriv[1]; CGeval(m, d, ctx, &p1); - if (p0.costLSresult = 2; // no improvement, initial convergence } else { ctx->LSresult = 0; // SUCCESS @@ -1135,32 +1137,32 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) { } // save direction - int dir = (p1.deriv[0]<0 ? +1 : -1); + int dir = (p1.deriv[0] < 0 ? +1 : -1); // SANITY CHECKS /* - // descent direction - if( mju_dot(ctx->grad, ctx->search, m->nv)>=0 ) + // descent direction + if( mju_dot(ctx->grad, ctx->search, m->nv)>=0 ) printf("NOT A DESCENT: grad %g search %g dot %g\n", mju_norm(ctx->grad, m->nv), mju_norm(ctx->search, m->nv), mju_dot(ctx->grad, ctx->search, m->nv)); - // 2nd derivative for Newton cone - if( ctx->flg_Newton && ctx->ncone ) - { + // 2nd derivative for Newton cone + if( ctx->flg_Newton && ctx->ncone ) + { mjtNum dd = -p0.deriv[0]/p0.deriv[1]; if( mju_abs(dd-1)>1e-6 ) printf("2nd DERIVATIVE FAIL: d0 %g d1 %g alpha %g\n", p0.deriv[0], p0.deriv[1], dd); - } + } - // cost and gradient at 0: full-space vs. linesearch - mjtNum grd = mju_dot(ctx->grad, ctx->search, m->nv); - if( mju_abs(p0.cost-ctx->cost)/mjMAX(mjMINVAL,mju_abs(p0.cost+ctx->cost)) > 1e-6 || + // cost and gradient at 0: full-space vs. linesearch + mjtNum grd = mju_dot(ctx->grad, ctx->search, m->nv); + if( mju_abs(p0.cost-ctx->cost)/mjMAX(mjMINVAL,mju_abs(p0.cost+ctx->cost)) > 1e-6 || mju_abs(p0.deriv[0]-grd)/mjMAX(mjMINVAL,mju_abs(p0.deriv[0]+grd)) > 1e-6 ) - { + { printf("LSiter = %d:\n", ctx->LSiter); printf("COST: %g %g %g\n", p0.cost, ctx->cost, @@ -1168,12 +1170,12 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) { printf("GRAD: %g %g %g\n", p0.deriv[0], grd, mju_abs(p0.deriv[0]-grd)/mjMAX(mjMINVAL,mju_abs(p0.deriv[0]+grd))); - } - */ + } + */ // one-sided search int p2update = 0; - while (p1.deriv[0]*dir<=-gtol && ctx->LSiterLSiter < LSmaxiter) { // save current p2 = p1; p2update = 1; @@ -1183,14 +1185,14 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) { CGeval(m, d, ctx, &p1); // check for convergence - if (mju_abs(p1.deriv[0])LSslope = mju_abs(p1.deriv[0])*slopescl; return p1.alpha; // SUCCESS } } // check for failure to bracket - if (ctx->LSiter>=LSmaxiter) { + if (ctx->LSiter >= LSmaxiter) { ctx->LSresult = 3; // could not bracket ctx->LSslope = mju_abs(p1.deriv[0])*slopescl; return p1.alpha; @@ -1209,7 +1211,7 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) { CGeval(m, d, ctx, &p1next); // bracketed search - while (ctx->LSiterLSiter < LSmaxiter) { // evaluate at midpoint pmid.alpha = 0.5*(p1.alpha + p2.alpha); CGeval(m, d, ctx, &pmid); @@ -1220,14 +1222,14 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) { // check candidates for convergence mjtNum bestcost = 0; int bestind = -1; - for (int i=0; i<3; i++) { - if (mju_abs(candidates[i].deriv[0])=0) { + if (bestind >= 0) { ctx->LSslope = mju_abs(candidates[bestind].deriv[0])*slopescl; return candidates[bestind].alpha; // SUCCESS } @@ -1238,7 +1240,7 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) { // no update possible: numerical accuracy reached, use midpoint if (!b1 && !b2) { - if (pmid.costLSresult = 0; // SUCCESS } else { ctx->LSresult = 7; // no improvement, could not bracket @@ -1250,11 +1252,11 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) { } // choose bracket with best cost - if (p1.cost<=p2.cost && p1.costLSresult = 4; // improvement but no convergence ctx->LSslope = mju_abs(p1.deriv[0])*slopescl; return p1.alpha; - } else if (p2.cost<=p1.cost && p2.costLSresult = 4; // improvement but no convergence ctx->LSslope = mju_abs(p2.deriv[0])*slopescl; return p2.alpha; @@ -1281,8 +1283,8 @@ static void HessianCone(const mjModel* m, mjData* d, mjCGContext* ctx) { mju_copy(ctx->Hcone, ctx->H, ctx->nnz); // add contributions - for (int i=0; iefc_state[i]==mjCNSTRSTATE_CONE) { + for (int i=0; i < nefc; i++) { + if (d->efc_state[i] == mjCNSTRSTATE_CONE) { mjContact* con = d->contact + d->efc_id[i]; int dim = con->dim; @@ -1297,14 +1299,14 @@ static void HessianCone(const mjModel* m, mjData* d, mjCGContext* ctx) { // compute LTJ = L'*J for this contact mju_zero(LTJ, dim*nnz); - for (int r=0; refc_J+d->efc_J_rowadr[i+r], local[r*dim+c], nnz); } } // update - for (int r=0; refc_J_colind+d->efc_J_rowadr[i+r], nnz*sizeof(int)); @@ -1320,14 +1322,14 @@ static void HessianCone(const mjModel* m, mjData* d, mjCGContext* ctx) { else { // compute LTJ = L'*J for this contact row mju_zero(LTJ, dim*nv); - for (int r=0; refc_J+(i+r)*nv, local[r*dim+c], nv); } } // update - for (int r=0; rHcone, LTJ+r*nv, nv, 1); } } @@ -1352,8 +1354,8 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) { // compute D corresponding to quad states mjtNum* D = mj_stackAlloc(d, nefc); - for (int i=0; iefc_state[i]==mjCNSTRSTATE_QUADRATIC) { + for (int i=0; i < nefc; i++) { + if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) { D[i] = d->efc_D[i]; } else { D[i] = 0; @@ -1381,7 +1383,7 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) { d); // rank-defficient, SHOULD NOT OCCUR - if (rank!=nv) { + if (rank != nv) { mju_error("Rank-defficient Hessian in HessianDirect"); } @@ -1390,7 +1392,7 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) { // count nnz ctx->nnz = 0; - for (int i=0; innz += ctx->rownnz[i]; } if (ctx->nnz > nv*nv) { // SHOULD NOT OCCUR @@ -1438,21 +1440,21 @@ static void HessianIncremental(const mjModel* m, mjData* d, ctx->nupdate = 0; // update H factorization - for (int i=0; iefc_state[i]==mjCNSTRSTATE_QUADRATIC) { + if (oldstate[i] != mjCNSTRSTATE_QUADRATIC && d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) { flag_update = 1; } // subtract quad - else if (oldstate[i]==mjCNSTRSTATE_QUADRATIC && d->efc_state[i]!=mjCNSTRSTATE_QUADRATIC) { + else if (oldstate[i] == mjCNSTRSTATE_QUADRATIC && d->efc_state[i] != mjCNSTRSTATE_QUADRATIC) { flag_update = 0; } // perform update if flagged - if (flag_update!=-1) { + if (flag_update != -1) { // update with vec = J(i,:)*sqrt(D[i])) if (mj_isSparse(m)) { // get nnz and adr of row i @@ -1473,7 +1475,7 @@ static void HessianIncremental(const mjModel* m, mjData* d, ctx->nupdate++; // recompute H directly if accuracy lost - if (rankefc_state[i]!=oldstate[i]); + for (int i=0; i < nefc; i++) { + nchange += (d->efc_state[i] != oldstate[i]); } // scale improvement, save stats, count @@ -1570,7 +1572,7 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New ctx.nactive, nchange, ctx.LSiter, ctx.nupdate); // termination - if (improvementopt.tolerance || gradientopt.tolerance) { + if (improvement < m->opt.tolerance || gradient < m->opt.tolerance) { break; } @@ -1584,12 +1586,12 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New mju_max(mjMINVAL, mju_dot(gradold, Mgradold, nv)); // reset if negative - if (beta<0) { + if (beta < 0) { beta = 0; } // update - for (int i=0; ibody_dofadr[body] + m->body_dofnum[body] - 1; // backward pass over dof ancestor chain - while (da>=0) { + while (da >= 0) { // construct rotation jacobian if (jacr) { jacr[da] = cdof[6*da]; @@ -167,7 +167,7 @@ void mj_jacSubtreeCom(const mjModel* m, mjData* d, mjtNum* jacp, int body) { mju_zero(jacp, 3*nv); // forward pass starting from body - for (int b=body; bnbody; b++) { + for (int b=body; b < m->nbody; b++) { // end of body subtree, break from the loop if (b > body && m->body_parentid[b] < body) { break; @@ -213,7 +213,7 @@ void mj_jacPointAxis(const mjModel* m, mjData* d, mjtNum* jacPoint, mjtNum* jacA // jacAxis_col = cross(jacr_col, axis) if (jacAxis) { - for (int i=0; i=0) { + while (da >= 0) { // find chain index for this dof - while (ci>=0 && chain[ci]>da) { + while (ci >= 0 && chain[ci] > da) { ci--; } // make sure we found it; SHOULD NOT OCCUR - if (chain[ci]!=da) { + if (chain[ci] != da) { mju_error("dof index %d not found in chain", da); } @@ -311,7 +311,7 @@ void mj_jacSparseSimple(const mjModel* m, const mjData* d, // process dofs int ci = start; int end = m->body_dofadr[body] + m->body_dofnum[body]; - for (int da=m->body_dofadr[body]; dabody_dofadr[body]; da < end; da++) { // construct rotation jacobian if (jacdifr) { // plus sign @@ -389,11 +389,11 @@ int mj_jacDifPair(const mjModel* m, const mjData* d, int* chain, if (issimple) { // first body mj_jacSparseSimple(m, d, jacdifp, jacdifr, pos1, b1, 0, NV, - b1body_dofnum[b2]); + b1 < b2 ? 0 : m->body_dofnum[b2]); // second body mj_jacSparseSimple(m, d, jacdifp, jacdifr, pos2, b2, 1, NV, - b2body_dofnum[b1]); + b2 < b1 ? 0 : m->body_dofnum[b1]); } // regular processing @@ -442,186 +442,186 @@ static int _getnumadr(const mjModel* m, mjtObj type, int** padr, int* mapadr) { // get address list and size for object type switch (type) { - case mjOBJ_BODY: - case mjOBJ_XBODY: - *mapadr -= mjLOAD_MULTIPLE*m->nbody; - *padr = m->name_bodyadr; - num = m->nbody; - mjFALLTHROUGH; + case mjOBJ_BODY: + case mjOBJ_XBODY: + *mapadr -= mjLOAD_MULTIPLE*m->nbody; + *padr = m->name_bodyadr; + num = m->nbody; + mjFALLTHROUGH; - case mjOBJ_JOINT: - *mapadr -= mjLOAD_MULTIPLE*m->njnt; - if (num < 0) { - *padr = m->name_jntadr; - num = m->njnt; - } - mjFALLTHROUGH; + case mjOBJ_JOINT: + *mapadr -= mjLOAD_MULTIPLE*m->njnt; + if (num < 0) { + *padr = m->name_jntadr; + num = m->njnt; + } + mjFALLTHROUGH; - case mjOBJ_GEOM: - *mapadr -= mjLOAD_MULTIPLE*m->ngeom; - if (num < 0) { - *padr = m->name_geomadr; - num = m->ngeom; - } - mjFALLTHROUGH; + case mjOBJ_GEOM: + *mapadr -= mjLOAD_MULTIPLE*m->ngeom; + if (num < 0) { + *padr = m->name_geomadr; + num = m->ngeom; + } + mjFALLTHROUGH; - case mjOBJ_SITE: - *mapadr -= mjLOAD_MULTIPLE*m->nsite; - if (num < 0) { - *padr = m->name_siteadr; - num = m->nsite; - } - mjFALLTHROUGH; + case mjOBJ_SITE: + *mapadr -= mjLOAD_MULTIPLE*m->nsite; + if (num < 0) { + *padr = m->name_siteadr; + num = m->nsite; + } + mjFALLTHROUGH; - case mjOBJ_CAMERA: - *mapadr -= mjLOAD_MULTIPLE*m->ncam; - if (num < 0) { - *padr = m->name_camadr; - num = m->ncam; - } - mjFALLTHROUGH; + case mjOBJ_CAMERA: + *mapadr -= mjLOAD_MULTIPLE*m->ncam; + if (num < 0) { + *padr = m->name_camadr; + num = m->ncam; + } + mjFALLTHROUGH; - case mjOBJ_LIGHT: - *mapadr -= mjLOAD_MULTIPLE*m->nlight; - if (num < 0) { - *padr = m->name_lightadr; - num = m->nlight; - } - mjFALLTHROUGH; + case mjOBJ_LIGHT: + *mapadr -= mjLOAD_MULTIPLE*m->nlight; + if (num < 0) { + *padr = m->name_lightadr; + num = m->nlight; + } + mjFALLTHROUGH; - case mjOBJ_MESH: - *mapadr -= mjLOAD_MULTIPLE*m->nmesh; - if (num < 0) { - *padr = m->name_meshadr; - num = m->nmesh; - } - mjFALLTHROUGH; + case mjOBJ_MESH: + *mapadr -= mjLOAD_MULTIPLE*m->nmesh; + if (num < 0) { + *padr = m->name_meshadr; + num = m->nmesh; + } + mjFALLTHROUGH; - case mjOBJ_SKIN: - *mapadr -= mjLOAD_MULTIPLE*m->nskin; - if (num < 0) { - *padr = m->name_skinadr; - num = m->nskin; - } - mjFALLTHROUGH; + case mjOBJ_SKIN: + *mapadr -= mjLOAD_MULTIPLE*m->nskin; + if (num < 0) { + *padr = m->name_skinadr; + num = m->nskin; + } + mjFALLTHROUGH; - case mjOBJ_HFIELD: - *mapadr -= mjLOAD_MULTIPLE*m->nhfield; - if (num < 0) { - *padr = m->name_hfieldadr; - num = m->nhfield; - } - mjFALLTHROUGH; + case mjOBJ_HFIELD: + *mapadr -= mjLOAD_MULTIPLE*m->nhfield; + if (num < 0) { + *padr = m->name_hfieldadr; + num = m->nhfield; + } + mjFALLTHROUGH; - case mjOBJ_TEXTURE: - *mapadr -= mjLOAD_MULTIPLE*m->ntex; - if (num < 0) { - *padr = m->name_texadr; - num = m->ntex; - } - mjFALLTHROUGH; + case mjOBJ_TEXTURE: + *mapadr -= mjLOAD_MULTIPLE*m->ntex; + if (num < 0) { + *padr = m->name_texadr; + num = m->ntex; + } + mjFALLTHROUGH; - case mjOBJ_MATERIAL: - *mapadr -= mjLOAD_MULTIPLE*m->nmat; - if (num < 0) { - *padr = m->name_matadr; - num = m->nmat; - } - mjFALLTHROUGH; + case mjOBJ_MATERIAL: + *mapadr -= mjLOAD_MULTIPLE*m->nmat; + if (num < 0) { + *padr = m->name_matadr; + num = m->nmat; + } + mjFALLTHROUGH; - case mjOBJ_PAIR: - *mapadr -= mjLOAD_MULTIPLE*m->npair; - if (num < 0) { - *padr = m->name_pairadr; - num = m->npair; - } - mjFALLTHROUGH; + case mjOBJ_PAIR: + *mapadr -= mjLOAD_MULTIPLE*m->npair; + if (num < 0) { + *padr = m->name_pairadr; + num = m->npair; + } + mjFALLTHROUGH; - case mjOBJ_EXCLUDE: - *mapadr -= mjLOAD_MULTIPLE*m->nexclude; - if (num < 0) { - *padr = m->name_excludeadr; - num = m->nexclude; - } - mjFALLTHROUGH; + case mjOBJ_EXCLUDE: + *mapadr -= mjLOAD_MULTIPLE*m->nexclude; + if (num < 0) { + *padr = m->name_excludeadr; + num = m->nexclude; + } + mjFALLTHROUGH; - case mjOBJ_EQUALITY: - *mapadr -= mjLOAD_MULTIPLE*m->neq; - if (num < 0) { - *padr = m->name_eqadr; - num = m->neq; - } - mjFALLTHROUGH; + case mjOBJ_EQUALITY: + *mapadr -= mjLOAD_MULTIPLE*m->neq; + if (num < 0) { + *padr = m->name_eqadr; + num = m->neq; + } + mjFALLTHROUGH; - case mjOBJ_TENDON: - *mapadr -= mjLOAD_MULTIPLE*m->ntendon; - if (num < 0) { - *padr = m->name_tendonadr; - num = m->ntendon; - } - mjFALLTHROUGH; + case mjOBJ_TENDON: + *mapadr -= mjLOAD_MULTIPLE*m->ntendon; + if (num < 0) { + *padr = m->name_tendonadr; + num = m->ntendon; + } + mjFALLTHROUGH; - case mjOBJ_ACTUATOR: - *mapadr -= mjLOAD_MULTIPLE*m->nu; - if (num < 0) { - *padr = m->name_actuatoradr; - num = m->nu; - } - mjFALLTHROUGH; + case mjOBJ_ACTUATOR: + *mapadr -= mjLOAD_MULTIPLE*m->nu; + if (num < 0) { + *padr = m->name_actuatoradr; + num = m->nu; + } + mjFALLTHROUGH; - case mjOBJ_SENSOR: - *mapadr -= mjLOAD_MULTIPLE*m->nsensor; - if (num < 0) { - *padr = m->name_sensoradr; - num = m->nsensor; - } - mjFALLTHROUGH; + case mjOBJ_SENSOR: + *mapadr -= mjLOAD_MULTIPLE*m->nsensor; + if (num < 0) { + *padr = m->name_sensoradr; + num = m->nsensor; + } + mjFALLTHROUGH; - case mjOBJ_NUMERIC: - *mapadr -= mjLOAD_MULTIPLE*m->nnumeric; - if (num < 0) { - *padr = m->name_numericadr; - num = m->nnumeric; - } - mjFALLTHROUGH; + case mjOBJ_NUMERIC: + *mapadr -= mjLOAD_MULTIPLE*m->nnumeric; + if (num < 0) { + *padr = m->name_numericadr; + num = m->nnumeric; + } + mjFALLTHROUGH; - case mjOBJ_TEXT: - *mapadr -= mjLOAD_MULTIPLE*m->ntext; - if (num < 0) { - *padr = m->name_textadr; - num = m->ntext; - } - mjFALLTHROUGH; + case mjOBJ_TEXT: + *mapadr -= mjLOAD_MULTIPLE*m->ntext; + if (num < 0) { + *padr = m->name_textadr; + num = m->ntext; + } + mjFALLTHROUGH; - case mjOBJ_TUPLE: - *mapadr -= mjLOAD_MULTIPLE*m->ntuple; - if (num < 0) { - *padr = m->name_tupleadr; - num = m->ntuple; - } - mjFALLTHROUGH; + case mjOBJ_TUPLE: + *mapadr -= mjLOAD_MULTIPLE*m->ntuple; + if (num < 0) { + *padr = m->name_tupleadr; + num = m->ntuple; + } + mjFALLTHROUGH; - case mjOBJ_KEY: - *mapadr -= mjLOAD_MULTIPLE*m->nkey; - if (num < 0) { - *padr = m->name_keyadr; - num = m->nkey; - } - mjFALLTHROUGH; + case mjOBJ_KEY: + *mapadr -= mjLOAD_MULTIPLE*m->nkey; + if (num < 0) { + *padr = m->name_keyadr; + num = m->nkey; + } + mjFALLTHROUGH; - case mjOBJ_PLUGIN: - *mapadr -= mjLOAD_MULTIPLE*m->nplugin; - if (num < 0) { - *padr = m->name_pluginadr; - num = m->nplugin; - } - mjFALLTHROUGH; + case mjOBJ_PLUGIN: + *mapadr -= mjLOAD_MULTIPLE*m->nplugin; + if (num < 0) { + *padr = m->name_pluginadr; + num = m->nplugin; + } + mjFALLTHROUGH; - default: - if (num < 0) { - *padr = 0; - num = 0; - } + default: + if (num < 0) { + *padr = 0; + num = 0; + } } return num; @@ -653,15 +653,15 @@ int mj_name2id(const mjModel* m, int type, const char* name) { do { int j = m->names_map[mapadr + i]; - if (j<0) { + if (j < 0) { return -1; } if (!strncmp(name, m->names+adr[j], m->nnames-adr[j])) { return j; } - if ((++i)==num) i = 0; - } while (i!=hash); + if ((++i) == num)i = 0; + } while (i != hash); } return -1; } @@ -678,7 +678,7 @@ const char* mj_id2name(const mjModel* m, int type, int id) { int num = _getnumadr(m, type, &adr, &mapadr); // id is in [0, num) and the found name is not the empty string "\0" - if (id>=0 && idnames[adr[id]]) { + if (id >= 0 && id < num && m->names[adr[id]]) { return m->names+adr[id]; } @@ -694,9 +694,9 @@ void mj_fullM(const mjModel* m, mjtNum* dst, const mjtNum* M) { int adr = 0, nv = m->nv; mju_zero(dst, nv*nv); - for (int i=0; i=0) { + while (j >= 0) { dst[i*nv+j] = M[adr]; dst[j*nv+i] = M[adr]; j = m->dof_parentid[j]; @@ -715,10 +715,10 @@ void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) mju_zero(res, nv); - for (int i=0; idof_simplenum[i]>=4) { + if (m->dof_simplenum[i] >= 4) { // init __m256d result, val1, val2; @@ -753,7 +753,7 @@ void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) // off-diagonal int j = m->dof_parentid[i]; adr++; - while (j>=0) { + while (j >= 0) { res[i] += M[adr]*vec[j]; res[j] += M[adr]*vec[i]; @@ -776,10 +776,10 @@ void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) mju_zero(res, nv); - for (int i=0; idof_simplenum[i]>=4) { + if (m->dof_simplenum[i] >= 4) { // init __m256d result, val1, val2; @@ -814,7 +814,7 @@ void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) // off-diagonal int j = m->dof_parentid[i]; adr++; - while (j>=0) { + while (j >= 0) { res[i] += qLD[adr]*vec[j]; // advance to next element @@ -837,7 +837,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, if (rownnz && rowadr && colind) { // special processing of simple dofs int simplecnt = 0; - for (int i=0; idof_simplenum[i]) { // count simple simplecnt++; @@ -854,8 +854,8 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, // find dof in row, add adr = rowadr[i]; int end = adr + rownnz[i]; - while (adrqM[m->dof_Madr[i]]; break; } else { @@ -863,7 +863,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, } // not found: error - if (adr>=end) { + if (adr >= end) { mju_error("mj_addM sparse: dst row expected to be empty"); } } @@ -871,7 +871,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, } // done if all simple - if (simplecnt==nv) { + if (simplecnt == nv) { return; } @@ -885,13 +885,13 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, mjtNum* sparse_buf = mj_stackAlloc(d, nv); // convert M into sparse format, lower-triangular - for (int i=0; idof_simplenum[i]) { // backward pass over dofs: construct M_row(i) in reverse order adr = m->dof_Madr[i]; int j = i; adr1 = 0; - while (j>=0) { + while (j >= 0) { // assign M[i*nv+adr1] = d->qM[adr]; M_colind[i*nv+adr1] = j; @@ -909,7 +909,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, M_rowadr[i] = i*nv; // reverse order - for (int k=0; kdof_simplenum[i]) { - for (int k=nv*i; kdof_simplenum[i]) { int new_nnz = mju_combineSparse(dst + rowadr[i], M + M_rowadr[i], nv, 1, 1, @@ -951,13 +951,13 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, // dense else { - for (int i=0; idof_Madr[i]; int j = i; - while (j>=0) { + while (j >= 0) { // add dst[i*nv+j] += d->qM[adr]; - if (jqM[adr]; } @@ -1052,7 +1052,7 @@ void mj_applyFT(const mjModel* m, mjData* d, mjtNum* qforce = mj_stackAlloc(d, nv); // make sure body is in range - if (body<0 || body>=m->nbody) { + if (body < 0 || body >= m->nbody) { mju_error("Invalid body %d in applyFT", body); } @@ -1076,7 +1076,7 @@ void mj_applyFT(const mjModel* m, mjData* d, // accumulate xfrc_applied in qfrc void mj_xfrcAccumulate(const mjModel* m, mjData* d, mjtNum* qfrc) { - for (int i=1; inbody; i++) { + for (int i=1; i < m->nbody; i++) { if (!mju_isZero(d->xfrc_applied+6*i, 6)) { mj_applyFT(m, d, d->xfrc_applied+6*i, d->xfrc_applied+6*i+3, d->xipos+3*i, i, qfrc); } @@ -1092,35 +1092,35 @@ void mj_objectVelocity(const mjModel* m, const mjData* d, const mjtNum *pos = 0, *rot = 0; // body-inertial - if (objtype==mjOBJ_BODY) { + if (objtype == mjOBJ_BODY) { bodyid = objid; pos = d->xipos+3*objid; rot = (flg_local ? d->ximat+9*objid : 0); } // body-regular - else if (objtype==mjOBJ_XBODY) { + else if (objtype == mjOBJ_XBODY) { bodyid = objid; pos = d->xpos+3*objid; rot = (flg_local ? d->xmat+9*objid : 0); } // geom - else if (objtype==mjOBJ_GEOM) { + else if (objtype == mjOBJ_GEOM) { bodyid = m->geom_bodyid[objid]; pos = d->geom_xpos+3*objid; rot = (flg_local ? d->geom_xmat+9*objid : 0); } // site - else if (objtype==mjOBJ_SITE) { + else if (objtype == mjOBJ_SITE) { bodyid = m->site_bodyid[objid]; pos = d->site_xpos+3*objid; rot = (flg_local ? d->site_xmat+9*objid : 0); } // camera - else if (objtype==mjOBJ_CAMERA) { + else if (objtype == mjOBJ_CAMERA) { bodyid = m->cam_bodyid[objid]; pos = d->cam_xpos+3*objid; rot = (flg_local ? d->cam_xmat+9*objid : 0); @@ -1145,35 +1145,35 @@ void mj_objectAcceleration(const mjModel* m, const mjData* d, mjtNum correction[3], vel[6]; // body-inertial - if (objtype==mjOBJ_BODY) { + if (objtype == mjOBJ_BODY) { bodyid = objid; pos = d->xipos+3*objid; rot = (flg_local ? d->ximat+9*objid : 0); } // body-regular - else if (objtype==mjOBJ_XBODY) { + else if (objtype == mjOBJ_XBODY) { bodyid = objid; pos = d->xpos+3*objid; rot = (flg_local ? d->xmat+9*objid : 0); } // geom - else if (objtype==mjOBJ_GEOM) { + else if (objtype == mjOBJ_GEOM) { bodyid = m->geom_bodyid[objid]; pos = d->geom_xpos+3*objid; rot = (flg_local ? d->geom_xmat+9*objid : 0); } // site - else if (objtype==mjOBJ_SITE) { + else if (objtype == mjOBJ_SITE) { bodyid = m->site_bodyid[objid]; pos = d->site_xpos+3*objid; rot = (flg_local ? d->site_xmat+9*objid : 0); } // camera - else if (objtype==mjOBJ_CAMERA) { + else if (objtype == mjOBJ_CAMERA) { bodyid = m->cam_bodyid[objid]; pos = d->cam_xpos+3*objid; rot = (flg_local ? d->cam_xmat+9*objid : 0); @@ -1207,7 +1207,7 @@ void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6] mju_zero(result, 6); // make sure contact is valid - if (id>=0 && idncon && d->contact[id].efc_address>=0) { + if (id >= 0 && id < d->ncon && d->contact[id].efc_address >= 0) { // get contact pointer con = d->contact + id; @@ -1225,14 +1225,14 @@ void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6] void mj_differentiatePos(const mjModel* m, mjtNum* qvel, mjtNum dt, const mjtNum* qpos1, const mjtNum* qpos2) { // loop over joints - for (int j=0; jnjnt; j++) { + for (int j=0; j < m->njnt; j++) { // get addresses in qpos and qvel int padr = m->jnt_qposadr[j]; int vadr = m->jnt_dofadr[j]; switch (m->jnt_type[j]) { case mjJNT_FREE: - for (int i=0; i<3; i++) { + for (int i=0; i < 3; i++) { qvel[vadr+i] = (qpos2[padr+i] - qpos1[padr+i]) / dt; } vadr += 3; @@ -1259,7 +1259,7 @@ void mj_differentiatePos(const mjModel* m, mjtNum* qvel, mjtNum dt, // integrate qpos with given qvel void mj_integratePos(const mjModel* m, mjtNum* qpos, const mjtNum* qvel, mjtNum dt) { // loop over joints - for (int j=0; jnjnt; j++) { + for (int j=0; j < m->njnt; j++) { // get addresses in qpos and qvel int padr = m->jnt_qposadr[j]; int vadr = m->jnt_dofadr[j]; @@ -1267,7 +1267,7 @@ void mj_integratePos(const mjModel* m, mjtNum* qpos, const mjtNum* qvel, mjtNum switch (m->jnt_type[j]) { case mjJNT_FREE: // position update - for (int i=0; i<3; i++) { + for (int i=0; i < 3; i++) { qpos[padr+i] += dt * qvel[vadr+i]; } padr += 3; @@ -1294,9 +1294,9 @@ void mj_integratePos(const mjModel* m, mjtNum* qpos, const mjtNum* qvel, mjtNum // normalize all quaternions in qpos-type vector void mj_normalizeQuat(const mjModel* m, mjtNum* qpos) { // find quaternion fields and normalize - for (int i=0; injnt; i++) { - if (m->jnt_type[i]==mjJNT_BALL || m->jnt_type[i]==mjJNT_FREE) { - mju_normalize4(qpos+m->jnt_qposadr[i]+3*(m->jnt_type[i]==mjJNT_FREE)); + for (int i=0; i < m->njnt; i++) { + if (m->jnt_type[i] == mjJNT_BALL || m->jnt_type[i] == mjJNT_FREE) { + mju_normalize4(qpos+m->jnt_qposadr[i]+3*(m->jnt_type[i] == mjJNT_FREE)); } } } @@ -1312,13 +1312,13 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9], // position if (xpos && pos) { // compute - if (sameframe==0) { + if (sameframe == 0) { mju_rotVecMat(xpos, pos, d->xmat+9*body); mju_addTo3(xpos, d->xpos+3*body); } // copy body position - else if (sameframe==1) { + else if (sameframe == 1) { mju_copy3(xpos, d->xpos+3*body); } @@ -1331,13 +1331,13 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9], // orientation if (xmat && quat) { // compute - if (sameframe==0) { + if (sameframe == 0) { mju_mulQuat(tmp, d->xquat+4*body, quat); mju_quat2Mat(xmat, tmp); } // copy body orientation - else if (sameframe==1) { + else if (sameframe == 1) { mju_copy(xmat, d->xmat+9*body, 9); } @@ -1354,7 +1354,7 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9], mjtNum mj_getTotalmass(const mjModel* m) { mjtNum res = 0; - for (int i=1; inbody; i++) { + for (int i=1; i < m->nbody; i++) { res += m->body_mass[i]; } @@ -1369,7 +1369,7 @@ void mj_setTotalmass(mjModel* m, mjtNum newmass) { mjtNum scale = mjMAX(mjMINVAL, newmass / mjMAX(mjMINVAL, mj_getTotalmass(m))); // scale all masses and inertias - for (int i=1; inbody; i++) { + for (int i=1; i < m->nbody; i++) { m->body_mass[i] *= scale; m->body_inertia[3*i] *= scale; m->body_inertia[3*i+1] *= scale; @@ -1385,7 +1385,7 @@ void mj_setTotalmass(mjModel* m, mjtNum newmass) { void mj_warning(mjData* d, int warning, int info) { // check type - if (warning<0 || warning>=mjNWARNING) { + if (warning < 0 || warning >= mjNWARNING) { mju_error("Invalid warning type %d", warning); } diff --git a/src/engine/engine_util_blas.c b/src/engine/engine_util_blas.c index 0becf416..f0b19daa 100644 --- a/src/engine/engine_util_blas.c +++ b/src/engine/engine_util_blas.c @@ -115,7 +115,7 @@ void mju_addScl3(mjtNum res[3], const mjtNum vec1[3], const mjtNum vec2[3], mjtN mjtNum mju_normalize3(mjtNum vec[3]) { mjtNum norm = mju_sqrt(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2]); - if (norm0) { + if (n > 0) { memset(res, 0, n*sizeof(mjtNum)); } } @@ -247,7 +247,7 @@ void mju_zero(mjtNum* res, int n) { // res = val void mju_fill(mjtNum* res, mjtNum val, int n) { - for (int i=0; i0) { + if (n > 0) { memcpy(res, vec, n*sizeof(mjtNum)); } } @@ -267,7 +267,7 @@ void mju_copy(mjtNum* res, const mjtNum* vec, int n) { mjtNum mju_sum(const mjtNum* vec, int n) { mjtNum res = 0; - for (int i=0; i=0) { + if (n_4 >= 0) { __m256d sclpar, val1, val1scl; // init sclpar = _mm256_set1_pd(scl); // parallel computation - while (i<=n_4) { + while (i <= n_4) { val1 = _mm256_loadu_pd(vec+i); val1scl = _mm256_mul_pd(val1, sclpar); _mm256_storeu_pd(res+i, val1scl); @@ -314,19 +314,19 @@ void mju_scl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n) { // process remaining int n_i = n - i; - if (n_i==3) { + if (n_i == 3) { res[i] = vec[i]*scl; res[i+1] = vec[i+1]*scl; res[i+2] = vec[i+2]*scl; - } else if (n_i==2) { + } else if (n_i == 2) { res[i] = vec[i]*scl; res[i+1] = vec[i+1]*scl; - } else if (n_i==1) { + } else if (n_i == 1) { res[i] = vec[i]*scl; } #else - for (; i=0) { + if (n_4 >= 0) { __m256d sum, val1, val2; // parallel computation - while (i<=n_4) { + while (i <= n_4) { val1 = _mm256_loadu_pd(vec1+i); val2 = _mm256_loadu_pd(vec2+i); sum = _mm256_add_pd(val1, val2); @@ -357,19 +357,19 @@ void mju_add(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, int n) { // process remaining int n_i = n - i; - if (n_i==3) { + if (n_i == 3) { res[i] = vec1[i] + vec2[i]; res[i+1] = vec1[i+1] + vec2[i+1]; res[i+2] = vec1[i+2] + vec2[i+2]; - } else if (n_i==2) { + } else if (n_i == 2) { res[i] = vec1[i] + vec2[i]; res[i+1] = vec1[i+1] + vec2[i+1]; - } else if (n_i==1) { + } else if (n_i == 1) { res[i] = vec1[i] + vec2[i]; } #else - for (; i=0) { + if (n_4 >= 0) { __m256d dif, val1, val2; // parallel computation - while (i<=n_4) { + while (i <= n_4) { val1 = _mm256_loadu_pd(vec1+i); val2 = _mm256_loadu_pd(vec2+i); dif = _mm256_sub_pd(val1, val2); @@ -400,19 +400,19 @@ void mju_sub(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, int n) { // process remaining int n_i = n - i; - if (n_i==3) { + if (n_i == 3) { res[i] = vec1[i] - vec2[i]; res[i+1] = vec1[i+1] - vec2[i+1]; res[i+2] = vec1[i+2] - vec2[i+2]; - } else if (n_i==2) { + } else if (n_i == 2) { res[i] = vec1[i] - vec2[i]; res[i+1] = vec1[i+1] - vec2[i+1]; - } else if (n_i==1) { + } else if (n_i == 1) { res[i] = vec1[i] - vec2[i]; } #else - for (; i=0) { + if (n_4 >= 0) { __m256d sum, val1, val2; // parallel computation - while (i<=n_4) { + while (i <= n_4) { val1 = _mm256_loadu_pd(res+i); val2 = _mm256_loadu_pd(vec+i); sum = _mm256_add_pd(val1, val2); @@ -443,19 +443,19 @@ void mju_addTo(mjtNum* res, const mjtNum* vec, int n) { // process remaining int n_i = n - i; - if (n_i==3) { + if (n_i == 3) { res[i] += vec[i]; res[i+1] += vec[i+1]; res[i+2] += vec[i+2]; - } else if (n_i==2) { + } else if (n_i == 2) { res[i] += vec[i]; res[i+1] += vec[i+1]; - } else if (n_i==1) { + } else if (n_i == 1) { res[i] += vec[i]; } #else - for (; i=0) { + if (n_4 >= 0) { __m256d dif, val1, val2; // parallel computation - while (i<=n_4) { + while (i <= n_4) { val1 = _mm256_loadu_pd(res+i); val2 = _mm256_loadu_pd(vec+i); dif = _mm256_sub_pd(val1, val2); @@ -486,19 +486,19 @@ void mju_subFrom(mjtNum* res, const mjtNum* vec, int n) { // process remaining int n_i = n - i; - if (n_i==3) { + if (n_i == 3) { res[i] -= vec[i]; res[i+1] -= vec[i+1]; res[i+2] -= vec[i+2]; - } else if (n_i==2) { + } else if (n_i == 2) { res[i] -= vec[i]; res[i+1] -= vec[i+1]; - } else if (n_i==1) { + } else if (n_i == 1) { res[i] -= vec[i]; } #else - for (; i=0) { + if (n_4 >= 0) { __m256d sclpar, sum, val1, val2, val2scl; // init sclpar = _mm256_set1_pd(scl); // parallel computation - while (i<=n_4) { + while (i <= n_4) { val1 = _mm256_loadu_pd(res+i); val2 = _mm256_loadu_pd(vec+i); val2scl = _mm256_mul_pd(val2, sclpar); @@ -533,19 +533,19 @@ void mju_addToScl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n) { // process remaining int n_i = n - i; - if (n_i==3) { + if (n_i == 3) { res[i] += vec[i]*scl; res[i+1] += vec[i+1]*scl; res[i+2] += vec[i+2]*scl; - } else if (n_i==2) { + } else if (n_i == 2) { res[i] += vec[i]*scl; res[i+1] += vec[i+1]*scl; - } else if (n_i==1) { + } else if (n_i == 1) { res[i] += vec[i]*scl; } #else - for (; i=0) { + if (n_4 >= 0) { __m256d sclpar, sum, val1, val2, val2scl; // init sclpar = _mm256_set1_pd(scl); // parallel computation - while (i<=n_4) { + while (i <= n_4) { val1 = _mm256_loadu_pd(vec1+i); val2 = _mm256_loadu_pd(vec2+i); val2scl = _mm256_mul_pd(val2, sclpar); @@ -578,19 +578,19 @@ void mju_addScl(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, mjtNum scl, // process remaining int n_i = n - i; - if (n_i==3) { + if (n_i == 3) { res[i] = vec1[i] + vec2[i]*scl; res[i+1] = vec1[i+1] + vec2[i+1]*scl; res[i+2] = vec1[i+2] + vec2[i+2]*scl; - } else if (n_i==2) { + } else if (n_i == 2) { res[i] = vec1[i] + vec2[i]*scl; res[i+1] = vec1[i+1] + vec2[i+1]*scl; - } else if (n_i==1) { + } else if (n_i == 1) { res[i] = vec1[i] + vec2[i]*scl; } #else - for (; i=0) { + if (n_4 >= 0) { __m256d sum, prod, val1, val2; __m128d vlow, vhigh, high64; @@ -646,7 +646,7 @@ mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, int n) { i = 4; // parallel computation - while (i<=n_4) { + while (i <= n_4) { val1 = _mm256_loadu_pd(vec1+i); val2 = _mm256_loadu_pd(vec2+i); prod = _mm256_mul_pd(val1, val2); @@ -671,7 +671,7 @@ mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, int n) { mjtNum res2 = 0; mjtNum res3 = 0; - for (; i<=n_4; i+=4) { + for (; i <= n_4; i+=4) { res0 += vec1[i] * vec2[i]; res1 += vec1[i+1] * vec2[i+1]; res2 += vec1[i+2] * vec2[i+2]; @@ -682,11 +682,11 @@ mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, int n) { // process remaining int n_i = n - i; - if (n_i==3) { + if (n_i == 3) { res += vec1[i]*vec2[i] + vec1[i+1]*vec2[i+1] + vec1[i+2]*vec2[i+2]; - } else if (n_i==2) { + } else if (n_i == 2) { res += vec1[i]*vec2[i] + vec1[i+1]*vec2[i+1]; - } else if (n_i==1) { + } else if (n_i == 1) { res += vec1[i]*vec2[i]; } return res; @@ -696,7 +696,7 @@ mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, int n) { // multiply matrix and vector void mju_mulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, int nc) { - for (int r=0; r=0 && a<=1 && b>=0 && b<=1) ? 1 : 0); + return ((a >= 0 && a <= 1 && b >= 0 && b <= 1) ? 1 : 0); } @@ -63,7 +63,7 @@ static mjtNum length_circle(const mjtNum* p0, const mjtNum* p1, int ind, mjtNum // flip if necessary cross = p0[1]*p1[0]-p0[0]*p1[1]; - if ((cross>0 && ind) || (cross<0 && !ind)) { + if ((cross > 0 && ind) || (cross < 0 && !ind)) { angle = 2*mjPI - angle; } @@ -85,37 +85,37 @@ static mjtNum wrap_circle(mjtNum* pnt, const mjtNum* d, const mjtNum* sd, mjtNum int sgn; // either point inside circle or circle too small: no wrap - if (sqlen01) { + } else if (a > 1) { a = 1; } tmp[0] = a*dif[0] + d[0]; tmp[1] = a*dif[1] + d[1]; // check for intersection and side - if (tmp[0]*tmp[0]+tmp[1]*tmp[1]>sqrad && (!sd || mju_dot(sd, tmp, 2)>=0)) { + if (tmp[0]*tmp[0]+tmp[1]*tmp[1] > sqrad && (!sd || mju_dot(sd, tmp, 2) >= 0)) { return -1; } // construct the two solutions, compute goodness - for (int i=0; i<2; i++) { + for (int i=0; i < 2; i++) { sqrt0 = mju_sqrt(sqlen0 - sqrad); sqrt1 = mju_sqrt(sqlen1 - sqrad); - sgn = (i==0 ? 1 : -1); + sgn = (i == 0 ? 1 : -1); sol[i][0][0] = (d[0]*sqrad + sgn*rad*d[1]*sqrt0)/sqlen0; sol[i][0][1] = (d[1]*sqrad - sgn*rad*d[0]*sqrt0)/sqlen0; @@ -139,7 +139,7 @@ static mjtNum wrap_circle(mjtNum* pnt, const mjtNum* d, const mjtNum* sd, mjtNum } // select the better solution - int i = (good[0]>good[1] ? 0 : 1); + int i = (good[0] > good[1] ? 0 : 1); pnt[0] = sol[i][0][0]; pnt[1] = sol[i][0][1]; pnt[2] = sol[i][1][0]; @@ -173,20 +173,20 @@ static mjtNum wrap_inside(mjtNum* pnt, const mjtNum* d, mjtNum rad) { mjtNum dd = dif[0]*dif[0] + dif[1]*dif[1]; // either point inside circle or circle too small: no wrap - if (len0<=rad || len1<=rad || radmjMINVAL) { + if (dd > mjMINVAL) { // find nearest point on line segment to origin: d0 + a*dif mjtNum a = -(dif[0]*d[0]+dif[1]*d[1])/dd; // in segment - if (a>0 && a<1) { + if (a > 0 && a < 1) { mjtNum tmp[2]; mju_addScl(tmp, d, dif, a, 2); - if (mju_norm(tmp, 2)<=rad) { + if (mju_norm(tmp, 2) <= rad) { return -1; } } @@ -204,9 +204,9 @@ static mjtNum wrap_inside(mjtNum* pnt, const mjtNum* d, mjtNum rad) { mjtNum A = rad/len0; mjtNum B = rad/len1; mjtNum cosG = (len0*len0 + len1*len1 - dd) / (2*len0*len1); - if (cosG<-1+mjMINVAL) { + if (cosG < -1+mjMINVAL) { return -1; - } else if (cosG>1-mjMINVAL) { + } else if (cosG > 1-mjMINVAL) { return 0; } mjtNum G = mju_acos(cosG); @@ -216,20 +216,20 @@ static mjtNum wrap_inside(mjtNum* pnt, const mjtNum* d, mjtNum rad) { mjtNum f = mju_asin(A*z) + mju_asin(B*z) - 2*mju_asin(z) + G; // make sure init is not on the other side - if (f>0) { + if (f > 0) { return 0; } // Newton method int iter; - for (iter=0; itertolerance; iter++) { + for (iter=0; iter < maxiter && mju_abs(f) > tolerance; iter++) { // derivative mjtNum df = A/mju_max(mjMINVAL, mju_sqrt(1-z*z*A*A)) + B/mju_max(mjMINVAL, mju_sqrt(1-z*z*B*B)) - 2/mju_max(mjMINVAL, mju_sqrt(1-z*z)); // check sign; SHOULD NOT OCCUR - if (df>-mjMINVAL) { + if (df > -mjMINVAL) { return 0; } @@ -237,7 +237,7 @@ static mjtNum wrap_inside(mjtNum* pnt, const mjtNum* d, mjtNum rad) { mjtNum z1 = z - f/df; // make sure we are moving to the left; SHOULD NOT OCCUR - if (z1>z) { + if (z1 > z) { return 0; } @@ -246,13 +246,13 @@ static mjtNum wrap_inside(mjtNum* pnt, const mjtNum* d, mjtNum rad) { f = mju_asin(A*z) + mju_asin(B*z) - 2*mju_asin(z) + G; // exit if positive; SHOULD NOT OCCUR - if (f>tolerance) { + if (f > tolerance) { return 0; } } // check convergence - if (iter>=maxiter) { + if (iter >= maxiter) { return 0; } @@ -286,7 +286,7 @@ mjtNum mju_wrap(mjtNum* wpnt, const mjtNum* x0, const mjtNum* x1, mjtNum L0, L1; // check object type; SHOULD NOT OCCUR - if (type!=mjWRAP_SPHERE && type!=mjWRAP_CYLINDER) { + if (type != mjWRAP_SPHERE && type != mjWRAP_CYLINDER) { mju_error("mju_wrap: unknown wrapping object type %d", type); } @@ -297,12 +297,12 @@ mjtNum mju_wrap(mjtNum* wpnt, const mjtNum* x0, const mjtNum* x1, mju_mulMatTVec(p[1], xmat, tmp, 3, 3); // too close to origin: return - if (mju_norm3(p[0])mju_abs(axis[0][0]) && - mju_abs(axis[0][1])>mju_abs(axis[0][2])) { + if (mju_abs(axis[0][1]) > mju_abs(axis[0][0]) && + mju_abs(axis[0][1]) > mju_abs(axis[0][2])) { i = 1; } - if (mju_abs(axis[0][2])>mju_abs(axis[0][0]) && - mju_abs(axis[0][2])>mju_abs(axis[0][1])) { + if (mju_abs(axis[0][2]) > mju_abs(axis[0][0]) && + mju_abs(axis[0][2]) > mju_abs(axis[0][1])) { i = 2; } @@ -374,7 +374,7 @@ mjtNum mju_wrap(mjtNum* wpnt, const mjtNum* x0, const mjtNum* x1, } // apply inside wrap - if (side && sd[0]==0 && sd[1]==0) { + if (side && sd[0] == 0 && sd[1] == 0) { wlen = wrap_inside(pnt, d, size[0]); } @@ -384,12 +384,12 @@ mjtNum mju_wrap(mjtNum* wpnt, const mjtNum* x0, const mjtNum* x1, } // no wrap - if (wlen<0) { + if (wlen < 0) { return -1; } // reconstruct 3D points in local frame: res - for (int i=0; i<2; i++) { + for (int i=0; i < 2; i++) { // res = axis0*d0 + axis1*d1 mju_scl3(res+3*i, axis[0], pnt[2*i]); mju_scl3(tmp, axis[1], pnt[2*i+1]); @@ -397,7 +397,7 @@ mjtNum mju_wrap(mjtNum* wpnt, const mjtNum* x0, const mjtNum* x1, } // cylinder: correct along z - if (type==mjWRAP_CYLINDER) { + if (type == mjWRAP_CYLINDER) { // set vertical coordinates L0 = mju_sqrt((p[0][0]-res[0])*(p[0][0]-res[0]) + (p[0][1]-res[1])*(p[0][1]-res[1])); L1 = mju_sqrt((p[1][0]-res[3])*(p[1][0]-res[3]) + (p[1][1]-res[4])*(p[1][1]-res[4])); @@ -466,7 +466,7 @@ mjtNum mju_muscleGain(mjtNum len, mjtNum vel, const mjtNum lengthrange[2], mjtNum fvmax = prm[8]; // scale force if negative - if (force<0) { + if (force < 0) { force = scale / mjMAX(mjMINVAL, acc0); } @@ -484,16 +484,16 @@ mjtNum mju_muscleGain(mjtNum len, mjtNum vel, const mjtNum lengthrange[2], // length curve mjtNum FL = 0; - if (L>=lmin && L<=a) { + if (L >= lmin && L <= a) { x = (L-lmin) / mjMAX(mjMINVAL, a-lmin); FL = 0.5*x*x; - } else if (L<=1) { + } else if (L <= 1) { x = (1-L) / mjMAX(mjMINVAL, 1-a); FL = 1 - 0.5*x*x; - } else if (L<=b) { + } else if (L <= b) { x = (L-1) / mjMAX(mjMINVAL, b-1); FL = 1 - 0.5*x*x; - } else if (L<=lmax) { + } else if (L <= lmax) { x = (lmax-L) / mjMAX(mjMINVAL, lmax-b); FL = 0.5*x*x; } @@ -501,11 +501,11 @@ mjtNum mju_muscleGain(mjtNum len, mjtNum vel, const mjtNum lengthrange[2], // velocity curve mjtNum FV; mjtNum y = fvmax-1; - if (V<=-1) { + if (V <= -1) { FV = 0; - } else if (V<=0) { + } else if (V <= 0) { FV = (V+1)*(V+1); - } else if (V<=y) { + } else if (V <= y) { FV = fvmax - (y-V)*(y-V) / mjMAX(mjMINVAL, y); } else { FV = fvmax; @@ -528,7 +528,7 @@ mjtNum mju_muscleBias(mjtNum len, const mjtNum lengthrange[2], mjtNum fpmax = prm[7]; // scale force if negative - if (force<0) { + if (force < 0) { force = scale / mjMAX(mjMINVAL, acc0); } @@ -540,9 +540,9 @@ mjtNum mju_muscleBias(mjtNum len, const mjtNum lengthrange[2], // half-quadratic to (L0+lmax)/2, linear beyond mjtNum b = 0.5*(1+lmax); - if (L<=1) { + if (L <= 1) { return 0; - } else if (L<=b) { + } else if (L <= b) { mjtNum x = (L-1) / mjMAX(mjMINVAL, b-1); return -force*fpmax*0.5*x*x; } else { @@ -606,7 +606,7 @@ void mju_encodePyramid(mjtNum* pyramid, const mjtNum* force, const mjtNum* mu, i // arbitary redundancy resolution: // pyramid0_i + pyramid1_i = force_normal/(dim-1) = a // pyramid0_i - pyramid1_i = force_tangent_i/mu_i = b - for (int i=0; imjMINVAL) { + if (det > mjMINVAL) { // compute w = sqrt(det)/2 w = mju_sqrt(det)/2; @@ -664,7 +664,7 @@ mjtNum mju_springDamper(mjtNum pos0, mjtNum vel0, mjtNum k, mjtNum b, mjtNum t) // critical damping // pos(t) = exp(-b*t/2) * (c1 + c2*t) - else if (det<=mjMINVAL && det>=-mjMINVAL) { + else if (det <= mjMINVAL && det >= -mjMINVAL) { // compute coefficients c1 = pos0; c2 = vel0 + b*c1/2; @@ -692,8 +692,8 @@ mjtNum mju_springDamper(mjtNum pos0, mjtNum vel0, mjtNum k, mjtNum b, mjtNum t) // print matrix to screen void mju_printMat(const mjtNum* mat, int nr, int nc) { - for (int r=0; rmax) { + } else if (x > max) { return max; } else { return x; @@ -755,9 +755,9 @@ mjtNum mju_clip(mjtNum x, mjtNum min, mjtNum max) { // sign function mjtNum mju_sign(mjtNum x) { - if (x<0) { + if (x < 0) { return -1; - } else if (x>0) { + } else if (x > 0) { return 1; } else { return 0; @@ -972,13 +972,13 @@ const char* mju_writeNumBytes(size_t nbytes) { int i; static mjTHREADLOCAL char message[20]; static const char suffix[] = " KMGTPE"; - for (i=0; i<6; i++) { + for (i=0; i < 6; i++) { const size_t bits = (size_t)(1) << (10*(6-i)); if (nbytes >= bits && !(nbytes & (bits - 1))) { break; } } - if (i<6) { + if (i < 6) { mjSNPRINTF(message, "%zu%c", nbytes >> (10*(6-i)), suffix[6-i]); } else { mjSNPRINTF(message, "%zu", nbytes >> (10*(6-i))); @@ -1042,15 +1042,15 @@ const char* mju_warningText(int warning, size_t info) { // return 1 if nan or abs(x)>mjMAXVAL, 0 otherwise int mju_isBad(mjtNum x) { - return (x!=x || x>mjMAXVAL || x<-mjMAXVAL); + return (x != x || x > mjMAXVAL || x < -mjMAXVAL); } // return 1 if all elements are 0 int mju_isZero(mjtNum* vec, int n) { - for (int i=0; i=1.0 || w==0); + } while (w >= 1.0 || w == 0); w = mju_sqrt((-2.0 * mju_log(w)) / w); if (num2) { @@ -1083,7 +1083,7 @@ mjtNum mju_standardNormal(mjtNum* num2) { // convert from float to mjtNum void mju_f2n(mjtNum* res, const float* vec, int n) { - for (int i=0; i=0 && list[j]>x) { + while (j >= 0 && list[j] > x) { list[j+1] = list[j]; j--; } @@ -1133,10 +1133,10 @@ void mju_insertionSort(mjtNum* list, int n) { // integer insertion sort, increasing order void mju_insertionSortInt(int* list, int n) { - for (int i=1; i=0 && list[j]>x) { + while (j >= 0 && list[j] > x) { list[j+1] = list[j]; j--; } @@ -1152,7 +1152,7 @@ mjtNum mju_Halton(int index, int base) { mjtNum b = (mjtNum)base; mjtNum f = 1/b, hn = 0; - while (n0>0) { + while (n0 > 0) { int n1 = n0/base; int r = n0 - n1*base; hn += f*r; @@ -1167,7 +1167,7 @@ mjtNum mju_Halton(int index, int base) { // Call strncpy, then set dst[n-1] = 0. char* mju_strncpy(char *dst, const char *src, int n) { - if (dst && src && n>0) { + if (dst && src && n > 0) { strncpy(dst, src, n); dst[n-1] = 0; } @@ -1180,10 +1180,10 @@ char* mju_strncpy(char *dst, const char *src, int n) { // sigmoid function over 0<=x<=1 using quintic polynomial mjtNum mju_sigmoid(mjtNum x) { // fast return - if (x<=0) { + if (x <= 0) { return 0; } - if (x>=1) { + if (x >= 1) { return 1; } diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index 8af9b28c..8c6a51fd 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -35,7 +35,7 @@ int mju_cholFactor(mjtNum* mat, int n, mjtNum mindiag) { mjtNum tmp; // in-place Cholesky factorization - for (int j=0; j=0; i--) { - if (i= 0; i--) { + if (i < n-1) { + for (int j=i+1; j < n; j++) { res[i] -= mat[j*n+i] * res[j]; } } @@ -99,12 +99,12 @@ int mju_cholUpdate(mjtNum* mat, mjtNum* x, int n, int flg_plus) { int rank = n; mjtNum r, c, cinv, s, Lkk, tmp; - for (int k=0; k0 && colind[rowadr[r]+rownnz[r]-1]>r) { + while (rownnz[r] > 0 && colind[rowadr[r]+rownnz[r]-1] > r) { rownnz[r]--; } // check - if (rownnz[r]==0 || colind[rowadr[r]+rownnz[r]-1]!=r) { + if (rownnz[r] == 0 || colind[rowadr[r]+rownnz[r]-1] != r) { mju_error("Matrix must have non-zero diagonal in mju_cholFactorSparse"); } } // backpass over rows - for (int r=n-1; r>=0; r--) { + for (int r=n-1; r >= 0; r--) { // get rownnz and rowadr for row r int nnz = rownnz[r], adr = rowadr[r]; // update row r diagonal mjtNum tmp = mat[adr+nnz-1]; - if (tmp=0; i--) { + for (int i=n-1; i >= 0; i--) { if (res[i]) { // get rowadr[i], rownnz[i] const int adr = rowadr[i], nnz = rownnz[i]; @@ -222,19 +222,19 @@ void mju_cholSolveSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int mjtNum tmp = res[i]; // x(j) -= L(i,j)*x(i), j=0:i-1 - for (int j=0; j1) { + if (nnz > 1) { res[i] -= mju_dotSparse(mat+adr, res, nnz-1, colind+adr); // modulo AVX, the above line does // for (int j=0; j=0) { + while (i >= 0) { // get rownnz and rowadr for this row int nnz = rownnz[x_ind[i]], adr = rowadr[x_ind[i]]; // compute quantities mjtNum tmp = mat[adr+nnz-1]*mat[adr+nnz-1] + (flg_plus ? x[i]*x[i] : -x[i]*x[i]); - if (tmp0 ? mju_dot(mat+adr_jj-width_jj, mat+adr_jj-width_jj, width_jj) : 0; + mjtNum left_ij = width_jj > 0 ? mju_dot(mat+adr_jj-width_jj, mat+adr_jj-width_jj, width_jj) : 0; mjtNum Ljj = diagadd + diagmul*mat[adr_jj] + mat[adr_jj] - left_ij; // update mindiag - if (Ljjj, sparse part - for (int i=j+1; i<=j+height; i++) { + for (int i=j+1; i <= j+height; i++) { // number of non-zeros left of (i,j) int width_ij = mjMIN(j, nband-1-i+j); @@ -351,18 +351,18 @@ mjtNum mju_cholFactorBand(mjtNum* mat, int ntotal, int nband, int ndense, int adr_ij = (i+1)*nband-1-i+j; // in-place computation of L(i,j) - left_ij = width_ij>0 ? mju_dot(mat+adr_jj-width_ij, mat+adr_ij-width_ij, width_ij) : 0; + left_ij = width_ij > 0 ? mju_dot(mat+adr_jj-width_ij, mat+adr_ij-width_ij, width_ij) : 0; mat[adr_ij] = scale * (mat[adr_ij] - left_ij); } // compute L(i,j) for i>j, dense part - for (int i=nsparse; i0 ? mju_dot(mat+adr_jj-width_jj, mat+adr_ij-width_jj, width_jj) : 0; + left_ij = width_jj > 0 ? mju_dot(mat+adr_jj-width_jj, mat+adr_ij-width_jj, width_jj) : 0; mat[adr_ij] = scale * (mat[adr_ij] - left_ij); } @@ -371,7 +371,7 @@ mjtNum mju_cholFactorBand(mjtNum* mat, int ntotal, int nband, int ndense, } // dense part - for (int j=nsparse; jj - for (int i=j+1; i=nsparse; i--) { - for (int j=i+1; j= nsparse; i--) { + for (int j=i+1; j < ntotal; j++) { res[i] -= mat[nsparse*nband+(j-nsparse)*ntotal+i] * res[j]; } @@ -457,16 +457,16 @@ void mju_cholSolveBand(mjtNum* res, const mjtNum* mat, const mjtNum* vec, } // sparse part - for (int i=nsparse-1; i>=0; i--) { + for (int i=nsparse-1; i >= 0; i--) { // number of non-zeros below (i,i), sparse part height = mjMIN(nsparse-1-i, nband-1); // sparse rows - for (int j=i+1; j<=i+height; j++) + for (int j=i+1; j <= i+height; j++) res[i] -= mat[(j+1)*nband-1-(j-i)] * res[j]; // dense rows - for (int j=nsparse; j=0; i--) { + for (int i=n-1; i >= 0; i--) { // get address of last remaining element of row i, adjust remaining counter int ii = rowadr[i] + remaining[i] - 1; remaining[i]--; // make sure ii is on diagonal - if (colind[ii]!=i) { + if (colind[ii] != i) { mju_error("missing diagonal element in mju_factorLUSparse"); } // make sure diagonal is not too small - if (mju_abs(LU[ii])=0; j--) { + for (int j=i-1; j >= 0; j--) { // get address of last remaining element of row j int ji = rowadr[j] + remaining[j] - 1; // process row j if (j,i) is non-zero - if (colind[ji]==i) { + if (colind[ji] == i) { // adjust remaining counter remaining[j]--; @@ -627,15 +627,15 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch, // (j,k) = (j,k) - (i,k) * (j,i) for kcolind[jcnt]) { + else if (colind[icnt] > colind[jcnt]) { // advance j counter jcnt++; } @@ -647,7 +647,7 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch, } // make sure both rows fully processed - if (icnt!=rowadr[i]+remaining[i] || jcnt!=rowadr[j]+remaining[j]) { + if (icnt != rowadr[i]+remaining[i] || jcnt != rowadr[j]+remaining[j]) { mju_error("row processing incomplete in mju_factorLUSparse"); } } @@ -655,8 +655,8 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch, } // make sure remaining points to diagonal - for (int i=0; i=0; i--) { + for (int i=n-1; i >= 0; i--) { // init: diagonal of (U+I) is 1 res[i] = vec[i]; // res[i] -= sum_k>i res[k]*LU(i,k) int j = rownnz[i] - 1; - while (colind[rowadr[i]+j]>i) { + while (colind[rowadr[i]+j] > i) { res[i] -= res[colind[rowadr[i]+j]] * LU[rowadr[i]+j]; j--; } // make sure j points to diagonal - if (colind[rowadr[i]+j]!=i) { + if (colind[rowadr[i]+j] != i) { mju_error("diagonal of U not reached in mju_factorLUSparse"); } } //------------------ solve L*res(new) = res - for (int i=0; ifabs(D[2]) && fabs(D[1])>fabs(D[5])) { + if (fabs(D[1]) > fabs(D[2]) && fabs(D[1]) > fabs(D[5])) { rk = 0; // row ck = 1; // column rotk = 2; // rotation axis - } else if (fabs(D[2])>fabs(D[5])) { + } else if (fabs(D[2]) > fabs(D[5])) { rk = 0; ck = 2; rotk = 1; @@ -747,13 +747,13 @@ int mju_eig3(mjtNum* eigval, mjtNum* eigvec, mjtNum quat[4], const mjtNum mat[9] } // terminate if max off-diagonal element too small - if (fabs(D[3*rk+ck])=0) { + if (tau >= 0) { t = 1.0/(tau + mju_sqrt(1 + tau*tau)); } else { t = -1.0/(-tau + mju_sqrt(1 + tau*tau)); @@ -761,14 +761,14 @@ int mju_eig3(mjtNum* eigval, mjtNum* eigvec, mjtNum quat[4], const mjtNum mat[9] c = 1.0/mju_sqrt(1 + t*t); // terminate if cosine too close to 1 - if (c>1.0-eigEPS) { + if (c > 1.0-eigEPS) { break; } // express rotation as quaternion tmp[1] = tmp[2] = tmp[3] = 0; - tmp[rotk+1] = (tau>=0 ? -mju_sqrt(0.5-0.5*c) : mju_sqrt(0.5-0.5*c)); - if (rotk==1) { + tmp[rotk+1] = (tau >= 0 ? -mju_sqrt(0.5-0.5*c) : mju_sqrt(0.5-0.5*c)); + if (rotk == 1) { tmp[rotk+1] = -tmp[rotk+1]; } tmp[0] = mju_sqrt(1.0 - tmp[rotk+1]*tmp[rotk+1]); @@ -780,7 +780,7 @@ int mju_eig3(mjtNum* eigval, mjtNum* eigvec, mjtNum quat[4], const mjtNum mat[9] } // sort eigenvalues in decreasing order (bubblesort: 0, 1, 0) - for (int j=0; j<3; j++) { + for (int j=0; j < 3; j++) { int j1 = j%2; // lead index if (eigval[j1] < eigval[j1+1]) { @@ -825,12 +825,12 @@ int mju_QCQP2(mjtNum* res, const mjtNum* Ain, const mjtNum* bin, // Newton iteration la = 0; - for (int iter=0; iter<20; iter++) { + for (int iter=0; iter < 20; iter++) { // det(A+la) det = (A11+la)*(A22+la) - A12*A12; // check SPD, with 1e-10 threshold - if (det<1e-10) { + if (det < 1e-10) { res[0] = 0; res[1] = 0; return 0; @@ -850,7 +850,7 @@ int mju_QCQP2(mjtNum* res, const mjtNum* Ain, const mjtNum* bin, val = v1*v1 + v2*v2 - r*r; // check for convergence, or initial solution inside constraint set - if (val<1e-10) { + if (val < 1e-10) { break; } @@ -859,7 +859,7 @@ int mju_QCQP2(mjtNum* res, const mjtNum* Ain, const mjtNum* bin, // compute update, exit if too small mjtNum delta = -val/deriv; - if (delta<1e-10) { + if (delta < 1e-10) { break; } @@ -871,7 +871,7 @@ int mju_QCQP2(mjtNum* res, const mjtNum* Ain, const mjtNum* bin, res[0] = v1*d[0]; res[1] = v2*d[1]; - return (la!=0); + return (la != 0); } @@ -897,7 +897,7 @@ int mju_QCQP3(mjtNum* res, const mjtNum* Ain, const mjtNum* bin, // Newton iteration la = 0; - for (int iter=0; iter<20; iter++) { + for (int iter=0; iter < 20; iter++) { // unscaled P P11 = (A22+la)*(A33+la) - A23*A23; P22 = (A11+la)*(A33+la) - A13*A13; @@ -910,7 +910,7 @@ int mju_QCQP3(mjtNum* res, const mjtNum* Ain, const mjtNum* bin, det = (A11+la)*P11 + A12*P12 + A13*P13; // check SPD, with 1e-10 threshold - if (det<1e-10) { + if (det < 1e-10) { res[0] = 0; res[1] = 0; res[2] = 0; @@ -937,7 +937,7 @@ int mju_QCQP3(mjtNum* res, const mjtNum* Ain, const mjtNum* bin, val = v1*v1 + v2*v2 + v3*v3 - r*r; // check for convergence, or initial solution inside constraint set - if (val<1e-10) { + if (val < 1e-10) { break; } @@ -947,7 +947,7 @@ int mju_QCQP3(mjtNum* res, const mjtNum* Ain, const mjtNum* bin, // compute update, exit if too small mjtNum delta = -val/deriv; - if (delta<1e-10) { + if (delta < 1e-10) { break; } @@ -960,7 +960,7 @@ int mju_QCQP3(mjtNum* res, const mjtNum* Ain, const mjtNum* bin, res[1] = v2*d[1]; res[2] = v3*d[2]; - return (la!=0); + return (la != 0); } @@ -974,25 +974,25 @@ int mju_QCQP(mjtNum* res, const mjtNum* Ain, const mjtNum* bin, mjtNum la, val, deriv, tmp[5]; // check size - if (n>5) { + if (n > 5) { mju_error("mju_QCQP supports n up to 5"); } // scale A,b so that constraint becomes x'*x <= r*r - for (int i=0; i= upper[i]) { mju_error("mju_boxQP: upper bounds must be stricly larger than lower bounds"); } @@ -1200,14 +1200,14 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs } // full index set (no clamping) - for (int i=0; i 0 ) || ( upper && res[i] == upper[i] && grad[i] < 0 ); } // build index of free dimensions, count them nfree = 0; - for (int i=0; iupper[i]) { + } else if (upper && candidate[i] > upper[i]) { candidate[i] = upper[i]; } } @@ -1352,7 +1352,7 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs // increment and break if step is too small nstep++; step = step*backtrack; - if (step= rownnz[r]) r++; @@ -369,7 +369,7 @@ void mju_transposeSparse(mjtNum* res, const mjtNum* mat, int nr, int nc, } // shift back row addresses - for (int i = nc-1; i>0; i--) { + for (int i = nc-1; i > 0; i--) { res_rowadr[i] = res_rowadr[i-1]; } @@ -387,9 +387,9 @@ void mju_superSparse(int nr, int* rowsuper, } // find match to child - for (int r=0; r=0; r--) { + for (int r=nr-2; r >= 0; r--) { if (rowsuper[r]) { rowsuper[r] += rowsuper[r+1]; } @@ -424,14 +424,14 @@ void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr, int nchain = 0; int* res_colind = NULL; - for (int r=0; r0 && rowsuperT[r-1]>0) { + if (rowsuperT && r > 0 && rowsuperT[r-1] > 0) { res_rownnz[r] = res_rownnz[r - 1]; // fill in upper triangle - for (int j=0; j r) { + if (col_mat > r) { break; } @@ -472,7 +472,7 @@ void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr, chain[inew + nnewchain++] = col_mat; } - while (adr0 && res_colind[nchain-1]==r) { + if (nchain > 0 && res_colind[nchain-1] == r) { nchain_end = nchain - 1; } - for (int j=0; j0 && rowsuperT[i-1]) { + if (rowsuperT && i > 0 && rowsuperT[i-1]) { res_rownnz[i] = res_rownnz[i-1]; memcpy(cols, res_colind+res_rowadr[i-1], res_rownnz[i]*sizeof(int)); } // iterate through each row of M' int end = rowadrT[i] + rownnzT[i]; - for (int r = rowadrT[i]; ri) { + if (cc > i) { break; } @@ -566,16 +566,16 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT, markers[cc] = 1; // since i is the rightmost column, it can be inserted at the end - if (cc==i) { + if (cc == i) { cols[res_rownnz[i]++] = cc; continue; } // insert col in order via binary search int l = 0, h = res_rownnz[i]; - while (l> 1; - if (cols[m]mjPI) { + if (speed > mjPI) { speed -= 2*mjPI; } speed /= dt; @@ -143,7 +143,7 @@ void mju_subQuat(mjtNum res[3], const mjtNum qa[4], const mjtNum qb[4]) { // convert quaternion to 3D rotation matrix void mju_quat2Mat(mjtNum res[9], const mjtNum quat[4]) { // null quat: identity - if (quat[0]==1 && quat[1]==0 && quat[2]==0 && quat[3]==0) { + if (quat[0] == 1 && quat[1] == 0 && quat[2] == 0 && quat[3] == 0) { res[0] = 1; res[1] = 0; res[2] = 0; @@ -186,7 +186,7 @@ void mju_quat2Mat(mjtNum res[9], const mjtNum quat[4]) { // convert 3D rotation matrix to quaternion void mju_mat2Quat(mjtNum quat[4], const mjtNum mat[9]) { // q0 largest - if (mat[0]+mat[4]+mat[8]>0) { + if (mat[0]+mat[4]+mat[8] > 0) { quat[0] = 0.5 * mju_sqrt(1 + mat[0] + mat[4] + mat[8]); quat[1] = 0.25 * (mat[7] - mat[5]) / quat[0]; quat[2] = 0.25 * (mat[2] - mat[6]) / quat[0]; @@ -194,7 +194,7 @@ void mju_mat2Quat(mjtNum quat[4], const mjtNum mat[9]) { } // q1 largest - else if (mat[0]>mat[4] && mat[0]>mat[8]) { + else if (mat[0] > mat[4] && mat[0] > mat[8]) { quat[1] = 0.5 * mju_sqrt(1 + mat[0] - mat[4] - mat[8]); quat[0] = 0.25 * (mat[7] - mat[5]) / quat[1]; quat[2] = 0.25 * (mat[1] + mat[3]) / quat[1]; @@ -202,7 +202,7 @@ void mju_mat2Quat(mjtNum quat[4], const mjtNum mat[9]) { } // q2 largest - else if (mat[4]>mat[8]) { + else if (mat[4] > mat[8]) { quat[2] = 0.5 * mju_sqrt(1 - mat[0] + mat[4] - mat[8]); quat[0] = 0.25 * (mat[2] - mat[6]) / quat[2]; quat[1] = 0.25 * (mat[1] + mat[3]) / quat[2]; @@ -255,7 +255,7 @@ void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]) { mju_zero3(quat+1); // normalize vector; if too small, no rotation - if (mju_normalize3(vn)-0.5) { + if (frame[1] < 0.5 && frame[1] > -0.5) { frame[4] = 1; } else { frame[5] = 1; diff --git a/src/engine/engine_vfs.c b/src/engine/engine_vfs.c index b6f5aea3..6371283e 100644 --- a/src/engine/engine_vfs.c +++ b/src/engine/engine_vfs.c @@ -29,17 +29,17 @@ static void vfs_strippath(char* newname, const char* oldname) { // find last delimiter int i; - for (i=sz-1; i>=0; i--) { - if (oldname[i]=='\\' || oldname[i]=='/') { + for (i=sz-1; i >= 0; i--) { + if (oldname[i] == '\\' || oldname[i] == '/') { break; } } // check resulting length - if (sz-(i+1)>=mjMAXVFSNAME) { + if (sz-(i+1) >= mjMAXVFSNAME) { mju_error("Filename too long in VFS"); } - if (sz-(i+1)<=0) { + if (sz-(i+1) <= 0) { mju_error("Empty filename in VFS"); } @@ -47,8 +47,8 @@ static void vfs_strippath(char* newname, const char* oldname) { mju_strncpy(newname, oldname+i+1, mjMAXVFSNAME); // make lowercase - for (int j=strlen(newname)-1; j>=0; j--) { - if (newname[j]>='A' && newname[j]<='Z') { + for (int j=strlen(newname)-1; j >= 0; j--) { + if (newname[j] >= 'A' && newname[j] <= 'Z') { newname[j] = (char)(((int)newname[j]) +'a' - 'A'); } } @@ -66,7 +66,7 @@ void mj_defaultVFS(mjVFS* vfs) { // add file to VFS, return 0: success, 1: full, 2: repeated name, -1: failed to load int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename) { // check vfs size - if (vfs->nfile>=mjMAXVFS-1) { + if (vfs->nfile >= mjMAXVFS-1) { return 1; } @@ -84,8 +84,8 @@ int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename) { vfs_strippath(newname, filename); // check for repeated name - for (int i=0; infile; i++) { - if (strncmp(newname, vfs->filename[i], mjMAXVFSNAME)==0) { + for (int i=0; i < vfs->nfile; i++) { + if (strncmp(newname, vfs->filename[i], mjMAXVFSNAME) == 0) { return 2; } } @@ -111,12 +111,12 @@ int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename) { // make empty file in VFS, return 0: success, 1: full, 2: repeated name int mj_makeEmptyFileVFS(mjVFS* vfs, const char* filename, int filesize) { // check vfs size - if (vfs->nfile>=mjMAXVFS-1) { + if (vfs->nfile >= mjMAXVFS-1) { return 1; } // check filesize - if (filesize<=0) { + if (filesize <= 0) { mju_error("mj_makeEmptyFileVFS expects positive filesize"); } @@ -125,8 +125,8 @@ int mj_makeEmptyFileVFS(mjVFS* vfs, const char* filename, int filesize) { vfs_strippath(newname, filename); // check for repeated name - for (int i=0; infile; i++) { - if (strncmp(newname, vfs->filename[i], mjMAXVFSNAME)==0) { + for (int i=0; i < vfs->nfile; i++) { + if (strncmp(newname, vfs->filename[i], mjMAXVFSNAME) == 0) { return 2; } } @@ -156,8 +156,8 @@ int mj_findFileVFS(const mjVFS* vfs, const char* filename) { char newname[mjMAXVFSNAME]; vfs_strippath(newname, filename); // find specific file - for (int i=0; infile; i++) { - if (strncmp(newname, vfs->filename[i], mjMAXVFSNAME)==0) { + for (int i=0; i < vfs->nfile; i++) { + if (strncmp(newname, vfs->filename[i], mjMAXVFSNAME) == 0) { return i; } } @@ -174,13 +174,13 @@ int mj_deleteFileVFS(mjVFS* vfs, const char* filename) { vfs_strippath(newname, filename); // find specified file - for (int i=0; infile; i++) { - if (strncmp(newname, vfs->filename[i], mjMAXVFSNAME)==0) { + for (int i=0; i < vfs->nfile; i++) { + if (strncmp(newname, vfs->filename[i], mjMAXVFSNAME) == 0) { // free buffer mju_free(vfs->filedata[i]); // scroll remaining files forward - for (int j=i; jnfile-1; j++) { + for (int j=i; j < vfs->nfile-1; j++) { mjSTRNCPY(vfs->filename[j], vfs->filename[j+1]); vfs->filesize[j] = vfs->filesize[j+1]; vfs->filedata[j] = vfs->filedata[j+1]; @@ -204,7 +204,7 @@ int mj_deleteFileVFS(mjVFS* vfs, const char* filename) { // delete all files from VFS void mj_deleteVFS(mjVFS* vfs) { - for (int i=0; infile; i++) { + for (int i=0; i < vfs->nfile; i++) { mju_free(vfs->filedata[i]); } diff --git a/src/engine/engine_vis_init.c b/src/engine/engine_vis_init.c index 1a889c19..c2a9c975 100644 --- a/src/engine/engine_vis_init.c +++ b/src/engine/engine_vis_init.c @@ -119,7 +119,7 @@ void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) { mjv_freeScene(scn); // allocate geom buffers - if (maxgeom>0) { + if (maxgeom > 0) { // allocate scn->maxgeom = maxgeom; scn->geoms = (mjvGeom*) mju_malloc(maxgeom*sizeof(mjvGeom)); @@ -132,8 +132,8 @@ void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) { } // set default OpenGL options - for (int i=0; iflags[i] = (mjRNDSTRING[i][1][0]=='1'); + for (int i=0; i < mjNRNDFLAG; i++) { + scn->flags[i] = (mjRNDSTRING[i][1][0] == '1'); } // set default model transformation @@ -152,7 +152,7 @@ void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) { // compute number of vertices in all skins int nskin = m->nskin; int totvert = 0; - for (int i=0; iskin_vertnum[i]; } @@ -173,7 +173,7 @@ void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) { } // copy constant data - for (int i=0; iskinfacenum[i] = m->skin_facenum[i]; scn->skinvertadr[i] = m->skin_vertadr[i]; scn->skinvertnum[i] = m->skin_vertnum[i]; @@ -214,8 +214,8 @@ void mjv_defaultOption(mjvOption* vopt) { vopt->label = mjLABEL_NONE; vopt->frame = mjFRAME_NONE; - for (int i=0; igeomgroup[i] = state; vopt->sitegroup[i] = state; vopt->jointgroup[i] = state; @@ -224,8 +224,8 @@ void mjv_defaultOption(mjvOption* vopt) { vopt->skingroup[i] = state; } - for (int i=0; iflags[i] = (mjVISSTRING[i][1][0]=='1'); + for (int i=0; i < mjNVISFLAG; i++) { + vopt->flags[i] = (mjVISSTRING[i][1][0] == '1'); } vopt->bvh_depth = 1; @@ -326,9 +326,9 @@ void mjv_defaultFigure(mjvFigure* fig) { mjSTRNCPY(fig->minwidth, "XXX"); // set line colors - for (int n=0; nlinergb[n][0] = _linergb[n][0]; fig->linergb[n][1] = _linergb[n][1]; fig->linergb[n][2] = _linergb[n][2]; @@ -348,7 +348,7 @@ void mjv_defaultFigure(mjvFigure* fig) { // compute rbound for mjvGeom float mjv_rbound(const mjvGeom* geom) { // model geom: return - if (geom->objtype==mjOBJ_GEOM) { + if (geom->objtype == mjOBJ_GEOM) { return geom->modelrbound; } diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index aad3a82a..cdfc6279 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -37,7 +37,7 @@ void mjv_room2model(mjtNum* modelpos, mjtNum* modelquat, const mjtNum* roompos, mjtNum translate[3], rotate[4], invpos[3], invquat[4]; // check scale - if (scn->scalescale < mjMINVAL) { mju_error("mjvScene scale too small in mjv_room2model"); } @@ -72,7 +72,7 @@ void mjv_model2room(mjtNum* roompos, mjtNum* roomquat, const mjtNum* modelpos, mjtNum translate[3], rotate[4]; // check scale - if (scn->scalescale < mjMINVAL) { mju_error("mjvScene scale too small in mjv_model2room"); } @@ -104,7 +104,7 @@ void mjv_cameraInModel(mjtNum* headpos, mjtNum* forward, mjtNum* up, const mjvSc mjtNum modelpos[3], modelquat[4], modelmat[9]; // check znear - if (scn->camera[0].frustum_nearcamera[1].frustum_nearcamera[0].frustum_near < mjMINVAL || scn->camera[1].frustum_near < mjMINVAL) { mju_error("mjvScene frustum_near too small in mjv_cameraInModel"); } @@ -120,7 +120,7 @@ void mjv_cameraInModel(mjtNum* headpos, mjtNum* forward, mjtNum* up, const mjvSc } // average over cameras - for (int n=0; n<2; n++) { + for (int n=0; n < 2; n++) { // convert pos, fwd, u mju_f2n(pos, scn->camera[n].pos, 3); mju_f2n(fwd, scn->camera[n].forward, 3); @@ -177,7 +177,7 @@ void mjv_cameraInRoom(mjtNum* headpos, mjtNum* forward, mjtNum* up, const mjvSce mjtNum pos[3], fwd[3], u[3]; // check znear - if (scn->camera[0].frustum_nearcamera[1].frustum_nearcamera[0].frustum_near < mjMINVAL || scn->camera[1].frustum_near < mjMINVAL) { mju_error("mjvScene frustum_near too small in mjv_cameraInRoom"); } @@ -193,7 +193,7 @@ void mjv_cameraInRoom(mjtNum* headpos, mjtNum* forward, mjtNum* up, const mjvSce } // average over cameras - for (int n=0; n<2; n++) { + for (int n=0; n < 2; n++) { // convert pos, fwd, u mju_f2n(pos, scn->camera[n].pos, 3); mju_f2n(fwd, scn->camera[n].forward, 3); @@ -227,7 +227,7 @@ mjtNum mjv_frustumHeight(const mjvScene* scn) { mjtNum height; // check znear - if (scn->camera[0].frustum_nearcamera[1].frustum_nearcamera[0].frustum_near < mjMINVAL || scn->camera[1].frustum_near < mjMINVAL) { mju_error("mjvScene frustum_near too small in mjv_frustumHeight"); } @@ -311,7 +311,7 @@ void mjv_moveCamera(const mjModel* m, int action, mjtNum reldx, mjtNum reldy, mjtNum vec[3], dif[3], scl; // fixed camera: nothing to do - if (cam->type==mjCAMERA_FIXED) { + if (cam->type == mjCAMERA_FIXED) { return; } @@ -326,7 +326,7 @@ void mjv_moveCamera(const mjModel* m, int action, mjtNum reldx, mjtNum reldy, case mjMOUSE_MOVE_V: case mjMOUSE_MOVE_H: // do not move lookat point of tracking camera - if (cam->type==mjCAMERA_TRACKING) { + if (cam->type == mjCAMERA_TRACKING) { return; } @@ -405,7 +405,7 @@ void mjv_movePerturb(const mjModel* m, const mjData* d, int action, mjtNum reldx mju_mulQuat(xiquat, d->xquat+4*sel, m->body_iquat+4*sel); // limit rotation relative to selected body - if (sel>0 && selnbody) { + if (sel > 0 && sel < m->nbody) { // q2 = neg(selbody) * refquat mjtNum q2[4]; mju_negQuat(q1, xiquat); @@ -417,7 +417,7 @@ void mjv_movePerturb(const mjModel* m, const mjData* d, int action, mjtNum reldx scl = mju_normalize3(dif); // check limit: +/- 90 deg allowed - if (scl<-mjPI*0.5 || scl>mjPI*0.5) { + if (scl < -mjPI*0.5 || scl > mjPI*0.5) { // clamp angle scl = mju_max(-mjPI*0.5, mju_min(mjPI*0.5, scl)); @@ -467,8 +467,8 @@ void mjv_moveModel(const mjModel* m, int action, mjtNum reldx, mjtNum reldy, case mjMOUSE_ROTATE_V: case mjMOUSE_ROTATE_H: // construct rotation vector - for (int i=0; i<3; i++) { - if (action==mjMOUSE_ROTATE_V) { + for (int i=0; i < 3; i++) { + if (action == mjMOUSE_ROTATE_V) { vec[i] = roomup[i]*reldx + roomright[i]*reldy; } else { vec[i] = roomforward[i]*reldx + roomright[i]*reldy; @@ -489,22 +489,22 @@ void mjv_moveModel(const mjModel* m, int action, mjtNum reldx, mjtNum reldy, break; case mjMOUSE_MOVE_V: - for (int i=0; i<3; i++) { + for (int i=0; i < 3; i++) { scn->translate[i] += (float)(roomright[i]*reldx - roomup[i]*reldy) * m->stat.extent; } break; case mjMOUSE_MOVE_H: - for (int i=0; i<3; i++) { + for (int i=0; i < 3; i++) { scn->translate[i] += (float)(roomright[i]*reldx - roomforward[i]*reldy) * m->stat.extent; } break; case mjMOUSE_ZOOM: scn->scale += (float)(mju_log(1 + scn->scale/3) * reldy * 3); - if (scn->scale<0.01f) { + if (scn->scale < 0.01f) { scn->scale = 0.01f; - } else if (scn->scale>100.0f) { + } else if (scn->scale > 100.0f) { scn->scale = 100.0f; } break; @@ -528,7 +528,7 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur mjtNum* jacM2 = mj_stackAlloc(d, 3*nv); // invalid selected body: return - if (sel<=0 || sel>=m->nbody) { + if (sel <= 0 || sel >= m->nbody) { return; } @@ -570,7 +570,7 @@ void mjv_applyPerturbPose(const mjModel* m, mjData* d, const mjvPerturb* pert, i mjtNum *Rpos, *Rquat, *Cpos, *Cquat; // exit if nothing to do - if (sel<=0 || sel>=m->nbody || !(pert->active | pert->active2)) { + if (sel <= 0 || sel >= m->nbody || !(pert->active | pert->active2)) { return; } @@ -582,23 +582,23 @@ void mjv_applyPerturbPose(const mjModel* m, mjData* d, const mjvPerturb* pert, i mju_mulPose(refpos, refquat, pert->refpos, pert->refquat, pos1, quat1); // mocap body - if (m->body_mocapid[sel]>=0) { + if (m->body_mocapid[sel] >= 0) { // copy ref pose into mocap pose mju_copy3(d->mocap_pos + 3*m->body_mocapid[sel], refpos); mju_copy4(d->mocap_quat + 4*m->body_mocapid[sel], refquat); } // floating body, paused - else if (flg_paused && m->body_jntnum[sel]==1 && - m->jnt_type[m->body_jntadr[sel]]==mjJNT_FREE) { + else if (flg_paused && m->body_jntnum[sel] == 1 && + m->jnt_type[m->body_jntadr[sel]] == mjJNT_FREE) { // copy ref pose into qpos mju_copy3(d->qpos + m->jnt_qposadr[m->body_jntadr[sel]], refpos); mju_copy4(d->qpos + m->jnt_qposadr[m->body_jntadr[sel]] + 3, refquat); } // child of floating body, paused - else if (flg_paused && m->body_jntnum[rootid]==1 && - m->jnt_type[m->body_jntadr[rootid]]==mjJNT_FREE) { + else if (flg_paused && m->body_jntnum[rootid] == 1 && + m->jnt_type[m->body_jntadr[rootid]] == mjJNT_FREE) { // get pointers to root Rpos = d->qpos + m->jnt_qposadr[m->body_jntadr[rootid]]; Rquat = Rpos + 3; @@ -621,7 +621,7 @@ void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb* pert) int sel = pert->select; // exit if nothing to do - if (sel<0 || sel>=m->nbody || !(pert->active | pert->active2)) { + if (sel < 0 || sel >= m->nbody || !(pert->active | pert->active2)) { return; } @@ -763,7 +763,7 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt, mjtNum skindist = -1; *skinid = -1; if (vopt->flags[mjVIS_SKIN]) { - for (int i=0; inskin; i++) { + for (int i=0; i < m->nskin; i++) { // process one skin int vertid; mjtNum newdist = mju_raySkin(m->skin_facenum[i], m->skin_vertnum[i], @@ -772,24 +772,24 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt, pos, ray, &vertid); // update if closer intersection found - if (newdist>=0 && (newdist= 0 && (newdist < skindist || skindist < 0)) { // assign result skindist = newdist; // find body with largest weight for this vertex float bestweight = -1; for (int j=m->skin_boneadr[i]; - jskin_boneadr[i]+m->skin_bonenum[i]; + j < m->skin_boneadr[i]+m->skin_bonenum[i]; j++) { for (int k=m->skin_bonevertadr[j]; - kskin_bonevertadr[j]+m->skin_bonevertnum[j]; + k < m->skin_bonevertadr[j]+m->skin_bonevertnum[j]; k++) { // get vertex id and weight int vid = m->skin_bonevertid[k]; float vweight = m->skin_bonevertweight[k]; // update if matching id and bigger weight - if (vid==vertid && vweight>bestweight) { + if (vid == vertid && vweight > bestweight) { bestweight = vweight; bodyid = m->skin_bonebodyid[j]; *skinid = i; @@ -801,12 +801,12 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt, } // no intersection - if (geomdist<0 && skindist<0) { + if (geomdist < 0 && skindist < 0) { return -1; } // geom only, or geom closer than skin - else if (geomdist>=0 && (skindist<0 || skindist>geomdist)) { + else if (geomdist >= 0 && (skindist < 0 || skindist > geomdist)) { mju_addScl3(selpnt, pos, ray, geomdist); *skinid = -1; return m->geom_bodyid[*geomid]; diff --git a/src/engine/engine_vis_state.c b/src/engine/engine_vis_state.c index e3dbd213..2020dc46 100644 --- a/src/engine/engine_vis_state.c +++ b/src/engine/engine_vis_state.c @@ -141,8 +141,8 @@ void mjv_assignFromSceneState(const mjvSceneState* scnstate, mjModel* m, mjData* memset(m, 0, sizeof(mjModel)); #ifdef MEMORY_SANITIZER - // Tell msan to treat the entire buffer as uninitialized - __msan_allocated_memory(m, sizeof(mjModel)); + // Tell msan to treat the entire buffer as uninitialized + __msan_allocated_memory(m, sizeof(mjModel)); #endif #define X(var) @@ -166,8 +166,8 @@ void mjv_assignFromSceneState(const mjvSceneState* scnstate, mjModel* m, mjData* memset(d, 0, sizeof(mjData)); #ifdef MEMORY_SANITIZER - // Tell msan to treat the entire buffer as uninitialized - __msan_allocated_memory(d, sizeof(mjData)); + // Tell msan to treat the entire buffer as uninitialized + __msan_allocated_memory(d, sizeof(mjData)); #endif memcpy(d->warning, scnstate->data.warning, sizeof(d->warning)); @@ -247,7 +247,7 @@ void mjv_updateSceneState(const mjModel* m, mjData* d, const mjvOption* opt, if (m->nplugin) { const int nslot = mjp_pluginCount(); // iterate over plugins, call visualize if defined - for (int i=0; inplugin; i++) { + for (int i=0; i < m->nplugin; i++) { const int slot = m->plugin[i]; const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot); if (!plugin) { diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index d824c259..19edfc0b 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -67,18 +67,18 @@ static void makeLabel(const mjModel* m, mjtObj type, int id, char* label) { // return if there is no space in buffer -#define START \ - if ( scn->ngeom>=scn->maxgeom ) { \ - mj_warning(d, mjWARN_VGEOMFULL, scn->maxgeom); \ - return; \ - } else { \ - thisgeom = scn->geoms + scn->ngeom; \ - mjv_initGeom(thisgeom, mjGEOM_NONE, NULL, NULL, NULL, NULL); \ - thisgeom->objtype = objtype; \ - thisgeom->objid = i; \ - thisgeom->category = category; \ - thisgeom->segid = scn->ngeom; \ - } +#define START \ + if ( scn->ngeom>=scn->maxgeom ) { \ + mj_warning(d, mjWARN_VGEOMFULL, scn->maxgeom); \ + return; \ + } else { \ + thisgeom = scn->geoms + scn->ngeom; \ + mjv_initGeom(thisgeom, mjGEOM_NONE, NULL, NULL, NULL, NULL); \ + thisgeom->objtype = objtype; \ + thisgeom->objid = i; \ + thisgeom->category = category; \ + thisgeom->segid = scn->ngeom; \ + } // advance counter @@ -98,12 +98,12 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags, mjtByte split; // fast return if all relevant features are disabled - if (!flags[mjVIS_CONTACTPOINT] && !flags[mjVIS_CONTACTFORCE] && vopt->frame!=mjFRAME_CONTACT) { + if (!flags[mjVIS_CONTACTPOINT] && !flags[mjVIS_CONTACTFORCE] && vopt->frame != mjFRAME_CONTACT) { return; } // loop over contacts included in impulse solver - for (int i=0; incon; i++) { + for (int i=0; i < d->ncon; i++) { // get pointer con = d->contact + i; @@ -122,14 +122,14 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags, mju_n2f(thisgeom->mat, mat, 9); // different colors for included and excluded contacts - if (d->contact[i].efc_address>=0) { + if (d->contact[i].efc_address >= 0) { f2f(thisgeom->rgba, m->vis.rgba.contactpoint, 4); } else { f2f(thisgeom->rgba, m->vis.rgba.contactgap, 4); } // label contacting geom names or ids - if (vopt->label==mjLABEL_CONTACTPOINT) { + if (vopt->label == mjLABEL_CONTACTPOINT) { const char* name1 = mj_id2name(m, mjOBJ_GEOM, con->geom1); char id1[10]; mjSNPRINTF(id1, "%d", con->geom1); @@ -143,18 +143,18 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags, } // contact frame - if (vopt->frame==mjFRAME_CONTACT) { + if (vopt->frame == mjFRAME_CONTACT) { // set length and width of axis cylinders using half regular frame scaling framelength = m->vis.scale.framelength * scl / 2; framewidth = m->vis.scale.framewidth * scl / 2; // draw the three axes (separate geoms) - for (int j=0; j<3; j++) { + for (int j=0; j < 3; j++) { START // prepare axis - for (int k=0; k<3; k++) { - axis[k] = (j==k ? framelength : 0); + for (int k=0; k < 3; k++) { + axis[k] = (j == k ? framelength : 0); } mju_mulMatVec(vec, mat, axis, 3, 3); @@ -168,8 +168,8 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags, con->pos[2] + vec[2]); // set color: R, G or B depending on axis - for (int k=0; k<3; k++) { - thisgeom->rgba[k] = (j==k ? 0.9 : 0); + for (int k=0; k < 3; k++) { + thisgeom->rgba[k] = (j == k ? 0.9 : 0); } thisgeom->rgba[3] = 1; @@ -178,7 +178,7 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags, } // nothing else to do for excluded contacts - if (d->contact[i].efc_address<0) { + if (d->contact[i].efc_address < 0) { continue; } @@ -193,12 +193,12 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags, // get force, fill zeros if only normal mju_zero3(frc); mju_copy(frc, confrc, mjMIN(3, con->dim)); - if (mju_norm3(frc)dim>1); + split = (flags[mjVIS_CONTACTSPLIT] && con->dim > 1); for (int j = (split ? 1 : 0); j < (split ? 3 : 1); j++) { // set vec to combined, normal or friction force, in world frame switch (j) { @@ -225,14 +225,14 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags, body2 = m->geom_bodyid[con->geom2]; // make sure arrow points towards body with higher id - if (body1>body2) { + if (body1 > body2) { mju_scl3(vec, vec, -1); } // one-directional arrow for friction and world, symmetric otherwise START mjv_makeConnector(thisgeom, - body1>0 && body2>0 && !split ? mjGEOM_ARROW2 : mjGEOM_ARROW, + body1 > 0 && body2 > 0 && !split ? mjGEOM_ARROW2 : mjGEOM_ARROW, m->vis.scale.forcewidth * scl, con->pos[0], con->pos[1], @@ -240,8 +240,8 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags, con->pos[0] + vec[0], con->pos[1] + vec[1], con->pos[2] + vec[2]); - f2f(thisgeom->rgba, j==2 ? m->vis.rgba.contactfriction : m->vis.rgba.contactforce, 4); - if (vopt->label==mjLABEL_CONTACTFORCE && j==(split ? 1 : 0)) { + f2f(thisgeom->rgba, j == 2 ? m->vis.rgba.contactfriction : m->vis.rgba.contactforce, 4); + if (vopt->label == mjLABEL_CONTACTFORCE && j == (split ? 1 : 0)) { mjSNPRINTF(thisgeom->label, "%-.3g", mju_norm3(frc)); } FINISH @@ -256,7 +256,7 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags, static void setMaterial(const mjModel* m, mjvGeom* geom, int matid, const float* rgba, const mjtByte* flags) { // set material properties if given - if (matid>=0) { + if (matid >= 0) { f2f(geom->texrepeat, m->mat_texrepeat + 2*matid, 2); f2f(geom->rgba, m->mat_rgba + 4*matid, 4); geom->texuniform = m->mat_texuniform[matid]; @@ -273,17 +273,17 @@ static void setMaterial(const mjModel* m, mjvGeom* geom, int matid, const float* } // use rgba if different from default, or no material given - if (rgba[0]!=0.5f || rgba[1]!=0.5f || rgba[2]!=0.5f || rgba[3]!=1.0f || matid<0) { + if (rgba[0] != 0.5f || rgba[1] != 0.5f || rgba[2] != 0.5f || rgba[3] != 1.0f || matid < 0) { f2f(geom->rgba, rgba, 4); } // set texture - if (flags[mjVIS_TEXTURE] && matid>=0) { + if (flags[mjVIS_TEXTURE] && matid >= 0) { geom->texid = m->mat_texid[matid]; } // scale alpha for dynamic geoms only - if (flags[mjVIS_TRANSPARENT] && (geom->category==mjCAT_DYNAMIC)) { + if (flags[mjVIS_TRANSPARENT] && (geom->category == mjCAT_DYNAMIC)) { geom->rgba[3] *= m->vis.map.alpha; } } @@ -300,9 +300,9 @@ void mjv_makeConnector(mjvGeom* geom, int type, mjtNum width, mjtNum quat[4], mat[9], dif[3] = {b0-a0, b1-a1, b2-a2}; // require connector-compatible type - if (type!=mjGEOM_CAPSULE && type!=mjGEOM_CYLINDER && - type!=mjGEOM_ARROW && type!=mjGEOM_ARROW1 && type!=mjGEOM_ARROW2 - && type!=mjGEOM_LINE) { + if (type != mjGEOM_CAPSULE && type != mjGEOM_CYLINDER && + type != mjGEOM_ARROW && type != mjGEOM_ARROW1 && type != mjGEOM_ARROW2 + && type != mjGEOM_LINE) { mju_error("Invalid geom type %d for connector", type); } @@ -314,7 +314,7 @@ void mjv_makeConnector(mjvGeom* geom, int type, mjtNum width, geom->size[2] = (float)mju_norm3(dif); // cylinder and capsule are centered, and size[0] is "radius" - if (type==mjGEOM_CAPSULE || type==mjGEOM_CYLINDER) { + if (type == mjGEOM_CAPSULE || type == mjGEOM_CYLINDER) { geom->pos[0] = 0.5*(a0 + b0); geom->pos[1] = 0.5*(a1 + b1); geom->pos[2] = 0.5*(a2 + b2); @@ -345,26 +345,26 @@ void mjv_initGeom(mjvGeom* geom, int type, const mjtNum* size, // set size (for XYZ scaling) if (size) { switch (type) { - case mjGEOM_SPHERE: - geom->size[0] = (float)size[0]; - geom->size[1] = (float)size[0]; - geom->size[2] = (float)size[0]; - break; + case mjGEOM_SPHERE: + geom->size[0] = (float)size[0]; + geom->size[1] = (float)size[0]; + geom->size[2] = (float)size[0]; + break; - case mjGEOM_CAPSULE: - geom->size[0] = (float)size[0]; - geom->size[1] = (float)size[0]; - geom->size[2] = (float)size[1]; - break; + case mjGEOM_CAPSULE: + geom->size[0] = (float)size[0]; + geom->size[1] = (float)size[0]; + geom->size[2] = (float)size[1]; + break; - case mjGEOM_CYLINDER: - geom->size[0] = (float)size[0]; - geom->size[1] = (float)size[0]; - geom->size[2] = (float)size[1]; - break; + case mjGEOM_CYLINDER: + geom->size[0] = (float)size[0]; + geom->size[1] = (float)size[0]; + geom->size[2] = (float)size[1]; + break; - default: - mju_n2f(geom->size, size, 3); + default: + mju_n2f(geom->size, size, 3); } } else { geom->size[0] = 0.1f; @@ -451,7 +451,7 @@ static void mixcolor(float rgba[4], const float ref[4], int flg1, int flg2) { // a body is static if it is welded to the world and is not a mocap body static int bodycategory(const mjModel* m, int bodyid) { - if (m->body_weldid[bodyid]==0 && m->body_mocapid[bodyid]==-1) { + if (m->body_weldid[bodyid] == 0 && m->body_mocapid[bodyid] == -1) { return mjCAT_STATIC; } else { return mjCAT_DYNAMIC; @@ -471,35 +471,35 @@ static void drawBoundingBox(mjvGeom* thisgeom, mjData* d, mjvScene* scn, if (xmat != NULL) { mju_rotVecMat(x, aabb, xmat); mju_addTo3(x, xpos); - for (int j=0; j<3; j++) { - for (int k=0; k<3; k++) { + for (int j=0; j < 3; j++) { + for (int k=0; k < 3; k++) { dist[k][j] = aabb[k+3] * xmat[3*j+k]; } } } else { mju_copy3(x, aabb); mju_addTo3(x, xpos); - for (int j=0; j<3; j++) { + for (int j=0; j < 3; j++) { mju_zero3(dist[j]); dist[j][j] = aabb[j+3]; } } int split[3] = {1, 2, 4}; - for (int v=0; v<8; v++) { + for (int v=0; v < 8; v++) { mjtNum from[3] = {x[0], x[1], x[2]}; - for (int k=0; k<3; k++) { + for (int k=0; k < 3; k++) { mju_addToScl3(from, dist[k], v&split[k] ? 1 : -1); } mjtNum to[3]; - for (int k=0; k<3; k++) { + for (int k=0; k < 3; k++) { mju_addScl3(to, from, dist[k], 2); if (!(v&split[k])) { START mjv_makeConnector(thisgeom, mjGEOM_LINE, 2, - from[0], from[1], from[2], - to[0], to[1], to[2]); + from[0], from[1], from[2], + to[0], to[1], to[2]); f2f(thisgeom->rgba, rgba, 4); FINISH } @@ -511,7 +511,7 @@ static void drawBoundingBox(mjvGeom* thisgeom, mjData* d, mjvScene* scn, // add abstract geoms void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, - const mjvPerturb* pert, int catmask, mjvScene* scn) { + const mjvPerturb* pert, int catmask, mjvScene* scn) { int objtype, category; mjtNum sz[3], mat[9], selpos[3]; mjtNum catenary[3*mjNCATENARY]; @@ -537,7 +537,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, objtype = mjOBJ_SKIN; category = mjCAT_DYNAMIC; if (vopt->flags[mjVIS_SKIN] && (category & catmask)) { - for (int i=0; inskin; i++) { + for (int i=0; i < m->nskin; i++) { START // construct geom, pos = first bone @@ -548,22 +548,22 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, setMaterial(m, thisgeom, m->skin_matid[i], m->skin_rgba+4*i, vopt->flags); // glow skin if selected - if (pert->skinselect==i) { + if (pert->skinselect == i) { markselected(&m->vis, thisgeom); } // set texcoord - if (m->skin_texcoordadr[i]>=0) { + if (m->skin_texcoordadr[i] >= 0) { thisgeom->texcoord = 1; } // skip if alpha is 0 - if (thisgeom->rgba[3]==0) { + if (thisgeom->rgba[3] == 0) { continue; } // vopt->label - if (vopt->label==mjLABEL_SKIN) { + if (vopt->label == mjLABEL_SKIN) { makeLabel(m, mjOBJ_SKIN, i, thisgeom->label); } @@ -576,7 +576,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, int bodyid = 0; float rgba[] = {1, 0, 0, 1}; for (int i = 0; i < m->nbvh; i++) { - int isleaf = m->bvh_child[2*i]==-1 && m->bvh_child[2*i+1]==-1; + 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) { @@ -621,7 +621,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, for (int b = 0; b < m->mesh_bvhnum[meshid]; b++) { int i = b + m->mesh_bvhadr[meshid]; - int isleaf = m->bvh_child[2*i]==-1 && m->bvh_child[2*i+1]==-1; + 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) { @@ -646,9 +646,9 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, objtype = mjOBJ_BODY; if (vopt->flags[mjVIS_INERTIA]) { int ellipsoid = m->vis.global.ellipsoidinertia == 1; - for (int i=1; inbody; i++) { + for (int i=1; i < m->nbody; i++) { // skip if mass too small or if this body is static and static bodies are masked - if (m->body_mass[i]>mjMINVAL && (bodycategory(m, i) & catmask)) { + if (m->body_mass[i] > mjMINVAL && (bodycategory(m, i) & catmask)) { START mjtNum Ixx = m->body_inertia[3*i+0]; @@ -682,12 +682,13 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjv_initGeom(thisgeom, type, sz, d->xipos+3*i, d->ximat+9*i, m->vis.rgba.inertia); // glow - if (pert->select==i) { + if (pert->select == i) { markselected(&m->vis, thisgeom); } // vopt->label - if (vopt->label==mjLABEL_BODY || (vopt->label==mjLABEL_SELECTION && pert->select==i)) { + if (vopt->label == mjLABEL_BODY || + (vopt->label == mjLABEL_SELECTION && pert->select == i)) { makeLabel(m, mjOBJ_BODY, i, thisgeom->label); } @@ -699,7 +700,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, // connector to mouse perturbation target objtype = mjOBJ_UNKNOWN; category = mjCAT_DECOR; - if (vopt->flags[mjVIS_PERTOBJ] && (category & catmask) && pert->select>0) { + if (vopt->flags[mjVIS_PERTOBJ] && (category & catmask) && pert->select > 0) { int i = pert->select; if ((pert->active | pert->active2) & mjPERT_TRANSLATE) { @@ -718,8 +719,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, // prepare color float rgba[4]; mixcolor(rgba, m->vis.rgba.constraint, - (pert->active & mjPERT_TRANSLATE)>0, - (pert->active2 & mjPERT_TRANSLATE)>0); + (pert->active & mjPERT_TRANSLATE) > 0, + (pert->active2 & mjPERT_TRANSLATE) > 0); f2f(thisgeom->rgba, rgba, 4); @@ -729,7 +730,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, START // construct geom - sz[0] = 2*sz[0]; + sz[0] = 2*sz[0]; sz[1] = sz[2] = sz[0]; mju_quat2Mat(mat, pert->refquat); mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, pert->refselpos, mat, rgba); @@ -743,8 +744,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, // prepare color, use inertia color float rgba[4]; mixcolor(rgba, m->vis.rgba.inertia, - (pert->active & mjPERT_ROTATE)>0, - (pert->active2 & mjPERT_ROTATE)>0); + (pert->active & mjPERT_ROTATE) > 0, + (pert->active2 & mjPERT_ROTATE) > 0); // construct geom sz[0] = sz[1] = sz[2] = scl; @@ -759,11 +760,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, objtype = mjOBJ_UNKNOWN; category = mjCAT_DECOR; if (category & catmask) { - for (int i = (vopt->frame==mjFRAME_WORLD ? 0 : 1); - i < (vopt->frame==mjFRAME_BODY ? m->nbody : 1); + for (int i = (vopt->frame == mjFRAME_WORLD ? 0 : 1); + i < (vopt->frame == mjFRAME_BODY ? m->nbody : 1); i++) { // set length(1) and width(0) of the axis cylinders - if (i==0) { + if (i == 0) { sz[1] = m->vis.scale.framelength * scl * 2; sz[0] = m->vis.scale.framewidth * scl * 2; } else { @@ -772,7 +773,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, } // skip if body is static and static bodies are masked - if (i>0 && bodycategory(m, i) & ~catmask) { + if (i > 0 && bodycategory(m, i) & ~catmask) { continue; } @@ -780,12 +781,12 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjtNum* xpos = vopt->flags[mjVIS_INERTIA] ? d->xipos+3*i : d->xpos+3*i; // draw the three axes (separate geoms) - for (int j=0; j<3; j++) { + for (int j=0; j < 3; j++) { START // prepare axis - for (int k=0; k<3; k++) { - axis[k] = (j==k ? sz[1] : 0); + for (int k=0; k < 3; k++) { + axis[k] = (j == k ? sz[1] : 0); } mju_mulMatVec(vec, xmat, axis, 3, 3); @@ -794,8 +795,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, xpos[0] + vec[0], xpos[1] + vec[1], xpos[2] + vec[2]); // set color: R, G or B depending on axis - for (int k=0; k<3; k++) { - thisgeom->rgba[k] = (j==k ? 0.9 : 0); + for (int k=0; k < 3; k++) { + thisgeom->rgba[k] = (j == k ? 0.9 : 0); } thisgeom->rgba[3] = 1; @@ -807,7 +808,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, // selection point objtype = mjOBJ_UNKNOWN; category = mjCAT_DECOR; - if ((category & catmask) && pert->select>0 && vopt->flags[mjVIS_SELECT]) { + if ((category & catmask) && pert->select > 0 && vopt->flags[mjVIS_SELECT]) { int i=0; // compute selection point in world coordinates @@ -820,7 +821,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mju_n2f(thisgeom->pos, selpos, 3); mju_n2f(thisgeom->mat, IDENTITY, 9); f2f(thisgeom->rgba, m->vis.rgba.selectpoint, 4); - if (vopt->label==mjLABEL_SELPNT) { + if (vopt->label == mjLABEL_SELPNT) { mjSNPRINTF( thisgeom->label, "%.3f %.3f %.3f (local %.3f %.3f %.3f)", selpos[0], selpos[1], selpos[2], @@ -832,10 +833,10 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, // label bodies when inertia boxes are not shown objtype = mjOBJ_UNKNOWN; category = mjCAT_DECOR; - if ((category & catmask) && (vopt->label==mjLABEL_SELECTION || vopt->label==mjLABEL_BODY) && + if ((category & catmask) && (vopt->label == mjLABEL_SELECTION || vopt->label == mjLABEL_BODY) && !vopt->flags[mjVIS_INERTIA]) { - for (int i=1; inbody; i++) { - if (vopt->label==mjLABEL_BODY || (vopt->label==mjLABEL_SELECTION && pert->select==i)) { + for (int i=1; i < m->nbody; i++) { + if (vopt->label == mjLABEL_BODY || (vopt->label == mjLABEL_SELECTION && pert->select == i)) { // skip if body is static and static bodies are masked if (bodycategory(m, i) & ~catmask) { continue; @@ -859,7 +860,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, objtype = mjOBJ_JOINT; category = mjCAT_DECOR; if (vopt->flags[mjVIS_JOINT] && (category & catmask)) { - for (int i=0; injnt; i++) { + for (int i=0; i < m->njnt; i++) { if (vopt->jointgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->jnt_group[i]))]) { // set length(1) and width(0) of the connectors sz[1] = m->vis.scale.jointlength * scl; @@ -887,7 +888,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, case mjJNT_SLIDE: case mjJNT_HINGE: mjv_makeConnector(thisgeom, - m->jnt_type[i]==mjJNT_SLIDE ? mjGEOM_ARROW : mjGEOM_ARROW1, sz[0], + m->jnt_type[i] == mjJNT_SLIDE ? mjGEOM_ARROW : mjGEOM_ARROW1, sz[0], d->xanchor[3*i+0], d->xanchor[3*i+1], d->xanchor[3*i+2], @@ -903,7 +904,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, f2f(thisgeom->rgba, m->vis.rgba.joint, 4); // vopt->label - if (vopt->label==mjLABEL_JOINT) { + if (vopt->label == mjLABEL_JOINT) { makeLabel(m, mjOBJ_JOINT, i, thisgeom->label); } @@ -916,7 +917,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, objtype = mjOBJ_ACTUATOR; category = mjCAT_DECOR; if (vopt->flags[mjVIS_ACTUATOR] && (category & catmask)) { - for (int i=0; inu; i++) { + for (int i=0; i < m->nu; i++) { if (vopt->actuatorgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->actuator_group[i]))]) { // determine extended range mjtNum rng[3] = {-1, 0, +1}; @@ -928,11 +929,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, rmin = m->actuator_actrange[2*i]; rmax = m->actuator_actrange[2*i+1]; } - if (rmin>=0) { + if (rmin >= 0) { rng[0] = -1; rng[1] = rmin; rng[2] = rmax; - } else if (rmax<=0) { + } else if (rmax <= 0) { rng[0] = rmin; rng[1] = rmax; rng[2] = +1; @@ -943,10 +944,10 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, } // adjust small ranges - if (rng[1]-rng[0]vis.rgba.actuatornegative[j] + amean*m->vis.rgba.actuator[j] + amax*m->vis.rgba.actuatorpositive[j]; @@ -981,13 +982,13 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, int j = m->actuator_trnid[2*i]; // slide and hinge joint actuators - if (m->actuator_trntype[i]==mjTRN_JOINT || - m->actuator_trntype[i]==mjTRN_JOINTINPARENT || - m->actuator_trntype[i]==mjTRN_SITE) { + if (m->actuator_trntype[i] == mjTRN_JOINT || + m->actuator_trntype[i] == mjTRN_JOINTINPARENT || + m->actuator_trntype[i] == mjTRN_SITE) { START // site actuators - if (m->actuator_trntype[i]==mjTRN_SITE) { + if (m->actuator_trntype[i] == mjTRN_SITE) { // inflate sizes by 5% mju_scl3(sz, m->site_size+3*j, 1.05); @@ -997,14 +998,14 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, d->site_xpos + 3*j, d->site_xmat + 9*j, thisgeom->rgba); - } else if (m->jnt_type[j]==mjJNT_HINGE || m->jnt_type[j]==mjJNT_SLIDE) { + } else if (m->jnt_type[j] == mjJNT_HINGE || m->jnt_type[j] == mjJNT_SLIDE) { // set length(1) and width(0) of the connectors sz[1] = m->vis.scale.actuatorlength * scl; sz[0] = m->vis.scale.actuatorwidth * scl; // make geom mjv_makeConnector(thisgeom, - m->jnt_type[j]==mjJNT_SLIDE ? mjGEOM_ARROW : mjGEOM_ARROW1, sz[0], + m->jnt_type[j] == mjJNT_SLIDE ? mjGEOM_ARROW : mjGEOM_ARROW1, sz[0], d->xanchor[3*j+0], d->xanchor[3*j+1], d->xanchor[3*j+2], @@ -1014,12 +1015,12 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, } // ball or free joint - else if (m->jnt_type[j]==mjJNT_BALL || m->jnt_type[j]==mjJNT_FREE) { + else if (m->jnt_type[j] == mjJNT_BALL || m->jnt_type[j] == mjJNT_FREE) { sz[0] = sz[1] = sz[2] = m->vis.scale.jointlength * scl * 0.33; // make geom mjv_initGeom(thisgeom, - m->jnt_type[j]==mjJNT_BALL ? mjGEOM_SPHERE : mjGEOM_BOX, sz, + m->jnt_type[j] == mjJNT_BALL ? mjGEOM_SPHERE : mjGEOM_BOX, sz, d->xanchor + 3*j, d->xmat + 9*m->jnt_bodyid[j], thisgeom->rgba); @@ -1029,7 +1030,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, f2f(thisgeom->rgba, rgba, 4); // vopt->label - if (vopt->label==mjLABEL_ACTUATOR) { + if (vopt->label == mjLABEL_ACTUATOR) { makeLabel(m, mjOBJ_ACTUATOR, i, thisgeom->label); } @@ -1037,11 +1038,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, } // body actuators - else if (m->actuator_trntype[i]==mjTRN_BODY) { + else if (m->actuator_trntype[i] == mjTRN_BODY) { // iterate over body's geoms int geomnum = m->body_geomnum[j]; int geomadr = m->body_geomadr[j]; - for (int k=geomadr; kgeom_type[k]; // add inflated geom if it is a regular primitive if (geomtype != mjGEOM_PLANE && geomtype != mjGEOM_HFIELD && geomtype != mjGEOM_MESH) { @@ -1065,13 +1066,13 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, } // spatial tendon actuators - else if (m->actuator_trntype[i]==mjTRN_TENDON && d->ten_wrapnum[j]) { - for (int k=d->ten_wrapadr[j]; kten_wrapadr[j]+d->ten_wrapnum[j]-1; k++) { - if (d->wrap_obj[k]!=-2 && d->wrap_obj[k+1]!=-2) { + else if (m->actuator_trntype[i] == mjTRN_TENDON && d->ten_wrapnum[j]) { + for (int k=d->ten_wrapadr[j]; k < d->ten_wrapadr[j]+d->ten_wrapnum[j]-1; k++) { + if (d->wrap_obj[k] != -2 && d->wrap_obj[k+1] != -2) { START // determine width: smaller for segments inside wrapping objects - if (d->wrap_obj[k]>=0 && d->wrap_obj[k+1]>=0) { + if (d->wrap_obj[k] >= 0 && d->wrap_obj[k+1] >= 0) { sz[0] = 0.5 * m->tendon_width[j]; } else { sz[0] = m->tendon_width[j]; @@ -1092,7 +1093,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, f2f(thisgeom->rgba, rgba, 4); // vopt->label: only the first segment - if (vopt->label==mjLABEL_ACTUATOR && k==d->ten_wrapadr[j]) { + if (vopt->label == mjLABEL_ACTUATOR && k == d->ten_wrapadr[j]) { makeLabel(m, mjOBJ_ACTUATOR, i, thisgeom->label); } @@ -1106,9 +1107,9 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, // geom int planeid = -1; - for (int i=0; ingeom; i++) { + for (int i=0; i < m->ngeom; i++) { // count planes, put current plane number in geom->dataid - if (m->geom_type[i]==mjGEOM_PLANE) { + if (m->geom_type[i] == mjGEOM_PLANE) { planeid++; } @@ -1139,37 +1140,37 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, setMaterial(m, thisgeom, m->geom_matid[i], m->geom_rgba+4*i, vopt->flags); // set texcoord - if (m->geom_type[i]==mjGEOM_MESH && - m->geom_dataid[i]>=0 && - m->mesh_texcoordadr[m->geom_dataid[i]]>=0) { + if (m->geom_type[i] == mjGEOM_MESH && + m->geom_dataid[i] >= 0 && + m->mesh_texcoordadr[m->geom_dataid[i]] >= 0) { thisgeom->texcoord = 1; } // skip if alpha is 0 - if (thisgeom->rgba[3]==0) { + if (thisgeom->rgba[3] == 0) { continue; } // glow geoms of selected body - if (pert->select>0 && pert->select==m->geom_bodyid[i]) { + if (pert->select > 0 && pert->select == m->geom_bodyid[i]) { markselected(&m->vis, thisgeom); } // vopt->label - if (vopt->label==mjLABEL_GEOM) { + if (vopt->label == mjLABEL_GEOM) { makeLabel(m, mjOBJ_GEOM, i, thisgeom->label); } // mesh: 2*i is original, 2*i+1 is convex hull - if (m->geom_type[i]==mjGEOM_MESH) { + if (m->geom_type[i] == mjGEOM_MESH) { thisgeom->dataid *= 2; - if (m->mesh_graphadr[m->geom_dataid[i]]>=0 && vopt->flags[mjVIS_CONVEXHULL]) { + if (m->mesh_graphadr[m->geom_dataid[i]] >= 0 && vopt->flags[mjVIS_CONVEXHULL]) { thisgeom->dataid += 1; } } // plane - else if (m->geom_type[i]==mjGEOM_PLANE) { + else if (m->geom_type[i] == mjGEOM_PLANE) { // use current planeid thisgeom->dataid = planeid; @@ -1177,9 +1178,9 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mju_copy3(tmp, d->geom_xpos+3*i); // re-center infinite plane - if (m->geom_size[3*i]<=0 || m->geom_size[3*i+1]<=0) { + if (m->geom_size[3*i] <= 0 || m->geom_size[3*i+1] <= 0) { // vec = headpos - geompos - for (int j=0; j<3; j++) { + for (int j=0; j < 3; j++) { vec[j] = 0.5*(scn->camera[0].pos[j] + scn->camera[1].pos[j]) - d->geom_xpos[3*i+j]; } @@ -1188,15 +1189,15 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mju_transpose(ax, d->geom_xmat+9*i, 3, 3); // loop over (x,y) - for (int k=0; k<2; k++) { - if (m->geom_size[3*i+k]<=0) { + for (int k=0; k < 2; k++) { + if (m->geom_size[3*i+k] <= 0) { // compute zfar mjtNum zfar = m->vis.map.zfar * m->stat.extent; // get size increment mjtNum sX; int matid = m->geom_matid[i]; - if (matid>=0 && m->mat_texrepeat[2*matid+k]>0) { + if (matid >= 0 && m->mat_texrepeat[2*matid+k] > 0) { sX = 2/m->mat_texrepeat[2*matid+k]; } else { sX = 2.1*zfar/(mjMAXPLANEGRID-2); @@ -1219,9 +1220,9 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, FINISH // set type and category: frame - objtype = mjOBJ_UNKNOWN; + objtype = mjOBJ_UNKNOWN; category = mjCAT_DECOR; - if (!(category & catmask) || vopt->frame!=mjFRAME_GEOM) { + if (!(category & catmask) || vopt->frame != mjFRAME_GEOM) { continue; } @@ -1229,12 +1230,12 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, objtype = mjOBJ_UNKNOWN; sz[0] = m->vis.scale.framewidth * scl; sz[1] = m->vis.scale.framelength * scl; - for (int j=0; j<3; j++) { + for (int j=0; j < 3; j++) { START // prepare axis - for (int k=0; k<3; k++) { - axis[k] = (j==k ? sz[1] : 0); + for (int k=0; k < 3; k++) { + axis[k] = (j == k ? sz[1] : 0); } mju_mulMatVec(vec, d->geom_xmat+9*i, axis, 3, 3); @@ -1248,8 +1249,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, d->geom_xpos[3*i+2] + vec[2]); // set color: R, G or B depending on axis - for (int k=0; k<3; k++) { - thisgeom->rgba[k] = (j==k ? 0.9 : 0); + for (int k=0; k < 3; k++) { + thisgeom->rgba[k] = (j == k ? 0.9 : 0); } thisgeom->rgba[3] = 1; @@ -1259,7 +1260,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, } // site - for (int i=0; insite; i++) { + for (int i=0; i < m->nsite; i++) { // set type and category objtype = mjOBJ_SITE; category = bodycategory(m, m->site_bodyid[i]); @@ -1281,25 +1282,25 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, setMaterial(m, thisgeom, m->site_matid[i], m->site_rgba+4*i, vopt->flags); // skip if alpha is 0 - if (thisgeom->rgba[3]==0) { + if (thisgeom->rgba[3] == 0) { continue; } // glow - if (pert->select>0 && pert->select==m->site_bodyid[i]) { + if (pert->select > 0 && pert->select == m->site_bodyid[i]) { markselected(&m->vis, thisgeom); } // vopt->label - if (vopt->label==mjLABEL_SITE) { + if (vopt->label == mjLABEL_SITE) { makeLabel(m, mjOBJ_SITE, i, thisgeom->label); } FINISH // set category for site frame - category = mjCAT_DECOR; - if (!(category & catmask) || vopt->frame!=mjFRAME_SITE) { + category = mjCAT_DECOR; + if (!(category & catmask) || vopt->frame != mjFRAME_SITE) { continue; } @@ -1307,12 +1308,12 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, objtype = mjOBJ_UNKNOWN; sz[0] = m->vis.scale.framewidth * scl; sz[1] = m->vis.scale.framelength * scl; - for (int j=0; j<3; j++) { + for (int j=0; j < 3; j++) { START // prepare axis - for (int k=0; k<3; k++) { - axis[k] = (j==k ? sz[1] : 0); + for (int k=0; k < 3; k++) { + axis[k] = (j == k ? sz[1] : 0); } mju_mulMatVec(vec, d->site_xmat+9*i, axis, 3, 3); @@ -1326,8 +1327,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, d->site_xpos[3*i+2] + vec[2]); // set color: R, G or B depending on axis - for (int k=0; k<3; k++) { - thisgeom->rgba[k] = (j==k ? 0.9 : 0); + for (int k=0; k < 3; k++) { + thisgeom->rgba[k] = (j == k ? 0.9 : 0); } thisgeom->rgba[3] = 1; @@ -1340,7 +1341,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, objtype = mjOBJ_CAMERA; category = mjCAT_DECOR; if (vopt->flags[mjVIS_CAMERA] && (category & catmask)) { - for (int i=0; incam; i++) { + for (int i=0; i < m->ncam; i++) { START // construct geom: camera body @@ -1353,7 +1354,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, f2f(thisgeom->rgba, m->vis.rgba.camera, 4); // vopt->label - if (vopt->label==mjLABEL_CAMERA) { + if (vopt->label == mjLABEL_CAMERA) { makeLabel(m, mjOBJ_CAMERA, i, thisgeom->label); } @@ -1374,15 +1375,15 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, thisgeom->size[2] = scl * m->vis.scale.camera * 0.3; mju_n2f(thisgeom->mat, d->cam_xmat+9*i, 9); f2f(thisgeom->rgba, m->vis.rgba.camera, 4); - for (int k=0; k<3; k++) { + for (int k=0; k < 3; k++) { thisgeom->rgba[k] *= 0.5; // make lens body darker } FINISH // set category for camera frame - category = mjCAT_DECOR; - if (!(category & catmask) || vopt->frame!=mjFRAME_CAMERA) { + category = mjCAT_DECOR; + if (!(category & catmask) || vopt->frame != mjFRAME_CAMERA) { continue; } @@ -1390,12 +1391,12 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, objtype = mjOBJ_UNKNOWN; sz[0] = m->vis.scale.framewidth * scl; sz[1] = m->vis.scale.framelength * scl; - for (int j=0; j<3; j++) { + for (int j=0; j < 3; j++) { START // prepare axis - for (int k=0; k<3; k++) { - axis[k] = (j==k ? sz[1] : 0); + for (int k=0; k < 3; k++) { + axis[k] = (j == k ? sz[1] : 0); } mju_mulMatVec(vec, d->cam_xmat+9*i, axis, 3, 3); @@ -1409,8 +1410,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, d->cam_xpos[3*i+2] + vec[2]); // set color: R, G or B depending on axis - for (int k=0; k<3; k++) { - thisgeom->rgba[k] = (j==k ? 0.9 : 0); + for (int k=0; k < 3; k++) { + thisgeom->rgba[k] = (j == k ? 0.9 : 0); } thisgeom->rgba[3] = 1; @@ -1423,7 +1424,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, objtype = mjOBJ_LIGHT; category = mjCAT_DECOR; if (vopt->flags[mjVIS_LIGHT] && (category & catmask)) { - for (int i=0; inlight; i++) { + for (int i=0; i < m->nlight; i++) { // make light frame mju_quatZ2Vec(quat, d->light_xdir+3*i); mju_quat2Mat(mat, quat); @@ -1443,15 +1444,15 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, f2f(thisgeom->rgba, m->vis.rgba.light, 4); // vopt->label - if (vopt->label==mjLABEL_LIGHT) { + if (vopt->label == mjLABEL_LIGHT) { makeLabel(m, mjOBJ_LIGHT, i, thisgeom->label); } FINISH // set category for light frame - category = mjCAT_DECOR; - if (!(category & catmask) || vopt->frame!=mjFRAME_LIGHT) { + category = mjCAT_DECOR; + if (!(category & catmask) || vopt->frame != mjFRAME_LIGHT) { continue; } @@ -1459,12 +1460,12 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, objtype = mjOBJ_UNKNOWN; sz[0] = m->vis.scale.framewidth * scl; sz[1] = m->vis.scale.framelength * scl; - for (int j=0; j<3; j++) { + for (int j=0; j < 3; j++) { START // prepare axis - for (int k=0; k<3; k++) { - axis[k] = (j==k ? sz[1] : 0); + for (int k=0; k < 3; k++) { + axis[k] = (j == k ? sz[1] : 0); } mju_mulMatVec(vec, mat, axis, 3, 3); @@ -1478,8 +1479,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, d->light_xpos[3*i+2] + vec[2]); // set color: R, G or B depending on axis - for (int k=0; k<3; k++) { - thisgeom->rgba[k] = (j==k ? 0.9 : 0); + for (int k=0; k < 3; k++) { + thisgeom->rgba[k] = (j == k ? 0.9 : 0); } thisgeom->rgba[3] = 1; @@ -1492,37 +1493,37 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, objtype = mjOBJ_TENDON; category = mjCAT_DYNAMIC; if (vopt->flags[mjVIS_TENDON] && (category & catmask)) { - for (int i=0; intendon; i++) { + for (int i=0; i < m->ntendon; i++) { if (vopt->tendongroup[mjMAX(0, mjMIN(mjNGROUP-1, m->tendon_group[i]))]) { // stiff tendon has a deadband spring int limitedspring = - m->tendon_stiffness[i] > 0 && // positive stiffness - m->tendon_lengthspring[2*i] == 0 && // range lower-bound is 0 - m->tendon_lengthspring[2*i+1] > 0; // range upper-bound is positive + m->tendon_stiffness[i] > 0 && // positive stiffness + m->tendon_lengthspring[2*i] == 0 && // range lower-bound is 0 + m->tendon_lengthspring[2*i+1] > 0; // range upper-bound is positive // non-stiff tendon has a length constraint int limitedconstraint = - m->tendon_stiffness[i] == 0 && // zero stiffness - m->tendon_limited[i] == 1 && // limited length range - m->tendon_range[2*i] == 0; // range lower-bound is 0 + m->tendon_stiffness[i] == 0 && // zero stiffness + m->tendon_limited[i] == 1 && // limited length range + m->tendon_range[2*i] == 0; // range lower-bound is 0 // conditions for drawing a catenary int draw_catenary = - !mjDISABLED(mjDSBL_GRAVITY) && // gravity enabled - mju_norm3(m->opt.gravity) > mjMINVAL && // gravity strictly nonzero - m->tendon_num[i] == 2 && // only two sites on the tendon - (limitedspring || limitedconstraint) && // either spring or constraint length limits - m->tendon_damping[i] == 0 && // no damping - m->tendon_frictionloss[i] == 0; // no frictionloss + !mjDISABLED(mjDSBL_GRAVITY) && // gravity enabled + mju_norm3(m->opt.gravity) > mjMINVAL && // gravity strictly nonzero + m->tendon_num[i] == 2 && // only two sites on the tendon + (limitedspring || limitedconstraint) && // either spring or constraint length limits + m->tendon_damping[i] == 0 && // no damping + m->tendon_frictionloss[i] == 0; // no frictionloss // conditions not met: draw straight lines if (!draw_catenary) { - for (int j=d->ten_wrapadr[i]; jten_wrapadr[i]+d->ten_wrapnum[i]-1; j++) { - if (d->wrap_obj[j]!=-2 && d->wrap_obj[j+1]!=-2) { + for (int j=d->ten_wrapadr[i]; j < d->ten_wrapadr[i]+d->ten_wrapnum[i]-1; j++) { + if (d->wrap_obj[j] != -2 && d->wrap_obj[j+1] != -2) { START // determine width: smaller for segments inside wrapping objects - if (d->wrap_obj[j]>=0 && d->wrap_obj[j+1]>=0) { + if (d->wrap_obj[j] >= 0 && d->wrap_obj[j+1] >= 0) { sz[0] = 0.5 * m->tendon_width[i]; } else { sz[0] = m->tendon_width[i]; @@ -1537,7 +1538,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, setMaterial(m, thisgeom, m->tendon_matid[i], m->tendon_rgba+4*i, vopt->flags); // vopt->label: only the first segment - if (vopt->label==mjLABEL_TENDON && j==d->ten_wrapadr[i]) { + if (vopt->label == mjLABEL_TENDON && j == d->ten_wrapadr[i]) { makeLabel(m, mjOBJ_TENDON, i, thisgeom->label); } @@ -1565,10 +1566,10 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, int npoints = mjv_catenary(x0, x1, m->opt.gravity, length, catenary); // draw npoints-1 segments - for (int j=0; jtendon_width[i]; + sz[0] = m->tendon_width[i]; // construct geom mjv_makeConnector(thisgeom, mjGEOM_CAPSULE, sz[0], @@ -1579,7 +1580,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, setMaterial(m, thisgeom, m->tendon_matid[i], m->tendon_rgba+4*i, vopt->flags); // vopt->label: only the first segment - if (vopt->label==mjLABEL_TENDON && npoints/2) { + if (vopt->label == mjLABEL_TENDON && npoints/2) { makeLabel(m, mjOBJ_TENDON, i, thisgeom->label); } @@ -1594,8 +1595,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, objtype = mjOBJ_ACTUATOR; category = mjCAT_DYNAMIC; if ((category & catmask)) { - for (int i=0; inu; i++) { - if (m->actuator_trntype[i]==mjTRN_SLIDERCRANK) { + for (int i=0; i < m->nu; i++) { + if (m->actuator_trntype[i] == mjTRN_SLIDERCRANK) { // get data int j = m->actuator_trnid[2*i]; // crank int k = m->actuator_trnid[2*i+1]; // slider @@ -1609,7 +1610,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, len = mju_dot3(vec, axis); det = len*len + rod*rod - mju_dot3(vec, vec); broken = 0; - if (det<0) { + if (det < 0) { det = 0; broken = 1; } @@ -1625,7 +1626,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, d->site_xpos[3*k], d->site_xpos[3*k+1], d->site_xpos[3*k+2], end[0], end[1], end[2]); f2f(thisgeom->rgba, m->vis.rgba.slidercrank, 4); - if (vopt->label==mjLABEL_ACTUATOR) { + if (vopt->label == mjLABEL_ACTUATOR) { makeLabel(m, mjOBJ_ACTUATOR, i, thisgeom->label); } FINISH @@ -1649,8 +1650,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, objtype = mjOBJ_UNKNOWN; category = mjCAT_DECOR; if (vopt->flags[mjVIS_COM] && (category & catmask)) { - for (int i=1; inbody; i++) { - if (m->body_rootid[i]==i) { + for (int i=1; i < m->nbody; i++) { + if (m->body_rootid[i] == i) { START thisgeom->type = mjGEOM_SPHERE; thisgeom->size[0] = thisgeom->size[1] = thisgeom->size[2] = scl * m->vis.scale.com; @@ -1666,18 +1667,18 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, objtype = mjOBJ_UNKNOWN; category = mjCAT_DECOR; if (vopt->flags[mjVIS_AUTOCONNECT] && (category & catmask)) { - for (int i=1; inbody; i++) { + for (int i=1; i < m->nbody; i++) { // do not connect to world - if (m->body_parentid[i]==0) { + if (m->body_parentid[i] == 0) { continue; } // start at body com, connect joint centers in reverse order cur = d->xipos+3*i; if (m->body_jntnum[i]) { - for (int j=m->body_jntadr[i]+m->body_jntnum[i]-1; j>=m->body_jntadr[i]; j--) { + for (int j=m->body_jntadr[i]+m->body_jntnum[i]-1; j >= m->body_jntadr[i]; j--) { START - nxt = d->xanchor+3*j; + nxt = d->xanchor+3*j; // construct geom mjv_makeConnector(thisgeom, mjGEOM_CAPSULE, scl * m->vis.scale.connect, @@ -1686,13 +1687,13 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, f2f(thisgeom->rgba, m->vis.rgba.connect, 4); FINISH - cur = nxt; + cur = nxt; } } // connect first joint (or com) to parent com START - nxt = d->xipos+3*m->body_parentid[i]; + nxt = d->xipos+3*m->body_parentid[i]; mjv_makeConnector(thisgeom, mjGEOM_CAPSULE, scl * m->vis.scale.connect, cur[0], cur[1], cur[2], nxt[0], nxt[1], nxt[2]); @@ -1705,14 +1706,14 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, objtype = mjOBJ_UNKNOWN; category = mjCAT_DECOR; if (vopt->flags[mjVIS_RANGEFINDER] && (category & catmask)) { - for (int i=0; insensor; i++) { - if (m->sensor_type[i]==mjSENS_RANGEFINDER) { + for (int i=0; i < m->nsensor; i++) { + if (m->sensor_type[i] == mjSENS_RANGEFINDER) { // sensor data mjtNum dst = d->sensordata[m->sensor_adr[i]]; int sid = m->sensor_objid[i]; // null output: nothing to render - if (dst<0) { + if (dst < 0) { continue; } @@ -1725,7 +1726,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, d->site_xpos[3*sid] + d->site_xmat[9*sid+2]*dst, d->site_xpos[3*sid+1] + d->site_xmat[9*sid+5]*dst, d->site_xpos[3*sid+2] + d->site_xmat[9*sid+8]*dst - ); + ); f2f(thisgeom->rgba, m->vis.rgba.rangefinder, 4); FINISH } @@ -1735,14 +1736,14 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, // external perturbations objtype = mjOBJ_UNKNOWN; category = mjCAT_DECOR; - for (int i=1; inbody; i++) { + for (int i=1; i < m->nbody; i++) { if (!mju_isZero(d->xfrc_applied+6*i, 6) && (category & catmask)) { // point of application and force mjtNum *xpos = d->xipos+3*i; xfrc = d->xfrc_applied+6*i; // force perturbation - if (vopt->flags[mjVIS_PERTFORCE] && mju_norm3(xfrc)>mjMINVAL) { + if (vopt->flags[mjVIS_PERTFORCE] && mju_norm3(xfrc) > mjMINVAL) { // map force to spatial vector in world frame mju_scl3(vec, xfrc, m->vis.map.force/m->stat.meanmass); @@ -1766,13 +1767,13 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, category = mjCAT_DECOR; if (vopt->flags[mjVIS_CONSTRAINT] && (category & catmask) && m->neq) { // connect or weld - for (int i=0; ineq; i++) { - if (m->eq_active[i] && (m->eq_type[i]==mjEQ_CONNECT || m->eq_type[i]==mjEQ_WELD)) { + for (int i=0; i < m->neq; i++) { + if (m->eq_active[i] && (m->eq_type[i] == mjEQ_CONNECT || m->eq_type[i] == mjEQ_WELD)) { // compute endpoints in global coordinates int j = m->eq_obj1id[i], k = m->eq_obj2id[i]; - mju_rotVecMat(vec, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i]==mjEQ_WELD), d->xmat+9*j); + mju_rotVecMat(vec, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i] == mjEQ_WELD), d->xmat+9*j); mju_addTo3(vec, d->xpos+3*j); - mju_rotVecMat(end, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i]==mjEQ_CONNECT), d->xmat+9*k); + mju_rotVecMat(end, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i] == mjEQ_CONNECT), d->xmat+9*k); mju_addTo3(end, d->xpos+3*k); // construct geom @@ -1780,14 +1781,14 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, START mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, vec, d->xmat+9*j, m->vis.rgba.connect); - if (vopt->label==mjLABEL_CONSTRAINT) { + if (vopt->label == mjLABEL_CONSTRAINT) { makeLabel(m, mjOBJ_EQUALITY, i, thisgeom->label); } FINISH START mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, end, d->xmat+9*k, m->vis.rgba.constraint); - if (vopt->label==mjLABEL_CONSTRAINT) { + if (vopt->label == mjLABEL_CONSTRAINT) { makeLabel(m, mjOBJ_EQUALITY, i, thisgeom->label); } FINISH @@ -1834,7 +1835,7 @@ void mjv_makeLights(const mjModel* m, mjData* d, mjvScene* scn) { } // remaining lights - for (int i=0; inlight && scn->nlightnlight && scn->nlight < mjMAXLIGHT; i++) { if (m->light_active[i]) { // get pointer thislight = scn->lights + scn->nlight; @@ -1872,7 +1873,7 @@ void mjv_updateCamera(const mjModel* m, mjData* d, mjvCamera* cam, mjvScene* scn mjtNum headpos[3], forward[3], up[3], right[3], ipd, fovy, znear, zfar; // return if nothing to do - if (!m || !cam || cam->type==mjCAMERA_USER) { + if (!m || !cam || cam->type == mjCAMERA_USER) { return; } @@ -1889,10 +1890,10 @@ void mjv_updateCamera(const mjModel* m, mjData* d, mjvCamera* cam, mjvScene* scn fovy = m->vis.global.fovy; // move lookat for tracking - if (cam->type==mjCAMERA_TRACKING) { + if (cam->type == mjCAMERA_TRACKING) { // get id and check int bid = cam->trackbodyid; - if (bid<0 || bid>=m->nbody) { + if (bid < 0 || bid >= m->nbody) { mju_error("Track body id is outside valid range"); } @@ -1919,41 +1920,41 @@ void mjv_updateCamera(const mjModel* m, mjData* d, mjvCamera* cam, mjvScene* scn break; case mjCAMERA_FIXED: { - // get id and check - int cid = cam->fixedcamid; - if (cid<0 || cid>=m->ncam) { - mju_error("Fixed camera id is outside valid range"); - } - - // get camera-specific ipd and fovy - ipd = m->cam_ipd[cid]; - fovy = m->cam_fovy[cid]; - - // get pointer to camera orientation matrix - mat = d->cam_xmat + 9*cid; - - // get frame - forward[0] = -mat[2]; - forward[1] = -mat[5]; - forward[2] = -mat[8]; - up[0] = mat[1]; - up[1] = mat[4]; - up[2] = mat[7]; - right[0] = mat[0]; - right[1] = mat[3]; - right[2] = mat[6]; - mju_copy3(headpos, d->cam_xpos + 3*cid); + // get id and check + int cid = cam->fixedcamid; + if (cid < 0 || cid >= m->ncam) { + mju_error("Fixed camera id is outside valid range"); } - break; + + // get camera-specific ipd and fovy + ipd = m->cam_ipd[cid]; + fovy = m->cam_fovy[cid]; + + // get pointer to camera orientation matrix + mat = d->cam_xmat + 9*cid; + + // get frame + forward[0] = -mat[2]; + forward[1] = -mat[5]; + forward[2] = -mat[8]; + up[0] = mat[1]; + up[1] = mat[4]; + up[2] = mat[7]; + right[0] = mat[0]; + right[1] = mat[3]; + right[2] = mat[6]; + mju_copy3(headpos, d->cam_xpos + 3*cid); + } + break; default: mju_error("Unknown camera type in mjv_updateCamera"); } // compute GL cameras - for (int view=0; view<2; view++) { + for (int view=0; view < 2; view++) { // set frame - for (int i=0; i<3; i++) { + for (int i=0; i < 3; i++) { scn->camera[view].pos[i] = (float)(headpos[i] + (view ? ipd : -ipd)*0.5*right[i]); scn->camera[view].forward[i] = (float)forward[i]; scn->camera[view].up[i] = (float)up[i]; @@ -1984,7 +1985,7 @@ void mjv_updateSkin(const mjModel* m, mjData* d, mjvScene* scn) { // update visible skins only void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvOption* opt) { // process skins - for (int i=0; inskin; i++) { + for (int i=0; i < m->nskin; i++) { // get info int vertadr = m->skin_vertadr[i]; int vertnum = m->skin_vertnum[i]; @@ -1998,8 +1999,8 @@ void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvO if (opt->skingroup[m->skin_group[i]]) { // accumulate positions from all bones for (int j=m->skin_boneadr[i]; - jskin_boneadr[i]+m->skin_bonenum[i]; - j++) { + j < m->skin_boneadr[i]+m->skin_bonenum[i]; + j++) { // get bind pose mjtNum bindpos[3] = { (mjtNum) m->skin_bonebindpos[3*j], @@ -2027,8 +2028,8 @@ void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvO // process all bone vertices for (int k=m->skin_bonevertadr[j]; - kskin_bonevertadr[j]+m->skin_bonevertnum[j]; - k++) { + k < m->skin_bonevertadr[j]+m->skin_bonevertnum[j]; + k++) { // vertex id and weight int vid = m->skin_bonevertid[k]; float vweight = m->skin_bonevertweight[k]; @@ -2053,7 +2054,7 @@ void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvO } // compute vertex normals from face normals - for (int k=faceadr; kskin_face[3*k], @@ -2063,7 +2064,7 @@ void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvO // get triangle edges mjtNum vec01[3], vec02[3]; - for (int r=0; r<3; r++) { + for (int r=0; r < 3; r++) { vec01[r] = scn->skinvert[3*(vertadr+vid[1])+r] - scn->skinvert[3*(vertadr+vid[0])+r]; vec02[r] = scn->skinvert[3*(vertadr+vid[2])+r] - scn->skinvert[3*(vertadr+vid[0])+r]; } @@ -2073,20 +2074,20 @@ void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvO mju_cross(nrm, vec01, vec02); // add normal to each vertex with weight = area - for (int r=0; r<3; r++) { - for (int t=0; t<3; t++) { + for (int r=0; r < 3; r++) { + for (int t=0; t < 3; t++) { scn->skinnormal[3*(vertadr+vid[r])+t] += nrm[t]; } } } // normalize normals - for (int k=vertadr; kskinnormal[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]*scn->skinnormal[3*k] + + 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; @@ -2097,7 +2098,7 @@ void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvO // inflate if (m->skin_inflate[i]) { float inflate = m->skin_inflate[i]; - for (int k=vertadr; kskinvert[3*k] += inflate*scn->skinnormal[3*k]; scn->skinvert[3*k+1] += inflate*scn->skinnormal[3*k+1]; scn->skinvert[3*k+2] += inflate*scn->skinnormal[3*k+2]; @@ -2119,7 +2120,7 @@ void mjv_updateScene(const mjModel* m, mjData* d, const mjvOption* opt, if (m->nplugin) { const int nslot = mjp_pluginCount(); // iterate over plugins, call visualize if defined - for (int i=0; inplugin; i++) { + for (int i=0; i < m->nplugin; i++) { const int slot = m->plugin[i]; const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot); if (!plugin) { @@ -2194,7 +2195,7 @@ static inline mjtNum solve_catenary(mjtNum v, mjtNum h, mjtNum length) { mjtNum b = intercept / mju_sqrt(24); // Newton steps to convergence (usually ~ 5 steps) - for (int i=0; i<50; i++) { + for (int i=0; i < 50; i++) { // get value and gradient mjtNum grad; mjtNum res = catenary_residual(b, intercept, &grad); @@ -2207,7 +2208,7 @@ static inline mjtNum solve_catenary(mjtNum v, mjtNum h, mjtNum length) { mjtNum step = -res / grad; // backtracking line-search is not essential but can reduce number of iterations - for (int j=0; j<10; j++) { + for (int j=0; j < 10; j++) { mjtNum new_res = catenary_residual(b + step, intercept, NULL); if (mju_abs(new_res) < mju_abs(res)) { break; @@ -2300,7 +2301,7 @@ int mjv_catenary(const mjtNum x0[3], const mjtNum x1[3], const mjtNum gravity[3] mju_copy3(catenary+0, x0); // hanging points - for (int i=1; i