chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add orthographic cameras.

Browse Source 
Orthographic cameras are specified by setting the `orthographic` attribute of the `<camera>` element. The `fovy` attribute is still used to specify the field-of-view, but its semantic is different for orthographic cameras. For orthographic cameras, the field-of-view is expressed in units of length, rather than degrees.

Other related changes:

* Fix bug in the ordering of `cam_xxx` elements in `mjModel`.
* Make camera visualization translucent only when the frustum is visualized.
* Added a button to `simulate` to toggle between perspective and orthographic free cameras.

https://youtu.be/ZXBTEIDWHhs

PiperOrigin-RevId: 642293435
Change-Id: Id090a421ad88ab404b5b27ddbbd6bfc81ad49bc5
This commit is contained in:
Yuval Tassa
2024-06-11 09:21:39 -07:00
committed by Copybara-Service
parent 171b0d6e06
commit 07fc95ca9a
23 changed files with 490 additions and 235 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_io.c
+2 -1
View File
@@ -182,6 +182,7 @@ static void setf4(float* rgba, float r, float g, float b, float a) {
// set visual options to default values
void mj_defaultVisual(mjVisual* vis) {
// global
vis->global.orthographic = 0;
vis->global.fovy = 45;
vis->global.ipd = 0.068;
vis->global.azimuth = 90;
@@ -257,7 +258,7 @@ void mj_defaultVisual(mjVisual* vis) {
setf4(vis->rgba.actuatornegative, .2, .6, .9, 1.);
setf4(vis->rgba.actuatorpositive, .9, .4, .2, 1.);
setf4(vis->rgba.com, .9, .9, .9, 1.);
setf4(vis->rgba.camera, .6, .9, .6, .3);
setf4(vis->rgba.camera, .6, .9, .6, 1);
setf4(vis->rgba.light, .6, .6, .9, 1.);
setf4(vis->rgba.selectpoint, .9, .9, .1, 1.);
setf4(vis->rgba.connect, .2, .2, .8, 1.);
src/engine/engine_vis_init.c
+10 -9
View File
@@ -359,15 +359,16 @@ void mjv_defaultCamera(mjvCamera* cam) {
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;
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;
cam->orthographic = m->vis.global.orthographic;
}
src/engine/engine_vis_interact.c
+53 -15
View File
@@ -223,16 +223,29 @@ void mjv_cameraInRoom(mjtNum* headpos, mjtNum* forward, mjtNum* up, const mjvSce
// get frustum height at unit distance from camera; average left and right OpenGL cameras
mjtNum mjv_frustumHeight(const mjvScene* scn) {
mjtNum height;
const mjvGLCamera* cam1 = scn->camera;
const mjvGLCamera* cam2 = scn->camera + 1;
// check znear
if (scn->camera[0].frustum_near < mjMINVAL || scn->camera[1].frustum_near < mjMINVAL) {
mjERROR("mjvScene frustum_near too small");
if (cam1->orthographic != cam2->orthographic) {
mjERROR("cannot average frustums of perspective and orthographic cameras");
}
// add normalized height for left and right cameras
height = (scn->camera[0].frustum_top-scn->camera[0].frustum_bottom)/scn->camera[0].frustum_near +
(scn->camera[1].frustum_top-scn->camera[1].frustum_bottom)/scn->camera[1].frustum_near;
// get height
mjtNum height;
if (!cam1->orthographic) {
// check znear
if (cam1->frustum_near < mjMINVAL || cam2->frustum_near < mjMINVAL) {
mjERROR("mjvScene frustum_near too small");
}
// add normalized height for left and right cameras
height = (cam1->frustum_top - cam1->frustum_bottom) / cam1->frustum_near +
(cam2->frustum_top - cam2->frustum_bottom) / cam2->frustum_near;
} else {
// add height for left and right cameras
height = (cam1->frustum_top - cam1->frustum_bottom) +
(cam2->frustum_top - cam2->frustum_bottom);
}
// average
return 0.5*height;
@@ -337,6 +350,9 @@ void mjv_moveCamera(const mjModel* m, int action, mjtNum reldx, mjtNum reldy,
mju_sub3(dif, cam->lookat, headpos);
scl = mjv_frustumHeight(scn) * mju_dot3(dif, forward);
// multiply by mystery coefficient TODO: b/346130949
if (cam->orthographic) scl *= 0.15;
// move lookat point in opposite direction
mju_addToScl3(cam->lookat, vec, -scl);
break;
@@ -563,6 +579,9 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur
mju_sub3(dif, pert->refselpos, headpos);
pert->scale = mjv_frustumHeight(scn) * mju_dot3(dif, forward);
// multiply by mystery coefficient TODO: b/346130949
if (scn->camera[0].orthographic) pert->scale *= 0.15;
mj_freeStack(d);
}
@@ -733,6 +752,12 @@ mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2)
cam.frustum_near = 0.5f * (cam1->frustum_near + cam2->frustum_near);
cam.frustum_far = 0.5f * (cam1->frustum_far + cam2->frustum_far);
if (cam1->orthographic != cam2->orthographic) {
mjERROR("cannot average perspective and orthographic cameras");
} else {
cam.orthographic = cam1->orthographic;
}
return cam;
}
@@ -755,17 +780,31 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
// compute frustum halfwidth so as to match viewport aspect ratio
mjtNum halfwidth = 0.5*aspectratio*(cam.frustum_top - cam.frustum_bottom);
// construct ray
// compute up and left offsets from normalized cursor
mjtNum d_up = cam.frustum_bottom + rely*(cam.frustum_top-cam.frustum_bottom);
mjtNum d_left = -(cam.frustum_center + (2*relx-1)*halfwidth);
// define ray
mjtNum ray[3];
mju_scl3(ray, forward, cam.frustum_near);
mju_addToScl3(ray, up, cam.frustum_bottom + rely*(cam.frustum_top-cam.frustum_bottom));
mju_addToScl3(ray, left, -(cam.frustum_center + (2*relx-1)*halfwidth));
mju_normalize3(ray);
// construct ray for orthographic camera: fixed direction, modify pos
if (cam.orthographic) {
mju_copy3(ray, forward);
mju_addToScl3(pos, up, d_up);
mju_addToScl3(pos, left, d_left);
}
// construct ray for perspective camera: fixed pos, modify direction
else {
mju_scl3(ray, forward, cam.frustum_near);
mju_addToScl3(ray, up, d_up);
mju_addToScl3(ray, left, d_left);
mju_normalize3(ray);
}
// find intersection with geoms
*geomid = -1;
mjtNum geomdist = mj_ray(m, d, pos, ray, vopt->geomgroup,
vopt->flags[mjVIS_STATIC], -1, geomid);
mjtNum geomdist = mj_ray(m, d, pos, ray, vopt->geomgroup, vopt->flags[mjVIS_STATIC], -1, geomid);
// find intersection with flexes
int flexbodyid = -1;
@@ -851,7 +890,6 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
}
}
// geom
if (best == 0) {
*flexid = -1;
src/engine/engine_vis_visualize.c
+140 -115
View File
@@ -511,11 +511,11 @@ static int bodycategory(const mjModel* m, int bodyid) {
// computes the camera frustum
static void getFrustum(float zver[2], float zhor[2], float znear,
const float K[4], const float sensorsize[2]) {
zhor[0] = znear / K[0] * (sensorsize[0]/2.f - K[2]);
zhor[1] = znear / K[0] * (sensorsize[0]/2.f + K[2]);
zver[0] = znear / K[1] * (sensorsize[1]/2.f - K[3]);
zver[1] = znear / K[1] * (sensorsize[1]/2.f + K[3]);
const float intrinsic[4], const float sensorsize[2]) {
zhor[0] = znear / intrinsic[0] * (sensorsize[0]/2.f - intrinsic[2]);
zhor[1] = znear / intrinsic[0] * (sensorsize[0]/2.f + intrinsic[2]);
zver[0] = znear / intrinsic[1] * (sensorsize[1]/2.f - intrinsic[3]);
zver[1] = znear / intrinsic[1] * (sensorsize[1]/2.f + intrinsic[3]);
}
@@ -1502,11 +1502,97 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
}
}
// cameras
// cameras and frustums
objtype = mjOBJ_CAMERA;
category = mjCAT_DECOR;
if (vopt->flags[mjVIS_CAMERA] && (category & catmask)) {
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 sensorsize is defined
if (m->cam_sensorsize[2*i+1] > 0) {
// 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];
// get frustum
getFrustum(zver, zhor, znear, m->cam_intrinsic + 4*i, m->cam_sensorsize + 2*i);
// 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
zhor[0] *= zfar / znear;
zhor[1] *= zfar / znear;
zver[0] *= zfar / znear;
zver[1] *= zfar / znear;
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++) {
START
mju_sub3(x, vfar[e], vnear[e]);
mju_sub3(y, vnear[(e+1)%4], vnear[e]);
mju_cross(z, x, y);
mjtNum tri1[3] = {mju_normalize3(x), mju_normalize3(y), mju_normalize3(z)};
mjtNum xmat1[9] = {x[0], y[0], z[0], x[1], y[1], z[1], x[2], y[2], z[2]};
mjv_initGeom(thisgeom, mjGEOM_TRIANGLE, tri1, vnear[e], xmat1, rgba);
FINISH
START
mju_sub3(y, vnear[(e+1)%4], vfar[e]);
mju_sub3(x, vfar[(e+1)%4], vfar[e]);
mju_cross(z, x, y);
mjtNum tri2[3] = {mju_normalize3(x), mju_normalize3(y), mju_normalize3(z)};
mjtNum xmat2[9] = {x[0], y[0], z[0], x[1], y[1], z[1], x[2], y[2], z[2]};
mjv_initGeom(thisgeom, mjGEOM_TRIANGLE, tri2, vfar[e], xmat2, rgba);
FINISH
START
mjv_connector(thisgeom, mjGEOM_LINE, 3, vnear[e], vnear[(e+1)%4]);
f2f(thisgeom->rgba, rgba, 4);
FINISH
START
mjv_connector(thisgeom, mjGEOM_LINE, 3, vfar[e], vfar[(e+1)%4]);
f2f(thisgeom->rgba, rgba, 4);
FINISH
START
mjv_connector(thisgeom, mjGEOM_LINE, 3, vnear[e], vfar[e]);
f2f(thisgeom->rgba, rgba, 4);
FINISH
}
}
START
// construct geom: camera body
@@ -1516,7 +1602,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
thisgeom->size[2] = scl * m->vis.scale.camera * 0.4;
mju_n2f(thisgeom->pos, d->cam_xpos+3*i, 3);
mju_n2f(thisgeom->mat, d->cam_xmat+9*i, 9);
f2f(thisgeom->rgba, m->vis.rgba.camera, 4);
f2f(thisgeom->rgba, cam_rgba, 4);
// vopt->label
if (vopt->label == mjLABEL_CAMERA) {
@@ -1539,7 +1625,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
thisgeom->size[1] = scl * m->vis.scale.camera * 0.4;
thisgeom->size[2] = scl * m->vis.scale.camera * 0.3;
mju_n2f(thisgeom->mat, d->cam_xmat+9*i, 9);
f2f(thisgeom->rgba, m->vis.rgba.camera, 4);
f2f(thisgeom->rgba, cam_rgba, 4);
for (int k=0; k < 3; k++) {
thisgeom->rgba[k] *= 0.5; // make lens body darker
}
@@ -1582,88 +1668,6 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
}
}
// camera frustum
if (vopt->flags[mjVIS_CAMERA]) {
objtype = mjOBJ_CAMERA;
category = mjCAT_DECOR;
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];
for (int i=0; i < m->ncam; i++) {
if (m->cam_sensorsize[2*i+1] == 0) {
continue;
}
getFrustum(zver, zhor, znear, m->cam_intrinsic + 4*i, m->cam_sensorsize + 2*i);
// 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
zhor[0] *= zfar / znear;
zhor[1] *= zfar / znear;
zver[0] *= zfar / znear;
zver[1] *= zfar / znear;
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++) {
START
mju_sub3(x, vfar[e], vnear[e]);
mju_sub3(y, vnear[(e+1)%4], vnear[e]);
mju_cross(z, x, y);
mjtNum tri1[3] = {mju_normalize3(x), mju_normalize3(y), mju_normalize3(z)};
mjtNum xmat1[9] = {x[0], y[0], z[0], x[1], y[1], z[1], x[2], y[2], z[2]};
mjv_initGeom(thisgeom, mjGEOM_TRIANGLE, tri1, vnear[e], xmat1, rgba);
FINISH
START
mju_sub3(y, vnear[(e+1)%4], vfar[e]);
mju_sub3(x, vfar[(e+1)%4], vfar[e]);
mju_cross(z, x, y);
mjtNum tri2[3] = {mju_normalize3(x), mju_normalize3(y), mju_normalize3(z)};
mjtNum xmat2[9] = {x[0], y[0], z[0], x[1], y[1], z[1], x[2], y[2], z[2]};
mjv_initGeom(thisgeom, mjGEOM_TRIANGLE, tri2, vfar[e], xmat2, rgba);
FINISH
START
mjv_connector(thisgeom, mjGEOM_LINE, 3, vnear[e], vnear[(e+1)%4]);
f2f(thisgeom->rgba, rgba, 4);
FINISH
START
mjv_connector(thisgeom, mjGEOM_LINE, 3, vfar[e], vfar[(e+1)%4]);
f2f(thisgeom->rgba, rgba, 4);
FINISH
START
mjv_connector(thisgeom, mjGEOM_LINE, 3, vnear[e], vfar[e]);
f2f(thisgeom->rgba, rgba, 4);
FINISH
}
}
}
// lights
objtype = mjOBJ_LIGHT;
@@ -2131,28 +2135,31 @@ void mjv_makeLights(const mjModel* m, const mjData* d, mjvScene* scn) {
// update camera only
void mjv_updateCamera(const mjModel* m, const mjData* d, mjvCamera* cam, mjvScene* scn) {
mjtNum ca, sa, ce, se, move[3], *mat;
mjtNum headpos[3], forward[3], up[3], right[3], ipd;
// return if nothing to do
if (!m || !cam || cam->type == mjCAMERA_USER) {
return;
}
// initialize frustum
float zver[2], zhor[2] = {0, 0};
float znear = m->vis.map.znear * m->stat.extent;
float zfar = m->vis.map.zfar * m->stat.extent;
// define extrinsics
mjtNum move[3];
mjtNum headpos[3], forward[3], up[3], right[3];
// get headpos, forward[3], up, right, ipd, fovy
// define intrinsics
int cid, orthographic = 0;
mjtNum fovy, ipd;
float* intrinsic = NULL;
float* sensorsize = NULL;
// get headpos, forward, up, right, ipd, fovy, orthographic, intrinsic
switch (cam->type) {
case mjCAMERA_FREE:
case mjCAMERA_TRACKING:
// get global ipd
ipd = m->vis.global.ipd;
// compute image size from global fovy
zver[0] = zver[1] = (float)znear * mju_tan(m->vis.global.fovy * (float)(mjPI/360.0));
// get orthographic, fovy
orthographic = m->vis.global.orthographic;
fovy = m->vis.global.fovy;
// move lookat for tracking
if (cam->type == mjCAMERA_TRACKING) {
@@ -2168,10 +2175,10 @@ void mjv_updateCamera(const mjModel* m, const mjData* d, mjvCamera* cam, mjvScen
}
// compute frame
ca = mju_cos(cam->azimuth/180.0*mjPI);
sa = mju_sin(cam->azimuth/180.0*mjPI);
ce = mju_cos(cam->elevation/180.0*mjPI);
se = mju_sin(cam->elevation/180.0*mjPI);
mjtNum ca = mju_cos(cam->azimuth/180.0*mjPI);
mjtNum sa = mju_sin(cam->azimuth/180.0*mjPI);
mjtNum ce = mju_cos(cam->elevation/180.0*mjPI);
mjtNum se = mju_sin(cam->elevation/180.0*mjPI);
forward[0] = ce*ca;
forward[1] = ce*sa;
forward[2] = se;
@@ -2184,25 +2191,27 @@ void mjv_updateCamera(const mjModel* m, const mjData* d, mjvCamera* cam, mjvScen
mju_addScl3(headpos, cam->lookat, forward, -cam->distance);
break;
case mjCAMERA_FIXED: {
// get id and check
int cid = cam->fixedcamid;
case mjCAMERA_FIXED:
// get id, check range
cid = cam->fixedcamid;
if (cid < 0 || cid >= m->ncam) {
mjERROR("fixed camera id is outside valid range");
}
// get camera-specific ipd and fovy
// get camera-specific ipd, orthographic, fovy
ipd = m->cam_ipd[cid];
// get frustum from intrinsics or from fovy
orthographic = m->cam_orthographic[cid];
fovy = m->cam_fovy[cid];
// if positive sensorsize, get sensorsize and intrinsic
if (m->cam_sensorsize[2*cid+1]) {
getFrustum(zver, zhor, znear, m->cam_intrinsic + 4*cid, m->cam_sensorsize + 2*cid);
} else {
zver[0] = zver[1] = (float)znear * mju_tan(m->cam_fovy[cid] * (float)(mjPI/360.0));
sensorsize = m->cam_sensorsize + 2*cid;
intrinsic = m->cam_intrinsic + 4*cid;
}
// get pointer to camera orientation matrix
mat = d->cam_xmat + 9*cid;
mjtNum* mat = d->cam_xmat + 9*cid;
// get frame
forward[0] = -mat[2];
@@ -2215,13 +2224,26 @@ void mjv_updateCamera(const mjModel* m, const mjData* d, mjvCamera* cam, mjvScen
right[1] = mat[3];
right[2] = mat[6];
mju_copy3(headpos, d->cam_xpos + 3*cid);
}
break;
break;
default:
mjERROR("unknown camera type");
}
// convert intrinsics to frustum parameters
float znear = m->vis.map.znear * m->stat.extent;
float zfar = m->vis.map.zfar * m->stat.extent;
float zver[2], zhor[2] = {0, 0};
if (orthographic){
zver[0] = zver[1] = fovy / 2;
} else {
if (!intrinsic) {
zver[0] = zver[1] = znear * mju_tan(fovy * mjPI/360.0);
} else {
getFrustum(zver, zhor, znear, intrinsic, sensorsize);
}
}
// compute GL cameras
for (int view=0; view < 2; view++) {
// set frame
@@ -2231,6 +2253,9 @@ void mjv_updateCamera(const mjModel* m, const mjData* d, mjvCamera* cam, mjvScen
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[1];
scn->camera[view].frustum_bottom = -zver[0];