Minor cleanup: Move geom-related functions together.
PiperOrigin-RevId: 813661319 Change-Id: I6cdf74ff57f39f932ddd39268f15f1128ddf1df4
This commit is contained in:
Haroon Qureshi
committed by
Copybara-Service
Copybara-Service
parent
328a4ab293
commit
f9d0bedb44
+369
-367
@@ -72,37 +72,6 @@ static void makeLabel(const mjModel* m, mjtObj type, int id, char* label) {
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
// 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) {
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// convert HSV to RGB
|
// convert HSV to RGB
|
||||||
static void hsv2rgb(float *RGB, float H, float S, float V) {
|
static void hsv2rgb(float *RGB, float H, float S, float V) {
|
||||||
float R, G, B;
|
float R, G, B;
|
||||||
@@ -150,6 +119,67 @@ static void islandColor(float rgba[4], int h) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// mix colors for perturbation object
|
||||||
|
static void mixcolor(float rgba[4], const float ref[4], int flg1, int flg2) {
|
||||||
|
rgba[0] = flg1 ? ref[0] : 0;
|
||||||
|
if (flg2) {
|
||||||
|
rgba[0] = mjMAX(rgba[0], ref[1]);
|
||||||
|
}
|
||||||
|
|
||||||
|
rgba[1] = flg1 ? ref[1] : 0;
|
||||||
|
if (flg2) {
|
||||||
|
rgba[1] = mjMAX(rgba[1], ref[0]);
|
||||||
|
}
|
||||||
|
|
||||||
|
rgba[2] = ref[2];
|
||||||
|
rgba[3] = ref[3];
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// a body is static if it is welded to the world and is not a mocap body
|
||||||
|
static int bodycategory(const mjModel* m, int bodyid) {
|
||||||
|
if (m->body_weldid[bodyid] == 0 && m->body_mocapid[bodyid] == -1) {
|
||||||
|
return mjCAT_STATIC;
|
||||||
|
} else {
|
||||||
|
return mjCAT_DYNAMIC;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
//----------------------------- 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) {
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// make a triangle in thisgeom at coordinates v0, v1, v2 with a given color
|
// make a triangle in thisgeom at coordinates v0, v1, v2 with a given color
|
||||||
static void makeTriangle(mjvGeom* thisgeom, const mjtNum v0[3], const mjtNum v1[3],
|
static void makeTriangle(mjvGeom* thisgeom, const mjtNum v0[3], const mjtNum v1[3],
|
||||||
@@ -167,6 +197,314 @@ static void makeTriangle(mjvGeom* thisgeom, const mjtNum v0[3], const mjtNum v1[
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// 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);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
//----------------------------- 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_orthographic[cid];
|
||||||
|
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
|
// add contact-related geoms in mjvObject
|
||||||
static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
|
static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
|
||||||
const mjvOption* vopt, mjvScene* scn) {
|
const mjvOption* vopt, mjvScene* scn) {
|
||||||
@@ -381,342 +719,6 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// 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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
//----------------------------- main API functions -------------------------------------------------
|
|
||||||
|
|
||||||
// set (type, size, pos, mat) connector-type geom between given points
|
|
||||||
// assume that mjv_initGeom was already called to set all other properties
|
|
||||||
void mjv_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);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// 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;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// mix colors for perturbation object
|
|
||||||
static void mixcolor(float rgba[4], const float ref[4], int flg1, int flg2) {
|
|
||||||
rgba[0] = flg1 ? ref[0] : 0;
|
|
||||||
if (flg2) {
|
|
||||||
rgba[0] = mjMAX(rgba[0], ref[1]);
|
|
||||||
}
|
|
||||||
|
|
||||||
rgba[1] = flg1 ? ref[1] : 0;
|
|
||||||
if (flg2) {
|
|
||||||
rgba[1] = mjMAX(rgba[1], ref[0]);
|
|
||||||
}
|
|
||||||
|
|
||||||
rgba[2] = ref[2];
|
|
||||||
rgba[3] = ref[3];
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// a body is static if it is welded to the world and is not a mocap body
|
|
||||||
static int bodycategory(const mjModel* m, int bodyid) {
|
|
||||||
if (m->body_weldid[bodyid] == 0 && m->body_mocapid[bodyid] == -1) {
|
|
||||||
return mjCAT_STATIC;
|
|
||||||
} else {
|
|
||||||
return mjCAT_DYNAMIC;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// 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_orthographic[cid];
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// add abstract geoms
|
// add abstract geoms
|
||||||
void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
|
void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
|
||||||
const mjvPerturb* pert, int catmask, mjvScene* scn) {
|
const mjvPerturb* pert, int catmask, mjvScene* scn) {
|
||||||
|
|||||||
Reference in New Issue
Block a user