Enable interior nodes for interpolated flex shell mode.
Previously shell mode required cellcount=1 along at least one axis. This CL adds support for cellcount > 1 in all three axes by pinning interior grid nodes to the parent body and reconstructing their positions from boundary nodes via Transfinite Interpolation (TFI). PiperOrigin-RevId: 924314800 Change-Id: I8c2438f4866dd4133feed65f535a1ab69f0c9188
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
4548e81e4d
commit
91c92279d2
@@ -277,10 +277,18 @@ static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int* v,
|
||||
mju_addToScl3(coord, m->flex_vert0 + 3*v[i], mju_abs(vweight[i]));
|
||||
}
|
||||
|
||||
int order = m->flex_interp[f];
|
||||
order = order < 0 ? -order : order;
|
||||
int interp = m->flex_interp[f];
|
||||
int order = interp < 0 ? -interp : interp;
|
||||
int npc = (order+1)*(order+1)*(order+1); // number of nodes per cell
|
||||
|
||||
// grid dimensions for shell mode
|
||||
int nx = 0, ny = 0, nz = 0;
|
||||
if (interp < 0) {
|
||||
nx = m->flex_cellnum[3*f+0] * order + 1;
|
||||
ny = m->flex_cellnum[3*f+1] * order + 1;
|
||||
nz = m->flex_cellnum[3*f+2] * order + 1;
|
||||
}
|
||||
|
||||
// cell lookup: get local coords and node indices
|
||||
mjtNum local[3];
|
||||
int nodeindices[27]; // max npc for quadratic: 3^3 = 27
|
||||
@@ -290,14 +298,46 @@ static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int* v,
|
||||
int nstart = m->flex_nodeadr[f];
|
||||
int nb = 0;
|
||||
|
||||
if (!m->flex_nodebodyid) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (npc > 27) {
|
||||
for (int j = 0; j < npc; j++) {
|
||||
mjtNum w = mju_evalBasis(local, j, order);
|
||||
if (w < 1e-5) {
|
||||
continue;
|
||||
}
|
||||
if (bweight) bweight[nb] = sign * w;
|
||||
body[nb++] = m->flex_nodebodyid[nstart + nodeindices[j]];
|
||||
|
||||
int idx = nodeindices[j];
|
||||
|
||||
// shell mode: map interior nodes to boundary
|
||||
if (interp < 0) {
|
||||
int k_idx = idx % nz;
|
||||
int rest = idx / nz;
|
||||
int j_idx = rest % ny;
|
||||
int i_idx = rest / ny;
|
||||
|
||||
if (i_idx > 0 && i_idx < nx-1 && j_idx > 0 && j_idx < ny-1 && k_idx > 0 && k_idx < nz-1) {
|
||||
mju_shellTFIWeights(nx, ny, nz, i_idx, j_idx, k_idx, sign * w, &nb, body, bweight, m->flex_nodebodyid, nstart);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
// add node, check for duplicates (especially needed when combining with TFI)
|
||||
int b = m->flex_nodebodyid[nstart + idx];
|
||||
int found = 0;
|
||||
for (int k = 0; k < nb; k++) {
|
||||
if (body[k] == b) {
|
||||
if (bweight) bweight[k] += sign * w;
|
||||
found = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!found) {
|
||||
if (bweight) bweight[nb] = sign * w;
|
||||
body[nb++] = b;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
mjtNum basis[27];
|
||||
@@ -308,11 +348,40 @@ static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int* v,
|
||||
if (w < 1e-5) {
|
||||
continue;
|
||||
}
|
||||
if (bweight) bweight[nb] = sign * w;
|
||||
body[nb++] = m->flex_nodebodyid[nstart + nodeindices[j]];
|
||||
|
||||
int idx = nodeindices[j];
|
||||
|
||||
// shell mode: map interior nodes to boundary
|
||||
if (interp < 0) {
|
||||
int k_idx = idx % nz;
|
||||
int rest = idx / nz;
|
||||
int j_idx = rest % ny;
|
||||
int i_idx = rest / ny;
|
||||
|
||||
if (i_idx > 0 && i_idx < nx-1 && j_idx > 0 && j_idx < ny-1 && k_idx > 0 && k_idx < nz-1) {
|
||||
mju_shellTFIWeights(nx, ny, nz, i_idx, j_idx, k_idx, sign * w, &nb, body, bweight, m->flex_nodebodyid, nstart);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
// add node, check for duplicates (especially needed when combining with TFI)
|
||||
int b = m->flex_nodebodyid[nstart + idx];
|
||||
int found = 0;
|
||||
for (int k = 0; k < nb; k++) {
|
||||
if (body[k] == b) {
|
||||
if (bweight) bweight[k] += sign * w;
|
||||
found = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!found) {
|
||||
if (bweight) bweight[nb] = sign * w;
|
||||
body[nb++] = b;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
return nb;
|
||||
}
|
||||
|
||||
|
||||
@@ -66,10 +66,10 @@ void mj_instantiateEquality(const mjModel* m, mjData* d);
|
||||
void mj_instantiateContact(const mjModel* m, mjData* d);
|
||||
|
||||
// compute Jacobian for contact, return number of DOFs affected
|
||||
int mj_contactJacobian(const mjModel* m, mjData* d, const mjContact* con, int dim,
|
||||
mjtNum* jac, mjtNum* jacdif, mjtNum* jacdifp,
|
||||
mjtNum* jacdifr, mjtNum* jac1p, mjtNum* jac2p,
|
||||
mjtNum* jac1r, mjtNum* jac2r, int* chain);
|
||||
MJAPI int mj_contactJacobian(const mjModel* m, mjData* d, const mjContact* con, int dim,
|
||||
mjtNum* jac, mjtNum* jacdif, mjtNum* jacdifp,
|
||||
mjtNum* jacdifr, mjtNum* jac1p, mjtNum* jac2p,
|
||||
mjtNum* jac1r, mjtNum* jac2r, int* chain);
|
||||
|
||||
|
||||
//------------------------ parameter computation/extraction ----------------------------------------
|
||||
|
||||
@@ -607,6 +607,11 @@ void mj_flex(const mjModel* m, mjData* d) {
|
||||
mjERROR("flex_interp_order mismatch");
|
||||
}
|
||||
|
||||
// shell mode: reconstruct interior node positions from boundary via TFI
|
||||
if (interp < 0) {
|
||||
mju_shellTrackInterior(nodexpos, nx_g, ny_g, nz_g);
|
||||
}
|
||||
|
||||
for (int i=vstart; i < vend; i++) {
|
||||
mju_zero3(d->flexvert_xpos+3*i);
|
||||
|
||||
|
||||
@@ -1020,6 +1020,23 @@ void mju_flexGatherState(const mjModel* m, const mjData* d, int f, mjtNum* xpos,
|
||||
mju_addTo3(vel + 3*i, cross);
|
||||
}
|
||||
}
|
||||
|
||||
// shell mode: reconstruct interior node positions and velocities via TFI
|
||||
int interp = m->flex_interp[f];
|
||||
if (interp < 0) {
|
||||
int order = -interp;
|
||||
int cx = m->flex_cellnum[3*f+0];
|
||||
int cy = m->flex_cellnum[3*f+1];
|
||||
int cz = m->flex_cellnum[3*f+2];
|
||||
int nx_g = cx * order + 1;
|
||||
int ny_g = cy * order + 1;
|
||||
int nz_g = cz * order + 1;
|
||||
|
||||
mju_shellTrackInterior(xpos, nx_g, ny_g, nz_g);
|
||||
if (vel) {
|
||||
mju_shellTrackInterior(vel, nx_g, ny_g, nz_g);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -898,6 +898,151 @@ void mju_flexFaceNormal2D(mjtNum normal[3], mjtNum t1[3], mjtNum t2[3],
|
||||
}
|
||||
|
||||
|
||||
// helper: get nodexpos value for node (i,j,k) in an nx*ny*nz grid
|
||||
static inline const mjtNum* nodeAt(const mjtNum* nodexpos, int ny, int nz, int i, int j, int k) {
|
||||
return nodexpos + 3*(i*ny*nz + j*nz + k);
|
||||
}
|
||||
|
||||
// reconstruct interior node positions from boundary nodes via Transfinite Interpolation
|
||||
void mju_shellTrackInterior(mjtNum* nodexpos, int nx, int ny, int nz) {
|
||||
// need at least 3 nodes in each direction to have interior nodes
|
||||
if (nx < 3 || ny < 3 || nz < 3) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (int i = 1; i < nx-1; i++) {
|
||||
for (int j = 1; j < ny-1; j++) {
|
||||
for (int k = 1; k < nz-1; k++) {
|
||||
// parametric coordinates in [0, 1]
|
||||
mjtNum s = (mjtNum)i / (nx-1);
|
||||
mjtNum t = (mjtNum)j / (ny-1);
|
||||
mjtNum u = (mjtNum)k / (nz-1);
|
||||
|
||||
mjtNum result[3] = {0, 0, 0};
|
||||
|
||||
// --- face contributions (bilinear interpolation on each face pair) ---
|
||||
// x-faces: i=0 and i=nx-1
|
||||
for (int d = 0; d < 3; d++) {
|
||||
result[d] += (1-s) * nodeAt(nodexpos, ny, nz, 0, j, k)[d]
|
||||
+ s * nodeAt(nodexpos, ny, nz, nx-1, j, k)[d];
|
||||
}
|
||||
// y-faces: j=0 and j=ny-1
|
||||
for (int d = 0; d < 3; d++) {
|
||||
result[d] += (1-t) * nodeAt(nodexpos, ny, nz, i, 0, k)[d]
|
||||
+ t * nodeAt(nodexpos, ny, nz, i, ny-1, k)[d];
|
||||
}
|
||||
// z-faces: k=0 and k=nz-1
|
||||
for (int d = 0; d < 3; d++) {
|
||||
result[d] += (1-u) * nodeAt(nodexpos, ny, nz, i, j, 0)[d]
|
||||
+ u * nodeAt(nodexpos, ny, nz, i, j, nz-1)[d];
|
||||
}
|
||||
|
||||
// --- edge corrections (subtract 12 edges, each linearly interpolated) ---
|
||||
// edges along x (4 edges: (j,k) at corners of y-z face)
|
||||
for (int d = 0; d < 3; d++) {
|
||||
result[d] -= (1-t)*(1-u) * nodeAt(nodexpos, ny, nz, i, 0, 0)[d];
|
||||
result[d] -= (1-t)* u * nodeAt(nodexpos, ny, nz, i, 0, nz-1)[d];
|
||||
result[d] -= t *(1-u) * nodeAt(nodexpos, ny, nz, i, ny-1, 0)[d];
|
||||
result[d] -= t * u * nodeAt(nodexpos, ny, nz, i, ny-1, nz-1)[d];
|
||||
}
|
||||
// edges along y (4 edges: (i,k) at corners of x-z face)
|
||||
for (int d = 0; d < 3; d++) {
|
||||
result[d] -= (1-s)*(1-u) * nodeAt(nodexpos, ny, nz, 0, j, 0)[d];
|
||||
result[d] -= (1-s)* u * nodeAt(nodexpos, ny, nz, 0, j, nz-1)[d];
|
||||
result[d] -= s *(1-u) * nodeAt(nodexpos, ny, nz, nx-1, j, 0)[d];
|
||||
result[d] -= s * u * nodeAt(nodexpos, ny, nz, nx-1, j, nz-1)[d];
|
||||
}
|
||||
// edges along z (4 edges: (i,j) at corners of x-y face)
|
||||
for (int d = 0; d < 3; d++) {
|
||||
result[d] -= (1-s)*(1-t) * nodeAt(nodexpos, ny, nz, 0, 0, k)[d];
|
||||
result[d] -= (1-s)* t * nodeAt(nodexpos, ny, nz, 0, ny-1, k)[d];
|
||||
result[d] -= s *(1-t) * nodeAt(nodexpos, ny, nz, nx-1, 0, k)[d];
|
||||
result[d] -= s * t * nodeAt(nodexpos, ny, nz, nx-1, ny-1, k)[d];
|
||||
}
|
||||
|
||||
// --- corner corrections (add 8 corners back) ---
|
||||
for (int d = 0; d < 3; d++) {
|
||||
result[d] += (1-s)*(1-t)*(1-u) * nodeAt(nodexpos, ny, nz, 0, 0, 0)[d];
|
||||
result[d] += (1-s)*(1-t)* u * nodeAt(nodexpos, ny, nz, 0, 0, nz-1)[d];
|
||||
result[d] += (1-s)* t *(1-u) * nodeAt(nodexpos, ny, nz, 0, ny-1, 0)[d];
|
||||
result[d] += (1-s)* t * u * nodeAt(nodexpos, ny, nz, 0, ny-1, nz-1)[d];
|
||||
result[d] += s *(1-t)*(1-u) * nodeAt(nodexpos, ny, nz, nx-1, 0, 0)[d];
|
||||
result[d] += s *(1-t)* u * nodeAt(nodexpos, ny, nz, nx-1, 0, nz-1)[d];
|
||||
result[d] += s * t *(1-u) * nodeAt(nodexpos, ny, nz, nx-1, ny-1, 0)[d];
|
||||
result[d] += s * t * u * nodeAt(nodexpos, ny, nz, nx-1, ny-1, nz-1)[d];
|
||||
}
|
||||
|
||||
// write result to interior node
|
||||
mju_copy3(nodexpos + 3*(i*ny*nz + j*nz + k), result);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// helper to accumulate weights in a sparse list
|
||||
static void addWeight(int* nb, int* body, mjtNum* bweight, int b, mjtNum w) {
|
||||
for (int i = 0; i < *nb; i++) {
|
||||
if (body[i] == b) {
|
||||
if (bweight) {
|
||||
bweight[i] += w;
|
||||
}
|
||||
return;
|
||||
}
|
||||
}
|
||||
body[*nb] = b;
|
||||
if (bweight) {
|
||||
bweight[*nb] = w;
|
||||
}
|
||||
(*nb)++;
|
||||
}
|
||||
|
||||
// compute TFI weights for an interior node (i,j,k) and distribute to boundary nodes
|
||||
void mju_shellTFIWeights(int nx, int ny, int nz, int i, int j, int k,
|
||||
mjtNum w, int* nb, int* body, mjtNum* bweight,
|
||||
const int* nodebodyid, int nstart) {
|
||||
mjtNum s = (mjtNum)i / (nx-1);
|
||||
mjtNum t = (mjtNum)j / (ny-1);
|
||||
mjtNum u = (mjtNum)k / (nz-1);
|
||||
|
||||
// face contributions
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + j*nz + k], w * (1-s));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + j*nz + k], w * s);
|
||||
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + 0*nz + k], w * (1-t));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + (ny-1)*nz + k], w * t);
|
||||
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + j*nz + 0], w * (1-u));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + j*nz + (nz-1)], w * u);
|
||||
|
||||
// edge corrections
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + 0*nz + 0], -w * (1-t)*(1-u));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + 0*nz + (nz-1)], -w * (1-t)*u);
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + (ny-1)*nz + 0], -w * t*(1-u));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + (ny-1)*nz + (nz-1)], -w * t*u);
|
||||
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + j*nz + 0], -w * (1-s)*(1-u));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + j*nz + (nz-1)], -w * (1-s)*u);
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + j*nz + 0], -w * s*(1-u));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + j*nz + (nz-1)], -w * s*u);
|
||||
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + 0*nz + k], -w * (1-s)*(1-t));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + (ny-1)*nz + k], -w * (1-s)*t);
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + 0*nz + k], -w * s*(1-t));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + (ny-1)*nz + k], -w * s*t);
|
||||
|
||||
// corner corrections
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + 0*nz + 0], w * (1-s)*(1-t)*(1-u));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + 0*nz + (nz-1)], w * (1-s)*(1-t)*u);
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + (ny-1)*nz + 0], w * (1-s)*t*(1-u));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + (ny-1)*nz + (nz-1)], w * (1-s)*t*u);
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + 0*nz + 0], w * s*(1-t)*(1-u));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + 0*nz + (nz-1)], w * s*(1-t)*u);
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + (ny-1)*nz + 0], w * s*t*(1-u));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + (ny-1)*nz + (nz-1)], w * s*t*u);
|
||||
}
|
||||
|
||||
|
||||
//------------------------------ actuator models ---------------------------------------------------
|
||||
|
||||
// normalized muscle length-gain curve
|
||||
|
||||
@@ -147,6 +147,14 @@ static inline mjtNum mju_flexDphi(mjtNum s, int i, int order) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
// reconstruct interior node positions from boundary nodes via Transfinite Interpolation
|
||||
MJAPI void mju_shellTrackInterior(mjtNum* nodexpos, int nx, int ny, int nz);
|
||||
|
||||
// compute TFI weights for an interior node (i,j,k) and distribute to boundary nodes
|
||||
MJAPI void mju_shellTFIWeights(int nx, int ny, int nz, int i, int j, int k,
|
||||
mjtNum w, int* nb, int* body, mjtNum* bweight,
|
||||
const int* nodebodyid, int nstart);
|
||||
|
||||
|
||||
// ----------------------------- Base64 ------------------------------------------------------------
|
||||
|
||||
|
||||
@@ -873,14 +873,21 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
|
||||
mju_cellLookup(coord, m->flex_cellnum+3*i, order, loc, nodeindices);
|
||||
|
||||
// find node with largest weight in this cell
|
||||
// in shell mode, skip interior nodes (pinned to worldbody)
|
||||
int nodeid = -1;
|
||||
int nstart = m->flex_nodeadr[i];
|
||||
mjtNum w = 0;
|
||||
int shell_mode = m->flex_interp[i] < 0;
|
||||
for (int j = 0; j < npc; j++) {
|
||||
mjtNum ww = mju_evalBasis(loc, j, order);
|
||||
int nid = nodeindices[j];
|
||||
// skip interior nodes in shell mode (they map to worldbody)
|
||||
if (shell_mode && m->body_dofnum[m->flex_nodebodyid[nstart + nid]] == 0) {
|
||||
continue;
|
||||
}
|
||||
if (ww > w) {
|
||||
w = ww;
|
||||
nodeid = nodeindices[j];
|
||||
nodeid = nid;
|
||||
}
|
||||
}
|
||||
flexbodyid = m->flex_nodebodyid[nstart + nodeid];
|
||||
|
||||
@@ -1496,6 +1496,8 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
|
||||
int NY = cy * order + 1;
|
||||
int NZ = cz * order + 1;
|
||||
|
||||
int shell_mode = m->flex_interp[f] < 0;
|
||||
|
||||
for (int i=0; i < NX; i++) {
|
||||
for (int j=0; j < NY; j++) {
|
||||
for (int k=0; k < NZ; k++) {
|
||||
@@ -1506,36 +1508,58 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
|
||||
continue;
|
||||
}
|
||||
|
||||
// shell mode: skip interior nodes entirely
|
||||
int is_boundary = (i == 0 || i == NX-1 ||
|
||||
j == 0 || j == NY-1 ||
|
||||
k == 0 || k == NZ-1);
|
||||
if (shell_mode && !is_boundary) {
|
||||
continue;
|
||||
}
|
||||
|
||||
int offset = 3*n0;
|
||||
int offset1 = 3*((i+1)*NY*NZ + j*NZ + k);
|
||||
int offset2 = 3*(i*NY*NZ + (j+1)*NZ + k);
|
||||
int offset3 = 3*(i*NY*NZ + j*NZ + (k+1));
|
||||
if (i < NX-1 && m->body_jntnum[bodyid[(i+1)*NY*NZ + j*NZ + k]] > 0) {
|
||||
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
|
||||
if (!thisgeom) {
|
||||
return;
|
||||
}
|
||||
|
||||
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset1);
|
||||
releaseGeom(&thisgeom, scn);
|
||||
// edge along i: draw if neighbor is also on boundary (shell) or has joints
|
||||
if (i < NX-1 && m->body_jntnum[bodyid[(i+1)*NY*NZ + j*NZ + k]] > 0) {
|
||||
int nb_boundary = ((i+1) == 0 || (i+1) == NX-1 ||
|
||||
j == 0 || j == NY-1 ||
|
||||
k == 0 || k == NZ-1);
|
||||
if (!shell_mode || nb_boundary) {
|
||||
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
|
||||
if (!thisgeom) {
|
||||
return;
|
||||
}
|
||||
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset1);
|
||||
releaseGeom(&thisgeom, scn);
|
||||
}
|
||||
}
|
||||
if (j < NY-1 && m->body_jntnum[bodyid[i*NY*NZ + (j+1)*NZ + k]] > 0) {
|
||||
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
|
||||
if (!thisgeom) {
|
||||
return;
|
||||
int nb_boundary = (i == 0 || i == NX-1 ||
|
||||
(j+1) == 0 || (j+1) == NY-1 ||
|
||||
k == 0 || k == NZ-1);
|
||||
if (!shell_mode || nb_boundary) {
|
||||
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
|
||||
if (!thisgeom) {
|
||||
return;
|
||||
}
|
||||
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset2);
|
||||
releaseGeom(&thisgeom, scn);
|
||||
}
|
||||
|
||||
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset2);
|
||||
releaseGeom(&thisgeom, scn);
|
||||
}
|
||||
if (k < NZ-1 && m->body_jntnum[bodyid[i*NY*NZ + j*NZ + (k+1)]] > 0) {
|
||||
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
|
||||
if (!thisgeom) {
|
||||
return;
|
||||
int nb_boundary = (i == 0 || i == NX-1 ||
|
||||
j == 0 || j == NY-1 ||
|
||||
(k+1) == 0 || (k+1) == NZ-1);
|
||||
if (!shell_mode || nb_boundary) {
|
||||
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
|
||||
if (!thisgeom) {
|
||||
return;
|
||||
}
|
||||
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset3);
|
||||
releaseGeom(&thisgeom, scn);
|
||||
}
|
||||
|
||||
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset3);
|
||||
releaseGeom(&thisgeom, scn);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user