Add mj_geomDistance API function and sensors for geometric distance, normal direction and segment between between two geoms. Fixes #51

PiperOrigin-RevId: 632458301 Change-Id: I65b7e5bb0df59008028114cf4a07e0d6365ff3d3
2024-05-10 04:51:46 -07:00
parent 31de1dad9d
commit 02d015458c
29 changed files with 1002 additions and 66 deletions
@@ -1974,6 +1974,20 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
        mjv_connector(thisgeom, mjGEOM_LINE, 3, from, to);
        f2f(thisgeom->rgba, m->vis.rgba.rangefinder, 4);
        FINISH
+      } else if (m->sensor_type[i] == mjSENS_GEOMFROMTO) {
+        // sensor data
+        mjtNum* fromto = d->sensordata + m->sensor_adr[i];
+
+        // null output: nothing to render
+        if (mju_isZero(fromto, 6)) {
+          continue;
+        }
+
+        // make ray
+        START
+        mjv_connector(thisgeom, mjGEOM_LINE, 3, fromto, fromto+3);
+        f2f(thisgeom->rgba, m->vis.rgba.rangefinder, 4);
+        FINISH
      }
    }
  }