Add mju_rayFlexNormal (not exposed in public header)

PiperOrigin-RevId: 847746174
Change-Id: Ia8f597d296bf6a3eed9bd48b1c39b2ff2b5fcf90

doc/APIreference/functions.rst

@@ -1170,16 +1170,14 @@ Similar semantics to mj_ray, but vec is an array of (nray x 3) directions.
 
 .. mujoco-include:: mj_ray
 
-Intersect ray ``(pnt+x*vec, x >= 0)`` with visible geoms, except geoms in bodyexclude.
+Intersect ray ``pnt+x*vec, x >= 0`` with geoms.
 
-Return geomid and distance (x) to nearest surface, or -1 if no intersection.
+- Return distance ``x`` to nearest surface, or -1 if no intersection.
-
+- If ``geomid`` is not NULL, write the id of the intersected geom or -1 if not intersection.
-geomgroup is an array of length mjNGROUP, where 1 means the group should be included. Pass geomgroup=NULL to skip
+- Exclude geoms in body with id ``bodyexclude``, use -1 to include all bodies.
-group exclusion.
+- ``geomgroup`` is an array of length :ref:`mjNGROUP<glNumeric>`, where 1 means the group should be included. Pass
-
+  NULL to skip geom group exclusion.
-If flg_static is 0, static geoms will be excluded.
+- If ``flg_static`` is 0, static geoms will be excluded.
-
-bodyexclude=-1 can be used to indicate that all bodies are included.
 
 *Nullable:* ``geomgroup``, ``geomid``
 

doc/APIreference/functions_override.rst

@@ -329,16 +329,14 @@ rays from a single point.
 
 .. _mj_ray:
 
-Intersect ray ``(pnt+x*vec, x >= 0)`` with visible geoms, except geoms in bodyexclude.
+Intersect ray ``pnt+x*vec, x >= 0`` with geoms.
 
-Return geomid and distance (x) to nearest surface, or -1 if no intersection.
+- Return distance ``x`` to nearest surface, or -1 if no intersection.
-
+- If ``geomid`` is not NULL, write the id of the intersected geom or -1 if not intersection.
-geomgroup is an array of length mjNGROUP, where 1 means the group should be included. Pass geomgroup=NULL to skip
+- Exclude geoms in body with id ``bodyexclude``, use -1 to include all bodies.
-group exclusion.
+- ``geomgroup`` is an array of length :ref:`mjNGROUP<glNumeric>`, where 1 means the group should be included. Pass
-
+  NULL to skip geom group exclusion.
-If flg_static is 0, static geoms will be excluded.
+- If ``flg_static`` is 0, static geoms will be excluded.
-
-bodyexclude=-1 can be used to indicate that all bodies are included.
 
 *Nullable:* ``geomgroup``, ``geomid``
 

doc/changelog.rst

@@ -41,6 +41,8 @@ Documentation
 
 Bug fixes
 ^^^^^^^^^
+- The ``vertid`` argument of :ref:`mju_rayFlex` and :ref:`mju_raySkin` was marked as nullable but was not; it is now
+  nullable.
 
 Version 3.4.0 (December 5, 2025)
 --------------------------------

src/engine/engine_ray.c

@@ -890,8 +890,7 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum pnt[3
 
 // intersect ray with signed distance field, compute normal if given
 static mjtNum mj_raySdfNormal(const mjModel* m, const mjData* d, int g,
-                              const mjtNum pnt[3], const mjtNum vec[3],
+                              const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]) {
-                              mjtNum normal[3]) {
   if (normal) mju_zero3(normal);
 
   mjtNum distance_total = 0;
@@ -1028,12 +1027,16 @@ mjtNum mju_rayGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3
 }
 
 
-// intersect ray with flex, return nearest vertex id
+// intersect ray with flex, return nearest vertex id, compute normal if given
-mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte flg_vert,
+mjtNum mju_rayFlexNormal(const mjModel* m, const mjData* d, int flex_layer,
-                   mjtByte flg_edge, mjtByte flg_face, mjtByte flg_skin, int flexid,
+                         mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face,
-                   const mjtNum pnt[3], const mjtNum vec[3], int vertid[1]) {
+                         mjtByte flg_skin, int flexid, const mjtNum pnt[3],
+                         const mjtNum vec[3], int vertid[1], mjtNum normal[3]) {
   int dim = m->flex_dim[flexid];
 
+  // clear normal if given
+  if (normal) mju_zero3(normal);
+
   // compute bounding box
   mjtNum box[3][2] = {{0, 0}, {0, 0}, {0, 0}};
   mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[flexid];
@@ -1086,6 +1089,7 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl
 
   // init solution
   mjtNum x = -1;
+  mjtNum normal_local[3];
 
   // check edges if rendered, or if skin
   if (flg_edge || (dim > 1 && flg_skin)) {
@@ -1106,22 +1110,25 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl
       mju_quat2Mat(mat, quat);
 
       // intersect ray with capsule
-      mjtNum sol = mju_rayGeom(pos, mat, size, pnt, vec, mjGEOM_CAPSULE);
+      mjtNum sol = mju_rayGeomNormal(pos, mat, size, pnt, vec, mjGEOM_CAPSULE,
+                                     normal ? normal_local : NULL);
 
       // update
       if (sol >= 0 && (x < 0 || sol < x)) {
         x = sol;
+        if (normal) mju_copy3(normal, normal_local);
 
         // construct intersection point
         mjtNum intersect[3];
         mju_addScl3(intersect, pnt, vec, sol);
 
         // find nearest vertex
-        if (mju_dist3(v1, intersect) < mju_dist3(v2, intersect)) {
+        if (vertid) {
-          *vertid = m->flex_edge[2*e];
+          if (mju_dist3(v1, intersect) < mju_dist3(v2, intersect)) {
-        }
+            *vertid = m->flex_edge[2*e];
-        else {
+          } else {
-          *vertid = m->flex_edge[2*e+1];
+            *vertid = m->flex_edge[2*e+1];
+          }
         }
       }
     }
@@ -1137,12 +1144,14 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl
       size[0] = radius;
 
       // intersect ray with sphere
-      mjtNum sol = mju_rayGeom(vpos, NULL, size, pnt, vec, mjGEOM_SPHERE);
+      mjtNum sol = mju_rayGeomNormal(vpos, NULL, size, pnt, vec, mjGEOM_SPHERE,
+                                     normal ? normal_local : NULL);
 
       // update
       if (sol >= 0 && (x < 0 || sol < x)) {
         x = sol;
-        *vertid = v;
+        if (normal) mju_copy3(normal, normal_local);
+        if (vertid) *vertid = v;
       }
     }
   }
@@ -1173,11 +1182,12 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl
           mju_copy3(v[j], vptr[i][j]);
 
         // intersect ray with triangle
-        mjtNum sol = ray_triangle(v, pnt, vec, b0, b1, NULL);
+        mjtNum sol = ray_triangle(v, pnt, vec, b0, b1, normal ? normal_local : NULL);
 
         // update
         if (sol >= 0 && (x < 0 || sol < x)) {
           x = sol;
+          if (normal) mju_copy3(normal, normal_local);
 
           // construct intersection point
           mjtNum intersect[3];
@@ -1189,12 +1199,14 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl
             mju_dist3(v[1], intersect),
             mju_dist3(v[2], intersect)
           };
-          if (dist[0] <= dist[1] && dist[0] <= dist[2]) {
+          if (vertid) {
-            *vertid = edata[vid[i][0]];
+            if (dist[0] <= dist[1] && dist[0] <= dist[2]) {
-          } else if (dist[1] <= dist[2]){
+              *vertid = edata[vid[i][0]];
-            *vertid = edata[vid[i][1]];
+            } else if (dist[1] <= dist[2]){
-          } else {
+              *vertid = edata[vid[i][1]];
-            *vertid = edata[vid[i][2]];
+            } else {
+              *vertid = edata[vid[i][2]];
+            }
           }
         }
       }
@@ -1205,6 +1217,16 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl
 }
 
 
+// intersect ray with flex, return nearest vertex id
+mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer,
+                   mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face,
+                   mjtByte flg_skin, int flexid, const mjtNum pnt[3],
+                   const mjtNum vec[3], int vertid[1]) {
+  return mju_rayFlexNormal(m, d, flex_layer, flg_vert, flg_edge, flg_face,
+                           flg_skin, flexid, pnt, vec, vertid, NULL);
+}
+
+
 // intersect ray with skin, return nearest vertex id
 mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
                    const mjtNum pnt[3], const mjtNum vec[3], int vertid[1]) {
@@ -1282,12 +1304,12 @@ mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
 
       // find nearest vertex
       mjtNum dist = mju_dist3(intersect, v[0]);
-      *vertid = face[3*i];
+      if (vertid) *vertid = face[3*i];
       for (int j=1; j < 3; j++) {
         mjtNum newdist = mju_dist3(intersect, v[j]);
         if (newdist < dist) {
           dist = newdist;
-          *vertid = face[3*i+j];
+          if (vertid) *vertid = face[3*i+j];
         }
       }
     }

src/engine/engine_ray.h

@@ -63,6 +63,13 @@ MJAPI mjtNum mj_rayNormal(const mjModel* m, const mjData* d, const mjtNum pnt[3]
                           const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
                           int geomid[1], mjtNum normal[3]);
 
+// intersect ray with flex, return nearest vertex id, compute normal if given
+MJAPI mjtNum mju_rayFlexNormal(const mjModel* m, const mjData* d,
+                               int flex_layer, mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face,
+                               mjtByte flg_skin, int flexid,
+                               const mjtNum pnt[3], const mjtNum vec[3],
+                               int vertid[1], mjtNum normal[3]);
+
 // intersect ray with flex, return nearest vertex id
 MJAPI mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte flg_vert,
                          mjtByte flg_edge, mjtByte flg_face, mjtByte flg_skin, int flexid,

test/engine/engine_ray_test.cc

@@ -477,19 +477,21 @@ static const char* const kBoxModel = "engine/testdata/ray/box.xml";
 static const char* const kMeshModel = "engine/testdata/ray/mesh.xml";
 static const char* const kSdfModel = "engine/testdata/ray/sdf.xml";
 static const char* const kHfieldModel = "engine/testdata/ray/hfield.xml";
+static const char* const kFlexModel = "engine/testdata/ray/flex.xml";
 
-TEST_F(RayTest, GeomNormal) {
+TEST_F(RayTest, RayNormal) {
-  for (const char* path :
+  for (const char* path : {kPlaneModel, kSphereModel, kCapsuleModel,
-       {kPlaneModel, kSphereModel, kCapsuleModel, kEllipsoidModel,
+                           kEllipsoidModel, kCylinderModel, kBoxModel,
-        kCylinderModel, kBoxModel, kMeshModel, kSdfModel, kHfieldModel}) {
+                           kMeshModel, kSdfModel, kHfieldModel, kFlexModel}) {
     const std::string xml_path = GetTestDataFilePath(path);
     char error[1024];
     mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error));
     ASSERT_THAT(m, NotNull()) << error;
 
-    // exactly one geom and one site in each model
+    // exactly one geom or one flex, and one site in each model
-    ASSERT_EQ(m->ngeom, 1) << path;
     ASSERT_EQ(m->nsite, 1) << path;
+    ASSERT_TRUE((m->ngeom == 1) != (m->nflex == 1)) << path;
+    bool is_flex = m->nflex == 1;
 
     mjData* d = mj_makeData(m);
 
@@ -514,13 +516,22 @@ TEST_F(RayTest, GeomNormal) {
       const mjtNum* pnt = d->site_xpos;
       const mjtNum vec[3] = {d->site_xmat[2], d->site_xmat[5], d->site_xmat[8]};
 
-      // compute ray length and normal
+      // compute ray length and normal, compare with sensor
       mjtNum r, normal[3];
-      int geomid;
+      if (!is_flex) {
-      r = mj_rayNormal(m, d, pnt, vec, NULL, 1, -1, &geomid, normal);
+        int geomid;
+        r = mj_rayNormal(m, d, pnt, vec, nullptr, 1, -1, &geomid, normal);
 
-      // compare with sensor
+        // compare with sensor, expect geomid to be 0
-      EXPECT_EQ(r, d->sensordata[0]) << path << ", time " << d->time;
+        EXPECT_EQ(r, d->sensordata[0]) << path << ", time " << d->time;
+        EXPECT_EQ(geomid, r >= 0 ? 0 : -1);
+      } else {
+        r = mju_rayFlexNormal(m, d, /*flex_layer*/ 0, /*flg_vert*/ 1,
+                              /*flg_edge*/ 1, /*flg_face*/ 1,
+                              /*flg_skin*/ 1, /*flex_id*/ 0,
+                              pnt, vec, nullptr, normal);
+        // no sensor comparison: rangefinders only intersect with geoms
+      }
 
       // if no intersection, skip
       if (r < 0) {
@@ -542,7 +553,12 @@ TEST_F(RayTest, GeomNormal) {
                            d->site_xmat[6 + i]};
         mjtNum dr, dpnt[3];
         mju_addScl3(dpnt, pnt, nudge, eps);
-        dr = mj_rayNormal(m, d, dpnt, vec, NULL, 1, -1, nullptr, nullptr);
+        if (!is_flex) {
+          dr = mj_rayNormal(m, d, dpnt, vec, NULL, 1, -1, nullptr, nullptr);
+        } else {
+          dr = mju_rayFlexNormal(m, d, 0, 1, 1, 1, 1, 0, dpnt, vec, nullptr,
+                                 nullptr);
+        }
         mju_addScl3(ds[i], dpnt, vec, dr);
       }
 
@@ -560,10 +576,9 @@ TEST_F(RayTest, GeomNormal) {
       mjtNum expected_neg[3] = {-expected[0], -expected[1], -expected[2]};
 
       // compare analytic with fin-diff approximation
-      EXPECT_THAT(normal, AnyOf(
+      EXPECT_THAT(normal, AnyOf(Pointwise(DoubleNear(100 * eps), expected),
-          Pointwise(DoubleNear(10*eps), expected),
+                                Pointwise(DoubleNear(100 * eps), expected_neg)))
-          Pointwise(DoubleNear(10*eps), expected_neg)
+          << path << ", time " << d->time;
-      )) << path << ", time " << d->time;
 
       // increment count
       ntest++;

test/engine/testdata/ray/flex.xml

@@ -0,0 +1,12 @@
+<mujoco>
+  <include file="ray_swing.xml"/>
+
+  <worldbody>
+    <!-- <body pos="-.5 .002 0" euler="10 50 -40" mocap="true"> -->
+      <flexcomp name="softbox" type="box" count="4 4 4" spacing=".2 .2 .2" scale="1 2 3"
+                radius=".01" dim="3" dof="radial">
+        <edge equality="true"/>
+      </flexcomp>
+    <!-- </body> -->
+  </worldbody>
+</mujoco>