Add frustum visualization for orthographic cameras.

PiperOrigin-RevId: 847698917
Change-Id: I13633e8ba2d4718b127a5838c404ab74c6180e3d

Author: yuvaltassa

doc/XMLreference.rst

@@ -2976,12 +2976,19 @@ and the +Y axis points up. Thus the frame position and orientation are the key a
    for most scenes and should likely be reduced. In either case, the horizontal field of view is computed automatically
    given the window size and the vertical field of view.
 
+.. _body-camera-resolution:
+
+:at:`resolution`: :at-val:`int(2), "1 1"`
+   Resolution of the camera in pixels [width height]. Note that these values are not used for rendering since those
+   dimensions are determined by the size of the rendering context. This attribute serves as a convenient
+   location to save the required resolution. Setting either value larger than 1
+   enables frustum visualization when the :ref:`mjVIS_CAMERA<mjtVisFlag>` visualization flag is active.
+
 .. _body-camera-sensorsize:
 
 :at:`sensorsize`: :at-val:`real(2), "0 0"`
    Size of the camera sensor in length units. When specified, all intrinsic attributes become active and :at:`fovy`
-   is ignored. The field-of-view is then computed automatically from the focal length and sensor size. This
-   also enables frustum visualization when the :ref:`mjVIS_CAMERA<mjtVisFlag>` visualization flag is active.
+   is ignored. The field-of-view is then computed automatically from the focal length and sensor size.
 
 .. _body-camera-focal:
 .. _body-camera-focalpixel:
@@ -2998,13 +3005,6 @@ and the +Y axis points up. Thus the frame position and orientation are the key a
    specified, the pixel value is used. At zero offset, the rendered image is centered on the camera's negative Z axis,
    as in a standard pinhole camera model.
 
-.. _body-camera-resolution:
-
-:at:`resolution`: :at-val:`int(2), "1 1"`
-   Resolution of the camera in pixels [width height]. Note that these values are not used for rendering since those
-   dimensions are determined by the size of the rendering context. This attribute serves as a convenient
-   location to save the required resolution when creating a context.
-
 .. _body-camera-ipd:
 
 :at:`ipd`: :at-val:`real, "0.068"`

doc/changelog.rst

@@ -18,6 +18,8 @@ Upcoming version (not yet released)
 
 General
 ^^^^^^^
+- Camera frustum visualization is now triggered by setting :ref:`resolution<body-camera-resolution>` to values larger
+  than 1. Relatedly, frustum visualization also works for :ref:`orthographic<body-camera-projection>` cameras.
 - Non-breaking ABI changes:
 
   - The type of the ``sig`` (signature) argument of :ref:`mj_stateSize` and related functions has been changed from

src/engine/engine_vis_visualize.c

@@ -539,21 +539,13 @@ void mjv_cameraFrustum(float zver[2], float zhor[2], float zclip[2], const mjMod
   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;
-    }
+    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 (zver) zver[0] = zver[1] = znear * mju_tan(fovy * mjPI/360.0);
+    if (zhor) zhor[0] = zhor[1] = 0.0f;
   }
 
   if (zclip) {
@@ -2232,8 +2224,8 @@ static void addCameraGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, m
     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) {
+    // draw frustum if resolution larger than (1, 1)
+    if (m->cam_resolution[2*i] > 1 || m->cam_resolution[2*i+1] > 1) {
       // when drawing frustum, make camera translucent
       cam_rgba[3] = 0.3;
 
@@ -2244,9 +2236,22 @@ static void addCameraGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, m
       mjtNum znear = m->vis.map.znear * m->stat.extent;
       mjtNum zfar = m->vis.scale.frustum * scl;
       float zver[2], zhor[2];
+      int orthographic = m->cam_projection[i] == mjPROJ_ORTHOGRAPHIC;
 
       // get frustum
-      getFrustum(zver, zhor, znear, m->cam_intrinsic + 4*i, m->cam_sensorsize + 2*i);
+      if (orthographic) {
+        float aspect = (float)m->cam_resolution[2*i] / m->cam_resolution[2*i+1];
+        zver[0] = zver[1] = m->cam_fovy[i] / 2;
+        zhor[0] = zhor[1] = m->cam_fovy[i] * aspect / 2;
+      } else if (m->cam_sensorsize[2*i] && m->cam_sensorsize[2*i+1]) {
+        // intrinsic-based perspective camera
+        getFrustum(zver, zhor, znear, m->cam_intrinsic+4*i, m->cam_sensorsize+2*i);
+      } else {
+        // fovy-based perspective camera
+        float aspect = (float)m->cam_resolution[2*i] / m->cam_resolution[2*i+1];
+        zver[0] = zver[1] = znear * mju_tan(m->cam_fovy[i] * mjPI / 360.0);
+        zhor[0] = zhor[1] = zver[0] * aspect;
+      }
 
       // frustum frame to convert from planes to vertex representation
       mjtNum* cam_xpos = d->cam_xpos+3*i;
@@ -2266,11 +2271,15 @@ static void addCameraGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, m
       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;
+      // vertices of the far plane: scale for perspective, average(width, height) for orthographic
+      if (!orthographic) {
+        zhor[0] *= zfar / znear;
+        zhor[1] *= zfar / znear;
+        zver[0] *= zfar / znear;
+        zver[1] *= zfar / znear;
+      } else {
+        zfar = (zhor[0] + zver[0]) / 2;
+      }
       mju_addScl3(center, cam_xpos, z, -zfar);
       mju_addScl3(vfar[0], center, x, -zhor[0]);
       mju_addScl3(vfar[1], center, x,  zhor[1]);

test/engine/testdata/vis_visualize/orthographic.xml

@@ -4,7 +4,7 @@
   <visual>
     <!-- free camera is orthorgaphic with a 5m vertical field-of-view -->
     <global orthographic="true" fovy="5"/>
-    <scale camera="1"/>
+    <scale camera="0.5"/>
   </visual>
 
   <asset>
@@ -26,9 +26,9 @@
     </body>
 
     <!-- box should pefectly fit within field-of-view -->
-    <camera name="fovy = 1" pos=".5 .5 2" projection="orthographic" fovy="1"/>
+    <camera name="fovy = 1" pos=".5 .5 2" projection="orthographic" fovy="1" resolution="640 480"/>
 
     <!-- 2 floor squares should pefectly fit within field-of-view -->
-    <camera name="fovy = 2" pos="0 0 2" fovy="2"/>   <!-- inherits projection="orthographic" from defaults -->
+    <camera name="fovy = 2" pos="0 0 3" fovy="2"/>   <!-- inherits projection="orthographic" from defaults -->
   </worldbody>
 </mujoco>