// Copyright 2021 DeepMind Technologies Limited // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "engine/engine_vis_visualize.h" #include #include #include #include #include #include #include "engine/engine_array_safety.h" #include "engine/engine_plugin.h" #include "engine/engine_support.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" #include "engine/engine_util_spatial.h" #include "engine/engine_vis_init.h" //----------------------------- utility functions and macros --------------------------------------- static const mjtNum IDENTITY[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; // copy float array static void f2f(float* dest, const float* src, int n) { memcpy(dest, src, n*sizeof(float)); } // make text label static void makeLabel(const mjModel* m, mjtObj type, int id, char* label) { const char* typestr = mju_type2Str(type); const char* namestr = mj_id2name(m, type, id); char txt[100]; // copy existing name or make numeric name if (namestr) { mjSNPRINTF(txt, "%s", namestr); } else if (typestr) { mjSNPRINTF(txt, "%s %d", typestr, id); } else { mjSNPRINTF(txt, "%d", id); } // copy result into label strncpy(label, txt, 99); label[99] = '\0'; } // return if there is no space in buffer #define START \ if ( scn->ngeom>=scn->maxgeom ) { \ mj_warning(d, mjWARN_VGEOMFULL, scn->maxgeom); \ return; \ } else { \ thisgeom = scn->geoms + scn->ngeom; \ mjv_initGeom(thisgeom, mjGEOM_NONE, NULL, NULL, NULL, NULL); \ thisgeom->objtype = objtype; \ thisgeom->objid = i; \ thisgeom->category = category; \ thisgeom->segid = scn->ngeom; \ } // advance counter #define FINISH { scn->ngeom++; } // assign pseudo-random rgba to constraint island using Halton sequence static void islandColor(float rgba[4], int islanddofadr) { rgba[0] = 0.1f + 0.8f*mju_Halton(islanddofadr + 1, 2); rgba[1] = 0.1f + 0.8f*mju_Halton(islanddofadr + 1, 3); rgba[2] = 0.1f + 0.8f*mju_Halton(islanddofadr + 1, 5); rgba[3] = 1; } // add contact-related geoms in mjvObject static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags, const mjvOption* vopt, mjvScene* scn) { int body1, body2; int objtype = mjOBJ_UNKNOWN, category = mjCAT_DECOR; mjtNum mat[9], tmp[9], vec[3], frc[3], confrc[6], axis[3]; mjtNum framewidth, framelength, scl = m->stat.meansize; mjContact* con; mjvGeom* thisgeom; mjtByte split; // fast return if all relevant features are disabled if (!flags[mjVIS_CONTACTPOINT] && !flags[mjVIS_CONTACTFORCE] && vopt->frame != mjFRAME_CONTACT) { return; } // loop over contacts for (int i=0; i < d->ncon; i++) { // get pointer con = d->contact + i; // mat = contact rotation matrix (normal along z) mju_copy(tmp, con->frame+3, 6); mju_copy(tmp+6, con->frame, 3); mju_transpose(mat, tmp, 3, 3); // contact point if (flags[mjVIS_CONTACTPOINT]) { START thisgeom->type = mjGEOM_CYLINDER; thisgeom->size[0] = thisgeom->size[1] = m->vis.scale.contactwidth * scl; thisgeom->size[2] = m->vis.scale.contactheight * scl; mju_n2f(thisgeom->pos, con->pos, 3); mju_n2f(thisgeom->mat, mat, 9); int efc_adr = d->contact[i].efc_address; // override standard colors if visualizing islands if (vopt->flags[mjVIS_ISLAND] && d->nisland && efc_adr >= 0) { // set color using island's first dof islandColor(thisgeom->rgba, d->island_dofadr[d->efc_island[efc_adr]]); } // otherwise regular colors (different for included and excluded contacts) else { if (efc_adr >= 0) { f2f(thisgeom->rgba, m->vis.rgba.contactpoint, 4); } else { f2f(thisgeom->rgba, m->vis.rgba.contactgap, 4); } } // label contacting geom names or ids if (vopt->label == mjLABEL_CONTACTPOINT) { const char* name1 = mj_id2name(m, mjOBJ_GEOM, con->geom1); char id1[10]; mjSNPRINTF(id1, "%d", con->geom1); const char* name2 = mj_id2name(m, mjOBJ_GEOM, con->geom2); char id2[10]; mjSNPRINTF(id2, "%d", con->geom2); mjSNPRINTF(thisgeom->label, "%s | %s", name1 ? name1 : id1 , name2 ? name2 : id2); } FINISH } // contact frame if (vopt->frame == mjFRAME_CONTACT) { // set length and width of axis cylinders using half regular frame scaling framelength = m->vis.scale.framelength * scl / 2; framewidth = m->vis.scale.framewidth * scl / 2; // draw the three axes (separate geoms) for (int j=0; j < 3; j++) { START // prepare axis for (int k=0; k < 3; k++) { axis[k] = (j == k ? framelength : 0); } mju_mulMatVec(vec, mat, axis, 3, 3); // create a cylinder mjtNum* from = con->pos; mjtNum to[3]; mju_add3(to, from, vec); mjv_connector(thisgeom, mjGEOM_CYLINDER, framewidth, from, to); // set color: R, G or B depending on axis for (int k=0; k < 3; k++) { thisgeom->rgba[k] = (j == k ? 0.9 : 0); } thisgeom->rgba[3] = 1; FINISH } } // nothing else to do for excluded contacts if (d->contact[i].efc_address < 0) { continue; } // mat = contact frame rotation matrix (normal along x) mju_transpose(mat, con->frame, 3, 3); // get contact force:torque in contact frame mj_contactForce(m, d, i, confrc); // contact force if (flags[mjVIS_CONTACTFORCE]) { // get force, fill zeros if only normal mju_zero3(frc); mju_copy(frc, confrc, mjMIN(3, con->dim)); if (mju_norm3(frc) < mjMINVAL) { continue; } // render combined or split split = (flags[mjVIS_CONTACTSPLIT] && con->dim > 1); for (int j = (split ? 1 : 0); j < (split ? 3 : 1); j++) { // set vec to combined, normal or friction force, in world frame switch (j) { case 0: // combined mju_mulMatVec(vec, mat, frc, 3, 3); break; case 1: // normal vec[0] = mat[0]*frc[0]; vec[1] = mat[3]*frc[0]; vec[2] = mat[6]*frc[0]; break; case 2: // friction vec[0] = mat[1]*frc[1] + mat[2]*frc[2]; vec[1] = mat[4]*frc[1] + mat[5]*frc[2]; vec[2] = mat[7]*frc[1] + mat[8]*frc[2]; break; } // scale vector mju_scl3(vec, vec, m->vis.map.force/m->stat.meanmass); // get body ids body1 = m->geom_bodyid[con->geom1]; body2 = m->geom_bodyid[con->geom2]; // make sure arrow points towards body with higher id if (body1 > body2) { mju_scl3(vec, vec, -1); } // one-directional arrow for friction and world, symmetric otherwise START mjtNum* from = con->pos; mjtNum to[3]; mju_add3(to, from, vec); mjv_connector(thisgeom, body1 > 0 && body2 > 0 && !split ? mjGEOM_ARROW2 : mjGEOM_ARROW, m->vis.scale.forcewidth * scl,from, to); f2f(thisgeom->rgba, j == 2 ? m->vis.rgba.contactfriction : m->vis.rgba.contactforce, 4); if (vopt->label == mjLABEL_CONTACTFORCE && j == (split ? 1 : 0)) { mjSNPRINTF(thisgeom->label, "%-.3g", mju_norm3(frc)); } FINISH } } } } // copy material fields from model to visual geom static void setMaterial(const mjModel* m, mjvGeom* geom, int matid, const float* rgba, const mjtByte* flags) { // set material properties if given if (matid >= 0) { f2f(geom->texrepeat, m->mat_texrepeat + 2*matid, 2); f2f(geom->rgba, m->mat_rgba + 4*matid, 4); geom->texuniform = m->mat_texuniform[matid]; geom->emission = m->mat_emission[matid]; geom->specular = m->mat_specular[matid]; geom->shininess = m->mat_shininess[matid]; geom->reflectance = m->mat_reflectance[matid]; } // otherwise clear texrepeat else { geom->texrepeat[0] = 0; geom->texrepeat[1] = 0; } // use rgba if different from default, or no material given if (rgba[0] != 0.5f || rgba[1] != 0.5f || rgba[2] != 0.5f || rgba[3] != 1.0f || matid < 0) { f2f(geom->rgba, rgba, 4); } // set texture if (flags[mjVIS_TEXTURE] && matid >= 0) { geom->texid = m->mat_texid[matid]; } // scale alpha for dynamic geoms only if (flags[mjVIS_TRANSPARENT] && (geom->category == mjCAT_DYNAMIC)) { geom->rgba[3] *= m->vis.map.alpha; } } //----------------------------- main API functions ------------------------------------------------- // set (type, size, pos, mat) connector-type geom between given points // assume that mjv_initGeom was already called to set all other properties void mjv_makeConnector(mjvGeom* geom, int type, mjtNum width, mjtNum a0, mjtNum a1, mjtNum a2, mjtNum b0, mjtNum b1, mjtNum b2) { mjtNum quat[4], mat[9], dif[3] = {b0-a0, b1-a1, b2-a2}; // require connector-compatible type if (type != mjGEOM_CAPSULE && type != mjGEOM_CYLINDER && type != mjGEOM_ARROW && type != mjGEOM_ARROW1 && type != mjGEOM_ARROW2 && type != mjGEOM_LINE) { mjERROR("invalid geom type %d for connector", type); } // assign type geom->type = type; // compute size for XYZ scaling geom->size[0] = geom->size[1] = (float)width; geom->size[2] = (float)mju_norm3(dif); // cylinder and capsule are centered, and size[0] is "radius" if (type == mjGEOM_CAPSULE || type == mjGEOM_CYLINDER) { geom->pos[0] = 0.5*(a0 + b0); geom->pos[1] = 0.5*(a1 + b1); geom->pos[2] = 0.5*(a2 + b2); geom->size[2] *= 0.5; } // arrow is not centered else { geom->pos[0] = a0; geom->pos[1] = a1; geom->pos[2] = a2; } // set mat to minimal rotation aligning b-a with z axis mju_quatZ2Vec(quat, dif); mju_quat2Mat(mat, quat); mju_n2f(geom->mat, mat, 9); } // set (type, size, pos, mat) connector-type geom between given points // assume that mjv_initGeom was already called to set all other properties void mjv_connector(mjvGeom* geom, int type, mjtNum width, const mjtNum from[3], const mjtNum to[3]) { mjv_makeConnector(geom, type, width, from[0], from[1], from[2], to[0], to[1], to[2]); } // initialize given fields when not NULL, set the rest to their default values void mjv_initGeom(mjvGeom* geom, int type, const mjtNum* size, const mjtNum* pos, const mjtNum* mat, const float* rgba) { // assign type geom->type = type; // set size (for XYZ scaling) if (size) { switch ((mjtGeom) type) { case mjGEOM_SPHERE: geom->size[0] = (float)size[0]; geom->size[1] = (float)size[0]; geom->size[2] = (float)size[0]; break; case mjGEOM_CAPSULE: geom->size[0] = (float)size[0]; geom->size[1] = (float)size[0]; geom->size[2] = (float)size[1]; break; case mjGEOM_CYLINDER: geom->size[0] = (float)size[0]; geom->size[1] = (float)size[0]; geom->size[2] = (float)size[1]; break; default: mju_n2f(geom->size, size, 3); } } else { geom->size[0] = 0.1f; geom->size[1] = 0.1f; geom->size[2] = 0.1f; } // set pos if (pos) { mju_n2f(geom->pos, pos, 3); } else { geom->pos[0] = 0; geom->pos[1] = 0; geom->pos[2] = 0; } // set mat if (mat) { mju_n2f(geom->mat, mat, 9); } else { geom->mat[0] = 1; geom->mat[1] = 0; geom->mat[2] = 0; geom->mat[3] = 0; geom->mat[4] = 1; geom->mat[5] = 0; geom->mat[6] = 0; geom->mat[7] = 0; geom->mat[8] = 1; } // set rgba if (rgba) { f2f(geom->rgba, rgba, 4); } else { geom->rgba[0] = 0.5; geom->rgba[1] = 0.5; geom->rgba[2] = 0.5; geom->rgba[3] = 1; } // set defaults that cannot be assigned via this function geom->dataid = -1; geom->texid = -1; geom->texuniform = 0; geom->texcoord = 0; geom->texrepeat[0] = 1; geom->texrepeat[1] = 1; geom->emission = 0; geom->specular = 0.5; geom->shininess = 0.5; geom->reflectance = 0; geom->label[0] = 0; geom->modelrbound = 0; } // mark geom as selected static void markselected(const mjVisual* vis, mjvGeom* geom) { // add emission geom->emission += vis->global.glow; } // mix colors for perturbation object static void mixcolor(float rgba[4], const float ref[4], int flg1, int flg2) { rgba[0] = flg1 ? ref[0] : 0; if (flg2) { rgba[0] = mjMAX(rgba[0], ref[1]); } rgba[1] = flg1 ? ref[1] : 0; if (flg2) { rgba[1] = mjMAX(rgba[1], ref[0]); } rgba[2] = ref[2]; rgba[3] = ref[3]; } // a body is static if it is welded to the world and is not a mocap body static int bodycategory(const mjModel* m, int bodyid) { if (m->body_weldid[bodyid] == 0 && m->body_mocapid[bodyid] == -1) { return mjCAT_STATIC; } else { return mjCAT_DYNAMIC; } } // draw bounding box static void drawBoundingBox(mjvGeom* thisgeom, mjData* d, mjvScene* scn, const mjtNum aabb[6], const mjtNum xpos[3], const mjtNum xmat[9], const float rgba[4], int i, int objtype, int category) { mjtNum x[3]; mjtNum dist[3][3]; if (xmat != NULL) { mju_rotVecMat(x, aabb, xmat); mju_addTo3(x, xpos); for (int j=0; j < 3; j++) { for (int k=0; k < 3; k++) { dist[k][j] = aabb[k+3] * xmat[3*j+k]; } } } else { mju_copy3(x, aabb); mju_addTo3(x, xpos); for (int j=0; j < 3; j++) { mju_zero3(dist[j]); dist[j][j] = aabb[j+3]; } } int split[3] = {1, 2, 4}; for (int v=0; v < 8; v++) { mjtNum from[3] = {x[0], x[1], x[2]}; for (int k=0; k < 3; k++) { mju_addToScl3(from, dist[k], v&split[k] ? 1 : -1); } mjtNum to[3]; for (int k=0; k < 3; k++) { mju_addScl3(to, from, dist[k], 2); if (!(v&split[k])) { START mjv_connector(thisgeom, mjGEOM_LINE, 2, from, to); f2f(thisgeom->rgba, rgba, 4); FINISH } } } } // add abstract geoms void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, const mjvPerturb* pert, int catmask, mjvScene* scn) { int objtype, category; mjtNum sz[3], mat[9], selpos[3]; mjtNum catenary[3*mjNCATENARY]; mjtNum *cur, *nxt, *xfrc; mjtNum vec[3], end[3], axis[3], rod, len, det, tmp[9], quat[4]; mjtByte broken; mjvGeom* thisgeom; mjvPerturb localpert; float scl = m->stat.meansize; // make default pert if missing if (!pert) { mjv_defaultPerturb(&localpert); pert = &localpert; } // clear mjCAT_STATIC bit if mjVIS_STATIC is not set if (!vopt->flags[mjVIS_STATIC]) { catmask &= (~mjCAT_STATIC); } // skin objtype = mjOBJ_SKIN; category = mjCAT_DYNAMIC; if (vopt->flags[mjVIS_SKIN] && (category & catmask)) { for (int i=0; i < m->nskin; i++) { START // construct geom, pos = first bone mjv_initGeom(thisgeom, mjGEOM_SKIN, NULL, d->xpos + 3*m->skin_bonebodyid[m->skin_boneadr[i]], NULL, NULL); // set material properties setMaterial(m, thisgeom, m->skin_matid[i], m->skin_rgba+4*i, vopt->flags); // glow skin if selected if (pert->skinselect == i) { markselected(&m->vis, thisgeom); } // set texcoord if (m->skin_texcoordadr[i] >= 0) { thisgeom->texcoord = 1; } // skip if alpha is 0 if (thisgeom->rgba[3] == 0) { continue; } // vopt->label if (vopt->label == mjLABEL_SKIN) { makeLabel(m, mjOBJ_SKIN, i, thisgeom->label); } FINISH } } // bounding volume hierarchy if (vopt->flags[mjVIS_MIDPHASE]) { int bodyid = 0; float rgba[] = {1, 0, 0, 1}; for (int i = 0; i < m->nbvh; i++) { int isleaf = m->bvh_child[2*i] == -1 && m->bvh_child[2*i+1] == -1; if (scn->ngeom >= scn->maxgeom) break; if (m->bvh_depth[i] != vopt->bvh_depth) { if (!isleaf || m->bvh_depth[i] > vopt->bvh_depth) { continue; } } // find geom number int geomid = m->bvh_geomid[i]; while (i >= m->body_bvhadr[bodyid] + m->body_bvhnum[bodyid]) { if (++bodyid >= m->nbody) { break; } } // stop after body bvh are finished if (bodyid >= m->nbody) { break; } // compute transformation mjtNum *aabb = isleaf ? m->geom_aabb + 6*geomid : m->bvh_aabb + 6*i; const mjtNum* xpos = isleaf ? d->geom_xpos + 3 * geomid : d->xipos + 3 * bodyid; const mjtNum* xmat = isleaf ? d->geom_xmat + 9 * geomid : d->ximat + 9 * bodyid; rgba[0] = d->bvh_active[i] ? 1 : 0; rgba[1] = d->bvh_active[i] ? 0 : 1; drawBoundingBox(thisgeom, d, scn, aabb, xpos, xmat, rgba, i, objtype, category); } } // mesh bounding volume hierarchy if (vopt->flags[mjVIS_MESHBVH]) { float rgba[] = {1, 0, 0, 1}; for (int geomid = 0; geomid < m->ngeom; geomid++) { int meshid = m->geom_dataid[geomid]; if (meshid == -1) { continue; } for (int b = 0; b < m->mesh_bvhnum[meshid]; b++) { int i = b + m->mesh_bvhadr[meshid]; int isleaf = m->bvh_child[2*i] == -1 && m->bvh_child[2*i+1] == -1; if (scn->ngeom >= scn->maxgeom) break; if (m->bvh_depth[i] != vopt->bvh_depth) { if (!isleaf || m->bvh_depth[i] > vopt->bvh_depth) { continue; } } // compute transformation const mjtNum *aabb = m->bvh_aabb + 6*i; const mjtNum* xpos = d->geom_xpos + 3 * geomid; const mjtNum* xmat = d->geom_xmat + 9 * geomid; rgba[0] = d->bvh_active[i] ? 1 : 0; rgba[1] = d->bvh_active[i] ? 0 : 1; drawBoundingBox(thisgeom, d, scn, aabb, xpos, xmat, rgba, i, objtype, category); } } } // inertia objtype = mjOBJ_BODY; if (vopt->flags[mjVIS_INERTIA]) { int ellipsoid = m->vis.global.ellipsoidinertia == 1; 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)) { START mjtNum Ixx = m->body_inertia[3*i+0]; mjtNum Iyy = m->body_inertia[3*i+1]; mjtNum Izz = m->body_inertia[3*i+2]; mjtNum mass = m->body_mass[i]; mjtNum scale_inertia = ellipsoid ? mju_sqrt(5) : mju_sqrt(3); sz[0] = mju_sqrt((Iyy + Izz - Ixx) / (2 * mass)) * scale_inertia; sz[1] = mju_sqrt((Ixx + Izz - Iyy) / (2 * mass)) * scale_inertia; sz[2] = mju_sqrt((Ixx + Iyy - Izz) / (2 * mass)) * scale_inertia; // scale with mass if enabled if (vopt->flags[mjVIS_SCLINERTIA]) { // density = mass / volume mjtNum scale_volume = ellipsoid ? 4.0/3.0*mjPI : 8.0; mjtNum volume = scale_volume * sz[0]*sz[1]*sz[2]; mjtNum density = mass / mju_max(mjMINVAL, volume); // scale = root3(density) mjtNum scale = mju_pow(density*0.001, 1.0/3.0); // scale sizes, so that box/ellipsoid with density of 1000 has same mass sz[0] *= scale; sz[1] *= scale; sz[2] *= scale; } // construct geom mjtGeom type = ellipsoid ? mjGEOM_ELLIPSOID : mjGEOM_BOX; mjv_initGeom(thisgeom, type, sz, d->xipos+3*i, d->ximat+9*i, m->vis.rgba.inertia); // glow if (pert->select == i) { markselected(&m->vis, thisgeom); } // vopt->label if (vopt->label == mjLABEL_BODY || (vopt->label == mjLABEL_SELECTION && pert->select == i)) { makeLabel(m, mjOBJ_BODY, i, thisgeom->label); } FINISH } } } // connector to mouse perturbation target objtype = mjOBJ_UNKNOWN; category = mjCAT_DECOR; if (vopt->flags[mjVIS_PERTOBJ] && (category & catmask) && pert->select > 0) { int i = pert->select; if ((pert->active | pert->active2) & mjPERT_TRANSLATE) { START // compute selection point in world coordinates mju_rotVecMat(selpos, pert->localpos, d->xmat+9*pert->select); mju_addTo3(selpos, d->xpos+3*pert->select); // construct geom sz[0] = scl * m->vis.scale.constraint; mjv_connector(thisgeom, mjGEOM_CAPSULE, sz[0], selpos, pert->refselpos); // prepare color float rgba[4]; mixcolor(rgba, m->vis.rgba.constraint, (pert->active & mjPERT_TRANSLATE) > 0, (pert->active2 & mjPERT_TRANSLATE) > 0); f2f(thisgeom->rgba, rgba, 4); FINISH // add small sphere at end-effector START // construct geom sz[0] = 2*sz[0]; sz[1] = sz[2] = sz[0]; mju_quat2Mat(mat, pert->refquat); mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, pert->refselpos, mat, rgba); FINISH } if ((pert->active | pert->active2) & mjPERT_ROTATE) { START // prepare color, use inertia color float rgba[4]; mixcolor(rgba, m->vis.rgba.inertia, (pert->active & mjPERT_ROTATE) > 0, (pert->active2 & mjPERT_ROTATE) > 0); // construct geom sz[0] = sz[1] = sz[2] = scl; mju_quat2Mat(mat, pert->refquat); mjv_initGeom(thisgeom, mjGEOM_BOX, sz, d->xipos+3*i, mat, rgba); FINISH } } // world and body frame objtype = mjOBJ_UNKNOWN; category = mjCAT_DECOR; if (category & catmask) { for (int i = (vopt->frame == mjFRAME_WORLD ? 0 : 1); i < (vopt->frame == mjFRAME_BODY ? m->nbody : 1); i++) { // set length(1) and width(0) of the axis cylinders if (i == 0) { sz[1] = m->vis.scale.framelength * scl * 2; sz[0] = m->vis.scale.framewidth * scl * 2; } else { sz[1] = m->vis.scale.framelength * scl; sz[0] = m->vis.scale.framewidth * scl; } // skip if body is static and static bodies are masked if (i > 0 && bodycategory(m, i) & ~catmask) { continue; } mjtNum* xmat = vopt->flags[mjVIS_INERTIA] ? d->ximat+9*i : d->xmat+9*i; mjtNum* xpos = vopt->flags[mjVIS_INERTIA] ? d->xipos+3*i : d->xpos+3*i; // draw the three axes (separate geoms) for (int j=0; j < 3; j++) { START // prepare axis for (int k=0; k < 3; k++) { axis[k] = (j == k ? sz[1] : 0); } mju_mulMatVec(vec, xmat, axis, 3, 3); // create a cylinder mjtNum* from = xpos; mjtNum to[3]; mju_add3(to, from, vec); mjv_connector(thisgeom, mjGEOM_CYLINDER, sz[0], from, to); // set color: R, G or B depending on axis for (int k=0; k < 3; k++) { thisgeom->rgba[k] = (j == k ? 0.9 : 0); } thisgeom->rgba[3] = 1; FINISH } } } // selection point objtype = mjOBJ_UNKNOWN; category = mjCAT_DECOR; if ((category & catmask) && pert->select > 0 && vopt->flags[mjVIS_SELECT]) { int i=0; // compute selection point in world coordinates mju_rotVecMat(selpos, pert->localpos, d->xmat+9*pert->select); mju_addTo3(selpos, d->xpos+3*pert->select); START thisgeom->type = mjGEOM_SPHERE; thisgeom->size[0] = thisgeom->size[1] = thisgeom->size[2] = scl * m->vis.scale.selectpoint; mju_n2f(thisgeom->pos, selpos, 3); mju_n2f(thisgeom->mat, IDENTITY, 9); f2f(thisgeom->rgba, m->vis.rgba.selectpoint, 4); if (vopt->label == mjLABEL_SELPNT) { mjSNPRINTF( thisgeom->label, "%.3f %.3f %.3f (local %.3f %.3f %.3f)", selpos[0], selpos[1], selpos[2], pert->localpos[0], pert->localpos[1], pert->localpos[2]); } FINISH } // label bodies when inertia boxes are not shown objtype = mjOBJ_UNKNOWN; category = mjCAT_DECOR; if ((category & catmask) && (vopt->label == mjLABEL_SELECTION || vopt->label == mjLABEL_BODY) && !vopt->flags[mjVIS_INERTIA]) { for (int i=1; i < m->nbody; i++) { if (vopt->label == mjLABEL_BODY || (vopt->label == mjLABEL_SELECTION && pert->select == i)) { // skip if body is static and static bodies are masked if (bodycategory(m, i) & ~catmask) { continue; } START // construct geom thisgeom->type = mjGEOM_LABEL; mju_n2f(thisgeom->pos, d->xpos+3*i, 3); mju_n2f(thisgeom->mat, d->xmat+9*i, 9); // vopt->label makeLabel(m, mjOBJ_BODY, i, thisgeom->label); FINISH } } } // joint objtype = mjOBJ_JOINT; category = mjCAT_DECOR; if (vopt->flags[mjVIS_JOINT] && (category & catmask)) { for (int i=0; i < m->njnt; i++) { if (vopt->jointgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->jnt_group[i]))]) { // set length(1) and width(0) of the connectors sz[1] = m->vis.scale.jointlength * scl; sz[0] = m->vis.scale.jointwidth * scl; START // set type, size, pos, mat depending on joint type int j = m->jnt_bodyid[i]; mjtNum* from; mjtNum to[3]; switch ((mjtJoint) m->jnt_type[i]) { case mjJNT_FREE: thisgeom->type = mjGEOM_BOX; thisgeom->size[0] = thisgeom->size[1] = thisgeom->size[2] = 0.3*sz[1]; mju_n2f(thisgeom->pos, d->xanchor+3*i, 3); mju_n2f(thisgeom->mat, d->xmat+9*j, 9); break; case mjJNT_BALL: thisgeom->type = mjGEOM_SPHERE; thisgeom->size[0] = thisgeom->size[1] = thisgeom->size[2] = 0.3*sz[1]; mju_n2f(thisgeom->pos, d->xanchor+3*i, 3); mju_n2f(thisgeom->mat, d->xmat+9*j, 9); break; case mjJNT_SLIDE: case mjJNT_HINGE: from = d->xanchor+3*i; mju_addScl3(to, from, d->xaxis+3*i, sz[1]); mjv_connector(thisgeom, m->jnt_type[i] == mjJNT_SLIDE ? mjGEOM_ARROW : mjGEOM_ARROW1, sz[0], from, to); break; default: mjERROR("unknown joint type %d", m->jnt_type[i]); } f2f(thisgeom->rgba, m->vis.rgba.joint, 4); // vopt->label if (vopt->label == mjLABEL_JOINT) { makeLabel(m, mjOBJ_JOINT, i, thisgeom->label); } FINISH } } } // actuator objtype = mjOBJ_ACTUATOR; category = mjCAT_DECOR; if (vopt->flags[mjVIS_ACTUATOR] && (category & catmask)) { for (int i=0; i < m->nu; i++) { if (vopt->actuatorgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->actuator_group[i]))]) { // determine extended range mjtNum rng[3] = {-1, 0, +1}; mjtNum rmin = -1, rmax = 1, act = 0; if (m->actuator_ctrllimited[i]) { rmin = m->actuator_ctrlrange[2*i]; rmax = m->actuator_ctrlrange[2*i+1]; } else if (vopt->flags[mjVIS_ACTIVATION] && m->actuator_actlimited[i]) { rmin = m->actuator_actrange[2*i]; rmax = m->actuator_actrange[2*i+1]; } if (rmin >= 0) { rng[0] = -1; rng[1] = rmin; rng[2] = rmax; } else if (rmax <= 0) { rng[0] = rmin; rng[1] = rmax; rng[2] = +1; } else { rng[0] = rmin; rng[1] = 0; rng[2] = rmax; } // adjust small ranges if (rng[1]-rng[0] < mjMINVAL) { rng[0] = rng[1] - mjMINVAL; } if (rng[2]-rng[1] < mjMINVAL) { rng[2] = rng[1] + mjMINVAL; } // clamp act to extended range if (vopt->flags[mjVIS_ACTIVATION] && m->actuator_dyntype[i]) { act = mju_clip(d->act[m->actuator_actadr[i] + m->actuator_actnum[i] - 1], rng[0], rng[2]); } else { act = mju_clip(d->ctrl[i], rng[0], rng[2]); } // compute interpolants float amin, amean, amax; if (act <= rng[1]) { amin = (rng[1]-act) / mjMAX(mjMINVAL, rng[1]-rng[0]); amean = 1 - amin; amax = 0; } else { amax = (act-rng[1]) / mjMAX(mjMINVAL, rng[2]-rng[1]); amean = 1 - amax; amin = 0; } // interpolated color float rgba[4]; for (int j=0; j < 4; j++) { rgba[j] = amin*m->vis.rgba.actuatornegative[j] + amean*m->vis.rgba.actuator[j] + amax*m->vis.rgba.actuatorpositive[j]; } // get transmission object id int j = m->actuator_trnid[2*i]; // slide and hinge joint actuators if (m->actuator_trntype[i] == mjTRN_JOINT || m->actuator_trntype[i] == mjTRN_JOINTINPARENT || m->actuator_trntype[i] == mjTRN_SITE) { START // site actuators if (m->actuator_trntype[i] == mjTRN_SITE) { // inflate sizes by 5% mju_scl3(sz, m->site_size+3*j, 1.05); // make geom mjv_initGeom(thisgeom, m->site_type[j], sz, d->site_xpos + 3*j, d->site_xmat + 9*j, thisgeom->rgba); } else if (m->jnt_type[j] == mjJNT_HINGE || m->jnt_type[j] == mjJNT_SLIDE) { // set length(1) and width(0) of the connectors sz[1] = m->vis.scale.actuatorlength * scl; sz[0] = m->vis.scale.actuatorwidth * scl; // make geom mjtNum* from = d->xanchor + 3*j; mjtNum to[3]; mju_addScl3(to, from, d->xaxis+3*j, sz[1]); mjv_connector(thisgeom, m->jnt_type[j] == mjJNT_SLIDE ? mjGEOM_ARROW : mjGEOM_ARROW1, sz[0], from, to); } // ball or free joint else if (m->jnt_type[j] == mjJNT_BALL || m->jnt_type[j] == mjJNT_FREE) { sz[0] = sz[1] = sz[2] = m->vis.scale.jointlength * scl * 0.33; // make geom mjv_initGeom(thisgeom, m->jnt_type[j] == mjJNT_BALL ? mjGEOM_SPHERE : mjGEOM_BOX, sz, d->xanchor + 3*j, d->xmat + 9*m->jnt_bodyid[j], thisgeom->rgba); } // set interpolated color f2f(thisgeom->rgba, rgba, 4); // vopt->label if (vopt->label == mjLABEL_ACTUATOR) { makeLabel(m, mjOBJ_ACTUATOR, i, thisgeom->label); } FINISH } // body actuators else if (m->actuator_trntype[i] == mjTRN_BODY) { // iterate over body's geoms int geomnum = m->body_geomnum[j]; int geomadr = m->body_geomadr[j]; for (int k=geomadr; k < geomadr+geomnum; k++) { int geomtype = m->geom_type[k]; // add inflated geom if it is a regular primitive if (geomtype != mjGEOM_PLANE && geomtype != mjGEOM_HFIELD && geomtype != mjGEOM_MESH) { START // inflate sizes by 5% mju_scl3(sz, m->geom_size+3*k, 1.05); // make geom mjv_initGeom(thisgeom, m->geom_type[k], sz, d->geom_xpos + 3*k, d->geom_xmat + 9*k, thisgeom->rgba); // set interpolated color f2f(thisgeom->rgba, rgba, 4); FINISH } } } // spatial tendon actuators 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) { START // 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]; } else { sz[0] = m->tendon_width[j]; } // increase width for actuator sz[0] *= m->vis.map.actuatortendon; // construct geom mjv_connector(thisgeom, mjGEOM_CAPSULE, sz[0], d->wrap_xpos+3*k, d->wrap_xpos+3*k+3); // set material if given setMaterial(m, thisgeom, m->tendon_matid[j], m->tendon_rgba+4*j, vopt->flags); // set interpolated color f2f(thisgeom->rgba, rgba, 4); // vopt->label: only the first segment if (vopt->label == mjLABEL_ACTUATOR && k == d->ten_wrapadr[j]) { makeLabel(m, mjOBJ_ACTUATOR, i, thisgeom->label); } FINISH } } } } } } // island labels objtype = mjOBJ_UNKNOWN; category = mjCAT_DECOR; if ((category & catmask) && (vopt->label == mjLABEL_ISLAND) && d->nisland) { for (int i=1; i < m->nbody; i++) { int weld_id = m->body_weldid[i]; if (m->body_dofnum[weld_id]) { int islandid = d->dof_island[m->body_dofadr[weld_id]]; if (islandid > -1) { START thisgeom->type = mjGEOM_LABEL; mju_n2f(thisgeom->pos, d->xipos+3*i, 3); mju_n2f(thisgeom->mat, d->ximat+9*i, 9); mjSNPRINTF(thisgeom->label, "%d", islandid); FINISH } } } } // geom int planeid = -1; for (int i=0; i < m->ngeom; i++) { // count planes, put current plane number in geom->dataid if (m->geom_type[i] == mjGEOM_PLANE) { planeid++; } // set type and category: geom objtype = mjOBJ_GEOM; category = bodycategory(m, m->geom_bodyid[i]); // skip if category is masked if (!(category & catmask)) { continue; } // get geom group and clamp int geomgroup = mjMAX(0, mjMIN(mjNGROUP-1, m->geom_group[i])); if (vopt->geomgroup[geomgroup]) { START // 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); thisgeom->dataid = m->geom_dataid[i]; // copy rbound from model thisgeom->modelrbound = (float)m->geom_rbound[i]; // set material properties, override if visualizing islands float* rgba = m->geom_rgba+4*i; float rgba_island[4] = {.5, .5, .5, 1}; int geom_matid = m->geom_matid[i]; if (vopt->flags[mjVIS_ISLAND] && d->nisland) { geom_matid = -1; rgba = rgba_island; int weld_id = m->body_weldid[m->geom_bodyid[i]]; if (m->body_dofnum[weld_id]) { int island = d->dof_island[m->body_dofadr[weld_id]]; if (island > -1) { // color using island's first dof islandColor(rgba_island, d->island_dofadr[island]); } } } setMaterial(m, thisgeom, geom_matid, rgba, vopt->flags); // set texcoord if (m->geom_type[i] == mjGEOM_MESH && m->geom_dataid[i] >= 0 && m->mesh_texcoordadr[m->geom_dataid[i]] >= 0) { thisgeom->texcoord = 1; } // skip if alpha is 0 if (thisgeom->rgba[3] == 0) { continue; } // glow geoms of selected body if (pert->select > 0 && pert->select == m->geom_bodyid[i]) { markselected(&m->vis, thisgeom); } // vopt->label if (vopt->label == mjLABEL_GEOM) { makeLabel(m, mjOBJ_GEOM, i, thisgeom->label); } // mesh: 2*i is original, 2*i+1 is convex hull if (m->geom_type[i] == mjGEOM_MESH) { thisgeom->dataid *= 2; if (m->mesh_graphadr[m->geom_dataid[i]] >= 0 && vopt->flags[mjVIS_CONVEXHULL] && (m->geom_contype[i] || m->geom_conaffinity[i])) { thisgeom->dataid += 1; } } // plane else if (m->geom_type[i] == mjGEOM_PLANE) { // use current planeid thisgeom->dataid = planeid; // save initial pos mju_copy3(tmp, d->geom_xpos+3*i); // re-center infinite plane if (m->geom_size[3*i] <= 0 || m->geom_size[3*i+1] <= 0) { // vec = headpos - geompos for (int j=0; j < 3; j++) { vec[j] = 0.5*(scn->camera[0].pos[j] + scn->camera[1].pos[j]) - d->geom_xpos[3*i+j]; } // construct axes mjtNum ax[9]; mju_transpose(ax, d->geom_xmat+9*i, 3, 3); // loop over (x,y) for (int k=0; k < 2; k++) { if (m->geom_size[3*i+k] <= 0) { // compute zfar mjtNum zfar = m->vis.map.zfar * m->stat.extent; // get size increment mjtNum sX; int matid = m->geom_matid[i]; if (matid >= 0 && m->mat_texrepeat[2*matid+k] > 0) { sX = 2/m->mat_texrepeat[2*matid+k]; } else { sX = 2.1*zfar/(mjMAXPLANEGRID-2); } // project on frame, round to integer increment of size mjtNum dX = mju_dot3(vec, ax+3*k); dX = 2*sX*mju_round(0.5*dX/sX); // translate mju_addToScl3(tmp, ax+3*k, dX); } } } // set final pos mju_n2f(thisgeom->pos, tmp, 3); } FINISH // set type and category: frame objtype = mjOBJ_UNKNOWN; category = mjCAT_DECOR; if (!(category & catmask) || vopt->frame != mjFRAME_GEOM) { continue; } // construct geom frame objtype = mjOBJ_UNKNOWN; sz[0] = m->vis.scale.framewidth * scl; sz[1] = m->vis.scale.framelength * scl; for (int j=0; j < 3; j++) { START // prepare axis for (int k=0; k < 3; k++) { axis[k] = (j == k ? sz[1] : 0); } mju_mulMatVec(vec, d->geom_xmat+9*i, axis, 3, 3); // create a cylinder mjtNum* from = d->geom_xpos+3*i; mjtNum to[3]; mju_add3(to, from, vec); mjv_connector(thisgeom, mjGEOM_CYLINDER, sz[0], from, to); // set color: R, G or B depending on axis for (int k=0; k < 3; k++) { thisgeom->rgba[k] = (j == k ? 0.9 : 0); } thisgeom->rgba[3] = 1; FINISH } } } // site for (int i=0; i < m->nsite; i++) { // set type and category objtype = mjOBJ_SITE; category = bodycategory(m, m->site_bodyid[i]); // skip if category is masked if (!(category & catmask)) { continue; } // show if group enabled if (vopt->sitegroup[mjMAX(0, mjMIN(mjNGROUP-1, m->site_group[i]))]) { START // 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); // set material if given setMaterial(m, thisgeom, m->site_matid[i], m->site_rgba+4*i, vopt->flags); // skip if alpha is 0 if (thisgeom->rgba[3] == 0) { continue; } // glow if (pert->select > 0 && pert->select == m->site_bodyid[i]) { markselected(&m->vis, thisgeom); } // vopt->label if (vopt->label == mjLABEL_SITE) { makeLabel(m, mjOBJ_SITE, i, thisgeom->label); } FINISH // set category for site frame category = mjCAT_DECOR; if (!(category & catmask) || vopt->frame != mjFRAME_SITE) { continue; } // construct site frame objtype = mjOBJ_UNKNOWN; sz[0] = m->vis.scale.framewidth * scl; sz[1] = m->vis.scale.framelength * scl; for (int j=0; j < 3; j++) { START // prepare axis for (int k=0; k < 3; k++) { axis[k] = (j == k ? sz[1] : 0); } mju_mulMatVec(vec, d->site_xmat+9*i, axis, 3, 3); // create a cylinder mjtNum* from = d->site_xpos+3*i; mjtNum to[3]; mju_add3(to, from, vec); mjv_connector(thisgeom, mjGEOM_CYLINDER, sz[0], from, to); // set color: R, G or B depending on axis for (int k=0; k < 3; k++) { thisgeom->rgba[k] = (j == k ? 0.9 : 0); } thisgeom->rgba[3] = 1; FINISH } } } // cameras objtype = mjOBJ_CAMERA; category = mjCAT_DECOR; if (vopt->flags[mjVIS_CAMERA] && (category & catmask)) { for (int i=0; i < m->ncam; i++) { START // construct geom: camera body thisgeom->type = mjGEOM_BOX; thisgeom->size[0] = scl * m->vis.scale.camera * 1.0; thisgeom->size[1] = scl * m->vis.scale.camera * 0.8; thisgeom->size[2] = scl * m->vis.scale.camera * 0.4; mju_n2f(thisgeom->pos, d->cam_xpos+3*i, 3); mju_n2f(thisgeom->mat, d->cam_xmat+9*i, 9); f2f(thisgeom->rgba, m->vis.rgba.camera, 4); // vopt->label if (vopt->label == mjLABEL_CAMERA) { makeLabel(m, mjOBJ_CAMERA, i, thisgeom->label); } FINISH START // 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]); thisgeom->pos[1] = (float)(d->cam_xpos[3*i+1] - scl*m->vis.scale.camera*0.6 * d->cam_xmat[9*i+5]); thisgeom->pos[2] = (float)(d->cam_xpos[3*i+2] - scl*m->vis.scale.camera*0.6 * d->cam_xmat[9*i+8]); thisgeom->type = mjGEOM_CYLINDER; thisgeom->size[0] = scl * m->vis.scale.camera * 0.4; thisgeom->size[1] = scl * m->vis.scale.camera * 0.4; thisgeom->size[2] = scl * m->vis.scale.camera * 0.3; mju_n2f(thisgeom->mat, d->cam_xmat+9*i, 9); f2f(thisgeom->rgba, m->vis.rgba.camera, 4); for (int k=0; k < 3; k++) { thisgeom->rgba[k] *= 0.5; // make lens body darker } FINISH // set category for camera frame category = mjCAT_DECOR; if (!(category & catmask) || vopt->frame != mjFRAME_CAMERA) { continue; } // construct camera frame objtype = mjOBJ_UNKNOWN; sz[0] = m->vis.scale.framewidth * scl; sz[1] = m->vis.scale.framelength * scl; for (int j=0; j < 3; j++) { START // prepare axis for (int k=0; k < 3; k++) { axis[k] = (j == k ? sz[1] : 0); } mju_mulMatVec(vec, d->cam_xmat+9*i, axis, 3, 3); // create a cylinder mjtNum* from = d->cam_xpos+3*i; mjtNum to[3]; mju_add3(to, from, vec); mjv_connector(thisgeom, mjGEOM_CYLINDER, sz[0], from, to); // set color: R, G or B depending on axis for (int k=0; k < 3; k++) { thisgeom->rgba[k] = (j == k ? 0.9 : 0); } thisgeom->rgba[3] = 1; FINISH } } } // lights objtype = mjOBJ_LIGHT; category = mjCAT_DECOR; if (vopt->flags[mjVIS_LIGHT] && (category & catmask)) { for (int i=0; i < m->nlight; i++) { // make light frame mju_quatZ2Vec(quat, d->light_xdir+3*i); mju_quat2Mat(mat, quat); // 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); START // construct geom thisgeom->type = mjGEOM_CYLINDER; thisgeom->size[0] = scl * m->vis.scale.light * 0.8; thisgeom->size[1] = scl * m->vis.scale.light * 0.8; thisgeom->size[2] = scl * m->vis.scale.light * 1.0; mju_n2f(thisgeom->pos, vec, 3); mju_n2f(thisgeom->mat, mat, 9); f2f(thisgeom->rgba, m->vis.rgba.light, 4); // vopt->label if (vopt->label == mjLABEL_LIGHT) { makeLabel(m, mjOBJ_LIGHT, i, thisgeom->label); } FINISH // set category for light frame category = mjCAT_DECOR; if (!(category & catmask) || vopt->frame != mjFRAME_LIGHT) { continue; } // construct light frame objtype = mjOBJ_UNKNOWN; sz[0] = m->vis.scale.framewidth * scl; sz[1] = m->vis.scale.framelength * scl; for (int j=0; j < 3; j++) { START // prepare axis for (int k=0; k < 3; k++) { axis[k] = (j == k ? sz[1] : 0); } mju_mulMatVec(vec, mat, axis, 3, 3); // create a cylinder mjtNum* from = d->light_xpos+3*i; mjtNum to[3]; mju_add3(to, from, vec); mjv_connector(thisgeom, mjGEOM_CYLINDER, sz[0], from, to); // set color: R, G or B depending on axis for (int k=0; k < 3; k++) { thisgeom->rgba[k] = (j == k ? 0.9 : 0); } thisgeom->rgba[3] = 1; FINISH } } } // spatial tendons objtype = mjOBJ_TENDON; category = mjCAT_DYNAMIC; if (vopt->flags[mjVIS_TENDON] && (category & catmask)) { for (int i=0; i < m->ntendon; i++) { if (vopt->tendongroup[mjMAX(0, mjMIN(mjNGROUP-1, m->tendon_group[i]))]) { // tendon has a deadband spring int limitedspring = m->tendon_stiffness[i] > 0 && // positive stiffness m->tendon_lengthspring[2*i] == 0 && // range lower-bound is 0 m->tendon_lengthspring[2*i+1] > 0; // range upper-bound is positive // tendon has a simple length constraint, but is currently not limited mjtNum ten_length = d->ten_length[i]; mjtNum lower = m->tendon_range[2*i]; mjtNum upper = m->tendon_range[2*i + 1]; int limitedconstraint = m->tendon_stiffness[i] == 0 && // zero stiffness m->tendon_limited[i] == 1 && // limited length range lower == 0 && // range lower-bound is 0 ten_length < upper; // current length is smaller than upper bound // conditions for drawing a catenary int draw_catenary = !mjDISABLED(mjDSBL_GRAVITY) && // gravity enabled mju_norm3(m->opt.gravity) > mjMINVAL && // gravity strictly nonzero m->tendon_num[i] == 2 && // only two sites on the tendon (limitedspring || limitedconstraint) && // either spring or constraint length limits m->tendon_damping[i] == 0 && // no damping m->tendon_frictionloss[i] == 0; // no frictionloss // conditions not met: draw straight lines 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) { START // 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]; } else { sz[0] = m->tendon_width[i]; } // construct geom mjv_connector(thisgeom, mjGEOM_CAPSULE, sz[0], d->wrap_xpos+3*j, d->wrap_xpos+3*j+3); // set material properties, override if visualizing islands float* rgba = m->tendon_rgba+4*i; float rgba_island[4] = {.5, .5, .5, 1}; int tendon_matid = m->tendon_matid[i]; if (vopt->flags[mjVIS_ISLAND] && d->nisland) { tendon_matid = -1; rgba = rgba_island; int frictional = m->tendon_frictionloss[i] > 0; int limited = m->tendon_limited[i] && (ten_length <= lower || ten_length >= upper); if (frictional || limited) { // search for tendon's island int island = -1; for (int k=0; k < d->nefc; k++) { int istendon = d->efc_type[k] == mjCNSTR_FRICTION_TENDON || d->efc_type[k] == mjCNSTR_LIMIT_TENDON; if (istendon && d->efc_id[k] == i) { island = d->efc_island[k]; break; } } if (island > -1) { // set color using island's first dof islandColor(rgba_island, d->island_dofadr[island]); } } } setMaterial(m, thisgeom, tendon_matid, rgba, vopt->flags); // vopt->label: only the first segment if (vopt->label == mjLABEL_TENDON && j == d->ten_wrapadr[i]) { makeLabel(m, mjOBJ_TENDON, i, thisgeom->label); } FINISH } } } // special case handling of string-like tendons under gravity else { // two hanging points: x0, x1 mjtNum x0[3], x1[3]; mju_copy3(x0, d->wrap_xpos + 3*d->ten_wrapadr[i]); mju_copy3(x1, d->wrap_xpos + 3*d->ten_wrapadr[i] + 3); // length of the tendon mjtNum length; if (limitedconstraint) { length = m->tendon_range[2*i+1]; } else { length = m->tendon_lengthspring[2*i+1]; } // points along catenary path int npoints = mjv_catenary(x0, x1, m->opt.gravity, length, catenary); // draw npoints-1 segments for (int j=0; j < npoints-1; j++) { START sz[0] = m->tendon_width[i]; // construct geom mjv_connector(thisgeom, mjGEOM_CAPSULE, sz[0], catenary+3*j, catenary+3*j+3); // set material if given setMaterial(m, thisgeom, m->tendon_matid[i], m->tendon_rgba+4*i, vopt->flags); // vopt->label: only the first segment if (vopt->label == mjLABEL_TENDON && npoints/2) { makeLabel(m, mjOBJ_TENDON, i, thisgeom->label); } FINISH } } } } } // slider-crank objtype = mjOBJ_ACTUATOR; category = mjCAT_DYNAMIC; if ((category & catmask)) { for (int i=0; i < m->nu; i++) { if (m->actuator_trntype[i] == mjTRN_SLIDERCRANK) { // get data int j = m->actuator_trnid[2*i]; // crank int k = m->actuator_trnid[2*i+1]; // slider rod = m->actuator_cranklength[i]; axis[0] = d->site_xmat[9*k+2]; axis[1] = d->site_xmat[9*k+5]; axis[2] = d->site_xmat[9*k+8]; // compute crank length mju_sub(vec, d->site_xpos+3*j, d->site_xpos+3*k, 3); len = mju_dot3(vec, axis); det = len*len + rod*rod - mju_dot3(vec, vec); broken = 0; if (det < 0) { det = 0; broken = 1; } len = len - mju_sqrt(det); // compute slider endpoint mju_scl3(end, axis, len); mju_addTo3(end, d->site_xpos+3*k); // render slider START mjv_connector(thisgeom, mjGEOM_CYLINDER, scl * m->vis.scale.slidercrank, d->site_xpos+3*k, end); f2f(thisgeom->rgba, m->vis.rgba.slidercrank, 4); if (vopt->label == mjLABEL_ACTUATOR) { makeLabel(m, mjOBJ_ACTUATOR, i, thisgeom->label); } FINISH // render crank START mjv_connector(thisgeom, mjGEOM_CAPSULE, scl * m->vis.scale.slidercrank/2.0, end, d->site_xpos+3*j); if (broken) { f2f(thisgeom->rgba, m->vis.rgba.crankbroken, 4); } else { f2f(thisgeom->rgba, m->vis.rgba.slidercrank, 4); } FINISH } } } // center of mass for root bodies objtype = mjOBJ_UNKNOWN; category = mjCAT_DECOR; if (vopt->flags[mjVIS_COM] && (category & catmask)) { for (int i=1; i < m->nbody; i++) { if (m->body_rootid[i] == i) { START 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); mju_n2f(thisgeom->mat, IDENTITY, 9); f2f(thisgeom->rgba, m->vis.rgba.com, 4); FINISH } } } // auto connect objtype = mjOBJ_UNKNOWN; category = mjCAT_DECOR; if (vopt->flags[mjVIS_AUTOCONNECT] && (category & catmask)) { for (int i=1; i < m->nbody; i++) { // do not connect to world if (m->body_parentid[i] == 0) { continue; } // start at body com, connect joint centers in reverse order 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--) { START nxt = d->xanchor+3*j; // construct geom mjv_connector(thisgeom, mjGEOM_CAPSULE, scl * m->vis.scale.connect, cur, nxt); f2f(thisgeom->rgba, m->vis.rgba.connect, 4); FINISH cur = nxt; } } // connect first joint (or com) to parent com START 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); FINISH } } // rangefinders objtype = mjOBJ_UNKNOWN; category = mjCAT_DECOR; if (vopt->flags[mjVIS_RANGEFINDER] && (category & catmask)) { for (int i=0; i < m->nsensor; i++) { if (m->sensor_type[i] == mjSENS_RANGEFINDER) { // sensor data mjtNum dst = d->sensordata[m->sensor_adr[i]]; int sid = m->sensor_objid[i]; // null output: nothing to render if (dst < 0) { continue; } // make ray START 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, from[2] + d->site_xmat[9*sid+8]*dst}; mjv_connector(thisgeom, mjGEOM_LINE, 3, from, to); f2f(thisgeom->rgba, m->vis.rgba.rangefinder, 4); FINISH } } } // external perturbations objtype = mjOBJ_UNKNOWN; category = mjCAT_DECOR; for (int i=1; i < m->nbody; i++) { if (!mju_isZero(d->xfrc_applied+6*i, 6) && (category & catmask)) { // point of application and force mjtNum *xpos = d->xipos+3*i; xfrc = d->xfrc_applied+6*i; // force perturbation if (vopt->flags[mjVIS_PERTFORCE] && mju_norm3(xfrc) > mjMINVAL) { // map force to spatial vector in world frame mju_scl3(vec, xfrc, m->vis.map.force/m->stat.meanmass); START mjtNum* from = xpos; mjtNum to[3]; mju_add3(to, from, vec); mjv_connector(thisgeom, mjGEOM_ARROW, m->vis.scale.forcewidth * scl, from, to); f2f(thisgeom->rgba, m->vis.rgba.force, 4); FINISH } } } // connect and distance constraints objtype = mjOBJ_EQUALITY; category = mjCAT_DECOR; if (vopt->flags[mjVIS_CONSTRAINT] && (category & catmask) && m->neq) { // connect or weld for (int i=0; i < m->neq; i++) { if (m->eq_active[i] && (m->eq_type[i] == mjEQ_CONNECT || m->eq_type[i] == mjEQ_WELD)) { // compute endpoints in global coordinates int j = m->eq_obj1id[i], k = m->eq_obj2id[i]; mju_rotVecMat(vec, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i] == mjEQ_WELD), d->xmat+9*j); mju_addTo3(vec, d->xpos+3*j); mju_rotVecMat(end, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i] == mjEQ_CONNECT), d->xmat+9*k); mju_addTo3(end, d->xpos+3*k); // construct geom sz[0] = scl * m->vis.scale.constraint; START mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, vec, d->xmat+9*j, m->vis.rgba.connect); if (vopt->label == mjLABEL_CONSTRAINT) { makeLabel(m, mjOBJ_EQUALITY, i, thisgeom->label); } FINISH START mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, end, d->xmat+9*k, m->vis.rgba.constraint); if (vopt->label == mjLABEL_CONSTRAINT) { makeLabel(m, mjOBJ_EQUALITY, i, thisgeom->label); } FINISH } } } // contact if (catmask & mjCAT_DECOR) { addContactGeom(m, d, vopt->flags, vopt, scn); } } #undef START #undef FINISH // make list of lights only void mjv_makeLights(const mjModel* m, mjData* d, mjvScene* scn) { mjvLight* thislight; // clear counter scn->nlight = 0; // headlight if (m->vis.headlight.active) { // get pointer thislight = scn->lights; // set default properties memset(thislight, 0, sizeof(mjvLight)); thislight->headlight = 1; thislight->directional = 1; thislight->castshadow = 0; // copy colors f2f(thislight->ambient, m->vis.headlight.ambient, 3); f2f(thislight->diffuse, m->vis.headlight.diffuse, 3); f2f(thislight->specular, m->vis.headlight.specular, 3); // advance counter scn->nlight++; } // remaining lights for (int i=0; i < m->nlight && scn->nlight < mjMAXLIGHT; i++) { if (m->light_active[i]) { // get pointer thislight = scn->lights + scn->nlight; // copy properties memset(thislight, 0, sizeof(mjvLight)); thislight->directional = m->light_directional[i]; thislight->castshadow = m->light_castshadow[i]; if (!thislight->directional) { f2f(thislight->attenuation, m->light_attenuation+3*i, 3); thislight->exponent = m->light_exponent[i]; thislight->cutoff = m->light_cutoff[i]; } // copy colors f2f(thislight->ambient, m->light_ambient+3*i, 3); f2f(thislight->diffuse, m->light_diffuse+3*i, 3); f2f(thislight->specular, m->light_specular+3*i, 3); // copy position and direction mju_n2f(thislight->pos, d->light_xpos+3*i, 3); mju_n2f(thislight->dir, d->light_xdir+3*i, 3); // advance counter scn->nlight++; } } } // update camera only void mjv_updateCamera(const mjModel* m, mjData* d, mjvCamera* cam, mjvScene* scn) { mjtNum ca, sa, ce, se, move[3], *mat; mjtNum headpos[3], forward[3], up[3], right[3], ipd, fovy, znear, zfar; // return if nothing to do if (!m || !cam || cam->type == mjCAMERA_USER) { return; } // get znear, zfar znear = m->vis.map.znear * m->stat.extent; zfar = m->vis.map.zfar * m->stat.extent; // get headpos, forward[3], up, right, ipd, fovy switch (cam->type) { case mjCAMERA_FREE: case mjCAMERA_TRACKING: // get global ipd and fovy ipd = m->vis.global.ipd; fovy = m->vis.global.fovy; // move lookat for tracking if (cam->type == mjCAMERA_TRACKING) { // get id and check int bid = cam->trackbodyid; if (bid < 0 || bid >= m->nbody) { mjERROR("track body id is outside valid range"); } // smooth tracking of subtree com mju_sub3(move, d->subtree_com + 3*cam->trackbodyid, cam->lookat); mju_addToScl3(cam->lookat, move, 0.2); // constant ??? } // compute frame ca = mju_cos(cam->azimuth/180.0*mjPI); sa = mju_sin(cam->azimuth/180.0*mjPI); ce = mju_cos(cam->elevation/180.0*mjPI); se = mju_sin(cam->elevation/180.0*mjPI); forward[0] = ce*ca; forward[1] = ce*sa; forward[2] = se; up[0] = -se*ca; up[1] = -se*sa; up[2] = ce; right[0] = sa; right[1] = -ca; right[2] = 0; mju_addScl3(headpos, cam->lookat, forward, -cam->distance); break; case mjCAMERA_FIXED: { // get id and check int cid = cam->fixedcamid; if (cid < 0 || cid >= m->ncam) { mjERROR("fixed camera id is outside valid range"); } // get camera-specific ipd and fovy ipd = m->cam_ipd[cid]; fovy = m->cam_fovy[cid]; // get pointer to camera orientation matrix mat = d->cam_xmat + 9*cid; // get frame forward[0] = -mat[2]; forward[1] = -mat[5]; forward[2] = -mat[8]; up[0] = mat[1]; up[1] = mat[4]; up[2] = mat[7]; right[0] = mat[0]; right[1] = mat[3]; right[2] = mat[6]; mju_copy3(headpos, d->cam_xpos + 3*cid); } break; default: mjERROR("unknown camera type"); } // compute GL cameras for (int view=0; view < 2; view++) { // set frame for (int i=0; i < 3; i++) { scn->camera[view].pos[i] = (float)(headpos[i] + (view ? ipd : -ipd)*0.5*right[i]); scn->camera[view].forward[i] = (float)forward[i]; scn->camera[view].up[i] = (float)up[i]; } // set symmetric frustum scn->camera[view].frustum_center = 0; scn->camera[view].frustum_top = (float)znear * tanf(fovy * (float)(mjPI/360.0)); scn->camera[view].frustum_bottom = -scn->camera[view].frustum_top; scn->camera[view].frustum_near = (float)znear; scn->camera[view].frustum_far = (float)zfar; } // disable model transformation (do not clear float data; user may need it later) scn->enabletransform = 0; } // update all skins, here for backward API compatibility void mjv_updateSkin(const mjModel* m, mjData* d, mjvScene* scn) { mjvOption opt; mjv_defaultOption(&opt); mjv_updateActiveSkin(m, d, scn, &opt); mju_warning("mjv_updateSkin is deprecated, please use mjv_updateActiveSkin."); } // update visible skins only void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvOption* opt) { // process skins for (int i=0; i < m->nskin; i++) { // get info int vertadr = m->skin_vertadr[i]; int vertnum = m->skin_vertnum[i]; int faceadr = m->skin_faceadr[i]; int facenum = m->skin_facenum[i]; // clear positions and normals memset(scn->skinvert + 3*vertadr, 0, 3*vertnum*sizeof(float)); memset(scn->skinnormal + 3*vertadr, 0, 3*vertnum*sizeof(float)); if (opt->skingroup[m->skin_group[i]]) { // accumulate positions from all bones for (int j=m->skin_boneadr[i]; j < m->skin_boneadr[i]+m->skin_bonenum[i]; j++) { // get bind pose mjtNum bindpos[3] = { (mjtNum) m->skin_bonebindpos[3*j], (mjtNum) m->skin_bonebindpos[3*j+1], (mjtNum) m->skin_bonebindpos[3*j+2] }; mjtNum bindquat[4] = { (mjtNum) m->skin_bonebindquat[4*j], (mjtNum) m->skin_bonebindquat[4*j+1], (mjtNum) m->skin_bonebindquat[4*j+2], (mjtNum) m->skin_bonebindquat[4*j+3] }; // compute rotation int bodyid = m->skin_bonebodyid[j]; mjtNum quat[4], quatneg[4], rotate[9]; mju_negQuat(quatneg, bindquat); mju_mulQuat(quat, d->xquat+4*bodyid, quatneg); mju_quat2Mat(rotate, quat); // compute translation mjtNum translate[3]; mju_rotVecMat(translate, bindpos, rotate); mju_sub3(translate, d->xpos+3*bodyid, translate); // process all bone vertices for (int k=m->skin_bonevertadr[j]; k < m->skin_bonevertadr[j]+m->skin_bonevertnum[j]; k++) { // vertex id and weight int vid = m->skin_bonevertid[k]; float vweight = m->skin_bonevertweight[k]; // get original position mjtNum pos[3] = { (mjtNum) m->skin_vert[3*(vertadr+vid)], (mjtNum) m->skin_vert[3*(vertadr+vid)+1], (mjtNum) m->skin_vert[3*(vertadr+vid)+2], }; // transform mjtNum pos1[3]; mju_rotVecMat(pos1, pos, rotate); mju_addTo3(pos1, translate); // accumulate position scn->skinvert[3*(vertadr+vid)] += vweight*(float)pos1[0]; scn->skinvert[3*(vertadr+vid)+1] += vweight*(float)pos1[1]; scn->skinvert[3*(vertadr+vid)+2] += vweight*(float)pos1[2]; } } // compute vertex normals from face normals for (int k=faceadr; k < faceadr+facenum; k++) { // get face vertex indices int vid[3] = { m->skin_face[3*k], m->skin_face[3*k+1], m->skin_face[3*k+2] }; // get triangle edges mjtNum vec01[3], vec02[3]; for (int r=0; r < 3; r++) { vec01[r] = scn->skinvert[3*(vertadr+vid[1])+r] - scn->skinvert[3*(vertadr+vid[0])+r]; vec02[r] = scn->skinvert[3*(vertadr+vid[2])+r] - scn->skinvert[3*(vertadr+vid[0])+r]; } // compute face normal mjtNum nrm[3]; mju_cross(nrm, vec01, vec02); // add normal to each vertex with weight = area for (int r=0; r < 3; r++) { for (int t=0; t < 3; t++) { scn->skinnormal[3*(vertadr+vid[r])+t] += nrm[t]; } } } // normalize normals for (int k=vertadr; k < vertadr+vertnum; k++) { float s = sqrtf( scn->skinnormal[3*k]*scn->skinnormal[3*k] + scn->skinnormal[3*k+1]*scn->skinnormal[3*k+1] + scn->skinnormal[3*k+2]*scn->skinnormal[3*k+2] ); float scl = 1/mjMAX(mjMINVAL, s); scn->skinnormal[3*k] *= scl; scn->skinnormal[3*k+1] *= scl; scn->skinnormal[3*k+2] *= scl; } // inflate if (m->skin_inflate[i]) { float inflate = m->skin_inflate[i]; for (int k=vertadr; k < vertadr+vertnum; k++) { scn->skinvert[3*k] += inflate*scn->skinnormal[3*k]; scn->skinvert[3*k+1] += inflate*scn->skinnormal[3*k+1]; scn->skinvert[3*k+2] += inflate*scn->skinnormal[3*k+2]; } } } } } // update entire scene void mjv_updateScene(const mjModel* m, mjData* d, const mjvOption* opt, const mjvPerturb* pert, mjvCamera* cam, int catmask, mjvScene* scn) { // clear geoms scn->ngeom = 0; // trigger plugin visualization hooks if (m->nplugin) { const int nslot = mjp_pluginCount(); // iterate over plugins, call visualize if defined for (int i=0; i < m->nplugin; i++) { const int slot = m->plugin[i]; const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot); if (!plugin) { mjERROR("invalid plugin slot: %d", slot); } if (plugin->visualize) { plugin->visualize(m, d, opt, scn, i); } } } // add all categories mjv_addGeoms(m, d, opt, pert, catmask, scn); // add lights mjv_makeLights(m, d, scn); // update camera mjv_updateCamera(m, d, cam, scn); // update skins if (opt->flags[mjVIS_SKIN]) { mjv_updateActiveSkin(m, d, scn, opt); } } //----------------------------------- catenary functions ------------------------------------------- // returns hyperbolic cosine and optionally computes hyperbolic sine static inline mjtNum cosh_sinh(mjtNum x, mjtNum *sinh) { mjtNum expx = mju_exp(x); if (sinh) { *sinh = 0.5 * (expx - 1/expx); } return 0.5 * (expx + 1/expx); } // returns intercept of the catenary equation static inline mjtNum catenary_intercept(mjtNum v, mjtNum h, mjtNum length) { return 1/mju_sqrt(mju_sqrt(length*length - v*v)/h - 1); } // returns residual of catenary equation and optionally computes its gradient w.r.t b static inline mjtNum catenary_residual(mjtNum b, mjtNum intercept, mjtNum *grad) { mjtNum a = 0.5 / b; mjtNum sinh, cosh = cosh_sinh(a, &sinh); if (grad) { *grad = (a*cosh - sinh) * mju_pow(2*b*sinh - 1, -1.5); } return 1/mju_sqrt(2*b*sinh - 1) - intercept; } // convergence tolerance for catenary solver static const mjtNum tolerance = 1e-9; // solve trancendental catenary equation using change of variables proposed in // https://math.stackexchange.com/a/1002996 static inline mjtNum solve_catenary(mjtNum v, mjtNum h, mjtNum length) { mjtNum intercept = catenary_intercept(v, h, length); // initial guess using linear approximation to catenary_residual mjtNum b = intercept / mju_sqrt(24); // Newton steps to convergence (usually ~ 5 steps) for (int i=0; i < 50; i++) { // get value and gradient mjtNum grad; mjtNum res = catenary_residual(b, intercept, &grad); if (mju_abs(res) < tolerance) { break; } // Newton step mjtNum step = -res / grad; // backtracking line-search is not essential but can reduce number of iterations for (int j=0; j < 10; j++) { mjtNum new_res = catenary_residual(b + step, intercept, NULL); if (mju_abs(new_res) < mju_abs(res)) { break; } else { step *= 0.5; } } // take step b += step; } return b; } // points along catenary of given length between x0 and x1, returns number of points int mjv_catenary(const mjtNum x0[3], const mjtNum x1[3], const mjtNum gravity[3], mjtNum length, mjtNum catenary[3*mjNCATENARY]) { mjtNum dist = mju_dist3(x0, x1); // tendon is stretched longer than length: draw straight line if (dist > length) { // copy start and end points mju_copy3(catenary+0, x0); mju_copy3(catenary+3, x1); return 2; } // tendon is shorter than length else { // normalized up vector mjtNum up[3]; mju_scl3(up, gravity, -1); mju_normalize3(up); // x0 to x1 mjtNum x01[3]; mju_sub3(x01, x1, x0); // make across orthonormal to up, points from x0 to x1 mjtNum across[3]; mju_copy3(across, x01); mjtNum tmp[3]; mju_scl3(tmp, up, mju_dot3(up, across)); mju_subFrom3(across, tmp); mjtNum norm = mju_normalize3(across); // if across is numerically tiny, just set to 0 if (norm < mjMINVAL) { mju_zero3(across); } // extents in the suspension plane mjtNum h = mju_dot3(x01, across); // horizontal suspension extent mjtNum v = mju_dot3(x01, up); // vertical height difference of x1 and x0 // near vertical tendon, use hanging bead approximation: 3 points if (length > 100*h) { // solve for location of bead hanging on tendon mjtNum d_up = -0.5*(mju_sqrt(length*length - h*h) - v); // down from x0 mjtNum d_across = h*d_up / (2*d_up - v); // across from x0 // start point mju_copy3(catenary+0, x0); // midpoint: bead location mju_copy3(catenary+3, x0); mju_addToScl3(catenary+3, up, d_up); mju_addToScl3(catenary+3, across, d_across); // end point mju_copy3(catenary+6, x1); return 3; } // compute catenary: mjNCATENARY points else { // b*h: scaled catenary flatness mjtNum bh = solve_catenary(v, h, length) * h; // horizontal and vertical offsets mjtNum h_offset = -0.5 * (mju_log((length+v) / (length-v)) * bh - h); mjtNum v_offset = -cosh_sinh(h_offset / bh, NULL) * bh; // start point mju_copy3(catenary+0, x0); // hanging points for (int i=1; i < mjNCATENARY-1; i++) { // linearly spaced horizontal offset mjtNum horizontal = i*h/mjNCATENARY; mju_addScl3(catenary+3*i, x0, across, horizontal); // vertical offset, evaluate catenary values mjtNum vertical = bh * cosh_sinh((horizontal - h_offset) / bh, NULL) + v_offset; mju_addToScl3(catenary+3*i, up, vertical); } // end point mju_copy3(catenary+3*(mjNCATENARY-1), x1); return mjNCATENARY; } } return 0; // SHOULD NOT OCCUR }