From ad5f07e060e28aa1d2635f427866053561806e5d Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 11 Sep 2025 04:09:52 -0700 Subject: [PATCH] Extract functions from mjv_updateCamera for calculating frames and frustums. PiperOrigin-RevId: 805770746 Change-Id: Id2a6022a2ee2e8b1a783debf54b470a9df81051a --- src/engine/engine_vis_visualize.c | 242 +++++++++++++++++++----------- src/engine/engine_vis_visualize.h | 13 +- 2 files changed, 168 insertions(+), 87 deletions(-) diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 22b7cfdb..cd4d7e6d 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -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) diff --git a/src/engine/engine_vis_visualize.h b/src/engine/engine_vis_visualize.h index bf548360..daad04a9 100644 --- a/src/engine/engine_vis_visualize.h +++ b/src/engine/engine_vis_visualize.h @@ -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);