chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity
Files
5c21c8d28bf6db5acb9eb98b42e79d19ff3e029e
Mujoco_WASM/src/engine/engine_vis_visualize.c
T
Yuval Tassa f712eed4ce Allow flex sleeping 
PiperOrigin-RevId: 917817500
Change-Id: Ia3bd5e52e7c2eaa3f70c81352d82c130b1d357f6
2026-05-19 07:15:27 -07:00

3646 lines
112 KiB
C
Raw Blame History

// 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 <math.h>
#include <stddef.h>
#include <string.h>
#include <mujoco/mjdata.h>
#include <mujoco/mjmacro.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjsan.h> // IWYU pragma: keep
#include <mujoco/mjvisualize.h>
#include "engine/engine_array_safety.h"
#include "engine/engine_core_util.h"
#include "engine/engine_memory.h"
#include "engine/engine_name.h"
#include "engine/engine_plugin.h"
#include "engine/engine_sleep.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"
#include "engine/engine_vis_interact.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, 100);
label[99] = '\0';
}
// convert HSV to RGB
void hsv2rgb(float *RGB, float H, float S, float V) {
float R, G, B;
if (S <= 0) {
R = G = B = V;
} else {
float hh = H * 6;
int i = (int)hh;
float ff = hh - i;
float p = V * (1 - S);
float q = V * (1 - (S * ff));
float t = V * (1 - (S * (1 - ff)));
if (i == 0) {
R=V; G=t; B=p;
} else if (i == 1) {
R=q; G=V; B=p;
} else if (i == 2) {
R=p; G=V; B=t;
} else if (i == 3) {
R=p; G=q; B=V;
} else if (i == 4) {
R=t; G=p; B=V;
} else {
R=V; G=p; B=q;
}
}
RGB[0] = R;
RGB[1] = G;
RGB[2] = B;
}
// assign pseudo-random rgba to constraint island using Halton sequence
static void islandColor(float rgba[4], int h, int awake) {
// default to gray R = G = B = 0.7;
float hue = 1.0f;
float saturation = 0.0f;
float value = 0.7f;
// island index given, use Halton sequence to generate pseudo-random color
if (h >= 0) {
// hue in [0, 1]
hue = mju_Halton(h + 1, 7);
// saturation in [0.5, 1.0]
saturation = .5 + .5*mju_Halton(h + 1, 3);
// value in [0.6, 1.0]
value = .6 + .4*mju_Halton(h + 1, 5);
}
// if asleep, decrease saturation and value
if (!awake) {
value *= 0.6;
saturation *= 0.7;
}
hsv2rgb(rgba, hue, saturation, value);
rgba[3] = 1;
}
// 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 or descendant thereof
static int bodycategory(const mjModel* m, int bodyid) {
if (m->body_weldid[bodyid] == 0 && m->body_mocapid[m->body_rootid[bodyid]] == -1) {
return mjCAT_STATIC;
} else {
return mjCAT_DYNAMIC;
}
}
//----------------------------- geom functions -----------------------------------------------------
// 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) {
if (!scn->status) {
mju_warning("Pre-allocated visual geom buffer is full. "
"Increase maxgeom above %d.", scn->maxgeom);
scn->status = 1;
}
return NULL;
}
mjvGeom* thisgeom = scn->geoms + scn->ngeom;
memset(thisgeom, 0, sizeof(mjvGeom));
mjv_initGeom(thisgeom, mjGEOM_NONE, NULL, NULL, NULL, NULL);
thisgeom->objtype = objtype;
thisgeom->objid = objid;
thisgeom->category = category;
thisgeom->segid = scn->ngeom;
return thisgeom;
}
// mark geom as used, set its pointer to NULL, increment scn->ngeom
void releaseGeom(mjvGeom** geom, mjvScene* scn) {
// check geom being released was most recently acquired, SHOULD NOT OCCUR
if (*geom != scn->geoms + scn->ngeom) {
mju_error("Unexpected geom pointer; did you call acquireGeom?");
}
scn->ngeom++;
*geom = NULL;
}
// add a triangle to the scene
static void addTriangle(mjvScene* scn, const mjtNum v0[3], const mjtNum v1[3],
const mjtNum v2[3], const float rgba[4],
int objid, int category, int objtype) {
mjvGeom* thisgeom = acquireGeom(scn, objid, category, objtype);
if (!thisgeom) {
return;
}
mjtNum e1[3] = {v1[0] - v0[0], v1[1] - v0[1], v1[2] - v0[2]};
mjtNum e2[3] = {v2[0] - v0[0], v2[1] - v0[1], v2[2] - v0[2]};
mjtNum normal[3];
mju_cross(normal, e1, e2);
mjtNum lengths[3] = {mju_normalize3(e1), mju_normalize3(e2), mju_normalize3(normal)};
mjtNum xmat[9] = {e1[0], e2[0], normal[0],
e1[1], e2[1], normal[1],
e1[2], e2[2], normal[2]};
mjv_initGeom(thisgeom, mjGEOM_TRIANGLE, lengths, v0, xmat, rgba);
releaseGeom(&thisgeom, scn);
}
// 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->rgba, m->mat_rgba + 4*matid, 4);
geom->emission = m->mat_emission[matid];
geom->specular = m->mat_specular[matid];
geom->shininess = m->mat_shininess[matid];
geom->reflectance = m->mat_reflectance[matid];
}
// 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->matid = matid;
}
// scale alpha for dynamic geoms only
if (flags[mjVIS_TRANSPARENT] && (geom->category == mjCAT_DYNAMIC)) {
geom->rgba[3] *= m->vis.map.alpha;
}
}
// 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]) {
mjtNum quat[4], mat[9], dif[3] = {to[0]-from[0], to[1]-from[1], to[2]-from[2]};
// 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*(from[0] + to[0]);
geom->pos[1] = 0.5*(from[1] + to[1]);
geom->pos[2] = 0.5*(from[2] + to[2]);
geom->size[2] *= 0.5;
}
// arrow is not centered
else {
geom->pos[0] = from[0];
geom->pos[1] = from[1];
geom->pos[2] = from[2];
}
// 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);
}
// add a connector to the scene
static void addConnector(mjvScene* scn, int type, mjtNum width,
const mjtNum from[3], const mjtNum to[3],
const float rgba[4], int objid, int category, int objtype) {
mjvGeom* thisgeom = acquireGeom(scn, objid, category, objtype);
if (!thisgeom) {
return;
}
mjv_connector(thisgeom, type, width, from, to);
if (rgba) f2f(thisgeom->rgba, rgba, 4);
releaseGeom(&thisgeom, scn);
}
// 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->matid = -1;
geom->texcoord = 0;
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;
}
// draw 3 cylinders representing a "frame" decor element
void addFrame(mjvScene* scn, int objid, const mjtNum pos[3], const mjtNum rot[9], float length,
float width) {
// draw separate geoms for each axis
for (int j=0; j < 3; j++) {
mjtNum axis[3];
for (int k=0; k < 3; k++) {
axis[k] = (j == k ? length : 0);
}
mjtNum vec[3];
mju_mulMatVec3(vec, rot, axis);
// create a cylinder
mjtNum to[3];
mju_add3(to, pos, vec);
mjvGeom* thisgeom = acquireGeom(scn, objid, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
}
mjv_connector(thisgeom, mjGEOM_CYLINDER, width, pos, to);
for (int k=0; k < 3; k++) {
thisgeom->rgba[k] = (j == k ? 0.9 : 0);
}
thisgeom->rgba[3] = 1;
releaseGeom(&thisgeom, scn);
}
}
//----------------------------- camera functions --------------------------------------------------
// computes the camera frustum
static void getFrustum(float zver[2], float zhor[2], float znear,
const float intrinsic[4], const float sensorsize[2]) {
if (zhor) {
zhor[0] = znear / intrinsic[0] * (sensorsize[0]/2.f - intrinsic[2]);
zhor[1] = znear / intrinsic[0] * (sensorsize[0]/2.f + intrinsic[2]);
}
if (zver) {
zver[0] = znear / intrinsic[1] * (sensorsize[1]/2.f - intrinsic[3]);
zver[1] = znear / intrinsic[1] * (sensorsize[1]/2.f + intrinsic[3]);
}
}
void mjv_cameraFrame(mjtNum headpos[3], mjtNum forward[3], mjtNum up[3], mjtNum right[3],
const mjData* d, const mjvCamera* cam) {
switch (cam->type) {
case mjCAMERA_FREE:
case mjCAMERA_TRACKING: {
const mjtNum ca = mju_cos(cam->azimuth/180.0*mjPI);
const mjtNum sa = mju_sin(cam->azimuth/180.0*mjPI);
const mjtNum ce = mju_cos(cam->elevation/180.0*mjPI);
const mjtNum se = mju_sin(cam->elevation/180.0*mjPI);
if (forward) {
forward[0] = ce*ca;
forward[1] = ce*sa;
forward[2] = se;
}
if (up) {
up[0] = -se*ca;
up[1] = -se*sa;
up[2] = ce;
}
if (right) {
right[0] = sa;
right[1] = -ca;
right[2] = 0;
}
if (headpos) {
mju_addScl3(headpos, cam->lookat, forward, -cam->distance);
}
break;
}
case mjCAMERA_FIXED: {
const int cid = cam->fixedcamid;
const mjtNum* mat = d->cam_xmat + 9*cid;
if (forward) {
forward[0] = -mat[2];
forward[1] = -mat[5];
forward[2] = -mat[8];
}
if (up) {
up[0] = mat[1];
up[1] = mat[4];
up[2] = mat[7];
}
if (right) {
right[0] = mat[0];
right[1] = mat[3];
right[2] = mat[6];
}
if (headpos) {
mju_copy3(headpos, d->cam_xpos + 3*cid);
}
break;
}
default: {
mjERROR("unknown camera type");
}
}
}
void mjv_cameraFrustum(float zver[2], float zhor[2], float zclip[2], const mjModel* m,
const mjvCamera* cam) {
mjtNum fovy;
int orthographic = 0, cid = 0;
float* intrinsic = NULL;
float* sensorsize = NULL;
// get ipd, fovy, orthographic, intrinsic
switch (cam->type) {
case mjCAMERA_FREE:
case mjCAMERA_TRACKING:
orthographic = m->vis.global.orthographic;
fovy = m->vis.global.fovy;
break;
case mjCAMERA_FIXED:
// get id, check range
cid = cam->fixedcamid;
if (cid < 0 || cid >= m->ncam) {
mjERROR("fixed camera id is outside valid range");
}
orthographic = m->cam_projection[cid] == mjPROJ_ORTHOGRAPHIC;
fovy = m->cam_fovy[cid];
// if positive sensorsize, get sensorsize and intrinsic
if (m->cam_sensorsize[2*cid+1]) {
sensorsize = m->cam_sensorsize + 2*cid;
intrinsic = m->cam_intrinsic + 4*cid;
}
break;
default:
mjERROR("unknown camera type");
}
const float znear = m->vis.map.znear * m->stat.extent;
if (orthographic) {
if (zver) zver[0] = zver[1] = fovy / 2;
if (zhor) zhor[0] = zhor[1] = 0.0f;
} else if (intrinsic) {
getFrustum(zver, zhor, znear, intrinsic, sensorsize);
} else {
if (zver) zver[0] = zver[1] = znear * mju_tan(fovy * mjPI/360.0);
if (zhor) zhor[0] = zhor[1] = 0.0f;
}
if (zclip) {
zclip[0] = znear;
zclip[1] = m->vis.map.zfar * m->stat.extent;
}
}
//----------------------------- main API functions -------------------------------------------------
// add contact-related geoms in mjvObject
static void addContactGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjvScene* scn,
int catmask) {
if (!vopt->flags[mjVIS_CONTACTPOINT] && !vopt->flags[mjVIS_CONTACTFORCE] &&
vopt->frame != mjFRAME_CONTACT) {
return;
}
int objtype = mjOBJ_UNKNOWN, category = mjCAT_DECOR;
mjtNum mat[9], tmp[9], vec[3], frc[3], confrc[6];
mjtNum framewidth, framelength, scl = m->stat.meansize;
mjContact* con;
mjvGeom* thisgeom;
mjtByte split;
// 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 (vopt->flags[mjVIS_CONTACTPOINT]) {
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
thisgeom->type = mjGEOM_CYLINDER;
thisgeom->size[0] = thisgeom->size[1] = m->vis.scale.contactwidth * scl;
float halfheight = m->vis.scale.contactheight * scl;
float halfdepth = -con->dist / 2;
thisgeom->size[2] = mjMAX(halfheight, halfdepth);
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] && efc_adr >= 0) {
// set hue using island's first dof
int h = d->nisland > 0 ? d->island_dofadr[d->efc_island[efc_adr]] : -1;
islandColor(thisgeom->rgba, h, /*awake*/1);
}
// 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) {
char contactlabel[2][48];
for (int k=0; k < 2; k++) {
// make geom label
if (con->geom[k] >= 0) {
const char* geomname = mj_id2name(m, mjOBJ_GEOM, con->geom[k]);
if (geomname) {
mjSNPRINTF(contactlabel[k], "%s", geomname);
} else {
mjSNPRINTF(contactlabel[k], "g%d", con->geom[k]);
}
}
// make flex elem or vert label
else {
const char* flexname = mj_id2name(m, mjOBJ_FLEX, con->flex[k]);
if (flexname) {
if (con->elem[k] >= 0) {
mjSNPRINTF(contactlabel[k], "%s.e%d", flexname, con->elem[k]);
} else {
mjSNPRINTF(contactlabel[k], "%s.v%d", flexname, con->vert[k]);
}
}
else {
if (con->elem[k] >= 0) {
mjSNPRINTF(contactlabel[k], "f%d.e%d", con->flex[k], con->elem[k]);
} else {
mjSNPRINTF(contactlabel[k], "f%d.v%d", con->flex[k], con->vert[k]);
}
}
}
}
mjSNPRINTF(thisgeom->label, "%s | %s", contactlabel[0], contactlabel[1]);
}
releaseGeom(&thisgeom, scn);
}
// mat = contact frame rotation matrix (normal along x)
mju_transpose(mat, con->frame, 3, 3);
// 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;
addFrame(scn, i, con->pos, mat, framelength, framewidth);
}
// nothing else to do for excluded contacts
if (d->contact[i].efc_address < 0) {
continue;
}
// get contact force:torque in contact frame
mj_contactForce(m, d, i, confrc);
// contact force
if (vopt->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 = (vopt->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 bodyflex ids
int bf[2];
for (int k=0; k < 2; k++) {
bf[k] = (con->geom[k] >= 0) ? m->geom_bodyid[con->geom[k]] :
m->nbody + con->flex[k];
}
// make sure arrow points towards bodyflex with higher id
if (bf[0] > bf[1]) {
mju_scl3(vec, vec, -1);
}
// one-directional arrow for friction and world, symmetric otherwise
thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
mjtNum* from = con->pos;
mjtNum to[3];
mju_add3(to, from, vec);
mjv_connector(thisgeom,
bf[0] > 0 && bf[1] > 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));
}
releaseGeom(&thisgeom, scn);
}
}
}
}
static void addFlexGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
const mjvPerturb* pert, int catmask, mjvScene* scn) {
const int category = mjCAT_DYNAMIC;
if (!(category & catmask)) {
return;
}
if (!vopt->flags[mjVIS_FLEXVERT] && !vopt->flags[mjVIS_FLEXEDGE] &&
!vopt->flags[mjVIS_FLEXFACE] && !vopt->flags[mjVIS_FLEXSKIN]) {
return;
}
for (int i=0; i < m->nflex; i++) {
if (!vopt->flexgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->flex_group[i]))]) {
continue;
}
mjvGeom* thisgeom = acquireGeom(scn, i, category, mjOBJ_FLEX);
if (!thisgeom) {
return;
}
// construct geom, pos = first vertex
mjv_initGeom(thisgeom, mjGEOM_FLEX, NULL,
d->flexvert_xpos + 3*m->flex_vertadr[i], NULL, NULL);
thisgeom->size[0] = m->flex_radius[i];
setMaterial(m, thisgeom, m->flex_matid[i], m->flex_rgba+4*i, vopt->flags);
// override if visualizing islands
if (vopt->flags[mjVIS_ISLAND]) {
// find first dynamic body in flex
int bodyid = -1;
if (m->flex_interp[i]) {
int nodeadr = m->flex_nodeadr[i];
for (int j=0; j < m->flex_nodenum[i] && bodyid < 0; j++) {
int b = m->flex_nodebodyid[nodeadr+j];
if (m->body_treeid[b] >= 0) bodyid = b;
}
} else {
int vertadr = m->flex_vertadr[i];
for (int j=0; j < m->flex_vertnum[i] && bodyid < 0; j++) {
int b = m->flex_vertbodyid[vertadr+j];
if (m->body_treeid[b] >= 0) bodyid = b;
}
}
if (bodyid >= 0) {
// strip material
thisgeom->matid = -1;
int weld_id = m->body_weldid[bodyid];
int dof = m->body_dofadr[weld_id];
int island = d->nisland ? d->dof_island[dof] : -1;
int h = island >= 0 ? d->island_dofadr[island] : -1;
int awake = d->body_awake[bodyid];
// if sleep is enabled, color by first tree dof
if (h == -1 && mjENABLED(mjENBL_SLEEP)) {
int tree = m->dof_treeid[dof];
if (!awake) tree = mj_sleepCycle(d->tree_asleep, m->ntree, tree);
h = m->tree_dofadr[tree];
}
islandColor(thisgeom->rgba, h, awake);
}
}
// set texcoord
if (m->flex_texcoordadr[i] >= 0) {
thisgeom->texcoord = 1;
} else {
thisgeom->matid = -1;
}
// glow flex if selected
if (pert->flexselect == i) {
markselected(&m->vis, thisgeom);
}
// skip if alpha is 0
if (thisgeom->rgba[3] == 0) {
continue;
}
// vopt->label
if (vopt->label == mjLABEL_FLEX) {
makeLabel(m, mjOBJ_FLEX, i, thisgeom->label);
}
releaseGeom(&thisgeom, scn);
}
}
static void addSkinGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
const mjvPerturb* pert, int catmask, mjvScene* scn) {
const int category = mjCAT_DYNAMIC;
if (!(category & catmask)) {
return;
}
if (!vopt->flags[mjVIS_SKIN]) {
return;
}
for (int i=0; i < m->nskin; i++) {
if (!vopt->skingroup[mjMAX(0, mjMIN(mjNGROUP-1, m->skin_group[i]))]) {
continue;
}
mjvGeom* thisgeom = acquireGeom(scn, i, category, mjOBJ_SKIN);
if (!thisgeom) {
return;
}
// 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);
}
releaseGeom(&thisgeom, scn);
}
}
static void addGeomGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
const mjvPerturb* pert, int catmask, mjvScene* scn) {
const int objtype = mjOBJ_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++;
}
// skip if category is masked
int category = bodycategory(m, m->geom_bodyid[i]);
if (!(category & catmask)) {
continue;
}
// skip if group is disabled
if (!vopt->geomgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->geom_group[i]))]) {
continue;
}
mjvGeom* thisgeom = acquireGeom(scn, i, category, objtype);
if (!thisgeom) {
return;
}
// 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
float* rgba = m->geom_rgba+4*i;
int geom_matid = m->geom_matid[i];
setMaterial(m, thisgeom, geom_matid, rgba, vopt->flags);
// override if visualizing islands
if (vopt->flags[mjVIS_ISLAND]) {
int weld_id = m->body_weldid[m->geom_bodyid[i]];
if (m->body_dofnum[weld_id]) {
// strip materials off moving geom
thisgeom->matid = -1;
// set hue using first island dof, -1 if no island
int dof = m->body_dofadr[weld_id];
int island = d->nisland ? d->dof_island[dof] : -1;
int h = island >= 0 ? d->island_dofadr[island] : -1;
int awake = d->body_awake[m->geom_bodyid[i]];
// if sleep is enabled, color by first tree dof
if (h == -1 && mjENABLED(mjENBL_SLEEP)) {
int tree = m->dof_treeid[dof];
if (!awake) tree = mj_sleepCycle(d->tree_asleep, m->ntree, tree);
h = m->tree_dofadr[tree];
}
islandColor(thisgeom->rgba, h, awake);
}
}
// set texcoord
if ((m->geom_type[i] == mjGEOM_MESH || m->geom_type[i] == mjGEOM_SDF) &&
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 || m->geom_type[i] == mjGEOM_SDF) {
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
mjtNum tmp[9];
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
mjtNum vec[3];
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);
}
releaseGeom(&thisgeom, scn);
}
}
static void addSiteGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
const mjvPerturb* pert, int catmask, mjvScene* scn) {
for (int i=0; i < m->nsite; i++) {
// skip if category is masked
int category = bodycategory(m, m->site_bodyid[i]);
if (!(category & catmask)) {
continue;
}
// skip if group disabled
if (!vopt->sitegroup[mjMAX(0, mjMIN(mjNGROUP-1, m->site_group[i]))]) {
continue;
}
mjvGeom* thisgeom = acquireGeom(scn, i, category, mjOBJ_SITE);
if (!thisgeom) {
return;
}
// 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);
}
releaseGeom(&thisgeom, scn);
}
}
static void addSpatialTendonGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, int catmask,
mjvScene* scn) {
const int category = mjCAT_DYNAMIC;
if (!(category & catmask)) {
return;
}
if (!vopt->flags[mjVIS_TENDON]) {
return;
}
for (int i=0; i < m->ntendon; i++) {
if (!vopt->tendongroup[mjMAX(0, mjMIN(mjNGROUP-1, m->tendon_group[i]))]) {
continue;
}
int has_stiffness = m->tendon_stiffness[i] ||
!mju_isZero(m->tendon_stiffnesspoly+mjNPOLY*i, mjNPOLY);
// tendon has a deadband spring
int limitedspring =
has_stiffness && // 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 =
!has_stiffness && // 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
int has_damping = m->tendon_damping[i] || !mju_isZero(m->tendon_dampingpoly+mjNPOLY*i, mjNPOLY);
// 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
!has_damping && // no damping
m->tendon_frictionloss[i] == 0; // no frictionloss
// no actuator
if (draw_catenary) {
for (int j=0; j < m->nu; j++) {
if (m->actuator_trntype[j] == mjTRN_TENDON && m->actuator_trnid[2*j] == i) {
draw_catenary = 0;
break;
}
}
}
// 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) {
mjvGeom* thisgeom = acquireGeom(scn, i, category, mjOBJ_TENDON);
if (!thisgeom) {
return;
}
// determine width: smaller for segments inside wrapping objects
mjtNum width;
if (d->wrap_obj[j] >= 0 && d->wrap_obj[j+1] >= 0) {
width = 0.5 * m->tendon_width[i];
} else {
width = m->tendon_width[i];
}
// construct geom
mjv_connector(thisgeom, mjGEOM_CAPSULE, width, d->wrap_xpos+3*j, d->wrap_xpos+3*j+3);
// set material properties
int tendon_matid = m->tendon_matid[i];
float rgba[4];
f2f(rgba, m->tendon_rgba+4*i, 4);
// if tendon has no material and the color is the default gray, re-color it using limit impedance
if (tendon_matid == -1 && rgba[0] == 0.5 && rgba[1] == 0.5 && rgba[2] == 0.5 && rgba[3] == 1) {
// loop over limit constraints, get impedance if this tendon is limited
mjtNum imp = 0;
int efc_start = d->ne + d->nf;
int efc_end = efc_start + d->nl;
for (int k=efc_start; k < efc_end; k++) {
if (d->efc_type[k] == mjCNSTR_LIMIT_TENDON && d->efc_id[k] == i) {
imp = d->efc_KBIP[4*k + 2];
}
}
// use impedance to mix tendon and constraint colors
rgba[0] = (1-imp) * rgba[0] + imp * m->vis.rgba.constraint[0];
rgba[1] = (1-imp) * rgba[1] + imp * m->vis.rgba.constraint[1];
rgba[2] = (1-imp) * rgba[2] + imp * m->vis.rgba.constraint[2];
}
setMaterial(m, thisgeom, tendon_matid, rgba, vopt->flags);
// override if visualizing islands
if (vopt->flags[mjVIS_ISLAND]) {
// strip material
thisgeom->matid = -1;
// set hue with first island dof, if constrained
int h = -1;
if (d->nisland && d->tendon_efcadr[i] >= 0) {
h = d->island_dofadr[d->efc_island[d->tendon_efcadr[i]]];
}
islandColor(thisgeom->rgba, h, 1);
}
// vopt->label: only the first segment
if (vopt->label == mjLABEL_TENDON && j == d->ten_wrapadr[i]) {
makeLabel(m, mjOBJ_TENDON, i, thisgeom->label);
}
releaseGeom(&thisgeom, scn);
}
}
}
// 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];
}
// get number of points along catenary path (capped at 100)
int ncatenary = mjMIN(m->vis.quality.numslices + 1, 100);
mjtNum catenary[300];
// points along catenary path
int npoints = mjv_catenary(x0, x1, m->opt.gravity, length, catenary, ncatenary);
// draw npoints-1 segments
for (int j=0; j < npoints-1; j++) {
mjvGeom* thisgeom = acquireGeom(scn, i, category, mjOBJ_TENDON);
if (!thisgeom) {
return;
}
// construct geom
mjv_connector(thisgeom, mjGEOM_CAPSULE, m->tendon_width[i], 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);
}
releaseGeom(&thisgeom, scn);
}
}
}
}
static void addSliderCrankGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, int catmask,
mjvScene* scn) {
const int category = mjCAT_DYNAMIC;
if (!(category & catmask)) {
return;
}
const float scl = m->stat.meansize;
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
mjtNum rod = m->actuator_cranklength[i];
mjtNum axis[3];
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
mjtNum vec[3];
mju_sub(vec, d->site_xpos+3*j, d->site_xpos+3*k, 3);
mjtNum len = mju_dot3(vec, axis);
mjtNum det = len*len + rod*rod - mju_dot3(vec, vec);
mjtByte broken = 0;
if (det < 0) {
det = 0;
broken = 1;
}
len = len - mju_sqrt(det);
// compute slider endpoint
mjtNum end[3];
mju_scl3(end, axis, len);
mju_addTo3(end, d->site_xpos+3*k);
// render slider
mjvGeom* thisgeom = acquireGeom(scn, i, category, mjOBJ_ACTUATOR);
if (!thisgeom) {
return;
}
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);
}
releaseGeom(&thisgeom, scn);
thisgeom = acquireGeom(scn, i, category, mjOBJ_ACTUATOR);
if (!thisgeom) {
return;
}
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);
}
releaseGeom(&thisgeom, scn);
}
}
}
static void addGeomFrameGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, int catmask,
mjvScene* scn) {
if (vopt->frame != mjFRAME_GEOM) {
return;
}
const float scl = m->stat.meansize;
for (int i=0; i < m->ngeom; i++) {
if (!(bodycategory(m, m->geom_bodyid[i]) & catmask)) {
continue;
}
if (!vopt->geomgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->geom_group[i]))]) {
continue;
}
// base element is invisible; don't show decors
int matid = m->geom_matid[i];
float* rgba = (matid >= 0) ? (m->mat_rgba + 4*matid) : (m->geom_rgba + 4*i);
if (rgba[3] == 0) {
continue;
}
// construct geom frame
mjtNum width = m->vis.scale.framewidth * scl;
mjtNum length = m->vis.scale.framelength * scl;
addFrame(scn, i, d->geom_xpos+3*i, d->geom_xmat+9*i, length, width);
}
}
static void addSiteFrameGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, int catmask,
mjvScene* scn) {
if (vopt->frame != mjFRAME_SITE) {
return;
}
const float scl = m->stat.meansize;
for (int i=0; i < m->nsite; i++) {
if (!(bodycategory(m, m->site_bodyid[i]) & catmask)) {
continue;
}
if (!vopt->sitegroup[mjMAX(0, mjMIN(mjNGROUP-1, m->site_group[i]))]) {
continue;
}
// base element is invisible; don't show decors
int matid = m->site_matid[i];
float* rgba = (matid >= 0) ? (m->mat_rgba + 4*matid) : (m->site_rgba + 4*i);
if (rgba[3] == 0) {
continue;
}
// construct site frame
mjtNum width = m->vis.scale.framewidth * scl;
mjtNum length = m->vis.scale.framelength * scl;
addFrame(scn, i, d->site_xpos+3*i, d->site_xmat+9*i, length, width);
}
}
static void addBodyBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjvScene* scn) {
if (!vopt->flags[mjVIS_BODYBVH]) {
return;
}
for (int i = 0; i < m->nbvhstatic; i++) {
int isleaf = m->bvh_child[2*i] == -1 && m->bvh_child[2*i+1] == -1;
if (m->bvh_depth[i] != vopt->bvh_depth) {
if (!isleaf || m->bvh_depth[i] > vopt->bvh_depth) {
continue;
}
}
// find geom number
int bodyid = 0;
int geomid = m->bvh_nodeid[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;
}
// get xpos, xmat, size
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;
const mjtNum* size = isleaf ? m->geom_aabb + 6*geomid + 3 : m->bvh_aabb + 6*i + 3;
// offset xpos with aabb center (not always at frame origin)
const mjtNum* center = isleaf ? m->geom_aabb + 6*geomid : m->bvh_aabb + 6*i;
mjtNum pos[3];
mju_mulMatVec3(pos, xmat, center);
mju_addTo3(pos, xpos);
// set box color
const float* rgba = m->vis.rgba.bv;
if (m->vis.global.bvactive && d->bvh_active[i]) {
rgba = m->vis.rgba.bvactive;
}
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
}
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, size, pos, xmat, rgba);
releaseGeom(&thisgeom, scn);
}
}
static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjvScene* scn) {
if (!vopt->flags[mjVIS_MESHBVH]) {
return;
}
for (int f=0; f < m->nflex; f++) {
if (m->flex_bvhnum[f] && vopt->flexgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->flex_group[f]))]) {
for (int i=m->flex_bvhadr[f]; i < m->flex_bvhadr[f]+m->flex_bvhnum[f]; i++) {
int isleaf = m->bvh_child[2*i] == -1 && m->bvh_child[2*i+1] == -1;
if (m->bvh_depth[i] != vopt->bvh_depth) {
if (!isleaf || m->bvh_depth[i] > vopt->bvh_depth) {
continue;
}
}
// get box data
mjtNum* aabb = d->bvh_aabb_dyn + 6*(i - m->nbvhstatic);
// set box color
const float* rgba = m->vis.rgba.bv;
if (m->vis.global.bvactive && d->bvh_active[i]) {
rgba = m->vis.rgba.bvactive;
}
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
}
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, aabb+3, aabb, NULL, rgba);
releaseGeom(&thisgeom, scn);
}
}
if (!m->flex_interp[f]) {
continue;
}
// control points box
mjtNum* xpos = mjSTACKALLOC(d, 3*m->flex_nodenum[f], mjtNum);
int nstart = m->flex_nodeadr[f];
int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
if (m->flex_centered[f]) {
for (int i=0; i < m->flex_nodenum[f]; i++) {
mju_copy3(xpos + 3*i, d->xpos + 3*bodyid[i]);
}
} else {
for (int i=0; i < m->flex_nodenum[f]; i++) {
mju_mulMatVec3(xpos + 3*i, d->xmat + 9*bodyid[i], m->flex_node + 3*(i+nstart));
mju_addTo3(xpos + 3*i, d->xpos + 3*bodyid[i]);
}
}
int cx = m->flex_cellnum[3*f+0];
int cy = m->flex_cellnum[3*f+1];
int cz = m->flex_cellnum[3*f+2];
int order = m->flex_interp[f];
order = order < 0 ? -order : order;
int NX = cx * order + 1;
int NY = cy * order + 1;
int NZ = cz * order + 1;
for (int i=0; i < NX; i++) {
for (int j=0; j < NY; j++) {
for (int k=0; k < NZ; k++) {
int n0 = i*NY*NZ + j*NZ + k;
// skip if this node is pinned (no joints on its body)
if (m->body_jntnum[bodyid[n0]] == 0) {
continue;
}
int offset = 3*n0;
int offset1 = 3*((i+1)*NY*NZ + j*NZ + k);
int offset2 = 3*(i*NY*NZ + (j+1)*NZ + k);
int offset3 = 3*(i*NY*NZ + j*NZ + (k+1));
if (i < NX-1 && m->body_jntnum[bodyid[(i+1)*NY*NZ + j*NZ + k]] > 0) {
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
}
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset1);
releaseGeom(&thisgeom, scn);
}
if (j < NY-1 && m->body_jntnum[bodyid[i*NY*NZ + (j+1)*NZ + k]] > 0) {
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
}
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset2);
releaseGeom(&thisgeom, scn);
}
if (k < NZ-1 && m->body_jntnum[bodyid[i*NY*NZ + j*NZ + (k+1)]] > 0) {
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
}
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset3);
releaseGeom(&thisgeom, scn);
}
}
}
}
}
}
static void addMeshBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjvScene* scn) {
if (!vopt->flags[mjVIS_MESHBVH]) {
return;
}
for (int geomid = 0; geomid < m->ngeom; geomid++) {
int meshid = m->geom_dataid[geomid];
// skip if not a mesh or if there is an octree
if (meshid == -1 || m->geom_type[geomid] == mjGEOM_SDF || m->mesh_octadr[meshid] >= 0) {
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 (m->bvh_depth[i] != vopt->bvh_depth) {
if (!isleaf || m->bvh_depth[i] > vopt->bvh_depth) {
continue;
}
}
// box color
const float* rgba = m->vis.rgba.bv;
if (m->vis.global.bvactive) {
if (d->bvh_active[i]) {
rgba = m->vis.rgba.bvactive;
} else {
// when marking active bvs, skip inactive volumes
continue;
}
}
// get xpos, xmat, size
const mjtNum* xpos = d->geom_xpos + 3 * geomid;
const mjtNum* xmat = d->geom_xmat + 9 * geomid;
const mjtNum* size = m->bvh_aabb + 6*i + 3;
// offset xpos with aabb center (not always at geom origin)
const mjtNum* center = m->bvh_aabb + 6*i;
mjtNum pos[3];
mju_mulMatVec3(pos, xmat, center);
mju_addTo3(pos, xpos);
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
}
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, size, pos, xmat, rgba);
releaseGeom(&thisgeom, scn);
}
}
}
static void addMeshOctreeGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjvScene* scn) {
if (!vopt->flags[mjVIS_MESHBVH]) {
return;
}
for (int geomid = 0; geomid < m->ngeom; geomid++) {
int meshid = m->geom_dataid[geomid];
if (meshid == -1 || m->geom_type[geomid] == mjGEOM_HFIELD || m->mesh_octadr[meshid] == -1) {
continue;
}
for (int b = 0; b < m->mesh_octnum[meshid]; b++) {
int i = b + m->mesh_octadr[meshid];
if (m->oct_depth[i] != vopt->bvh_depth) {
continue;
}
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
}
const float* rgba = m->vis.rgba.bv;
const mjtNum* xpos = d->geom_xpos + 3 * geomid;
const mjtNum* xmat = d->geom_xmat + 9 * geomid;
const mjtNum* size = m->oct_aabb + 6*i + 3;
// offset xpos with aabb center (not always at geom origin)
const mjtNum* center = m->oct_aabb + 6*i;
mjtNum pos[3];
mju_mulMatVec3(pos, xmat, center);
mju_addTo3(pos, xpos);
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, size, pos, xmat, rgba);
releaseGeom(&thisgeom, scn);
}
}
}
static void addTactileSensorGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
mjvScene* scn) {
if (!vopt->flags[mjVIS_CONTACTPOINT]) {
return;
}
for (int id = 0; id < m->nsensor; id++) {
if (m->sensor_type[id] == mjSENS_TACTILE) {
// get site id and frame
int mesh_id = m->sensor_objid[id];
int geom_id = m->sensor_refid[id];
mjtNum* geom_pos = d->geom_xpos + 3*geom_id;
mjtNum* geom_mat = d->geom_xmat + 9*geom_id;
mjtNum geom_quat[4];
mju_mat2Quat(geom_quat, geom_mat);
// get sensor data
mjtNum* sensordata = d->sensordata + m->sensor_adr[id];
int nchannel = m->sensor_dim[id] / m->mesh_vertnum[mesh_id];
// get maximum absolute normal force
mjtNum maxval = 0;
for (int j=0; j < m->mesh_vertnum[mesh_id]; j++) {
maxval = mju_max(maxval, mju_abs(sensordata[j]));
}
// if no normal force readings, quick return
if (!maxval || m->geom_rbound[geom_id] < mjMINVAL) {
continue;
}
// draw geoms
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++) {
// triangle in global frame
mjtNum pos[3][3];
for (int j = 0; j < 3; j++) {
mjtNum v[3] = {mesh_vert[3 * face[3 * i + j] + 0],
mesh_vert[3 * face[3 * i + j] + 1],
mesh_vert[3 * face[3 * i + j] + 2]};
mju_mulMatVec3(pos[j], geom_mat, v);
mju_addTo3(pos[j], geom_pos);
}
// color
float rgba[4] = {0, 0, 0, 1.0};
mjtNum nval[3] = {0, 0, 0};
for (int r = 0; r < mjMIN(nchannel, 3); r++) {
for (int j = 0; j < 3; j++) {
mjtNum val = sensordata[r*m->mesh_vertnum[mesh_id] + face[3*i+j]];
rgba[r] += mju_abs(val) / maxval;
if (val) {
nval[r] += 1;
}
}
if (nval[r]) {
rgba[r] /= nval[r];
}
}
if (rgba[0]==0 && rgba[1]==0 && rgba[2]==0) {
rgba[3] = .1;
}
// draw triangles, one per side
addTriangle(scn, pos[0], pos[1], pos[2], rgba, id, mjCAT_DECOR, mjOBJ_SENSOR);
}
}
}
}
static void addInertiaGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
const mjvPerturb* pert, int catmask, mjvScene* scn) {
if (!vopt->flags[mjVIS_INERTIA]) {
return;
}
int ellipsoid = m->vis.global.ellipsoidinertia == 1;
for (int i=1; i < m->nbody; i++) {
if (m->body_mass[i] <= mjMINVAL) {
continue;
}
if (!(bodycategory(m, i) & catmask)) {
continue;
}
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_BODY);
if (!thisgeom) {
return;
}
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);
mjtNum sz[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);
}
releaseGeom(&thisgeom, scn);
}
}
static void addPerturbGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
const mjvPerturb* pert, mjvScene* scn) {
if (!vopt->flags[mjVIS_PERTOBJ]) {
return;
}
if (pert->select <= 0) {
return;
}
const float scl = m->stat.meansize;
if ((pert->active | pert->active2) & mjPERT_TRANSLATE) {
mjvGeom* thisgeom = acquireGeom(scn, pert->select, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
}
// compute selection point in world coordinates
mjtNum selpos[3];
mju_mulMatVec3(selpos, d->xmat+9*pert->select, pert->localpos);
mju_addTo3(selpos, d->xpos+3*pert->select);
// construct geom
mjtNum sz[3];
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);
releaseGeom(&thisgeom, scn);
// add small sphere at end-effector
thisgeom = acquireGeom(scn, pert->select, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
}
// construct geom
sz[0] = 2*sz[0];
sz[1] = sz[2] = sz[0];
mjtNum mat[9];
mju_quat2Mat(mat, pert->refquat);
mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, pert->refselpos, mat, rgba);
releaseGeom(&thisgeom, scn);
}
if ((pert->active | pert->active2) & mjPERT_ROTATE) {
mjvGeom* thisgeom = acquireGeom(scn, pert->select, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
}
// 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: if body i has a collision aabb, use that
mjtNum pos[3] = {0};
mjtNum sz[3];
if (m->body_bvhnum[pert->select]) {
mjtNum* aabb = m->bvh_aabb+6*m->body_bvhadr[pert->select];
mju_copy3(sz, aabb+3);
mju_mulMatVec3(pos, d->ximat+9*pert->select, aabb);
}
// otherwise box of size meansize
else {
sz[0] = sz[1] = sz[2] = scl;
}
mjtNum mat[9];
mju_quat2Mat(mat, pert->refquat);
mju_addTo3(pos, d->xipos+3*pert->select);
mjv_initGeom(thisgeom, mjGEOM_BOX, sz, pos, mat, rgba);
releaseGeom(&thisgeom, scn);
}
}
static void addWorldBodyFrameGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, int catmask,
mjvScene* scn) {
const float scl = m->stat.meansize;
for (int i = (vopt->frame == mjFRAME_WORLD ? 0 : 1);
i < (vopt->frame == mjFRAME_BODY ? m->nbody : 1);
i++) {
// skip if body is static and static bodies are masked
if (i > 0 && bodycategory(m, i) & ~catmask) {
continue;
}
// set length(1) and width(0) of the axis cylinders
mjtNum sz[2];
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;
}
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;
addFrame(scn, i, xpos, xmat, sz[1], sz[0]);
}
}
static void addSelectionPointGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
const mjvPerturb* pert, mjvScene* scn) {
if (pert->select <= 0) {
return;
}
if (!vopt->flags[mjVIS_SELECT]) {
return;
}
const float scl = m->stat.meansize;
// compute selection point in world coordinates
mjtNum selpos[3];
mju_mulMatVec3(selpos, d->xmat+9*pert->select, pert->localpos);
mju_addTo3(selpos, d->xpos+3*pert->select);
mjvGeom* thisgeom = acquireGeom(scn, pert->select, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
}
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]);
}
releaseGeom(&thisgeom, scn);
}
static void addBodyLabelGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
const mjvPerturb* pert, int catmask, mjvScene* scn) {
if (vopt->flags[mjVIS_INERTIA]) {
return;
}
if (vopt->label != mjLABEL_SELECTION && vopt->label != mjLABEL_BODY) {
return;
}
for (int i=1; i < m->nbody; i++) {
if (vopt->label == mjLABEL_SELECTION && pert->select != i) {
continue;
}
if (bodycategory(m, i) & ~catmask) {
continue;
}
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
}
thisgeom->type = mjGEOM_LABEL;
mju_n2f(thisgeom->pos, d->xpos+3*i, 3);
mju_n2f(thisgeom->mat, d->xmat+9*i, 9);
makeLabel(m, mjOBJ_BODY, i, thisgeom->label);
releaseGeom(&thisgeom, scn);
}
}
static void addJointGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjvScene* scn) {
if (!vopt->flags[mjVIS_JOINT]) {
return;
}
const float scl = m->stat.meansize;
for (int i=0; i < m->njnt; i++) {
if (!vopt->jointgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->jnt_group[i]))]) {
continue;
}
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_JOINT);
if (!thisgeom) {
return;
}
// set sz = {width, length} of the connectors
mjtNum sz[2];
sz[1] = m->vis.scale.jointlength * scl;
sz[0] = m->vis.scale.jointwidth * scl;
// 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]);
}
// loop over limit constraints, get impedance if this joint is limited
mjtNum imp = 0;
int efc_start = d->ne + d->nf;
int efc_end = efc_start + d->nl;
for (int k=efc_start; k < efc_end; k++) {
if (d->efc_type[k] == mjCNSTR_LIMIT_JOINT && d->efc_id[k] == i) {
imp = d->efc_KBIP[4*k + 2];
}
}
// use impedance to mix joint and constraint colors
float rgba[4];
rgba[0] = (1-imp) * m->vis.rgba.joint[0] + imp * m->vis.rgba.constraint[0];
rgba[1] = (1-imp) * m->vis.rgba.joint[1] + imp * m->vis.rgba.constraint[1];
rgba[2] = (1-imp) * m->vis.rgba.joint[2] + imp * m->vis.rgba.constraint[2];
rgba[3] = 1;
f2f(thisgeom->rgba, rgba, 4);
// vopt->label
if (vopt->label == mjLABEL_JOINT) {
makeLabel(m, mjOBJ_JOINT, i, thisgeom->label);
}
releaseGeom(&thisgeom, scn);
}
}
static void addActuatorGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjvScene* scn) {
if (!vopt->flags[mjVIS_ACTUATOR]) {
return;
}
const float scl = m->stat.meansize;
for (int i=0; i < m->nu; i++) {
if (!vopt->actuatorgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->actuator_group[i]))]) {
continue;
}
if (mj_actuatorDisabled(m, i)) {
continue;
}
// 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) {
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_ACTUATOR);
if (!thisgeom) {
return;
}
// site actuators
mjtNum sz[3];
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);
}
releaseGeom(&thisgeom, scn);
}
// 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 && geomtype != mjGEOM_SDF) {
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_ACTUATOR);
if (!thisgeom) {
return;
}
// inflate sizes by 5%
mjtNum sz[3];
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);
releaseGeom(&thisgeom, scn);
}
}
}
// 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) {
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_ACTUATOR);
if (!thisgeom) {
return;
}
// determine width: smaller for segments inside wrapping objects
mjtNum width;
if (d->wrap_obj[k] >= 0 && d->wrap_obj[k+1] >= 0) {
width = 0.5 * m->tendon_width[j];
} else {
width = m->tendon_width[j];
}
// increase width for actuator
width *= m->vis.map.actuatortendon;
// construct geom
mjv_connector(thisgeom, mjGEOM_CAPSULE, width, 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);
}
releaseGeom(&thisgeom, scn);
}
}
}
}
}
static void addIslandLabelGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
mjvScene* scn) {
if (vopt->label != mjLABEL_ISLAND || !d->nisland) {
return;
}
for (int i=1; i < m->nbody; i++) {
int weld_id = m->body_weldid[i];
if (!m->body_dofnum[weld_id]) {
continue;
}
int islandid = d->dof_island[m->body_dofadr[weld_id]];
if (islandid <= -1) {
continue;
}
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
}
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);
releaseGeom(&thisgeom, scn);
}
}
static void addCameraGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjvScene* scn) {
if (!vopt->flags[mjVIS_CAMERA]) {
return;
}
const float scl = m->stat.meansize;
for (int i=0; i < m->ncam; i++) {
// copy camera rgba
float cam_rgba[4];
f2f(cam_rgba, m->vis.rgba.camera, 4);
// draw frustum if resolution larger than (1, 1)
if (m->cam_resolution[2*i] > 1 || m->cam_resolution[2*i+1] > 1) {
// when drawing frustum, make camera translucent
cam_rgba[3] = 0.3;
// locals
const float* rgba = m->vis.rgba.frustum;
mjtNum vnear[4][3], vfar[4][3];
mjtNum center[3];
mjtNum znear = m->vis.map.znear * m->stat.extent;
mjtNum zfar = m->vis.scale.frustum * scl;
float zver[2], zhor[2];
int orthographic = m->cam_projection[i] == mjPROJ_ORTHOGRAPHIC;
// get frustum
if (orthographic) {
float aspect = (float)m->cam_resolution[2*i] / m->cam_resolution[2*i+1];
zver[0] = zver[1] = m->cam_fovy[i] / 2;
zhor[0] = zhor[1] = m->cam_fovy[i] * aspect / 2;
} else if (m->cam_sensorsize[2*i] && m->cam_sensorsize[2*i+1]) {
// intrinsic-based perspective camera
getFrustum(zver, zhor, znear, m->cam_intrinsic+4*i, m->cam_sensorsize+2*i);
} else {
// fovy-based perspective camera
float aspect = (float)m->cam_resolution[2*i] / m->cam_resolution[2*i+1];
zver[0] = zver[1] = znear * mju_tan(m->cam_fovy[i] * mjPI / 360.0);
zhor[0] = zhor[1] = zver[0] * aspect;
}
// frustum frame to convert from planes to vertex representation
mjtNum* cam_xpos = d->cam_xpos+3*i;
mjtNum* cam_xmat = d->cam_xmat+9*i;
mjtNum x[] = {cam_xmat[0], cam_xmat[3], cam_xmat[6]};
mjtNum y[] = {cam_xmat[1], cam_xmat[4], cam_xmat[7]};
mjtNum z[] = {cam_xmat[2], cam_xmat[5], cam_xmat[8]};
// vertices of the near plane
mju_addScl3(center, cam_xpos, z, -znear);
mju_addScl3(vnear[0], center, x, -zhor[0]);
mju_addScl3(vnear[1], center, x, zhor[1]);
mju_addScl3(vnear[2], center, x, zhor[1]);
mju_addScl3(vnear[3], center, x, -zhor[0]);
mju_addToScl3(vnear[0], y, -zver[0]);
mju_addToScl3(vnear[1], y, -zver[0]);
mju_addToScl3(vnear[2], y, zver[1]);
mju_addToScl3(vnear[3], y, zver[1]);
// vertices of the far plane: scale for perspective, average(width, height) for orthographic
if (!orthographic) {
zhor[0] *= zfar / znear;
zhor[1] *= zfar / znear;
zver[0] *= zfar / znear;
zver[1] *= zfar / znear;
} else {
zfar = (zhor[0] + zver[0]) / 2;
}
mju_addScl3(center, cam_xpos, z, -zfar);
mju_addScl3(vfar[0], center, x, -zhor[0]);
mju_addScl3(vfar[1], center, x, zhor[1]);
mju_addScl3(vfar[2], center, x, zhor[1]);
mju_addScl3(vfar[3], center, x, -zhor[0]);
mju_addToScl3(vfar[0], y, -zver[0]);
mju_addToScl3(vfar[1], y, -zver[0]);
mju_addToScl3(vfar[2], y, zver[1]);
mju_addToScl3(vfar[3], y, zver[1]);
// triangulation and wireframe of the frustum
for (int e=0; e < 4; e++) {
addTriangle(scn, vnear[e], vfar[e], vnear[(e+1)%4], rgba,
i, mjCAT_DECOR, mjOBJ_CAMERA);
addTriangle(scn, vfar[e], vfar[(e+1)%4], vnear[(e+1)%4], rgba,
i, mjCAT_DECOR, mjOBJ_CAMERA);
addConnector(scn, mjGEOM_LINE, 3, vnear[e], vnear[(e+1)%4], rgba,
i, mjCAT_DECOR, mjOBJ_CAMERA);
addConnector(scn, mjGEOM_LINE, 3, vfar[e], vfar[(e+1)%4], rgba,
i, mjCAT_DECOR, mjOBJ_CAMERA);
addConnector(scn, mjGEOM_LINE, 3, vnear[e], vfar[e], rgba,
i, mjCAT_DECOR, mjOBJ_CAMERA);
}
}
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_CAMERA);
if (!thisgeom) {
return;
}
// 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, cam_rgba, 4);
// vopt->label
if (vopt->label == mjLABEL_CAMERA) {
makeLabel(m, mjOBJ_CAMERA, i, thisgeom->label);
}
releaseGeom(&thisgeom, scn);
thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_CAMERA);
if (!thisgeom) {
return;
}
// 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, cam_rgba, 4);
for (int k=0; k < 3; k++) {
thisgeom->rgba[k] *= 0.5; // make lens body darker
}
releaseGeom(&thisgeom, scn);
if (vopt->frame != mjFRAME_CAMERA) {
continue;
}
mjtNum width = m->vis.scale.framewidth * scl;
mjtNum length = m->vis.scale.framelength * scl;
addFrame(scn, i, d->cam_xpos+3*i, d->cam_xmat+9*i, length, width);
}
}
static void addLightGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjvScene* scn) {
if (!vopt->flags[mjVIS_LIGHT]) {
return;
}
const float scl = m->stat.meansize;
for (int i=0; i < m->nlight; i++) {
// make light frame
mjtNum quat[4];
mju_quatZ2Vec(quat, d->light_xdir+3*i);
mjtNum mat[9];
mju_quat2Mat(mat, quat);
// make light position: offset backward, to avoid casting shadow
mjtNum vec[3];
mju_addScl3(vec, d->light_xpos+3*i, d->light_xdir+3*i, -scl * m->vis.scale.light -0.0001);
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_LIGHT);
if (!thisgeom) {
return;
}
// 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);
}
releaseGeom(&thisgeom, scn);
if (vopt->frame != mjFRAME_LIGHT) {
continue;
}
mjtNum width = m->vis.scale.framewidth * scl;
mjtNum length = m->vis.scale.framelength * scl;
addFrame(scn, i, d->light_xpos+3*i, mat, length, width);
}
}
static void addCenterOfMassGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
mjvScene* scn) {
// center of mass for root bodies
if (!vopt->flags[mjVIS_COM]) {
return;
}
const float scl = m->stat.meansize;
for (int i=1; i < m->nbody; i++) {
if (m->body_rootid[i] == i) {
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
}
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);
releaseGeom(&thisgeom, scn);
}
}
}
static void addAutoConnectGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
mjvScene* scn) {
if (!vopt->flags[mjVIS_AUTOCONNECT]) {
return;
}
const float scl = m->stat.meansize;
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
mjtNum* 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--) {
mjtNum* nxt = d->xanchor+3*j;
// construct geom
addConnector(scn, mjGEOM_CAPSULE, scl * m->vis.scale.connect, cur, nxt,
m->vis.rgba.connect, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
cur = nxt;
}
}
// connect first joint (or com) to parent com
mjtNum* first = d->xipos+3*m->body_parentid[i];
addConnector(scn, mjGEOM_CAPSULE, scl * m->vis.scale.connect, cur, first,
m->vis.rgba.connect, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
}
}
static void addRangefinderGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
mjvScene* scn) {
if (!vopt->flags[mjVIS_RANGEFINDER]) {
return;
}
const float scl = m->stat.meansize;
mjtNum framewidth = m->vis.scale.framewidth * scl;
mjtNum framelength = m->vis.scale.framelength * scl;
for (int i=0; i < m->nsensor; i++) {
if (m->sensor_type[i] == mjSENS_RANGEFINDER) {
int objid = m->sensor_objid[i];
int adr = m->sensor_adr[i];
// get dataspec and compute field offsets
int dataspec = m->sensor_intprm[i*mjNSENS];
int size = mju_raydataSize(dataspec);
int offset[mjNRAYDATA] = {0};
int increment = 0;
for (int j=0; j < mjNRAYDATA; j++) {
offset[j] = increment;
if (dataspec & (1 << j)) {
increment += mjRAYDATA_SIZE[j];
}
}
// site-attached rangefinder
if (m->sensor_objtype[i] == mjOBJ_SITE) {
const mjtNum* ptr = d->sensordata + adr;
// get distance (if present)
mjtNum dist = -1;
if (dataspec & (1 << mjRAYDATA_DIST)) {
dist = ptr[offset[mjRAYDATA_DIST]];
}
// get point and draw line if dist is valid
mjtNum point[3] = {0};
if (dist >= 0) {
mjtNum* origin = d->site_xpos + 3*objid;
point[0] = origin[0] + d->site_xmat[9*objid+2]*dist;
point[1] = origin[1] + d->site_xmat[9*objid+5]*dist;
point[2] = origin[2] + d->site_xmat[9*objid+8]*dist;
addConnector(scn, mjGEOM_LINE, 3, origin, point, m->vis.rgba.rangefinder,
i, mjCAT_DECOR, mjOBJ_SENSOR);
}
// draw point if present and non-zero
if (dataspec & (1 << mjRAYDATA_POINT)) {
const mjtNum* point_data = ptr + offset[mjRAYDATA_POINT];
if (point_data[0] || point_data[1] || point_data[2]) {
mju_copy3(point, point_data);
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_SENSOR);
if (thisgeom) {
thisgeom->type = mjGEOM_SPHERE;
thisgeom->size[0] = thisgeom->size[1] = thisgeom->size[2] = 1.5 * framewidth;
mju_n2f(thisgeom->pos, point, 3);
mju_n2f(thisgeom->mat, IDENTITY, 9);
f2f(thisgeom->rgba, m->vis.rgba.rangefinder, 4);
releaseGeom(&thisgeom, scn);
}
}
}
// draw normal if present and point is valid
int valid_point = dist >= 0 || point[0] || point[1] || point[2];
if (valid_point && (dataspec & (1 << mjRAYDATA_NORMAL))) {
const mjtNum* normal = ptr + offset[mjRAYDATA_NORMAL];
mjtNum to[3];
mju_addScl3(to, point, normal, 2*framelength);
addConnector(scn, mjGEOM_ARROW1, framewidth, point, to,
m->vis.rgba.rangefinder, i, mjCAT_DECOR, mjOBJ_SENSOR);
}
}
// camera-attached rangefinder
else if (m->sensor_objtype[i] == mjOBJ_CAMERA) {
const int width = m->cam_resolution[2*objid];
const int height = m->cam_resolution[2*objid+1];
const mjtNum* cam_xpos = d->cam_xpos + 3*objid;
const mjtNum* cam_xmat = d->cam_xmat + 9*objid;
const int projection = m->cam_projection[objid];
// compute focal length in pixels using helper
mjtNum fx, fy, cx, cy, ortho_extent;
mju_camIntrinsics(m, objid, &fx, &fy, &cx, &cy, &ortho_extent);
// draw for each pixel
for (int row = 0; row < height; row++) {
for (int col = 0; col < width; col++) {
int idx = row*width + col;
const mjtNum* ptr = d->sensordata + adr + idx*size;
// get distance (if present)
mjtNum dist = -1;
if (dataspec & (1 << mjRAYDATA_DIST)) {
dist = ptr[offset[mjRAYDATA_DIST]];
}
// compute ray origin and direction
mjtNum origin[3], direction[3];
mju_camPixelRay(origin, direction, cam_xpos, cam_xmat,
col, row, fx, fy, cx, cy, projection, ortho_extent);
// get point and draw line if dist is valid
mjtNum point[3] = {0};
if (dist >= 0) {
mju_addScl3(point, origin, direction, dist);
addConnector(scn, mjGEOM_LINE, 3, origin, point, m->vis.rgba.rangefinder,
i, mjCAT_DECOR, mjOBJ_SENSOR);
}
// draw point if present and non-zero
if (dataspec & (1 << mjRAYDATA_POINT)) {
const mjtNum* point_data = ptr + offset[mjRAYDATA_POINT];
if (point_data[0] || point_data[1] || point_data[2]) {
mju_copy3(point, point_data);
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_SENSOR);
if (thisgeom) {
thisgeom->type = mjGEOM_SPHERE;
thisgeom->size[0] = thisgeom->size[1] = thisgeom->size[2] = 1.3 * framewidth;
mju_n2f(thisgeom->pos, point, 3);
mju_n2f(thisgeom->mat, IDENTITY, 9);
f2f(thisgeom->rgba, m->vis.rgba.rangefinder, 4);
releaseGeom(&thisgeom, scn);
}
}
}
// draw normal if present and point is valid
int valid_point = dist >= 0 || point[0] || point[1] || point[2];
if (valid_point && (dataspec & (1 << mjRAYDATA_NORMAL))) {
const mjtNum* normal = ptr + offset[mjRAYDATA_NORMAL];
mjtNum to[3];
mju_addScl3(to, point, normal, 2*framelength);
addConnector(scn, mjGEOM_ARROW1, framewidth, point, to,
m->vis.rgba.rangefinder, i, mjCAT_DECOR, mjOBJ_SENSOR);
}
}
}
}
} else if (m->sensor_type[i] == mjSENS_GEOMFROMTO) {
// sensor data
mjtNum* fromto = d->sensordata + m->sensor_adr[i];
// null output: nothing to render
if (mju_isZero(fromto, 6)) {
continue;
}
// make ray
addConnector(scn, mjGEOM_LINE, 3, fromto, fromto+3, m->vis.rgba.rangefinder,
i, mjCAT_DECOR, mjOBJ_SENSOR);
}
}
}
static void addExternalPerturbGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
mjvScene* scn) {
if (!vopt->flags[mjVIS_PERTFORCE]) {
return;
}
const float scl = m->stat.meansize;
for (int i=1; i < m->nbody; i++) {
if (mju_isZero(d->xfrc_applied+6*i, 6)) {
continue;
}
// force perturbation
mjtNum* xfrc = d->xfrc_applied+6*i;
if (mju_norm3(xfrc) <= mjMINVAL) {
continue;
}
mjtNum* from = d->xipos+3*i;
// map force to spatial vector in world frame
mjtNum vec[3];
mju_scl3(vec, xfrc, m->vis.map.force/m->stat.meanmass);
mjtNum to[3];
mju_add3(to, from, vec);
addConnector(scn, mjGEOM_ARROW, m->vis.scale.forcewidth * scl, from, to,
m->vis.rgba.force, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
}
}
static void addConstraintGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjvScene* scn) {
if (!vopt->flags[mjVIS_CONSTRAINT]) {
return;
}
// connect or weld
const float scl = m->stat.meansize;
for (int i=0; i < m->neq; i++) {
int is_weld = m->eq_type[i] == mjEQ_WELD;
int is_connect = m->eq_type[i] == mjEQ_CONNECT;
if (d->eq_active[i] && (is_connect || is_weld)) {
// compute endpoints in global coordinates
mjtNum vec[3], end[3];
mjtNum *xmat_j, *xmat_k;
int j = m->eq_obj1id[i], k = m->eq_obj2id[i];
if (m->eq_objtype[i] == mjOBJ_SITE) {
mju_copy3(vec, d->site_xpos+3*j);
mju_copy3(end, d->site_xpos+3*k);
xmat_j = d->site_xmat+9*j;
xmat_k = d->site_xmat+9*k;
} else {
mju_mulMatVec3(vec, d->xmat+9*j, m->eq_data+mjNEQDATA*i+3*is_weld);
mju_addTo3(vec, d->xpos+3*j);
mju_mulMatVec3(end, d->xmat+9*k, m->eq_data+mjNEQDATA*i+3*is_connect);
mju_addTo3(end, d->xpos+3*k);
xmat_j = d->xmat+9*j;
xmat_k = d->xmat+9*k;
}
// construct geom
mjtNum sz[3];
sz[0] = scl * m->vis.scale.constraint;
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_EQUALITY);
if (!thisgeom) {
return;
}
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);
thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_EQUALITY);
if (!thisgeom) {
return;
}
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);
}
releaseGeom(&thisgeom, scn);
}
}
}
// add abstract geoms
void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
const mjvPerturb* pert, int catmask, mjvScene* scn) {
// make default pert if missing
mjvPerturb localpert;
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);
}
addFlexGeoms(m, d, vopt, pert, catmask, scn);
addSkinGeoms(m, d, vopt, pert, catmask, scn);
addGeomGeoms(m, d, vopt, pert, catmask, scn);
addSiteGeoms(m, d, vopt, pert, catmask, scn);
addSpatialTendonGeoms(m, d, vopt, catmask, scn);
addSliderCrankGeoms(m, d, vopt, catmask, scn);
// remaining functions only add decor elements
if (!(catmask & mjCAT_DECOR)) {
return;
}
addGeomFrameGeoms(m, d, vopt, catmask, scn);
addSiteFrameGeoms(m, d, vopt, catmask, scn);
addBodyBvhGeoms(m, d, vopt, scn);
addFlexBvhGeoms(m, d, vopt, scn);
addMeshBvhGeoms(m, d, vopt, scn);
addMeshOctreeGeoms(m, d, vopt, scn);
addTactileSensorGeoms(m, d, vopt, scn);
addInertiaGeoms(m, d, vopt, pert, catmask, scn);
addPerturbGeoms(m, d, vopt, pert, scn);
addWorldBodyFrameGeoms(m, d, vopt, catmask, scn);
addSelectionPointGeoms(m, d, vopt, pert, scn);
addBodyLabelGeoms(m, d, vopt, pert, catmask, scn);
addJointGeoms(m, d, vopt, scn);
addActuatorGeoms(m, d, vopt, scn);
addIslandLabelGeoms(m, d, vopt, scn);
addCameraGeoms(m, d, vopt, scn);
addLightGeoms(m, d, vopt, scn);
addCenterOfMassGeoms(m, d, vopt, scn);
addAutoConnectGeoms(m, d, vopt, scn);
addRangefinderGeoms(m, d, vopt, scn);
addExternalPerturbGeoms(m, d, vopt, scn);
addConstraintGeoms(m, d, vopt, scn);
addContactGeoms(m, d, vopt, scn, catmask);
}
// make list of lights only
void mjv_makeLights(const mjModel* m, const 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->id = -1;
thislight->headlight = 1;
thislight->texid = -1;
thislight->type = mjLIGHT_DIRECTIONAL;
thislight->castshadow = 0;
thislight->bulbradius = 0.02;
thislight->intensity = 0;
thislight->range = 10;
// compute head position and gaze direction in model space
mjtNum hpos[3], hfwd[3];
mjv_cameraInModel(hpos, hfwd, NULL, scn);
mju_n2f(thislight->pos, hpos, 3);
mju_n2f(thislight->dir, hfwd, 3);
// 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->id = i;
thislight->type = m->light_type[i];
thislight->texid = m->light_texid[i];
thislight->castshadow = m->light_castshadow[i];
thislight->bulbradius = m->light_bulbradius[i];
thislight->intensity = m->light_intensity[i];
thislight->range = m->light_range[i];
if (thislight->type == mjLIGHT_SPOT) {
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, const mjData* d, mjvCamera* cam, mjvScene* scn) {
// return if nothing to do
if (!m || !cam || cam->type == mjCAMERA_USER) {
return;
}
// 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");
}
mju_copy3(cam->lookat, d->subtree_com + 3*bid);
}
// get camera frame
mjtNum headpos[3], forward[3], up[3], right[3];
mjv_cameraFrame(headpos, forward, up, right, d, cam);
// get camera frustum
float zver[2], zhor[2], zclip[2] = {0, 0};
mjv_cameraFrustum(zver, zhor, zclip, m, cam);
// get ipd, orthographic
int cid, orthographic = 0;
mjtNum ipd;
switch (cam->type) {
case mjCAMERA_FREE:
case mjCAMERA_TRACKING:
ipd = m->vis.global.ipd;
orthographic = m->vis.global.orthographic;
break;
case mjCAMERA_FIXED:
// get id, check range
cid = cam->fixedcamid;
if (cid < 0 || cid >= m->ncam) {
mjERROR("fixed camera id is outside valid range");
}
ipd = m->cam_ipd[cid];
orthographic = m->cam_projection[cid] == mjPROJ_ORTHOGRAPHIC;
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 orthographic
scn->camera[view].orthographic = orthographic;
// set symmetric frustum using intrinsic camera matrix
scn->camera[view].frustum_top = zver[0];
scn->camera[view].frustum_bottom = -zver[1];
scn->camera[view].frustum_center = (zhor[1] - zhor[0]) / 2;
scn->camera[view].frustum_width = (zhor[1] + zhor[0]) / 2;
scn->camera[view].frustum_near = zclip[0];
scn->camera[view].frustum_far = zclip[1];
}
// disable model transformation (do not clear float data; user may need it later)
scn->enabletransform = 0;
}
// construct face, flat normals
static void makeFace(float* _face, float* _normal, mjtNum radius, const mjtNum* vertxpos,
int nface, int i0, int i1, int i2) {
float* face = _face + 9*nface;
float* normal = _normal + 9*nface;
const mjtNum* v0 = vertxpos + 3*i0;
const mjtNum* v1 = vertxpos + 3*i1;
const mjtNum* v2 = vertxpos + 3*i2;
// compute normal
mjtNum v01[3] = {v1[0]-v0[0], v1[1]-v0[1], v1[2]-v0[2]};
mjtNum v02[3] = {v2[0]-v0[0], v2[1]-v0[1], v2[2]-v0[2]};
mjtNum nrm[3];
mju_cross(nrm, v01, v02);
mju_normalize3(nrm);
// set vertices: offset by radius*normal
mjtNum temp[3];
mju_addScl3(temp, v0, nrm, radius);
mju_n2f(face, temp, 3);
mju_addScl3(temp, v1, nrm, radius);
mju_n2f(face+3, temp, 3);
mju_addScl3(temp, v2, nrm, radius);
mju_n2f(face+6, temp, 3);
// set normals
mju_n2f(normal, nrm, 3);
mju_n2f(normal+3, nrm, 3);
mju_n2f(normal+6, nrm, 3);
}
// add face normal to vertices
static void addNormal(mjtNum* vertnorm, const mjtNum* vertxpos,
int i0, int i1, int i2) {
// compute normal*area
const mjtNum* v0 = vertxpos + 3*i0;
const mjtNum* v1 = vertxpos + 3*i1;
const mjtNum* v2 = vertxpos + 3*i2;
mjtNum v01[3] = {v1[0]-v0[0], v1[1]-v0[1], v1[2]-v0[2]};
mjtNum v02[3] = {v2[0]-v0[0], v2[1]-v0[1], v2[2]-v0[2]};
mjtNum nrm[3];
mju_cross(nrm, v01, v02);
mju_normalize3(nrm);
// accumulate at each vertex
mju_addTo3(vertnorm + 3*i0, nrm);
mju_addTo3(vertnorm + 3*i1, nrm);
mju_addTo3(vertnorm + 3*i2, nrm);
}
// construct face, smooth normals
static void makeSmooth(float* _face, float* _normal, mjtNum radius, mjtByte flg_flat,
const mjtNum* vertnorm, const mjtNum* vertxpos,
int nface, int i0, int i1, int i2) {
float* face = _face + 9*nface;
float* normal = _normal + 9*nface;
int ind[3] = {i0, i1, i2};
int sign = radius > 0 ? 1 : -1;
// flat shading
if (flg_flat) {
// compute face normal
const mjtNum* v0 = vertxpos + 3*i0;
const mjtNum* v1 = vertxpos + 3*i1;
const mjtNum* v2 = vertxpos + 3*i2;
mjtNum v01[3] = {v1[0]-v0[0], v1[1]-v0[1], v1[2]-v0[2]};
mjtNum v02[3] = {v2[0]-v0[0], v2[1]-v0[1], v2[2]-v0[2]};
mjtNum nrm[3];
mju_cross(nrm, v01, v02);
mju_normalize3(nrm);
// set all vertex normals equal to face normal
for (int k=0; k < 3; k++){
normal[3*k+0] = (float) (sign*nrm[0]);
normal[3*k+1] = (float) (sign*nrm[1]);
normal[3*k+2] = (float) (sign*nrm[2]);
}
}
// smooth shading
else {
for (int k=0; k < 3; k++){
normal[3*k+0] = (float) (sign*vertnorm[3*ind[k]+0]);
normal[3*k+1] = (float) (sign*vertnorm[3*ind[k]+1]);
normal[3*k+2] = (float) (sign*vertnorm[3*ind[k]+2]);
}
}
// set positions: vertices offset by radius*normal
for (int k=0; k < 3; k++){
face[3*k+0] = (float) (vertxpos[3*ind[k]+0] + radius*vertnorm[3*ind[k]+0]);
face[3*k+1] = (float) (vertxpos[3*ind[k]+1] + radius*vertnorm[3*ind[k]+1]);
face[3*k+2] = (float) (vertxpos[3*ind[k]+2] + radius*vertnorm[3*ind[k]+2]);
}
}
// construct side in 2D face
static void makeSide(float* _face, float* _normal, mjtNum radius,
const mjtNum* vertnorm, const mjtNum* vertxpos,
int nface, int i0, int i1) {
float* face = _face + 9*nface;
float* normal = _normal + 9*nface;
// compute normal
const mjtNum* v0 = vertxpos + 3*i0;
const mjtNum* v1 = vertxpos + 3*i1;
mjtNum v01[3] = {v1[0]-v0[0], v1[1]-v0[1], v1[2]-v0[2]};
mjtNum nrm[3];
mju_cross(nrm, v01, vertnorm+3*i1);
if (radius < 0) {
mju_scl3(nrm, nrm, -1);
}
mju_normalize3(nrm);
// set normals
for (int k=0; k < 3; k++){
normal[3*k+0] = (float) nrm[0];
normal[3*k+1] = (float) nrm[1];
normal[3*k+2] = (float) nrm[2];
}
// set positions
int ind[3] = {i0, i1, i1};
for (int k=0; k < 3; k++){
mjtNum sign = (k == 1 ? -1 : +1);
face[3*k+0] = (float) (vertxpos[3*ind[k]+0] + sign*radius*vertnorm[3*ind[k]+0]);
face[3*k+1] = (float) (vertxpos[3*ind[k]+1] + sign*radius*vertnorm[3*ind[k]+1]);
face[3*k+2] = (float) (vertxpos[3*ind[k]+2] + sign*radius*vertnorm[3*ind[k]+2]);
}
}
// copy texcoord for face
static void copyTex(float* dst, const float* src, int nface, int i0, int i1, int i2) {
if (!dst || !src) {
return;
}
dst[6*nface+0] = src[2*i0];
dst[6*nface+1] = src[2*i0+1];
dst[6*nface+2] = src[2*i1];
dst[6*nface+3] = src[2*i1+1];
dst[6*nface+4] = src[2*i2];
dst[6*nface+5] = src[2*i2+1];
}
// update visible flexes only
void mjv_updateActiveFlex(const mjModel* m, mjData* d, mjvScene* scn, const mjvOption* opt) {
// save flex visualization flags in scene (needed by renderer)
scn->flexvertopt = opt->flags[mjVIS_FLEXVERT];
scn->flexedgeopt = opt->flags[mjVIS_FLEXEDGE];
scn->flexfaceopt = opt->flags[mjVIS_FLEXFACE];
scn->flexskinopt = opt->flags[mjVIS_FLEXSKIN];
// convert vertex positions from mjtNum to float
for (int v=0; v < 3*m->nflexvert; v++) {
scn->flexvert[v] = (float) d->flexvert_xpos[v];
}
// construct faces
for (int f=0; f < m->nflex; f++) {
int dim = m->flex_dim[f];
mjtNum radius = m->flex_radius[f];
mjtByte flg_flat = m->flex_flatskin[f];
const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f];
float* face = scn->flexface + 9*scn->flexfaceadr[f];
float* normal = scn->flexnormal + 9*scn->flexfaceadr[f];
float* texdst = m->flex_texcoordadr[f] >= 0 ?
scn->flextexcoord + 6*scn->flexfaceadr[f] : NULL;
const float* texsrc = m->flex_texcoordadr[f] >= 0 ?
m->flex_texcoord + 2*m->flex_texcoordadr[f] : NULL;
// 1D, or face and skin disabled: no faces
if (dim == 1 || (!opt->flags[mjVIS_FLEXFACE] && !opt->flags[mjVIS_FLEXSKIN])) {
scn->flexfaceused[f] = 0;
}
// 2D or 3D face: faces from elements, flat normals, texture
else if (!opt->flags[mjVIS_FLEXSKIN]) {
int nface = 0;
for (int e=0; e < m->flex_elemnum[f]; e++) {
// in 3D, show only elements in selected layer
if (dim == 2 || m->flex_elemlayer[m->flex_elemadr[f]+e] == opt->flex_layer) {
// get element data
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
const int* tdata = m->flex_elemtexcoord + m->flex_elemdataadr[f] + e*(dim+1);
// triangles: two faces per element
if (dim == 2) {
makeFace(face, normal, radius, vertxpos, nface, edata[0], edata[1], edata[2]);
copyTex(texdst, texsrc, nface, tdata[0], tdata[1], tdata[2]);
nface++;
makeFace(face, normal, radius, vertxpos, nface, edata[0], edata[2], edata[1]);
copyTex(texdst, texsrc, nface, tdata[0], tdata[2], tdata[1]);
nface++;
}
// tetrahedra: four faces per element
else {
makeFace(face, normal, radius, vertxpos,
nface, edata[0], edata[1], edata[2]);
copyTex(texdst, texsrc, nface, tdata[0], tdata[1], tdata[2]);
nface++;
makeFace(face, normal, radius, vertxpos,
nface, edata[0], edata[2], edata[3]);
copyTex(texdst, texsrc, nface, tdata[0], tdata[2], tdata[3]);
nface++;
makeFace(face, normal, radius, vertxpos,
nface, edata[0], edata[3], edata[1]);
copyTex(texdst, texsrc, nface, tdata[0], tdata[3], tdata[1]);
nface++;
makeFace(face, normal, radius, vertxpos,
nface, edata[1], edata[3], edata[2]);
copyTex(texdst, texsrc, nface, tdata[1], tdata[3], tdata[2]);
nface++;
}
}
}
// save face count
scn->flexfaceused[f] = nface;
}
// 2D or 3D skin: faces from elements (2D) or shells (3D), smooth normals, texture
else {
// allocate and clear vertex normals for smoothing
mj_markStack(d);
mjtNum* vertnorm = mjSTACKALLOC(d, 3*m->flex_vertnum[f], mjtNum);
mju_zero(vertnorm, 3*m->flex_vertnum[f]);
// add vertex normals: top element sides in 2D, shell fragments in 3D
if (dim == 2) {
for (int e=0; e < m->flex_elemnum[f]; e++) {
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
addNormal(vertnorm, vertxpos, edata[0], edata[1], edata[2]);
}
} else {
for (int s=0; s < m->flex_shellnum[f]; s++) {
const int* sdata = m->flex_shell + m->flex_shelldataadr[f] + s*dim;
addNormal(vertnorm, vertxpos, sdata[0], sdata[1], sdata[2]);
}
}
// normalize vertex normals
for (int i=0; i < m->flex_vertnum[f]; i++) {
mju_normalize3(vertnorm+3*i);
}
// create faces, offset along smoothed vertex normals, and texcoord
int nface = 0;
if (dim == 2) {
for (int e=0; e < m->flex_elemnum[f]; e++) {
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
const int* tdata = m->flex_elemtexcoord + m->flex_elemdataadr[f] + e*(dim+1);
makeSmooth(face, normal, radius, flg_flat, vertnorm, vertxpos,
nface, edata[0], edata[1], edata[2]);
copyTex(texdst, texsrc, nface, tdata[0], tdata[1], tdata[2]);
nface++;
makeSmooth(face, normal, -radius, flg_flat, vertnorm, vertxpos,
nface, edata[0], edata[2], edata[1]);
copyTex(texdst, texsrc, nface, tdata[0], tdata[2], tdata[1]);
nface++;
}
} else {
for (int s=0; s < m->flex_shellnum[f]; s++) {
const int* sdata = m->flex_shell + m->flex_shelldataadr[f] + s*dim;
makeSmooth(face, normal, radius, flg_flat, vertnorm, vertxpos,
nface, sdata[0], sdata[1], sdata[2]);
copyTex(texdst, texsrc, nface, sdata[0], sdata[1], sdata[2]);
nface++;
}
}
// 2D: close sides using shell fragments
if (dim == 2) {
for (int s=0; s < m->flex_shellnum[f]; s++) {
const int* sdata = m->flex_shell + m->flex_shelldataadr[f] + s*dim;
makeSide(face, normal, radius, vertnorm, vertxpos,
nface, sdata[0], sdata[1]);
copyTex(texdst, texsrc, nface, sdata[0], sdata[1], sdata[1]);
nface++;
makeSide(face, normal, -radius, vertnorm, vertxpos,
nface, sdata[1], sdata[0]);
copyTex(texdst, texsrc, nface, sdata[1], sdata[0], sdata[0]);
nface++;
}
}
// save face count
scn->flexfaceused[f] = nface;
mj_freeStack(d);
}
// check face count, SHOULD NOT OCCUR
if (scn->flexfaceused[f] > scn->flexfacenum[f]) {
mju_error("too many flex faces in mjv_updateActiveFlex");
}
}
}
// update all skins, here for backward API compatibility
void mjv_updateSkin(const mjModel* m, const 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, const 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));
// update only if visible
if (opt->skingroup[mjMAX(0, mjMIN(mjNGROUP-1, 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_mulMatVec3(translate, rotate, bindpos);
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_mulMatVec3(pos1, rotate, pos);
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+0]*scn->skinnormal[3*k+0] +
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);
// update camera
mjv_updateCamera(m, d, cam, scn);
// add lights
mjv_makeLights(m, d, scn);
// update flexes
if (opt->flags[mjVIS_FLEXVERT] || opt->flags[mjVIS_FLEXEDGE] ||
opt->flags[mjVIS_FLEXFACE] || opt->flags[mjVIS_FLEXSKIN]) {
mjv_updateActiveFlex(m, d, scn, opt);
}
// 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 transcendental 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, int ncatenary) {
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 full catenary: ncatenary 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 < ncatenary-1; i++) {
// linearly spaced horizontal offset
mjtNum horizontal = i*h/ncatenary;
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*(ncatenary-1), x1);
return ncatenary;
}
}
return 0; // SHOULD NOT OCCUR
}
Reference in New Issue View Git Blame Copy Permalink