diff --git a/src/user/.clang-format b/src/user/.clang-format new file mode 100644 index 00000000..48b08c7b --- /dev/null +++ b/src/user/.clang-format @@ -0,0 +1,28 @@ +--- +BasedOnStyle: Google +Language: Cpp +ColumnLimit: 100 +IncludeBlocks: Preserve +AlignArrayOfStructures: Left +AlignConsecutiveAssignments: + Enabled: true + AlignCompound: true + PadOperators: true +AlignConsecutiveDeclarations: + Enabled: true +AlignConsecutiveShortCaseStatements: + Enabled: true +AlignEscapedNewlines: LeftWithLastLine +AlignTrailingComments: + Kind: Always + OverEmptyLines: 2 +AllowAllArgumentsOnNextLine: false +AllowShortBlocksOnASingleLine: Always +AllowShortFunctionsOnASingleLine: Inline +BreakBinaryOperations: RespectPrecedence +BinPackArguments: false +BinPackParameters: OnePerLine +IndentPPDirectives: None +MaxEmptyLinesToKeep: 2 +SortIncludes: false +... diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc index d1bebf58..dddd85e0 100644 --- a/src/user/user_composite.cc +++ b/src/user/user_composite.cc @@ -44,12 +44,11 @@ static bool comperr(char* error, const char* msg, int error_sz) { } - // constructor mjCComposite::mjCComposite(void) { // common properties prefix.clear(); - type = mjCOMPTYPE_PARTICLE; + type = mjCOMPTYPE_PARTICLE; count[0] = count[1] = count[2] = 1; mjuu_setvec(offset, 0, 0, 0); mjuu_setvec(quat, 1, 0, 0, 0); @@ -57,10 +56,10 @@ mjCComposite::mjCComposite(void) { // plugin variables mjs_defaultPlugin(&plugin); - plugin_name = ""; + plugin_name = ""; plugin_instance_name = ""; - plugin.plugin_name = (mjString*)&plugin_name; - plugin.name = (mjString*)&plugin_instance_name; + plugin.plugin_name = (mjString*)&plugin_name; + plugin.name = (mjString*)&plugin_instance_name; // cable curve[0] = curve[1] = curve[2] = mjCOMPSHAPE_ZERO; @@ -68,28 +67,25 @@ mjCComposite::mjCComposite(void) { initial = "ball"; // skin - skin = false; + skin = false; skintexcoord = false; skinmaterial.clear(); mjuu_setvec(skinrgba, 1, 1, 1, 1); skininflate = 0; skinsubgrid = 0; - skingroup = 0; + skingroup = 0; // clear add flags - for (int i=0; i < mjNCOMPKINDS; i++) { - add[i] = false; - } + for (int i = 0; i < mjNCOMPKINDS; i++) { add[i] = false; } // clear internal dim = 0; } - // create the array of default joint options, append new elements only for particles type bool mjCComposite::AddDefaultJoint(char* error, int error_sz) { - for (int i=0; i < mjNCOMPKINDS; i++) { + for (int i = 0; i < mjNCOMPKINDS; i++) { if (!defjoint[(mjtCompKind)i].empty() && type != mjCOMPTYPE_PARTICLE) { comperr(error, "Only particles are allowed to have multiple joints", error_sz); return false; @@ -103,13 +99,12 @@ bool mjCComposite::AddDefaultJoint(char* error, int error_sz) { } - // set defaults, after reading top-level info and skin void mjCComposite::SetDefault(void) { // set all default groups to 3 - for (int i=0; i < mjNCOMPKINDS; i++) { - def[i].spec.geom->group = 3; - def[i].spec.site->group = 3; + for (int i = 0; i < mjNCOMPKINDS; i++) { + def[i].spec.geom->group = 3; + def[i].spec.site->group = 3; def[i].spec.tendon->group = 3; } @@ -118,24 +113,21 @@ void mjCComposite::SetDefault(void) { // set default geom and tendon group to 0 if needed to be visible if (!skin || type == mjCOMPTYPE_CABLE) { - for (int i=0; i < mjNCOMPKINDS; i++) { - def[i].spec.geom->group = 0; + for (int i = 0; i < mjNCOMPKINDS; i++) { + def[i].spec.geom->group = 0; def[i].spec.tendon->group = 0; } } } - // make composite object bool mjCComposite::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz) { mjCModel* model = (mjCModel*)spec->element; // check counts - for (int i=0; i < 3; i++) { - if (count[i] < 1) { - return comperr(error, "Positive counts expected in composite", error_sz); - } + for (int i = 0; i < 3; i++) { + if (count[i] < 1) { return comperr(error, "Positive counts expected in composite", error_sz); } } // check cable sizes are nonzero if vertices are not prescribed @@ -148,20 +140,18 @@ bool mjCComposite::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz) if (count[0] > 1) { return comperr(error, "Either vertex or count can be specified, not both", error_sz); } - count[0] = uservert.size()/3; + count[0] = uservert.size() / 3; count[1] = 1; } // determine dimensionality, check singleton order bool first = false; - for (int i=0; i < 3; i++) { + for (int i = 0; i < 3; i++) { if (count[i] == 1) { first = true; } else { dim++; - if (first) { - return comperr(error, "Singleton counts must come last", error_sz); - } + if (first) { return comperr(error, "Singleton counts must come last", error_sz); } } } @@ -194,7 +184,7 @@ bool mjCComposite::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz) // overwrite plugin name if (plugin_instance_name.empty() && plugin.active) { - plugin_instance_name = "composite" + prefix; + plugin_instance_name = "composite" + prefix; (static_cast(plugin.element))->name = plugin_instance_name; } @@ -239,12 +229,9 @@ bool mjCComposite::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz) } - bool mjCComposite::MakeCable(mjCModel* model, mjsBody* body, char* error, int error_sz) { // check dim - if (dim != 1) { - return comperr(error, "Cable must be one-dimensional", error_sz); - } + if (dim != 1) { return comperr(error, "Cable must be one-dimensional", error_sz); } // check geom type if (def[0].spec.geom->type != mjGEOM_CYLINDER && @@ -260,18 +247,18 @@ bool mjCComposite::MakeCable(mjCModel* model, mjsBody* body, char* error, int er // populate uservert if not specified if (uservert.empty()) { - for (int ix=0; ix < count[0]; ix++) { + for (int ix = 0; ix < count[0]; ix++) { double v[3]; - for (int k=0; k < 3; k++) { + for (int k = 0; k < 3; k++) { switch (curve[k]) { case mjCOMPSHAPE_LINE: - v[k] = ix*size[0]/(count[0]-1); + v[k] = ix * size[0] / (count[0] - 1); break; case mjCOMPSHAPE_COS: - v[k] = size[1]*cos(mjPI*ix*size[2]/(count[0]-1)); + v[k] = size[1] * cos(mjPI * ix * size[2] / (count[0] - 1)); break; case mjCOMPSHAPE_SIN: - v[k] = size[1]*sin(mjPI*ix*size[2]/(count[0]-1)); + v[k] = size[1] * sin(mjPI * ix * size[2] / (count[0] - 1)); break; case mjCOMPSHAPE_ZERO: v[k] = 0; @@ -283,7 +270,7 @@ bool mjCComposite::MakeCable(mjCModel* model, mjsBody* body, char* error, int er } } mjuu_rotVecQuat(v, v, quat); - uservert.insert(uservert.end(), v, v+3); + uservert.insert(uservert.end(), v, v + 3); } } @@ -293,19 +280,19 @@ bool mjCComposite::MakeCable(mjCModel* model, mjsBody* body, char* error, int er mjuu_setvec(prev_quat, 1, 0, 0, 0); // add one body after the other - for (int ix=0; ix < count[0]-1; ix++) { + for (int ix = 0; ix < count[0] - 1; ix++) { body = AddCableBody(model, body, ix, normal, prev_quat); } // add skin if (def[0].spec.geom->type == mjGEOM_BOX) { if (skinsubgrid > 0) { - count[1]+=2; - MakeSkin2Subgrid(model, 2*def[0].spec.geom->size[2]); - count[1]-=2; + count[1] += 2; + MakeSkin2Subgrid(model, 2 * def[0].spec.geom->size[2]); + count[1] -= 2; } else { count[1]++; - MakeSkin2(model, 2*def[0].spec.geom->size[2]); + MakeSkin2(model, 2 * def[0].spec.geom->size[2]); count[1]--; } } @@ -313,34 +300,36 @@ bool mjCComposite::MakeCable(mjCModel* model, mjsBody* body, char* error, int er } - -mjsBody* mjCComposite::AddCableBody(mjCModel* model, mjsBody* body, int ix, - double normal[3], double prev_quat[4]) { - char txt_geom[100], txt_site[100], txt_slide[100]; - char this_body[100], next_body[100], this_joint[100]; +mjsBody* mjCComposite::AddCableBody( + mjCModel* model, mjsBody* body, int ix, double normal[3], double prev_quat[4]) { + char txt_geom[100], txt_site[100], txt_slide[100]; + char this_body[100], next_body[100], this_joint[100]; double dquat[4], this_quat[4]; // set flags - int lastidx = count[0]-2; - bool first = ix == 0; - bool last = ix == lastidx; - bool secondlast = ix == lastidx-1; + int lastidx = count[0] - 2; + bool first = ix == 0; + bool last = ix == lastidx; + bool secondlast = ix == lastidx - 1; // compute edge and tangent vectors double edge[3], tprev[3], tnext[3], length_prev = 0; - mjuu_setvec(edge, uservert[3*(ix+1)+0]-uservert[3*ix+0], - uservert[3*(ix+1)+1]-uservert[3*ix+1], - uservert[3*(ix+1)+2]-uservert[3*ix+2]); + mjuu_setvec(edge, + uservert[3 * (ix + 1) + 0] - uservert[3 * ix + 0], + uservert[3 * (ix + 1) + 1] - uservert[3 * ix + 1], + uservert[3 * (ix + 1) + 2] - uservert[3 * ix + 2]); if (!first) { - mjuu_setvec(tprev, uservert[3*ix+0]-uservert[3*(ix-1)+0], - uservert[3*ix+1]-uservert[3*(ix-1)+1], - uservert[3*ix+2]-uservert[3*(ix-1)+2]); + mjuu_setvec(tprev, + uservert[3 * ix + 0] - uservert[3 * (ix - 1) + 0], + uservert[3 * ix + 1] - uservert[3 * (ix - 1) + 1], + uservert[3 * ix + 2] - uservert[3 * (ix - 1) + 2]); length_prev = mjuu_normvec(tprev, 3); } if (!last) { - mjuu_setvec(tnext, uservert[3*(ix+2)+0]-uservert[3*(ix+1)+0], - uservert[3*(ix+2)+1]-uservert[3*(ix+1)+1], - uservert[3*(ix+2)+2]-uservert[3*(ix+1)+2]); + mjuu_setvec(tnext, + uservert[3 * (ix + 2) + 0] - uservert[3 * (ix + 1) + 0], + uservert[3 * (ix + 2) + 1] - uservert[3 * (ix + 1) + 1], + uservert[3 * (ix + 2) + 2] - uservert[3 * (ix + 1) + 2]); mjuu_normvec(tnext, 3); } @@ -350,7 +339,7 @@ mjsBody* mjCComposite::AddCableBody(mjCModel* model, mjsBody* body, int ix, // create body, joint, and geom names if (first) { mju::sprintf_arr(this_body, "%sB_first", prefix.c_str()); - mju::sprintf_arr(next_body, "%sB_%d", prefix.c_str(), ix+1); + mju::sprintf_arr(next_body, "%sB_%d", prefix.c_str(), ix + 1); mju::sprintf_arr(this_joint, "%sJ_first", prefix.c_str()); mju::sprintf_arr(txt_site, "%sS_first", prefix.c_str()); } else if (last) { @@ -358,13 +347,13 @@ mjsBody* mjCComposite::AddCableBody(mjCModel* model, mjsBody* body, int ix, mju::sprintf_arr(next_body, "%sB_first", prefix.c_str()); mju::sprintf_arr(this_joint, "%sJ_last", prefix.c_str()); mju::sprintf_arr(txt_site, "%sS_last", prefix.c_str()); - } else if (secondlast){ + } else if (secondlast) { mju::sprintf_arr(this_body, "%sB_%d", prefix.c_str(), ix); mju::sprintf_arr(next_body, "%sB_last", prefix.c_str()); mju::sprintf_arr(this_joint, "%sJ_%d", prefix.c_str(), ix); } else { mju::sprintf_arr(this_body, "%sB_%d", prefix.c_str(), ix); - mju::sprintf_arr(next_body, "%sB_%d", prefix.c_str(), ix+1); + mju::sprintf_arr(next_body, "%sB_%d", prefix.c_str(), ix + 1); mju::sprintf_arr(this_joint, "%sJ_%d", prefix.c_str(), ix); } mju::sprintf_arr(txt_geom, "%sG%d", prefix.c_str(), ix); @@ -374,13 +363,12 @@ mjsBody* mjCComposite::AddCableBody(mjCModel* model, mjsBody* body, int ix, body = mjs_addBody(body, 0); mjs_setName(body->element, this_body); if (first) { - mjuu_setvec(body->pos, offset[0]+uservert[3*ix], - offset[1]+uservert[3*ix+1], - offset[2]+uservert[3*ix+2]); + mjuu_setvec(body->pos, + offset[0] + uservert[3 * ix], + offset[1] + uservert[3 * ix + 1], + offset[2] + uservert[3 * ix + 2]); mjuu_copyvec(body->quat, this_quat, 4); - if (frame) { - mjs_setFrame(body->element, frame); - } + if (frame) { mjs_setFrame(body->element, frame); } } else { mjuu_setvec(body->pos, length_prev, 0, 0); double negquat[4] = {prev_quat[0], -prev_quat[1], -prev_quat[2], -prev_quat[3]}; @@ -392,21 +380,20 @@ mjsBody* mjCComposite::AddCableBody(mjCModel* model, mjsBody* body, int ix, mjsGeom* geom = mjs_addGeom(body, &def[0].spec); mjs_setDefault(geom->element, mjs_getDefault(body->element)); mjs_setName(geom->element, txt_geom); - if (def[0].spec.geom->type == mjGEOM_CYLINDER || - def[0].spec.geom->type == mjGEOM_CAPSULE) { + if (def[0].spec.geom->type == mjGEOM_CYLINDER || def[0].spec.geom->type == mjGEOM_CAPSULE) { mjuu_zerovec(geom->fromto, 6); geom->fromto[3] = length; } else if (def[0].spec.geom->type == mjGEOM_BOX) { mjuu_zerovec(geom->pos, 3); - geom->pos[0] = length/2; - geom->size[0] = length/2; + geom->pos[0] = length / 2; + geom->size[0] = length / 2; } // add plugin if (plugin.active) { mjsPlugin* pplugin = &body->plugin; - pplugin->active = true; - pplugin->element = plugin.element; + pplugin->active = true; + pplugin->element = plugin.element; mjs_setString(pplugin->plugin_name, mjs_getString(plugin.plugin_name)); mjs_setString(pplugin->name, plugin_instance_name.c_str()); } @@ -419,8 +406,8 @@ mjsBody* mjCComposite::AddCableBody(mjCModel* model, mjsBody* body, int ix, mjsJoint* jnt = mjs_addJoint(body, &defjoint[mjCOMPKIND_JOINT][0].spec); mjs_setDefault(jnt->element, mjs_getDefault(body->element)); jnt->type = (first && strcmp(initial.c_str(), "free") == 0) ? mjJNT_FREE : mjJNT_BALL; - if (jnt->type == mjJNT_FREE) mjuu_zerovec(jnt->damping, 1+mjNPOLY); - jnt->armature = jnt->type == mjJNT_FREE ? 0 : jnt->armature; + if (jnt->type == mjJNT_FREE) mjuu_zerovec(jnt->damping, 1 + mjNPOLY); + jnt->armature = jnt->type == mjJNT_FREE ? 0 : jnt->armature; jnt->frictionloss = jnt->type == mjJNT_FREE ? 0 : jnt->frictionloss; mjs_setName(jnt->element, this_joint); } @@ -445,7 +432,6 @@ mjsBody* mjCComposite::AddCableBody(mjCModel* model, mjsBody* body, int ix, } - // copy local vectors to skin void mjCComposite::CopyIntoSkin(mjsSkin* skin) { mjs_setInt(skin->face, face.data(), face.size()); @@ -454,10 +440,10 @@ void mjCComposite::CopyIntoSkin(mjsSkin* skin) { mjs_setFloat(skin->bindquat, bindquat.data(), bindquat.size()); mjs_setFloat(skin->texcoord, texcoord.data(), texcoord.size()); - for (int i=0; i < vertid.size(); i++) { + for (int i = 0; i < vertid.size(); i++) { mjs_appendIntVec(skin->vertid, vertid[i].data(), vertid[i].size()); } - for (int i=0; i < vertweight.size(); i++) { + for (int i = 0; i < vertweight.size(); i++) { mjs_appendFloatVec(skin->vertweight, vertweight[i].data(), vertweight[i].size()); } @@ -471,11 +457,10 @@ void mjCComposite::CopyIntoSkin(mjsSkin* skin) { } - // add skin to 2D void mjCComposite::MakeSkin2(mjCModel* model, mjtNum inflate) { char txt[100]; - int N = count[0]*count[1]; + int N = count[0] * count[1]; // add skin, set name and material mjsSkin* skin = mjs_addSkin(&model->spec); @@ -484,12 +469,12 @@ void mjCComposite::MakeSkin2(mjCModel* model, mjtNum inflate) { mjs_setString(skin->material, skinmaterial.c_str()); mjuu_copyvec(skin->rgba, skinrgba, 4); skin->inflate = inflate; - skin->group = skingroup; + skin->group = skingroup; // populate mesh: two sides - for (int i=0; i < 2; i++) { - for (int ix=0; ix < count[0]; ix++) { - for (int iy=0; iy < count[1]; iy++) { + for (int i = 0; i < 2; i++) { + for (int ix = 0; ix < count[0]; ix++) { + for (int iy = 0; iy < count[1]; iy++) { // vertex vert.push_back(0); vert.push_back(0); @@ -497,66 +482,66 @@ void mjCComposite::MakeSkin2(mjCModel* model, mjtNum inflate) { // texture coordinate if (skintexcoord) { - texcoord.push_back(ix/(float)(count[0]-1)); - texcoord.push_back(iy/(float)(count[1]-1)); + texcoord.push_back(ix / (float)(count[0] - 1)); + texcoord.push_back(iy / (float)(count[1] - 1)); } // face - if (ix < count[0]-1 && iy < count[1]-1) { - face.push_back(i*N + ix*count[1]+iy); - face.push_back(i*N + (ix+1)*count[1]+iy+(i == 1)); - face.push_back(i*N + (ix+1)*count[1]+iy+(i == 0)); + if (ix < count[0] - 1 && iy < count[1] - 1) { + face.push_back(i * N + ix * count[1] + iy); + face.push_back(i * N + (ix + 1) * count[1] + iy + (i == 1)); + face.push_back(i * N + (ix + 1) * count[1] + iy + (i == 0)); - face.push_back(i*N + ix*count[1]+iy); - face.push_back(i*N + (ix+(i == 0))*count[1]+iy+1); - face.push_back(i*N + (ix+(i == 1))*count[1]+iy+1); + face.push_back(i * N + ix * count[1] + iy); + face.push_back(i * N + (ix + (i == 0)) * count[1] + iy + 1); + face.push_back(i * N + (ix + (i == 1)) * count[1] + iy + 1); } } } } // add thin triangles: X direction, iy = 0 - for (int ix=0; ix < count[0]-1; ix++) { - face.push_back(ix*count[1]); - face.push_back(N + (ix+1)*count[1]); - face.push_back((ix+1)*count[1]); + for (int ix = 0; ix < count[0] - 1; ix++) { + face.push_back(ix * count[1]); + face.push_back(N + (ix + 1) * count[1]); + face.push_back((ix + 1) * count[1]); - face.push_back(ix*count[1]); - face.push_back(N + ix*count[1]); - face.push_back(N + (ix+1)*count[1]); + face.push_back(ix * count[1]); + face.push_back(N + ix * count[1]); + face.push_back(N + (ix + 1) * count[1]); } // add thin triangles: X direction, iy = count[1]-1 - for (int ix=0; ix < count[0]-1; ix++) { - face.push_back(ix*count[1] + count[1]-1); - face.push_back((ix+1)*count[1] + count[1]-1); - face.push_back(N + (ix+1)*count[1] + count[1]-1); + for (int ix = 0; ix < count[0] - 1; ix++) { + face.push_back(ix * count[1] + count[1] - 1); + face.push_back((ix + 1) * count[1] + count[1] - 1); + face.push_back(N + (ix + 1) * count[1] + count[1] - 1); - face.push_back(ix*count[1] + count[1]-1); - face.push_back(N + (ix+1)*count[1] + count[1]-1); - face.push_back(N + ix*count[1] + count[1]-1); + face.push_back(ix * count[1] + count[1] - 1); + face.push_back(N + (ix + 1) * count[1] + count[1] - 1); + face.push_back(N + ix * count[1] + count[1] - 1); } // add thin triangles: Y direction, ix = 0 - for (int iy=0; iy < count[1]-1; iy++) { + for (int iy = 0; iy < count[1] - 1; iy++) { face.push_back(iy); - face.push_back(iy+1); - face.push_back(N + iy+1); + face.push_back(iy + 1); + face.push_back(N + iy + 1); face.push_back(iy); - face.push_back(N + iy+1); + face.push_back(N + iy + 1); face.push_back(N + iy); } // add thin triangles: Y direction, ix = count[0]-1 - for (int iy=0; iy < count[1]-1; iy++) { - face.push_back(iy + (count[0]-1)*count[1]); - face.push_back(N + iy+1 + (count[0]-1)*count[1]); - face.push_back(iy+1 + (count[0]-1)*count[1]); + for (int iy = 0; iy < count[1] - 1; iy++) { + face.push_back(iy + (count[0] - 1) * count[1]); + face.push_back(N + iy + 1 + (count[0] - 1) * count[1]); + face.push_back(iy + 1 + (count[0] - 1) * count[1]); - face.push_back(iy + (count[0]-1)*count[1]); - face.push_back(N + iy + (count[0]-1)*count[1]); - face.push_back(N + iy+1 + (count[0]-1)*count[1]); + face.push_back(iy + (count[0] - 1) * count[1]); + face.push_back(N + iy + (count[0] - 1) * count[1]); + face.push_back(N + iy + 1 + (count[0] - 1) * count[1]); } // couple with bones @@ -567,19 +552,18 @@ void mjCComposite::MakeSkin2(mjCModel* model, mjtNum inflate) { } - // add bones to 1D void mjCComposite::MakeCableBones(mjCModel* model, mjsSkin* skin) { char this_body[100]; - int N = count[0]*count[1]; + int N = count[0] * count[1]; // populate bones - for (int ix=0; ix < count[0]; ix++) { - for (int iy=0; iy < count[1]; iy++) { + for (int ix = 0; ix < count[0]; ix++) { + for (int iy = 0; iy < count[1]; iy++) { // body name if (ix == 0) { mju::sprintf_arr(this_body, "%sB_first", prefix.c_str()); - } else if (ix >= count[0]-2) { + } else if (ix >= count[0] - 2) { mju::sprintf_arr(this_body, "%sB_last", prefix.c_str()); } else { mju::sprintf_arr(this_body, "%sB_%d", prefix.c_str(), ix); @@ -588,39 +572,42 @@ void mjCComposite::MakeCableBones(mjCModel* model, mjsSkin* skin) { // bind pose if (iy == 0) { mjs_appendString(skin->bodyname, this_body); - bindpos.push_back((ix == count[0]-1) ? -2*def[0].spec.geom->size[0] : 0); + bindpos.push_back((ix == count[0] - 1) ? -2 * def[0].spec.geom->size[0] : 0); bindpos.push_back(-def[0].spec.geom->size[1]); bindpos.push_back(0); - bindquat.push_back(1); bindquat.push_back(0); - bindquat.push_back(0); bindquat.push_back(0); + bindquat.push_back(1); + bindquat.push_back(0); + bindquat.push_back(0); + bindquat.push_back(0); } else { mjs_appendString(skin->bodyname, this_body); - bindpos.push_back((ix == count[0]-1) ? -2*def[0].spec.geom->size[0] : 0); + bindpos.push_back((ix == count[0] - 1) ? -2 * def[0].spec.geom->size[0] : 0); bindpos.push_back(def[0].spec.geom->size[1]); bindpos.push_back(0); - bindquat.push_back(1); bindquat.push_back(0); - bindquat.push_back(0); bindquat.push_back(0); + bindquat.push_back(1); + bindquat.push_back(0); + bindquat.push_back(0); + bindquat.push_back(0); } // create vertid and vertweight - vertid.push_back({ix*count[1]+iy, N + ix*count[1]+iy}); + vertid.push_back({ix * count[1] + iy, N + ix * count[1] + iy}); vertweight.push_back({1, 1}); } } } - void mjCComposite::MakeCableBonesSubgrid(mjCModel* model, mjsSkin* skin) { // populate bones - for (int ix=0; ix < count[0]; ix++) { - for (int iy=0; iy < count[1]; iy++) { + for (int ix = 0; ix < count[0]; ix++) { + for (int iy = 0; iy < count[1]; iy++) { char txt[100]; // body name if (ix == 0) { mju::sprintf_arr(txt, "%sB_first", prefix.c_str()); - } else if (ix >= count[0]-2) { + } else if (ix >= count[0] - 2) { mju::sprintf_arr(txt, "%sB_last", prefix.c_str()); } else { mju::sprintf_arr(txt, "%sB_%d", prefix.c_str(), ix); @@ -628,15 +615,15 @@ void mjCComposite::MakeCableBonesSubgrid(mjCModel* model, mjsSkin* skin) { // bind pose if (iy == 0) { - bindpos.push_back((ix == count[0]-1) ? -2*def[0].spec.geom->size[0] : 0); + bindpos.push_back((ix == count[0] - 1) ? -2 * def[0].spec.geom->size[0] : 0); bindpos.push_back(-def[0].Geom().spec.size[1]); bindpos.push_back(0); } else if (iy == 2) { - bindpos.push_back((ix == count[0]-1) ? -2*def[0].spec.geom->size[0] : 0); + bindpos.push_back((ix == count[0] - 1) ? -2 * def[0].spec.geom->size[0] : 0); bindpos.push_back(def[0].Geom().spec.size[1]); bindpos.push_back(0); } else { - bindpos.push_back((ix == count[0]-1) ? -2*def[0].spec.geom->size[0] : 0); + bindpos.push_back((ix == count[0] - 1) ? -2 * def[0].spec.geom->size[0] : 0); bindpos.push_back(0); bindpos.push_back(0); } @@ -654,8 +641,8 @@ void mjCComposite::MakeCableBonesSubgrid(mjCModel* model, mjsSkin* skin) { } - //------------------------------------- subgrid matrices +// clang-format off // C = W * [f; f_x; f_y; f_xy] static const mjtNum subW[16*16] = { @@ -883,53 +870,53 @@ static const mjtNum subD22[] = { 9, -1, 6, -1, 5, 1, 10, 1, -1, 9, -0.5, 2, -0.5, 1, 0.5, 10, 0.5, -1 }; - +// clang-format on // add skin to 2D, with subgrid void mjCComposite::MakeSkin2Subgrid(mjCModel* model, mjtNum inflate) { // assemble pointers to Dxx matrices const mjtNum* Dp[3][3] = { - {subD00, subD01, subD02}, - {subD10, subD11, subD12}, - {subD20, subD21, subD22} + {subD00, subD01, subD02}, + {subD10, subD11, subD12}, + {subD20, subD21, subD22} }; // allocate - const int N = (2+skinsubgrid)*(2+skinsubgrid); - mjtNum* XY = (mjtNum*) mju_malloc(N*16*sizeof(mjtNum)); - mjtNum* XY_W = (mjtNum*) mju_malloc(N*16*sizeof(mjtNum)); - mjtNum* Weight = (mjtNum*) mju_malloc(9*N*16*sizeof(mjtNum)); - mjtNum* D = (mjtNum*) mju_malloc(16*16*sizeof(mjtNum)); + const int N = (2 + skinsubgrid) * (2 + skinsubgrid); + mjtNum* XY = (mjtNum*)mju_malloc(N * 16 * sizeof(mjtNum)); + mjtNum* XY_W = (mjtNum*)mju_malloc(N * 16 * sizeof(mjtNum)); + mjtNum* Weight = (mjtNum*)mju_malloc(9 * N * 16 * sizeof(mjtNum)); + mjtNum* D = (mjtNum*)mju_malloc(16 * 16 * sizeof(mjtNum)); // XY matrix - const mjtNum step = 1.0/(1+skinsubgrid); - int rxy = 0; - for (int sx=0; sx <= 1+skinsubgrid; sx++) { - for (int sy=0; sy <= 1+skinsubgrid; sy++) { + const mjtNum step = 1.0 / (1 + skinsubgrid); + int rxy = 0; + for (int sx = 0; sx <= 1 + skinsubgrid; sx++) { + for (int sy = 0; sy <= 1 + skinsubgrid; sy++) { // compute x, y - mjtNum x = sx*step; - mjtNum y = sy*step; + mjtNum x = sx * step; + mjtNum y = sy * step; // make XY-row - XY[16*rxy + 0] = 1; - XY[16*rxy + 1] = y; - XY[16*rxy + 2] = y*y; - XY[16*rxy + 3] = y*y*y; + XY[16 * rxy + 0] = 1; + XY[16 * rxy + 1] = y; + XY[16 * rxy + 2] = y * y; + XY[16 * rxy + 3] = y * y * y; - XY[16*rxy + 4] = x*1; - XY[16*rxy + 5] = x*y; - XY[16*rxy + 6] = x*y*y; - XY[16*rxy + 7] = x*y*y*y; + XY[16 * rxy + 4] = x * 1; + XY[16 * rxy + 5] = x * y; + XY[16 * rxy + 6] = x * y * y; + XY[16 * rxy + 7] = x * y * y * y; - XY[16*rxy + 8] = x*x*1; - XY[16*rxy + 9] = x*x*y; - XY[16*rxy + 10] = x*x*y*y; - XY[16*rxy + 11] = x*x*y*y*y; + XY[16 * rxy + 8] = x * x * 1; + XY[16 * rxy + 9] = x * x * y; + XY[16 * rxy + 10] = x * x * y * y; + XY[16 * rxy + 11] = x * x * y * y * y; - XY[16*rxy + 12] = x*x*x*1; - XY[16*rxy + 13] = x*x*x*y; - XY[16*rxy + 14] = x*x*x*y*y; - XY[16*rxy + 15] = x*x*x*y*y*y; + XY[16 * rxy + 12] = x * x * x * 1; + XY[16 * rxy + 13] = x * x * x * y; + XY[16 * rxy + 14] = x * x * x * y * y; + XY[16 * rxy + 15] = x * x * x * y * y * y; // advance row rxy++; @@ -940,17 +927,17 @@ void mjCComposite::MakeSkin2Subgrid(mjCModel* model, mjtNum inflate) { mju_mulMatMat(XY_W, XY, subW, N, 16, 16); // Weight matrices - for (int dx=0; dx < 3; dx++) { - for (int dy=0; dy < 3; dy++) { + for (int dx = 0; dx < 3; dx++) { + for (int dy = 0; dy < 3; dy++) { // make dense D - mju_zero(D, 16*16); + mju_zero(D, 16 * 16); int cnt = 0; - int r = 0, c; + int r = 0, c; while (r < 16) { // scan row while ((c = mju_round(Dp[dx][dy][cnt])) != -1) { - D[r*16+c] = Dp[dx][dy][cnt+1]; - cnt +=2; + D[r * 16 + c] = Dp[dx][dy][cnt + 1]; + cnt += 2; } // advance @@ -959,130 +946,128 @@ void mjCComposite::MakeSkin2Subgrid(mjCModel* model, mjtNum inflate) { } // Weight(d) = XY * W * D(d) - mju_mulMatMat(Weight + (dx*3+dy)*N*16, XY_W, D, N, 16, 16); + mju_mulMatMat(Weight + (dx * 3 + dy) * N * 16, XY_W, D, N, 16, 16); } } // add skin, set name and material - char txt[100]; + char txt[100]; mjsSkin* skin = mjs_addSkin(&model->spec); mju::sprintf_arr(txt, "%sSkin", prefix.c_str()); mjs_setName(skin->element, txt); mjs_setString(skin->material, skinmaterial.c_str()); mjuu_copyvec(skin->rgba, skinrgba, 4); skin->inflate = inflate; - skin->group = skingroup; + skin->group = skingroup; // populate mesh: two sides - mjtNum S = 0; - int C0 = count[0] + (count[0]-1)*skinsubgrid; - int C1 = count[1] + (count[1]-1)*skinsubgrid; - int NN = C0*C1; - for (int i=0; i < 2; i++) { - for (int ix=0; ix < C0; ix++) { - for (int iy=0; iy < C1; iy++) { + mjtNum S = 0; + int C0 = count[0] + (count[0] - 1) * skinsubgrid; + int C1 = count[1] + (count[1] - 1) * skinsubgrid; + int NN = C0 * C1; + for (int i = 0; i < 2; i++) { + for (int ix = 0; ix < C0; ix++) { + for (int iy = 0; iy < C1; iy++) { // vertex - vert.push_back(ix*S); - vert.push_back(iy*S); + vert.push_back(ix * S); + vert.push_back(iy * S); vert.push_back(0); // texture coordinate if (skintexcoord) { - texcoord.push_back(ix/(float)(C0-1)); - texcoord.push_back(iy/(float)(C1-1)); + texcoord.push_back(ix / (float)(C0 - 1)); + texcoord.push_back(iy / (float)(C1 - 1)); } // face - if (ix < C0-1 && iy < C1-1) { - face.push_back(i*NN + ix*C1+iy); - face.push_back(i*NN + (ix+1)*C1+iy+(i == 1)); - face.push_back(i*NN + (ix+1)*C1+iy+(i == 0)); + if (ix < C0 - 1 && iy < C1 - 1) { + face.push_back(i * NN + ix * C1 + iy); + face.push_back(i * NN + (ix + 1) * C1 + iy + (i == 1)); + face.push_back(i * NN + (ix + 1) * C1 + iy + (i == 0)); - face.push_back(i*NN + ix*C1+iy); - face.push_back(i*NN + (ix+(i == 0))*C1+iy+1); - face.push_back(i*NN + (ix+(i == 1))*C1+iy+1); + face.push_back(i * NN + ix * C1 + iy); + face.push_back(i * NN + (ix + (i == 0)) * C1 + iy + 1); + face.push_back(i * NN + (ix + (i == 1)) * C1 + iy + 1); } } } } // add thin triangles: X direction, iy = 0 - for (int ix=0; ix < C0-1; ix++) { - face.push_back(ix*C1); - face.push_back(NN + (ix+1)*C1); - face.push_back((ix+1)*C1); + for (int ix = 0; ix < C0 - 1; ix++) { + face.push_back(ix * C1); + face.push_back(NN + (ix + 1) * C1); + face.push_back((ix + 1) * C1); - face.push_back(ix*C1); - face.push_back(NN + ix*C1); - face.push_back(NN + (ix+1)*C1); + face.push_back(ix * C1); + face.push_back(NN + ix * C1); + face.push_back(NN + (ix + 1) * C1); } // add thin triangles: X direction, iy = C1-1 - for (int ix=0; ix < C0-1; ix++) { - face.push_back(ix*C1 + C1-1); - face.push_back((ix+1)*C1 + C1-1); - face.push_back(NN + (ix+1)*C1 + C1-1); + for (int ix = 0; ix < C0 - 1; ix++) { + face.push_back(ix * C1 + C1 - 1); + face.push_back((ix + 1) * C1 + C1 - 1); + face.push_back(NN + (ix + 1) * C1 + C1 - 1); - face.push_back(ix*C1 + C1-1); - face.push_back(NN + (ix+1)*C1 + C1-1); - face.push_back(NN + ix*C1 + C1-1); + face.push_back(ix * C1 + C1 - 1); + face.push_back(NN + (ix + 1) * C1 + C1 - 1); + face.push_back(NN + ix * C1 + C1 - 1); } // add thin triangles: Y direction, ix = 0 - for (int iy=0; iy < C1-1; iy++) { + for (int iy = 0; iy < C1 - 1; iy++) { face.push_back(iy); - face.push_back(iy+1); - face.push_back(NN + iy+1); + face.push_back(iy + 1); + face.push_back(NN + iy + 1); face.push_back(iy); - face.push_back(NN + iy+1); + face.push_back(NN + iy + 1); face.push_back(NN + iy); } // add thin triangles: Y direction, ix = C0-1 - for (int iy=0; iy < C1-1; iy++) { - face.push_back(iy + (C0-1)*C1); - face.push_back(NN + iy+1 + (C0-1)*C1); - face.push_back(iy+1 + (C0-1)*C1); + for (int iy = 0; iy < C1 - 1; iy++) { + face.push_back(iy + (C0 - 1) * C1); + face.push_back(NN + iy + 1 + (C0 - 1) * C1); + face.push_back(iy + 1 + (C0 - 1) * C1); - face.push_back(iy + (C0-1)*C1); - face.push_back(NN + iy + (C0-1)*C1); - face.push_back(NN + iy+1 + (C0-1)*C1); + face.push_back(iy + (C0 - 1) * C1); + face.push_back(NN + iy + (C0 - 1) * C1); + face.push_back(NN + iy + 1 + (C0 - 1) * C1); } MakeCableBonesSubgrid(model, skin); // bind vertices to bones: one big square at a time - for (int ix=0; ix < count[0]-1; ix++) { - for (int iy=0; iy < count[1]-1; iy++) { + for (int ix = 0; ix < count[0] - 1; ix++) { + for (int iy = 0; iy < count[1] - 1; iy++) { // determine d for Weight indexing - int d = 3 * (ix == 0 ? 0 : (ix == count[0]-2 ? 2 : 1)) + - (iy == 0 ? 0 : (iy == count[1]-2 ? 2 : 1)); + int d = 3 * (ix == 0 ? 0 : (ix == count[0] - 2 ? 2 : 1)) + + (iy == 0 ? 0 : (iy == count[1] - 2 ? 2 : 1)); // precompute 16 bone indices for big square int boneid[16]; int cnt = 0; - for (int dx=-1; dx < 3; dx++) { - for (int dy=-1; dy < 3; dy++) { - boneid[cnt++] = (ix+dx)*count[1] + (iy+dy); - } + for (int dx = -1; dx < 3; dx++) { + for (int dy = -1; dy < 3; dy++) { boneid[cnt++] = (ix + dx) * count[1] + (iy + dy); } } // process subgrid, top-rigth owns last index - for (int dx=0; dx < 1+skinsubgrid+(ix == count[0]-2); dx++) { - for (int dy=0; dy < 1+skinsubgrid+(iy == count[1]-2); dy++) { + for (int dx = 0; dx < 1 + skinsubgrid + (ix == count[0] - 2); dx++) { + for (int dy = 0; dy < 1 + skinsubgrid + (iy == count[1] - 2); dy++) { // recover vertex id - int vid = (ix*(1+skinsubgrid)+dx)*C1 + iy*(1+skinsubgrid)+dy; + int vid = (ix * (1 + skinsubgrid) + dx) * C1 + iy * (1 + skinsubgrid) + dy; // determine row in Weight - int n = dx*(2+skinsubgrid) + dy; + int n = dx * (2 + skinsubgrid) + dy; // add vertex to 16 bones - for (int bi=0; bi < 16; bi++) { - mjtNum w = Weight[d*N*16 + n*16 + bi]; + for (int bi = 0; bi < 16; bi++) { + mjtNum w = Weight[d * N * 16 + n * 16 + bi]; if (w) { vertid[boneid[bi]].push_back(vid); - vertid[boneid[bi]].push_back(vid+NN); + vertid[boneid[bi]].push_back(vid + NN); vertweight[boneid[bi]].push_back((float)w); vertweight[boneid[bi]].push_back((float)w); } @@ -1100,4 +1085,3 @@ void mjCComposite::MakeSkin2Subgrid(mjCModel* model, mjtNum inflate) { mju_free(Weight); mju_free(D); } -