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

Extract functions from mjv_updateCamera for calculating frames and frustums.

Browse Source 
PiperOrigin-RevId: 805770746
Change-Id: Id2a6022a2ee2e8b1a783debf54b470a9df81051a
This commit is contained in:
Haroon Qureshi
2025-09-11 04:09:52 -07:00
committed by Copybara-Service
parent 017e7d5a73
commit ad5f07e060
2 changed files with 168 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_vis_visualize.c
+156 -86
View File
@@ -558,10 +558,138 @@ 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 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]);
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;
}
}
@@ -2379,110 +2507,52 @@ void mjv_updateCamera(const mjModel* m, const mjData* d, mjvCamera* cam, mjvScen
return;
}
// define extrinsics
mjtNum move[3];
// move lookat for tracking
if (cam->type == mjCAMERA_TRACKING) {
// get id and check
int bid = cam->trackbodyid;
if (bid < 0 || bid >= m->nbody) {
mjERROR("track body id is outside valid range");
}
// smooth tracking of subtree com
mjtNum move[3];
mju_sub3(move, d->subtree_com + 3*cam->trackbodyid, cam->lookat);
mju_addToScl3(cam->lookat, move, 0.2); // constant ???
}
// get camera frame
mjtNum headpos[3], forward[3], up[3], right[3];
mjv_cameraFrame(headpos, forward, up, right, d, cam);
// define intrinsics
// 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 fovy, ipd;
float* intrinsic = NULL;
float* sensorsize = NULL;
mjtNum ipd;
// 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;
// get orthographic, fovy
orthographic = m->vis.global.orthographic;
fovy = m->vis.global.fovy;
// move lookat for tracking
if (cam->type == mjCAMERA_TRACKING) {
// get id and check
int bid = cam->trackbodyid;
if (bid < 0 || bid >= m->nbody) {
mjERROR("track body id is outside valid range");
}
// smooth tracking of subtree com
mju_sub3(move, d->subtree_com + 3*cam->trackbodyid, cam->lookat);
mju_addToScl3(cam->lookat, move, 0.2); // constant ???
}
// compute frame
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;
up[0] = -se*ca;
up[1] = -se*sa;
up[2] = ce;
right[0] = sa;
right[1] = -ca;
right[2] = 0;
mju_addScl3(headpos, cam->lookat, forward, -cam->distance);
break;
case mjCAMERA_FIXED:
// get id, check range
cid = cam->fixedcamid;
if (cid < 0 || cid >= m->ncam) {
mjERROR("fixed camera id is outside valid range");
}
// get camera-specific ipd, orthographic, fovy
ipd = m->cam_ipd[cid];
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;
}
// get pointer to camera orientation matrix
mjtNum* mat = d->cam_xmat + 9*cid;
// get frame
forward[0] = -mat[2];
forward[1] = -mat[5];
forward[2] = -mat[8];
up[0] = mat[1];
up[1] = mat[4];
up[2] = mat[7];
right[0] = mat[0];
right[1] = mat[3];
right[2] = mat[6];
mju_copy3(headpos, d->cam_xpos + 3*cid);
break;
default:
mjERROR("unknown camera type");
}
// 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
@@ -2500,8 +2570,8 @@ void mjv_updateCamera(const mjModel* m, const mjData* d, mjvCamera* cam, mjvScen
scn->camera[view].frustum_bottom = -zver[0];
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 = znear;
scn->camera[view].frustum_far = zfar;
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)
src/engine/engine_vis_visualize.h
+12 -1
View File
@@ -54,7 +54,18 @@ MJAPI void mjv_updateActiveFlex(const mjModel* m, mjData* d, mjvScene* scn, cons
MJAPI void mjv_updateSkin(const mjModel* m, const mjData* d, mjvScene* scn);
// update visible skins only
MJAPI void mjv_updateActiveSkin(const mjModel* m, const mjData* d, mjvScene* scn, const mjvOption* opt);
MJAPI void mjv_updateActiveSkin(const mjModel* m, const mjData* d, mjvScene* scn,
const mjvOption* opt);
// computes the camera position, forward, up, and right vectors
// Nullable: headpos, forward, up, right
void mjv_cameraFrame(mjtNum headpos[3], mjtNum forward[3], mjtNum up[3], mjtNum right[3],
const mjData* d, const mjvCamera* cam);
// computes the camera frustums, i.e. vertical, horizontal, and clip planes
// Nullable: zver, zhor, zclip
void mjv_cameraFrustum(float zver[2], float zhor[2], float zclip[2], const mjModel* m,
const mjvCamera* cam);
int mjv_catenary(const mjtNum x0[3], const mjtNum x1[3], const mjtNum gravity[3], mjtNum length,
mjtNum* catenary, int ncatenary);