chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add .clang-format to user/, format user_composite

Browse Source 
PiperOrigin-RevId: 958225464
Change-Id: Iad7eb52dbecf7585f98375c90863ca707c494590
This commit is contained in:
Yuval Tassa
2026-08-03 01:21:09 -07:00
committed by Copybara-Service
parent 4278c7b0cd
commit 760477f406
2 changed files with 253 additions and 241 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/.clang-format
+28
View File
@@ -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
...
src/user/user_composite.cc
+225 -241
View File
@@ -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<mjCPlugin*>(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);
}