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

Merge geomsExhausted into acquireGeom.

Browse Source 
PiperOrigin-RevId: 813347956
Change-Id: I07bd566388383cc5c49e675f5b85fffb379ef6d1
This commit is contained in:
Haroon Qureshi
2025-09-30 11:18:47 -07:00
committed by Copybara-Service
parent de0ae73da7
commit 328a4ab293
1 changed files with 104 additions and 142 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_vis_visualize.c
+104 -142
View File
@@ -72,20 +72,11 @@ static void makeLabel(const mjModel* m, mjtObj type, int id, char* label) {
// returns 1 if there are no more geoms available in the scene, 0 otherwise
static inline int geomsExhausted(mjvScene* scn) {
if ( scn->ngeom>=scn->maxgeom ) {
scn->status = 1;
return 1;
}
return 0;
}
// acquires and initializes the next available geom in the scene // acquires and initializes the next available geom in the scene
mjvGeom* acquireGeom(mjvScene* scn, int objid, int category, int objtype) { mjvGeom* acquireGeom(mjvScene* scn, int objid, int category, int objtype) {
// check for overflow, SHOULD NOT OCCUR // check for overflow, SHOULD NOT OCCUR
if (scn->ngeom >= scn->maxgeom) { if (scn->ngeom >= scn->maxgeom) {
mju_error("Cannot acquire geom; all geoms are in use."); scn->status = 1;
return NULL; return NULL;
} }
@@ -203,11 +194,11 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
// contact point // contact point
if (flags[mjVIS_CONTACTPOINT]) { if (flags[mjVIS_CONTACTPOINT]) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
thisgeom->type = mjGEOM_CYLINDER; thisgeom->type = mjGEOM_CYLINDER;
thisgeom->size[0] = thisgeom->size[1] = m->vis.scale.contactwidth * scl; thisgeom->size[0] = thisgeom->size[1] = m->vis.scale.contactwidth * scl;
float halfheight = m->vis.scale.contactheight * scl; float halfheight = m->vis.scale.contactheight * scl;
@@ -288,12 +279,11 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
// draw the three axes (separate geoms) // draw the three axes (separate geoms)
for (int j=0; j < 3; j++) { for (int j=0; j < 3; j++) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
// prepare axis // prepare axis
for (int k=0; k < 3; k++) { for (int k=0; k < 3; k++) {
axis[k] = (j == k ? framelength : 0); axis[k] = (j == k ? framelength : 0);
@@ -369,11 +359,11 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
} }
// one-directional arrow for friction and world, symmetric otherwise // one-directional arrow for friction and world, symmetric otherwise
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
mjtNum* from = con->pos; mjtNum* from = con->pos;
mjtNum to[3]; mjtNum to[3];
mju_add3(to, from, vec); mju_add3(to, from, vec);
@@ -759,12 +749,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
(category & catmask)) { (category & catmask)) {
for (int i=0; i < m->nflex; i++) { for (int i=0; i < m->nflex; i++) {
if (vopt->flexgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->flex_group[i]))]) { if (vopt->flexgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->flex_group[i]))]) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
// construct geom, pos = first vertex // construct geom, pos = first vertex
mjv_initGeom(thisgeom, mjGEOM_FLEX, NULL, mjv_initGeom(thisgeom, mjGEOM_FLEX, NULL,
d->flexvert_xpos + 3*m->flex_vertadr[i], NULL, NULL); d->flexvert_xpos + 3*m->flex_vertadr[i], NULL, NULL);
@@ -808,12 +797,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
if (vopt->flags[mjVIS_SKIN] && (category & catmask)) { if (vopt->flags[mjVIS_SKIN] && (category & catmask)) {
for (int i=0; i < m->nskin; i++) { for (int i=0; i < m->nskin; i++) {
if (vopt->skingroup[mjMAX(0, mjMIN(mjNGROUP-1, m->skin_group[i]))]) { if (vopt->skingroup[mjMAX(0, mjMIN(mjNGROUP-1, m->skin_group[i]))]) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
// construct geom, pos = first bone // construct geom, pos = first bone
mjv_initGeom(thisgeom, mjGEOM_SKIN, NULL, mjv_initGeom(thisgeom, mjGEOM_SKIN, NULL,
d->xpos + 3*m->skin_bonebodyid[m->skin_boneadr[i]], NULL, NULL); d->xpos + 3*m->skin_bonebodyid[m->skin_boneadr[i]], NULL, NULL);
@@ -889,14 +877,13 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
rgba = m->vis.rgba.bvactive; rgba = m->vis.rgba.bvactive;
} }
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, size, pos, xmat, rgba); mjv_initGeom(thisgeom, mjGEOM_LINEBOX, size, pos, xmat, rgba);
releaseGeom(&thisgeom, scn); releaseGeom(&thisgeom, scn);
} }
} }
@@ -923,11 +910,10 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
rgba = m->vis.rgba.bvactive; rgba = m->vis.rgba.bvactive;
} }
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, aabb+3, aabb, NULL, rgba); mjv_initGeom(thisgeom, mjGEOM_LINEBOX, aabb+3, aabb, NULL, rgba);
releaseGeom(&thisgeom, scn); releaseGeom(&thisgeom, scn);
} }
@@ -955,29 +941,29 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
for (int j=0; j < 2; j++) { for (int j=0; j < 2; j++) {
for (int k=0; k < 2; k++) { for (int k=0; k < 2; k++) {
if (i == 0) { if (i == 0) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*(i+1)+2*j+k)); mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*(i+1)+2*j+k));
releaseGeom(&thisgeom, scn); releaseGeom(&thisgeom, scn);
} }
if (j == 0) { if (j == 0) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*i+2*(j+1)+k)); mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*i+2*(j+1)+k));
releaseGeom(&thisgeom, scn); releaseGeom(&thisgeom, scn);
} }
if (k == 0) { if (k == 0) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*i+2*j+(k+1))); mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*i+2*j+(k+1)));
releaseGeom(&thisgeom, scn); releaseGeom(&thisgeom, scn);
} }
@@ -1029,11 +1015,10 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
mju_mulMatVec3(pos, xmat, center); mju_mulMatVec3(pos, xmat, center);
mju_addTo3(pos, xpos); mju_addTo3(pos, xpos);
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, size, pos, xmat, rgba); mjv_initGeom(thisgeom, mjGEOM_LINEBOX, size, pos, xmat, rgba);
releaseGeom(&thisgeom, scn); releaseGeom(&thisgeom, scn);
} }
@@ -1053,11 +1038,10 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
for (int b = 0; b < m->mesh_octnum[meshid]; b++) { for (int b = 0; b < m->mesh_octnum[meshid]; b++) {
int i = b + m->mesh_octadr[meshid]; int i = b + m->mesh_octadr[meshid];
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
if (m->oct_depth[i] != vopt->bvh_depth) { if (m->oct_depth[i] != vopt->bvh_depth) {
continue; continue;
} }
@@ -1114,7 +1098,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
float* mesh_vert = m->mesh_vert + 3*m->mesh_vertadr[mesh_id]; float* mesh_vert = m->mesh_vert + 3*m->mesh_vertadr[mesh_id];
int* face = m->mesh_face + 3*m->mesh_faceadr[mesh_id]; int* face = m->mesh_face + 3*m->mesh_faceadr[mesh_id];
for (int i=0; i < m->mesh_facenum[mesh_id]; i++) { for (int i=0; i < m->mesh_facenum[mesh_id]; i++) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
@@ -1149,7 +1134,6 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
} }
// draw triangles, one per side // draw triangles, one per side
thisgeom = acquireGeom(scn, i, category, objtype);
makeTriangle(thisgeom, pos[0], pos[1], pos[2], rgba); makeTriangle(thisgeom, pos[0], pos[1], pos[2], rgba);
thisgeom->objid = id; thisgeom->objid = id;
releaseGeom(&thisgeom, scn); releaseGeom(&thisgeom, scn);
@@ -1165,12 +1149,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
for (int i=1; i < m->nbody; 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 // 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)) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
mjtNum Ixx = m->body_inertia[3*i+0]; mjtNum Ixx = m->body_inertia[3*i+0];
mjtNum Iyy = m->body_inertia[3*i+1]; mjtNum Iyy = m->body_inertia[3*i+1];
mjtNum Izz = m->body_inertia[3*i+2]; mjtNum Izz = m->body_inertia[3*i+2];
@@ -1224,12 +1207,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
int i = pert->select; int i = pert->select;
if ((pert->active | pert->active2) & mjPERT_TRANSLATE) { if ((pert->active | pert->active2) & mjPERT_TRANSLATE) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
// compute selection point in world coordinates // compute selection point in world coordinates
mju_mulMatVec3(selpos, d->xmat+9*pert->select, pert->localpos); mju_mulMatVec3(selpos, d->xmat+9*pert->select, pert->localpos);
mju_addTo3(selpos, d->xpos+3*pert->select); mju_addTo3(selpos, d->xpos+3*pert->select);
@@ -1249,12 +1231,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
releaseGeom(&thisgeom, scn); releaseGeom(&thisgeom, scn);
// add small sphere at end-effector // add small sphere at end-effector
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
// construct geom // construct geom
sz[0] = 2*sz[0]; sz[0] = 2*sz[0];
sz[1] = sz[2] = sz[0]; sz[1] = sz[2] = sz[0];
@@ -1265,12 +1246,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
} }
if ((pert->active | pert->active2) & mjPERT_ROTATE) { if ((pert->active | pert->active2) & mjPERT_ROTATE) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
// prepare color, use inertia color // prepare color, use inertia color
float rgba[4]; float rgba[4];
mixcolor(rgba, m->vis.rgba.inertia, mixcolor(rgba, m->vis.rgba.inertia,
@@ -1323,12 +1303,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
// draw the three axes (separate geoms) // draw the three axes (separate geoms)
for (int j=0; j < 3; j++) { for (int j=0; j < 3; j++) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
// prepare axis // prepare axis
for (int k=0; k < 3; k++) { for (int k=0; k < 3; k++) {
axis[k] = (j == k ? sz[1] : 0); axis[k] = (j == k ? sz[1] : 0);
@@ -1362,11 +1341,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
mju_mulMatVec3(selpos, d->xmat+9*pert->select, pert->localpos); mju_mulMatVec3(selpos, d->xmat+9*pert->select, pert->localpos);
mju_addTo3(selpos, d->xpos+3*pert->select); mju_addTo3(selpos, d->xpos+3*pert->select);
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
thisgeom->type = mjGEOM_SPHERE; thisgeom->type = mjGEOM_SPHERE;
thisgeom->size[0] = thisgeom->size[1] = thisgeom->size[2] = scl * m->vis.scale.selectpoint; thisgeom->size[0] = thisgeom->size[1] = thisgeom->size[2] = scl * m->vis.scale.selectpoint;
mju_n2f(thisgeom->pos, selpos, 3); mju_n2f(thisgeom->pos, selpos, 3);
@@ -1392,12 +1371,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
if (bodycategory(m, i) & ~catmask) { if (bodycategory(m, i) & ~catmask) {
continue; continue;
} }
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
// construct geom // construct geom
thisgeom->type = mjGEOM_LABEL; thisgeom->type = mjGEOM_LABEL;
mju_n2f(thisgeom->pos, d->xpos+3*i, 3); mju_n2f(thisgeom->pos, d->xpos+3*i, 3);
@@ -1421,12 +1399,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
sz[1] = m->vis.scale.jointlength * scl; sz[1] = m->vis.scale.jointlength * scl;
sz[0] = m->vis.scale.jointwidth * scl; sz[0] = m->vis.scale.jointwidth * scl;
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
// set type, size, pos, mat depending on joint type // set type, size, pos, mat depending on joint type
int j = m->jnt_bodyid[i]; int j = m->jnt_bodyid[i];
mjtNum* from; mjtNum* from;
@@ -1564,12 +1541,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
if (m->actuator_trntype[i] == mjTRN_JOINT || if (m->actuator_trntype[i] == mjTRN_JOINT ||
m->actuator_trntype[i] == mjTRN_JOINTINPARENT || m->actuator_trntype[i] == mjTRN_JOINTINPARENT ||
m->actuator_trntype[i] == mjTRN_SITE) { m->actuator_trntype[i] == mjTRN_SITE) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
// site actuators // site actuators
if (m->actuator_trntype[i] == mjTRN_SITE) { if (m->actuator_trntype[i] == mjTRN_SITE) {
// inflate sizes by 5% // inflate sizes by 5%
@@ -1627,11 +1603,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
// add inflated geom if it is a regular primitive // add inflated geom if it is a regular primitive
if (geomtype != mjGEOM_PLANE && geomtype != mjGEOM_HFIELD && if (geomtype != mjGEOM_PLANE && geomtype != mjGEOM_HFIELD &&
geomtype != mjGEOM_MESH && geomtype != mjGEOM_SDF) { geomtype != mjGEOM_MESH && geomtype != mjGEOM_SDF) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
// inflate sizes by 5% // inflate sizes by 5%
mju_scl3(sz, m->geom_size+3*k, 1.05); mju_scl3(sz, m->geom_size+3*k, 1.05);
@@ -1654,12 +1630,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
else if (m->actuator_trntype[i] == mjTRN_TENDON && d->ten_wrapnum[j]) { 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++) { 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) { if (d->wrap_obj[k] != -2 && d->wrap_obj[k+1] != -2) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
// determine width: smaller for segments inside wrapping objects // 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]; sz[0] = 0.5 * m->tendon_width[j];
@@ -1701,12 +1676,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
if (m->body_dofnum[weld_id]) { if (m->body_dofnum[weld_id]) {
int islandid = d->dof_island[m->body_dofadr[weld_id]]; int islandid = d->dof_island[m->body_dofadr[weld_id]];
if (islandid > -1) { if (islandid > -1) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
thisgeom->type = mjGEOM_LABEL; thisgeom->type = mjGEOM_LABEL;
mju_n2f(thisgeom->pos, d->xipos+3*i, 3); mju_n2f(thisgeom->pos, d->xipos+3*i, 3);
mju_n2f(thisgeom->mat, d->ximat+9*i, 9); mju_n2f(thisgeom->mat, d->ximat+9*i, 9);
@@ -1739,12 +1713,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
int geomgroup = mjMAX(0, mjMIN(mjNGROUP-1, m->geom_group[i])); int geomgroup = mjMAX(0, mjMIN(mjNGROUP-1, m->geom_group[i]));
if (vopt->geomgroup[geomgroup]) { if (vopt->geomgroup[geomgroup]) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
// construct geom // construct geom
mjv_initGeom(thisgeom, m->geom_type[i], m->geom_size+3*i, mjv_initGeom(thisgeom, m->geom_type[i], m->geom_size+3*i,
d->geom_xpos+3*i, d->geom_xmat+9*i, NULL); d->geom_xpos+3*i, d->geom_xmat+9*i, NULL);
@@ -1865,12 +1838,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
sz[0] = m->vis.scale.framewidth * scl; sz[0] = m->vis.scale.framewidth * scl;
sz[1] = m->vis.scale.framelength * scl; sz[1] = m->vis.scale.framelength * scl;
for (int j=0; j < 3; j++) { for (int j=0; j < 3; j++) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
// prepare axis // prepare axis
for (int k=0; k < 3; k++) { for (int k=0; k < 3; k++) {
axis[k] = (j == k ? sz[1] : 0); axis[k] = (j == k ? sz[1] : 0);
@@ -1907,12 +1879,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
// show if group enabled // show if group enabled
if (vopt->sitegroup[mjMAX(0, mjMIN(mjNGROUP-1, m->site_group[i]))]) { if (vopt->sitegroup[mjMAX(0, mjMIN(mjNGROUP-1, m->site_group[i]))]) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
// construct geom // construct geom
mjv_initGeom(thisgeom, m->site_type[i], m->site_size+3*i, mjv_initGeom(thisgeom, m->site_type[i], m->site_size+3*i,
d->site_xpos+3*i, d->site_xmat+9*i, NULL); d->site_xpos+3*i, d->site_xmat+9*i, NULL);
@@ -1948,12 +1919,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
sz[0] = m->vis.scale.framewidth * scl; sz[0] = m->vis.scale.framewidth * scl;
sz[1] = m->vis.scale.framelength * scl; sz[1] = m->vis.scale.framelength * scl;
for (int j=0; j < 3; j++) { for (int j=0; j < 3; j++) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
// prepare axis // prepare axis
for (int k=0; k < 3; k++) { for (int k=0; k < 3; k++) {
axis[k] = (j == k ? sz[1] : 0); axis[k] = (j == k ? sz[1] : 0);
@@ -2037,52 +2007,52 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
// triangulation and wireframe of the frustum // triangulation and wireframe of the frustum
for (int e=0; e < 4; e++) { for (int e=0; e < 4; e++) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
makeTriangle(thisgeom, vnear[e], vfar[e], vnear[(e+1)%4], rgba); makeTriangle(thisgeom, vnear[e], vfar[e], vnear[(e+1)%4], rgba);
releaseGeom(&thisgeom, scn); releaseGeom(&thisgeom, scn);
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
makeTriangle(thisgeom, vfar[e], vfar[(e+1)%4], vnear[(e+1)%4], rgba); makeTriangle(thisgeom, vfar[e], vfar[(e+1)%4], vnear[(e+1)%4], rgba);
releaseGeom(&thisgeom, scn); releaseGeom(&thisgeom, scn);
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_connector(thisgeom, mjGEOM_LINE, 3, vnear[e], vnear[(e+1)%4]); mjv_connector(thisgeom, mjGEOM_LINE, 3, vnear[e], vnear[(e+1)%4]);
f2f(thisgeom->rgba, rgba, 4); f2f(thisgeom->rgba, rgba, 4);
releaseGeom(&thisgeom, scn); releaseGeom(&thisgeom, scn);
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_connector(thisgeom, mjGEOM_LINE, 3, vfar[e], vfar[(e+1)%4]); mjv_connector(thisgeom, mjGEOM_LINE, 3, vfar[e], vfar[(e+1)%4]);
f2f(thisgeom->rgba, rgba, 4); f2f(thisgeom->rgba, rgba, 4);
releaseGeom(&thisgeom, scn); releaseGeom(&thisgeom, scn);
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_connector(thisgeom, mjGEOM_LINE, 3, vnear[e], vfar[e]); mjv_connector(thisgeom, mjGEOM_LINE, 3, vnear[e], vfar[e]);
f2f(thisgeom->rgba, rgba, 4); f2f(thisgeom->rgba, rgba, 4);
releaseGeom(&thisgeom, scn); releaseGeom(&thisgeom, scn);
} }
} }
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
// construct geom: camera body // construct geom: camera body
thisgeom->type = mjGEOM_BOX; thisgeom->type = mjGEOM_BOX;
thisgeom->size[0] = scl * m->vis.scale.camera * 1.0; thisgeom->size[0] = scl * m->vis.scale.camera * 1.0;
@@ -2099,12 +2069,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
releaseGeom(&thisgeom, scn); releaseGeom(&thisgeom, scn);
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
// construct geom: lens // construct geom: lens
thisgeom->pos[0] = (float)(d->cam_xpos[3*i] - thisgeom->pos[0] = (float)(d->cam_xpos[3*i] -
scl*m->vis.scale.camera*0.6 * d->cam_xmat[9*i+2]); scl*m->vis.scale.camera*0.6 * d->cam_xmat[9*i+2]);
@@ -2135,12 +2104,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
sz[0] = m->vis.scale.framewidth * scl; sz[0] = m->vis.scale.framewidth * scl;
sz[1] = m->vis.scale.framelength * scl; sz[1] = m->vis.scale.framelength * scl;
for (int j=0; j < 3; j++) { for (int j=0; j < 3; j++) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
// prepare axis // prepare axis
for (int k=0; k < 3; k++) { for (int k=0; k < 3; k++) {
axis[k] = (j == k ? sz[1] : 0); axis[k] = (j == k ? sz[1] : 0);
@@ -2177,12 +2145,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
// make light position: offset backward, to avoid casting shadow // make light position: offset backward, to avoid casting shadow
mju_addScl3(vec, d->light_xpos+3*i, d->light_xdir+3*i, -scl * m->vis.scale.light -0.0001); mju_addScl3(vec, d->light_xpos+3*i, d->light_xdir+3*i, -scl * m->vis.scale.light -0.0001);
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
// construct geom // construct geom
thisgeom->type = mjGEOM_CYLINDER; thisgeom->type = mjGEOM_CYLINDER;
thisgeom->size[0] = scl * m->vis.scale.light * 0.8; thisgeom->size[0] = scl * m->vis.scale.light * 0.8;
@@ -2210,12 +2177,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
sz[0] = m->vis.scale.framewidth * scl; sz[0] = m->vis.scale.framewidth * scl;
sz[1] = m->vis.scale.framelength * scl; sz[1] = m->vis.scale.framelength * scl;
for (int j=0; j < 3; j++) { for (int j=0; j < 3; j++) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
// prepare axis // prepare axis
for (int k=0; k < 3; k++) { for (int k=0; k < 3; k++) {
axis[k] = (j == k ? sz[1] : 0); axis[k] = (j == k ? sz[1] : 0);
@@ -2285,12 +2251,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
if (!draw_catenary) { if (!draw_catenary) {
for (int j=d->ten_wrapadr[i]; j < d->ten_wrapadr[i]+d->ten_wrapnum[i]-1; j++) { 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) { if (d->wrap_obj[j] != -2 && d->wrap_obj[j+1] != -2) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
// determine width: smaller for segments inside wrapping objects // 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]; sz[0] = 0.5 * m->tendon_width[i];
@@ -2373,12 +2338,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
// draw npoints-1 segments // draw npoints-1 segments
for (int j=0; j < npoints-1; j++) { for (int j=0; j < npoints-1; j++) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
sz[0] = m->tendon_width[i]; sz[0] = m->tendon_width[i];
// construct geom // construct geom
@@ -2429,11 +2393,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
mju_addTo3(end, d->site_xpos+3*k); mju_addTo3(end, d->site_xpos+3*k);
// render slider // render slider
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_connector(thisgeom, mjGEOM_CYLINDER, scl * m->vis.scale.slidercrank, mjv_connector(thisgeom, mjGEOM_CYLINDER, scl * m->vis.scale.slidercrank,
d->site_xpos+3*k, end); d->site_xpos+3*k, end);
f2f(thisgeom->rgba, m->vis.rgba.slidercrank, 4); f2f(thisgeom->rgba, m->vis.rgba.slidercrank, 4);
@@ -2442,12 +2406,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
} }
releaseGeom(&thisgeom, scn); releaseGeom(&thisgeom, scn);
// render crank thisgeom = acquireGeom(scn, i, category, objtype);
if (geomsExhausted(scn)) { if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_connector(thisgeom, mjGEOM_CAPSULE, scl * m->vis.scale.slidercrank/2.0, mjv_connector(thisgeom, mjGEOM_CAPSULE, scl * m->vis.scale.slidercrank/2.0,
end, d->site_xpos+3*j); end, d->site_xpos+3*j);
if (broken) { if (broken) {
@@ -2466,11 +2429,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
if (vopt->flags[mjVIS_COM] && (category & catmask)) { if (vopt->flags[mjVIS_COM] && (category & catmask)) {
for (int i=1; i < m->nbody; i++) { for (int i=1; i < m->nbody; i++) {
if (m->body_rootid[i] == i) { if (m->body_rootid[i] == i) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
thisgeom->type = mjGEOM_SPHERE; thisgeom->type = mjGEOM_SPHERE;
thisgeom->size[0] = thisgeom->size[1] = thisgeom->size[2] = scl * m->vis.scale.com; thisgeom->size[0] = thisgeom->size[1] = thisgeom->size[2] = scl * m->vis.scale.com;
mju_n2f(thisgeom->pos, d->subtree_com+3*i, 3); mju_n2f(thisgeom->pos, d->subtree_com+3*i, 3);
@@ -2495,11 +2458,10 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
cur = d->xipos+3*i; cur = d->xipos+3*i;
if (m->body_jntnum[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--) {
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
nxt = d->xanchor+3*j; nxt = d->xanchor+3*j;
// construct geom // construct geom
@@ -2512,11 +2474,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
} }
// connect first joint (or com) to parent com // connect first joint (or com) to parent com
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
nxt = d->xipos+3*m->body_parentid[i]; nxt = d->xipos+3*m->body_parentid[i];
mjv_connector(thisgeom, mjGEOM_CAPSULE, scl * m->vis.scale.connect, cur, nxt); mjv_connector(thisgeom, mjGEOM_CAPSULE, scl * m->vis.scale.connect, cur, nxt);
f2f(thisgeom->rgba, m->vis.rgba.connect, 4); f2f(thisgeom->rgba, m->vis.rgba.connect, 4);
@@ -2540,11 +2502,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
} }
// make ray // make ray
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
mjtNum* from = d->site_xpos+3*sid; mjtNum* from = d->site_xpos+3*sid;
mjtNum to[3] = {from[0] + d->site_xmat[9*sid+2]*dst, mjtNum to[3] = {from[0] + d->site_xmat[9*sid+2]*dst,
from[1] + d->site_xmat[9*sid+5]*dst, from[1] + d->site_xmat[9*sid+5]*dst,
@@ -2562,11 +2524,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
} }
// make ray // make ray
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_connector(thisgeom, mjGEOM_LINE, 3, fromto, fromto+3); mjv_connector(thisgeom, mjGEOM_LINE, 3, fromto, fromto+3);
f2f(thisgeom->rgba, m->vis.rgba.rangefinder, 4); f2f(thisgeom->rgba, m->vis.rgba.rangefinder, 4);
releaseGeom(&thisgeom, scn); releaseGeom(&thisgeom, scn);
@@ -2588,11 +2550,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
// map force to spatial vector in world frame // map force to spatial vector in world frame
mju_scl3(vec, xfrc, m->vis.map.force/m->stat.meanmass); mju_scl3(vec, xfrc, m->vis.map.force/m->stat.meanmass);
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
mjtNum* from = xpos; mjtNum* from = xpos;
mjtNum to[3]; mjtNum to[3];
mju_add3(to, from, vec); mju_add3(to, from, vec);
@@ -2632,22 +2594,22 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
// construct geom // construct geom
sz[0] = scl * m->vis.scale.constraint; sz[0] = scl * m->vis.scale.constraint;
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, vec, xmat_j, m->vis.rgba.connect); mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, vec, xmat_j, m->vis.rgba.connect);
if (vopt->label == mjLABEL_CONSTRAINT) { if (vopt->label == mjLABEL_CONSTRAINT) {
makeLabel(m, mjOBJ_EQUALITY, i, thisgeom->label); makeLabel(m, mjOBJ_EQUALITY, i, thisgeom->label);
} }
releaseGeom(&thisgeom, scn); releaseGeom(&thisgeom, scn);
if (geomsExhausted(scn)) { thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return; return;
} }
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, end, xmat_k, m->vis.rgba.constraint); mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, end, xmat_k, m->vis.rgba.constraint);
if (vopt->label == mjLABEL_CONSTRAINT) { if (vopt->label == mjLABEL_CONSTRAINT) {
makeLabel(m, mjOBJ_EQUALITY, i, thisgeom->label); makeLabel(m, mjOBJ_EQUALITY, i, thisgeom->label);