Yuval Tassa
4c066a26b5
Also remove unused ampersands from `mjVISSTRING` names, these are a remnant from a very old version of the UI framework. PiperOrigin-RevId: 593469811 Change-Id: I0f6f73c9f5d2c61b2280f395f76434b0710ce4f3
484 lines
12 KiB
C
484 lines
12 KiB
C
// 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_init.h"
|
|
|
|
#include <math.h>
|
|
#include <string.h>
|
|
|
|
#include <mujoco/mjmacro.h>
|
|
#include <mujoco/mjmodel.h>
|
|
#include <mujoco/mjvisualize.h>
|
|
#include "engine/engine_array_safety.h"
|
|
#include "engine/engine_util_errmem.h"
|
|
#include "engine/engine_util_misc.h"
|
|
|
|
#ifdef _MSC_VER
|
|
#pragma warning (disable: 4305) // disable MSVC warning: truncation from 'double' to 'float'
|
|
#endif
|
|
|
|
|
|
//--------------------------------- Strings --------------------------------------------------------
|
|
|
|
// label names
|
|
const char* mjLABELSTRING[mjNLABEL] = {
|
|
"None",
|
|
"Body",
|
|
"Joint",
|
|
"Geom",
|
|
"Site",
|
|
"Camera",
|
|
"Light",
|
|
"Tendon",
|
|
"Actuator",
|
|
"Constraint",
|
|
"Flex",
|
|
"Skin",
|
|
"Selection",
|
|
"SelPoint",
|
|
"Contact",
|
|
"ContactForce",
|
|
"Island"
|
|
};
|
|
|
|
|
|
// frame names
|
|
const char* mjFRAMESTRING[mjNFRAME] = {
|
|
"None",
|
|
"Body",
|
|
"Geom",
|
|
"Site",
|
|
"Camera",
|
|
"Light",
|
|
"Contact",
|
|
"World"
|
|
};
|
|
|
|
|
|
// visual options: {name, initial value, shortcut}
|
|
const char* mjVISSTRING[mjNVISFLAG][3] = {
|
|
{"Convex Hull", "0", "H"},
|
|
{"Texture", "1", "X"},
|
|
{"Joint", "0", "J"},
|
|
{"Camera", "0", "Q"},
|
|
{"Actuator", "0", "U"},
|
|
{"Activation", "0", ","},
|
|
{"Light", "0", "Z"},
|
|
{"Tendon", "1", "V"},
|
|
{"Range Finder", "1", "Y"},
|
|
{"Equality", "0", "E"},
|
|
{"Inertia", "0", "I"},
|
|
{"Scale Inertia", "0", "'"},
|
|
{"Perturb Force", "0", "B"},
|
|
{"Perturb Object", "1", "O"},
|
|
{"Contact Point", "0", "C"},
|
|
{"Island", "1", ""}, // TODO(b/295296178): turn off after islands are on by default.
|
|
{"Contact Force", "0", "F"},
|
|
{"Contact Split", "0", "P"},
|
|
{"Transparent", "0", "T"},
|
|
{"Auto Connect", "0", "A"},
|
|
{"Center of Mass", "0", "M"},
|
|
{"Select Point", "0", ""},
|
|
{"Static Body", "1", "D"},
|
|
{"Skin", "1", ";"},
|
|
{"Flex Vert", "0", ""},
|
|
{"Flex Edge", "0", ""},
|
|
{"Flex Face", "0", ""},
|
|
{"Flex Skin", "1", ""},
|
|
{"Body Tree", "0", "`"},
|
|
{"Flex Tree", "0", ""},
|
|
{"Mesh Tree", "0", "\\"},
|
|
{"SDF iters", "0", ""}
|
|
};
|
|
|
|
|
|
// render options: {name, initial value, shortcut}
|
|
const char* mjRNDSTRING[mjNRNDFLAG][3] = {
|
|
{"Shadow", "1", "S"},
|
|
{"Wireframe", "0", "W"},
|
|
{"Reflection", "1", "R"},
|
|
{"Additive", "0", "L"},
|
|
{"Skybox", "1", "K"},
|
|
{"Fog", "0", "G"},
|
|
{"Haze", "1", "/"},
|
|
{"Segment", "0", ","},
|
|
{"Id Color", "0", ""},
|
|
{"Cull Face", "1", ""}
|
|
};
|
|
|
|
|
|
|
|
//--------------------------------- Implementation -------------------------------------------------
|
|
|
|
|
|
// allocate and init abstract scene
|
|
void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) {
|
|
// free previous
|
|
mjv_freeScene(scn);
|
|
|
|
// allocate geom buffers
|
|
if (maxgeom > 0) {
|
|
// allocate
|
|
scn->maxgeom = maxgeom;
|
|
scn->geoms = (mjvGeom*) mju_malloc(maxgeom*sizeof(mjvGeom));
|
|
scn->geomorder = (int*) mju_malloc(maxgeom*sizeof(int));
|
|
|
|
// check allocation
|
|
if (!scn->geoms || !scn->geomorder) {
|
|
mjERROR("could not allocate geom buffers");
|
|
}
|
|
}
|
|
|
|
// set default OpenGL options
|
|
for (int i=0; i < mjNRNDFLAG; i++) {
|
|
scn->flags[i] = (mjRNDSTRING[i][1][0] == '1');
|
|
}
|
|
|
|
// set default model transformation
|
|
scn->scale = 1;
|
|
scn->rotate[0] = 1;
|
|
|
|
// set number of flexes
|
|
scn->nflex = m ? m->nflex : 0;
|
|
|
|
// allocate flex data
|
|
if (scn->nflex) {
|
|
int nflex = scn->nflex;
|
|
|
|
// allocate fixed
|
|
scn->flexedgeadr = (int*) mju_malloc(nflex*sizeof(int));
|
|
scn->flexedgenum = (int*) mju_malloc(nflex*sizeof(int));
|
|
scn->flexvertadr = (int*) mju_malloc(nflex*sizeof(int));
|
|
scn->flexvertnum = (int*) mju_malloc(nflex*sizeof(int));
|
|
scn->flexfaceadr = (int*) mju_malloc(nflex*sizeof(int));
|
|
scn->flexfacenum = (int*) mju_malloc(nflex*sizeof(int));
|
|
scn->flexfaceused= (int*) mju_malloc(nflex*sizeof(int));
|
|
scn->flexedge = (int*) mju_malloc(2*m->nflexedge*sizeof(int));
|
|
scn->flexvert = (float*) mju_malloc(3*m->nflexvert*sizeof(float));
|
|
|
|
// count max number of flex faces to be rendered (depending on vis options)
|
|
int nface = 0;
|
|
for (int f=0; f < nflex; f++) {
|
|
// 1D : 0
|
|
if (m->flex_dim[f] == 0) {
|
|
scn->flexfacenum[f] = 0;
|
|
}
|
|
|
|
// 2D: 2*fragments + 2*elements
|
|
else if (m->flex_dim[f] == 2) {
|
|
scn->flexfacenum[f] = 2*m->flex_shellnum[f] + 2*m->flex_elemnum[f];
|
|
}
|
|
|
|
// 3D: max(fragments, 4*maxlayer)
|
|
else {
|
|
// find number of elements in biggest layer
|
|
int maxlayer = 0, layer = 0, nlayer = 1;
|
|
while (nlayer) {
|
|
// count elements in this layer
|
|
nlayer = 0;
|
|
for (int e=0; e < m->flex_elemnum[f]; e++) {
|
|
if (m->flex_elemlayer[m->flex_elemadr[f]+e] == layer) {
|
|
nlayer++;
|
|
}
|
|
}
|
|
|
|
// accumulate max over layers, advance layer
|
|
maxlayer = mjMAX(maxlayer, nlayer);
|
|
layer++;
|
|
}
|
|
|
|
scn->flexfacenum[f] = mjMAX(m->flex_shellnum[f], 4*maxlayer);
|
|
}
|
|
|
|
// accumulate over flexes
|
|
nface += scn->flexfacenum[f];
|
|
}
|
|
|
|
// allocate face-related
|
|
scn->flexface = nface ? (float*) mju_malloc(9*nface*sizeof(float)) : NULL;
|
|
scn->flexnormal = nface ? (float*) mju_malloc(9*nface*sizeof(float)) : NULL;
|
|
scn->flextexcoord = nface ? (float*) mju_malloc(6*nface*sizeof(float)) : NULL;
|
|
|
|
// check allocation
|
|
if (!scn->flexedgeadr ||
|
|
!scn->flexedgenum ||
|
|
!scn->flexfaceadr ||
|
|
!scn->flexfacenum ||
|
|
!scn->flexfaceused||
|
|
!scn->flexvertadr ||
|
|
!scn->flexvertnum ||
|
|
!scn->flexedge ||
|
|
!scn->flexvert ||
|
|
(nface && !scn->flexface) ||
|
|
(nface && !scn->flexnormal) ||
|
|
(nface && !scn->flextexcoord)) {
|
|
mju_error("Could not allocate flex buffers");
|
|
}
|
|
|
|
// copy constant edge and vertex data
|
|
memcpy(scn->flexedgeadr, m->flex_edgeadr, nflex*sizeof(int));
|
|
memcpy(scn->flexedgenum, m->flex_edgenum, nflex*sizeof(int));
|
|
memcpy(scn->flexvertadr, m->flex_vertadr, nflex*sizeof(int));
|
|
memcpy(scn->flexvertnum, m->flex_vertnum, nflex*sizeof(int));
|
|
memcpy(scn->flexedge, m->flex_edge, 2*m->nflexedge*sizeof(int));
|
|
|
|
// compute flexfaceadr
|
|
for (int f=0; f < nflex; f++) {
|
|
scn->flexfaceadr[f] = f == 0 ? 0 : scn->flexfaceadr[f-1]+scn->flexfacenum[f-1];
|
|
}
|
|
}
|
|
|
|
// set number of skins
|
|
if (m) {
|
|
scn->nskin = m->nskin;
|
|
} else {
|
|
scn->nskin = 0;
|
|
}
|
|
|
|
// allocate skin data
|
|
if (scn->nskin) {
|
|
int nskin = m->nskin;
|
|
|
|
// allocate
|
|
scn->skinfacenum = (int*) mju_malloc(nskin*sizeof(int));
|
|
scn->skinvertadr = (int*) mju_malloc(nskin*sizeof(int));
|
|
scn->skinvertnum = (int*) mju_malloc(nskin*sizeof(int));
|
|
scn->skinvert = (float*) mju_malloc(3*m->nskinvert*sizeof(float));
|
|
scn->skinnormal = (float*) mju_malloc(3*m->nskinvert*sizeof(float));
|
|
|
|
// check allocation
|
|
if (!scn->skinfacenum ||
|
|
!scn->skinvertadr ||
|
|
!scn->skinvertnum ||
|
|
!scn->skinvert ||
|
|
!scn->skinnormal) {
|
|
mjERROR("could not allocate skin buffers");
|
|
}
|
|
|
|
// copy constant data
|
|
mju_copyInt(scn->skinfacenum, m->skin_facenum, nskin);
|
|
mju_copyInt(scn->skinvertadr, m->skin_vertadr, nskin);
|
|
mju_copyInt(scn->skinvertnum, m->skin_vertnum, nskin);
|
|
}
|
|
|
|
// mjvGeom, mjvLight, mjvGLCamera objects are invalid
|
|
}
|
|
|
|
|
|
|
|
// free abstract scene
|
|
void mjv_freeScene(mjvScene* scn) {
|
|
// free buffers allocated by mjv_makeScene
|
|
mju_free(scn->geoms);
|
|
mju_free(scn->geomorder);
|
|
|
|
mju_free(scn->flexedgeadr);
|
|
mju_free(scn->flexedgenum);
|
|
mju_free(scn->flexvertadr);
|
|
mju_free(scn->flexvertnum);
|
|
mju_free(scn->flexfaceadr);
|
|
mju_free(scn->flexfacenum);
|
|
mju_free(scn->flexfaceused);
|
|
mju_free(scn->flexedge);
|
|
mju_free(scn->flexvert);
|
|
mju_free(scn->flexface);
|
|
mju_free(scn->flexnormal);
|
|
mju_free(scn->flextexcoord);
|
|
|
|
mju_free(scn->skinfacenum);
|
|
mju_free(scn->skinvertadr);
|
|
mju_free(scn->skinvertnum);
|
|
mju_free(scn->skinvert);
|
|
mju_free(scn->skinnormal);
|
|
|
|
// clear data structure
|
|
mjv_defaultScene(scn);
|
|
}
|
|
|
|
|
|
|
|
// set default scene
|
|
void mjv_defaultScene(mjvScene* scn) {
|
|
memset(scn, 0, sizeof(mjvScene));
|
|
}
|
|
|
|
|
|
|
|
// set default visualization options
|
|
void mjv_defaultOption(mjvOption* vopt) {
|
|
vopt->label = mjLABEL_NONE;
|
|
vopt->frame = mjFRAME_NONE;
|
|
|
|
for (int i=0; i < mjNGROUP; i++) {
|
|
int state = (i < 3 ? 1 : 0);
|
|
vopt->geomgroup[i] = state;
|
|
vopt->sitegroup[i] = state;
|
|
vopt->jointgroup[i] = state;
|
|
vopt->tendongroup[i] = state;
|
|
vopt->actuatorgroup[i] = state;
|
|
vopt->flexgroup[i] = state;
|
|
vopt->skingroup[i] = state;
|
|
}
|
|
|
|
for (int i=0; i < mjNVISFLAG; i++) {
|
|
vopt->flags[i] = (mjVISSTRING[i][1][0] == '1');
|
|
}
|
|
|
|
vopt->bvh_depth = 1;
|
|
vopt->flex_layer = 0;
|
|
}
|
|
|
|
|
|
|
|
// set default camera
|
|
void mjv_defaultCamera(mjvCamera* cam) {
|
|
memset(cam, 0, sizeof(mjvCamera));
|
|
|
|
cam->type = mjCAMERA_FREE;
|
|
cam->fixedcamid = -1;
|
|
cam->trackbodyid = -1;
|
|
cam->distance = 2;
|
|
cam->azimuth = 90;
|
|
cam->elevation = -45;
|
|
}
|
|
|
|
|
|
|
|
// set default free camera
|
|
void mjv_defaultFreeCamera(const mjModel* m, mjvCamera* cam) {
|
|
memset(cam, 0, sizeof(mjvCamera));
|
|
|
|
cam->type = mjCAMERA_FREE;
|
|
cam->fixedcamid = -1;
|
|
cam->trackbodyid = -1;
|
|
cam->lookat[0] = m->stat.center[0];
|
|
cam->lookat[1] = m->stat.center[1];
|
|
cam->lookat[2] = m->stat.center[2];
|
|
cam->distance = 1.5 * m->stat.extent;
|
|
cam->azimuth = m->vis.global.azimuth;
|
|
cam->elevation = m->vis.global.elevation;
|
|
}
|
|
|
|
|
|
|
|
// set default perturbation
|
|
void mjv_defaultPerturb(mjvPerturb* pert) {
|
|
memset(pert, 0, sizeof(mjvPerturb));
|
|
|
|
pert->flexselect = -1;
|
|
pert->skinselect = -1;
|
|
pert->refquat[0] = 1;
|
|
pert->scale = 1;
|
|
}
|
|
|
|
|
|
|
|
// predefined line colors
|
|
static const float _linergb[8][3] = {
|
|
{1.0, 0.3, 0.3},
|
|
{0.1, 1.0, 0.1},
|
|
{0.3, 0.3, 1.0},
|
|
{0.1, 1.0, 1.0},
|
|
{1.0, 0.2, 1.0},
|
|
{1.0, 1.0, 0.1},
|
|
{1.0, 0.6, 0.2},
|
|
{0.6, 0.7, 0.4}
|
|
};
|
|
|
|
|
|
|
|
// set default figure
|
|
void mjv_defaultFigure(mjvFigure* fig) {
|
|
// set everything to zero
|
|
memset(fig, 0, sizeof(mjvFigure));
|
|
|
|
// disable highlight
|
|
fig->highlightid = -1;
|
|
|
|
// set enable flags
|
|
fig->flg_legend = 1;
|
|
fig->flg_ticklabel[0] = 1;
|
|
fig->flg_ticklabel[1] = 1;
|
|
fig->flg_extend = 1;
|
|
|
|
// set style
|
|
fig->linewidth = 3;
|
|
fig->gridwidth = 1;
|
|
fig->gridsize[0] = 2;
|
|
fig->gridsize[1] = 2;
|
|
fig->gridrgb[0] = 0.4f;
|
|
fig->gridrgb[1] = 0.4f;
|
|
fig->gridrgb[2] = 0.4f;
|
|
fig->figurergba[3] = 1;
|
|
fig->panergba[3] = 1;
|
|
fig->legendrgba[3] = 0.3f;
|
|
fig->textrgb[0] = 1;
|
|
fig->textrgb[1] = 1;
|
|
fig->textrgb[2] = 1;
|
|
fig->range[0][0] = 0;
|
|
fig->range[0][1] = 1;
|
|
fig->range[1][0] = 0;
|
|
fig->range[1][1] = 1;
|
|
mjSTRNCPY(fig->xformat, "%.0f");
|
|
mjSTRNCPY(fig->yformat, "%.2g");
|
|
mjSTRNCPY(fig->minwidth, "XXX");
|
|
|
|
// set line colors
|
|
for (int n=0; n < mjMAXLINE; n++) {
|
|
// predefined colors
|
|
if (n < 8) {
|
|
fig->linergb[n][0] = _linergb[n][0];
|
|
fig->linergb[n][1] = _linergb[n][1];
|
|
fig->linergb[n][2] = _linergb[n][2];
|
|
}
|
|
|
|
// automatically generated colors: Halton sequence
|
|
else {
|
|
fig->linergb[n][0] = 0.1f + 0.8f*mju_Halton(n, 2);
|
|
fig->linergb[n][1] = 0.1f + 0.8f*mju_Halton(n, 3);
|
|
fig->linergb[n][2] = 0.1f + 0.8f*mju_Halton(n, 5);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// compute rbound for mjvGeom
|
|
float mjv_rbound(const mjvGeom* geom) {
|
|
// model geom: return
|
|
if (geom->objtype == mjOBJ_GEOM) {
|
|
return geom->modelrbound;
|
|
}
|
|
|
|
// compute rbound according to type
|
|
const float* s = geom->size;
|
|
switch ((mjtMouse) geom->type) {
|
|
case mjGEOM_SPHERE:
|
|
return s[0];
|
|
|
|
case mjGEOM_CAPSULE:
|
|
return (s[0]+s[2]);
|
|
|
|
case mjGEOM_CYLINDER:
|
|
return sqrtf(s[0]*s[0] + s[2]*s[2]);
|
|
|
|
case mjGEOM_BOX:
|
|
return sqrtf(s[0]*s[0] + s[1]*s[1] + s[2]*s[2]);
|
|
break;
|
|
|
|
default: // not accurate for arrows, but they are not transparent
|
|
return mjMAX(s[0], mjMAX(s[1], s[2]));
|
|
}
|
|
}
|