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
mjvGeom* acquireGeom(mjvScene* scn, int objid, int category, int objtype) {
// check for overflow, SHOULD NOT OCCUR
if (scn->ngeom >= scn->maxgeom) {
mju_error("Cannot acquire geom; all geoms are in use.");
scn->status = 1;
return NULL;
}
@@ -203,11 +194,11 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
// contact point
if (flags[mjVIS_CONTACTPOINT]) {
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
thisgeom->type = mjGEOM_CYLINDER;
thisgeom->size[0] = thisgeom->size[1] = m->vis.scale.contactwidth * 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)
for (int j=0; j < 3; j++) {
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
// prepare axis
for (int k=0; k < 3; k++) {
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
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
mjtNum* from = con->pos;
mjtNum to[3];
mju_add3(to, from, vec);
@@ -759,12 +749,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
(category & catmask)) {
for (int i=0; i < m->nflex; 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;
}
thisgeom = acquireGeom(scn, i, category, objtype);
// construct geom, pos = first vertex
mjv_initGeom(thisgeom, mjGEOM_FLEX, 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)) {
for (int i=0; i < m->nskin; 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;
}
thisgeom = acquireGeom(scn, i, category, objtype);
// construct geom, pos = first bone
mjv_initGeom(thisgeom, mjGEOM_SKIN, 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;
}
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, size, pos, xmat, rgba);
releaseGeom(&thisgeom, scn);
}
}
@@ -923,11 +910,10 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
rgba = m->vis.rgba.bvactive;
}
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, aabb+3, aabb, NULL, rgba);
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 k=0; k < 2; k++) {
if (i == 0) {
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
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));
releaseGeom(&thisgeom, scn);
}
if (j == 0) {
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
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));
releaseGeom(&thisgeom, scn);
}
if (k == 0) {
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
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)));
releaseGeom(&thisgeom, scn);
}
@@ -1029,11 +1015,10 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
mju_mulMatVec3(pos, xmat, center);
mju_addTo3(pos, xpos);
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, size, pos, xmat, rgba);
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++) {
int i = b + m->mesh_octadr[meshid];
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
if (m->oct_depth[i] != vopt->bvh_depth) {
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];
int* face = m->mesh_face + 3*m->mesh_faceadr[mesh_id];
for (int i=0; i < m->mesh_facenum[mesh_id]; i++) {
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
@@ -1149,7 +1134,6 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
}
// draw triangles, one per side
thisgeom = acquireGeom(scn, i, category, objtype);
makeTriangle(thisgeom, pos[0], pos[1], pos[2], rgba);
thisgeom->objid = id;
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++) {
// 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 (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
mjtNum Ixx = m->body_inertia[3*i+0];
mjtNum Iyy = m->body_inertia[3*i+1];
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;
if ((pert->active | pert->active2) & mjPERT_TRANSLATE) {
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
// compute selection point in world coordinates
mju_mulMatVec3(selpos, d->xmat+9*pert->select, pert->localpos);
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);
// add small sphere at end-effector
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
// construct geom
sz[0] = 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 (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
// prepare color, use inertia color
float rgba[4];
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)
for (int j=0; j < 3; j++) {
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
// prepare axis
for (int k=0; k < 3; k++) {
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_addTo3(selpos, d->xpos+3*pert->select);
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
thisgeom->type = mjGEOM_SPHERE;
thisgeom->size[0] = thisgeom->size[1] = thisgeom->size[2] = scl * m->vis.scale.selectpoint;
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) {
continue;
}
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
// construct geom
thisgeom->type = mjGEOM_LABEL;
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[0] = m->vis.scale.jointwidth * scl;
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
// set type, size, pos, mat depending on joint type
int j = m->jnt_bodyid[i];
mjtNum* from;
@@ -1564,12 +1541,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
if (m->actuator_trntype[i] == mjTRN_JOINT ||
m->actuator_trntype[i] == mjTRN_JOINTINPARENT ||
m->actuator_trntype[i] == mjTRN_SITE) {
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
// site actuators
if (m->actuator_trntype[i] == mjTRN_SITE) {
// 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
if (geomtype != mjGEOM_PLANE && geomtype != mjGEOM_HFIELD &&
geomtype != mjGEOM_MESH && geomtype != mjGEOM_SDF) {
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
// inflate sizes by 5%
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]) {
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 (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
// determine width: smaller for segments inside wrapping objects
if (d->wrap_obj[k] >= 0 && d->wrap_obj[k+1] >= 0) {
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]) {
int islandid = d->dof_island[m->body_dofadr[weld_id]];
if (islandid > -1) {
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
thisgeom->type = mjGEOM_LABEL;
mju_n2f(thisgeom->pos, d->xipos+3*i, 3);
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]));
if (vopt->geomgroup[geomgroup]) {
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
// construct geom
mjv_initGeom(thisgeom, m->geom_type[i], m->geom_size+3*i,
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[1] = m->vis.scale.framelength * scl;
for (int j=0; j < 3; j++) {
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
// prepare axis
for (int k=0; k < 3; k++) {
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
if (vopt->sitegroup[mjMAX(0, mjMIN(mjNGROUP-1, m->site_group[i]))]) {
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
// construct geom
mjv_initGeom(thisgeom, m->site_type[i], m->site_size+3*i,
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[1] = m->vis.scale.framelength * scl;
for (int j=0; j < 3; j++) {
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
// prepare axis
for (int k=0; k < 3; k++) {
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
for (int e=0; e < 4; e++) {
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
makeTriangle(thisgeom, vnear[e], vfar[e], vnear[(e+1)%4], rgba);
releaseGeom(&thisgeom, scn);
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
makeTriangle(thisgeom, vfar[e], vfar[(e+1)%4], vnear[(e+1)%4], rgba);
releaseGeom(&thisgeom, scn);
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_connector(thisgeom, mjGEOM_LINE, 3, vnear[e], vnear[(e+1)%4]);
f2f(thisgeom->rgba, rgba, 4);
releaseGeom(&thisgeom, scn);
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_connector(thisgeom, mjGEOM_LINE, 3, vfar[e], vfar[(e+1)%4]);
f2f(thisgeom->rgba, rgba, 4);
releaseGeom(&thisgeom, scn);
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_connector(thisgeom, mjGEOM_LINE, 3, vnear[e], vfar[e]);
f2f(thisgeom->rgba, rgba, 4);
releaseGeom(&thisgeom, scn);
}
}
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
// construct geom: camera body
thisgeom->type = mjGEOM_BOX;
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);
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
// construct geom: lens
thisgeom->pos[0] = (float)(d->cam_xpos[3*i] -
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[1] = m->vis.scale.framelength * scl;
for (int j=0; j < 3; j++) {
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
// prepare axis
for (int k=0; k < 3; k++) {
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
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;
}
thisgeom = acquireGeom(scn, i, category, objtype);
// construct geom
thisgeom->type = mjGEOM_CYLINDER;
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[1] = m->vis.scale.framelength * scl;
for (int j=0; j < 3; j++) {
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
// prepare axis
for (int k=0; k < 3; k++) {
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) {
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 (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
// determine width: smaller for segments inside wrapping objects
if (d->wrap_obj[j] >= 0 && d->wrap_obj[j+1] >= 0) {
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
for (int j=0; j < npoints-1; j++) {
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
sz[0] = m->tendon_width[i];
// 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);
// render slider
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_connector(thisgeom, mjGEOM_CYLINDER, scl * m->vis.scale.slidercrank,
d->site_xpos+3*k, end);
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);
// render crank
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_connector(thisgeom, mjGEOM_CAPSULE, scl * m->vis.scale.slidercrank/2.0,
end, d->site_xpos+3*j);
if (broken) {
@@ -2466,11 +2429,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
if (vopt->flags[mjVIS_COM] && (category & catmask)) {
for (int i=1; i < m->nbody; i++) {
if (m->body_rootid[i] == i) {
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
thisgeom->type = mjGEOM_SPHERE;
thisgeom->size[0] = thisgeom->size[1] = thisgeom->size[2] = scl * m->vis.scale.com;
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;
if (m->body_jntnum[i]) {
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;
}
thisgeom = acquireGeom(scn, i, category, objtype);
nxt = d->xanchor+3*j;
// 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
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
nxt = d->xipos+3*m->body_parentid[i];
mjv_connector(thisgeom, mjGEOM_CAPSULE, scl * m->vis.scale.connect, cur, nxt);
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
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
mjtNum* from = d->site_xpos+3*sid;
mjtNum to[3] = {from[0] + d->site_xmat[9*sid+2]*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
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_connector(thisgeom, mjGEOM_LINE, 3, fromto, fromto+3);
f2f(thisgeom->rgba, m->vis.rgba.rangefinder, 4);
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
mju_scl3(vec, xfrc, m->vis.map.force/m->stat.meanmass);
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
mjtNum* from = xpos;
mjtNum to[3];
mju_add3(to, from, vec);
@@ -2632,22 +2594,22 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
// construct geom
sz[0] = scl * m->vis.scale.constraint;
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, vec, xmat_j, m->vis.rgba.connect);
if (vopt->label == mjLABEL_CONSTRAINT) {
makeLabel(m, mjOBJ_EQUALITY, i, thisgeom->label);
}
releaseGeom(&thisgeom, scn);
if (geomsExhausted(scn)) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom = acquireGeom(scn, i, category, objtype);
mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, end, xmat_k, m->vis.rgba.constraint);
if (vopt->label == mjLABEL_CONSTRAINT) {
makeLabel(m, mjOBJ_EQUALITY, i, thisgeom->label);