Add mj_rayMeshNormal (not exposed in public header)

PiperOrigin-RevId: 838707353
Change-Id: If31d2454e0668f750bcba21ee42f81ef962e9097

src/engine/engine_ray.c

@@ -130,7 +130,10 @@ static mjtNum ray_quad(mjtNum a, mjtNum b, mjtNum c, mjtNum x[2]) {
 
 // intersect ray with triangle
 mjtNum ray_triangle(mjtNum v[][3], const mjtNum lpnt[3], const mjtNum lvec[3],
-                    const mjtNum b0[3], const mjtNum b1[3]) {
+                    const mjtNum b0[3], const mjtNum b1[3], mjtNum normal[3]) {
+  // clear normal if given
+  if (normal) mju_zero3(normal);
+
   // dif = v[i] - lpnt
   mjtNum dif[3][3];
   for (int i=0; i < 3; i++) {
@@ -182,9 +185,17 @@ mjtNum ray_triangle(mjtNum v[][3], const mjtNum lpnt[3], const mjtNum lvec[3],
     return -1;
   }
 
-  return (-mju_dot3(dif[2], nrm) / denom);
+  // compute distance
-}
+  mjtNum x = -mju_dot3(dif[2], nrm) / denom;
 
+  // compute normal if given
+  if (normal) {
+    mju_normalize3(nrm);
+    mju_copy3(normal, nrm);
+  }
+
+  return x;
+}
 
 //---------------------------- geom-specific intersection functions --------------------------------
 
@@ -639,7 +650,7 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id,
         {dx*(c+1)-size[0], dy*(r+1)-size[1], data[(r+1)*ncol+(c+1)]*size[2]},
         {dx*(c+1)-size[0], dy*(r+0)-size[1], data[(r+0)*ncol+(c+1)]*size[2]}
       };
-      mjtNum sol = ray_triangle(va, lpnt, lvec, b0, b1);
+      mjtNum sol = ray_triangle(va, lpnt, lvec, b0, b1, NULL);
       if (sol >= 0 && (x < 0 || sol < x)) {
         x = sol;
       }
@@ -650,7 +661,7 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id,
         {dx*(c+1)-size[0], dy*(r+1)-size[1], data[(r+1)*ncol+(c+1)]*size[2]},
         {dx*(c+0)-size[0], dy*(r+1)-size[1], data[(r+1)*ncol+(c+0)]*size[2]}
       };
-      sol = ray_triangle(vb, lpnt, lvec, b0, b1);
+      sol = ray_triangle(vb, lpnt, lvec, b0, b1, NULL);
       if (sol >= 0 && (x < 0 || sol < x)) {
         x = sol;
       }
@@ -721,9 +732,13 @@ int mju_raySlab(const mjtNum aabb[6], const mjtNum xpos[3],
   return tmin < tmax;
 }
 
+
 // ray vs tree intersection
 mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum pnt[3],
-                   const mjtNum vec[3]) {
+                   const mjtNum vec[3], mjtNum normal[3]) {
+  // clear normal if given
+  if (normal) mju_zero3(normal);
+
   int mark_active = m->vis.global.bvactive;
   const int meshid = m->geom_dataid[id];
   const int bvhadr = m->mesh_bvhadr[meshid];
@@ -761,6 +776,7 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum pnt[3
 
   // init solution
   mjtNum x = -1, sol;
+  mjtNum normal_local[3];
 
   while (nstack) {
     // pop from stack
@@ -794,14 +810,13 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum pnt[3
       }
 
       // solve
-      sol = ray_triangle(v, lpnt, lvec, b0, b1);
+      sol = ray_triangle(v, lpnt, lvec, b0, b1, normal ? normal_local : NULL);
 
       // update
       if (sol >= 0 && (x < 0 || sol < x)) {
         x = sol;
-        if (mark_active) {
+        if (normal) mju_copy3(normal, normal_local);
-          d->bvh_active[node + bvhadr] = 1;
+        if (mark_active) d->bvh_active[node + bvhadr] = 1;
-        }
       }
       continue;
     }
@@ -823,6 +838,11 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum pnt[3
     }
   }
 
+  // rotate normal to global frame
+  if (normal && x >= 0) {
+    mju_mulMatVec3(normal, d->geom_xmat+9*id, normal);
+  }
+
   return x;
 }
 
@@ -886,10 +906,12 @@ mjtNum ray_sdf(const mjModel* m, const mjData* d, int g,
   return -1;
 }
 
+// intersect ray with mesh, compute normal if given
+mjtNum mj_rayMeshNormal(const mjModel* m, const mjData* d, int id, const mjtNum pnt[3],
+                        const mjtNum vec[3], mjtNum normal[3]) {
+  // clear normal if given
+  if (normal) mju_zero3(normal);
 
-// intersect ray with mesh
-mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int id,
-                  const mjtNum pnt[3], const mjtNum vec[3]) {
   // check geom type
   if (m->geom_type[id] != mjGEOM_MESH) {
     mjERROR("geom with mesh type expected");
@@ -900,7 +922,13 @@ mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int id,
     return -1;
   }
 
-  return mju_rayTree(m, d, id, pnt, vec);
+  return mju_rayTree(m, d, id, pnt, vec, normal);
+}
+
+// intersect ray with mesh
+mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int id, const mjtNum pnt[3],
+                  const mjtNum vec[3]) {
+  return mj_rayMeshNormal(m, d, id, pnt, vec, NULL);
 }
 
 // intersect ray and find normal with primitive geom, no meshes or hfields, compute normal if given
@@ -931,6 +959,7 @@ mjtNum mju_rayGeomNormal(const mjtNum pos[3], const mjtNum mat[9], const mjtNum
   }
 }
 
+
 // intersect ray with primitive geom, no meshes or hfields
 mjtNum mju_rayGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3],
                    const mjtNum pnt[3], const mjtNum vec[3], int geomtype) {
@@ -1075,14 +1104,14 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl
       int vid[4][3] = {{0, 1, 2}, {0, 1, 3}, {0, 2, 3}, {1, 2, 3}};
 
       // process triangles of this element
-      for (int i=0; i < (dim == 2?1:4); i++) {
+      for (int i = 0; i < (dim == 2 ? 1 : 4); i++) {
         // copy vertices into triangle representation
         mjtNum v[3][3];
         for (int j=0; j < 3; j++)
           mju_copy3(v[j], vptr[i][j]);
 
         // intersect ray with triangle
-        mjtNum sol = ray_triangle(v, pnt, vec, b0, b1);
+        mjtNum sol = ray_triangle(v, pnt, vec, b0, b1, NULL);
 
         // update
         if (sol >= 0 && (x < 0 || sol < x)) {
@@ -1179,7 +1208,7 @@ mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
     }
 
     // solve
-    mjtNum sol = ray_triangle(v, pnt, vec, b0, b1);
+    mjtNum sol = ray_triangle(v, pnt, vec, b0, b1, NULL);
 
     // update
     if (sol >= 0 && (x < 0 || sol < x)) {
@@ -1434,7 +1463,7 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co
 }
 
 
-// Performs multiple ray intersections with the precomputes bv and flags
+// performs multiple ray intersections with the precomputed bv and flags
 void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum vec[3],
                  const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
                  int* geomid, mjtNum* dist, int nray, mjtNum cutoff) {

src/engine/engine_ray.h

@@ -29,8 +29,8 @@ MJAPI void mju_multiRayPrepare(const mjModel* m, const mjData* d,
                                int bodyexclude, mjtNum cutoff, mjtNum* geom_ba,
                                int* geom_eliminate);
 
-// Intersect multiple rays emanating from a single source
+// intersect multiple rays emanating from a single source
-// Similar semantics to mj_ray, but vec is an array of (nray x 3) directions.
+// similar semantics to mj_ray, but vec is an array of (nray x 3) directions.
 MJAPI void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum vec[3],
                        const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
                        int* geomid, mjtNum* dist, int nray, mjtNum cutoff);
@@ -48,12 +48,16 @@ MJAPI mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int geomid,
 
 // intersect ray with triangle
 MJAPI mjtNum ray_triangle(mjtNum v[][3], const mjtNum lpnt[3], const mjtNum lvec[3],
-                          const mjtNum b0[3], const mjtNum b1[3]);
+                          const mjtNum b0[3], const mjtNum b1[3], mjtNum normal[3]);
 
 // intersect ray with mesh
 MJAPI mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int geomid,
                         const mjtNum pnt[3], const mjtNum vec[3]);
 
+// intersect ray with mesh, compute normal if given
+MJAPI mjtNum mj_rayMeshNormal(const mjModel* m, const mjData* d, int geomid,
+                              const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]);
+
 // intersect ray with primitive geom, no meshes or hfields
 MJAPI mjtNum mju_rayGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3],
                          const mjtNum pnt[3], const mjtNum vec[3], int geomtype);

test/engine/engine_ray_test.cc

@@ -350,7 +350,7 @@ mjtNum _rayMesh(const mjModel* m, const mjData* d, int geomid,
     }
 
     // solve
-    sol = ray_triangle(v, lpnt, lvec, b0, b1);
+    sol = ray_triangle(v, lpnt, lvec, b0, b1, nullptr);
 
     // update
     if (sol >= 0 && (x < 0 || sol < x)) {
@@ -474,10 +474,12 @@ static const char* const kCapsuleModel = "engine/testdata/ray/capsule.xml";
 static const char* const kEllipsoidModel = "engine/testdata/ray/ellipsoid.xml";
 static const char* const kCylinderModel = "engine/testdata/ray/cylinder.xml";
 static const char* const kBoxModel = "engine/testdata/ray/box.xml";
+static const char* const kMeshModel = "engine/testdata/ray/mesh.xml";
 
 TEST_F(RayTest, GeomNormal) {
-  for (const char* path : {kPlaneModel, kSphereModel, kCapsuleModel,
+  for (const char* path :
-                           kEllipsoidModel, kCylinderModel, kBoxModel}) {
+       {kPlaneModel, kSphereModel, kCapsuleModel, kEllipsoidModel,
+        kCylinderModel, kBoxModel, kMeshModel}) {
     const std::string xml_path = GetTestDataFilePath(path);
     char error[1024];
     mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error));
@@ -517,8 +519,14 @@ TEST_F(RayTest, GeomNormal) {
       const mjtNum vec[3] = {d->site_xmat[2], d->site_xmat[5], d->site_xmat[8]};
 
       // compute ray length and normal
-      mjtNum normal[3];
+      mjtNum r, normal[3];
-      mjtNum r = mju_rayGeomNormal(pos, mat, size, pnt, vec, type, normal);
+      switch (type) {
+        case mjGEOM_MESH:
+          r = mj_rayMeshNormal(m, d, 0, pnt, vec, normal);
+          break;
+        default:
+          r = mju_rayGeomNormal(pos, mat, size, pnt, vec, type, normal);
+      }
 
       // compare with sensor
       EXPECT_EQ(r, d->sensordata[0]) << path << ", time " << d->time;
@@ -540,9 +548,15 @@ TEST_F(RayTest, GeomNormal) {
         mjtNum nudge[3] = {d->site_xmat[0 + i],
                            d->site_xmat[3 + i],
                            d->site_xmat[6 + i]};
-        mjtNum dpnt[3];
+        mjtNum dr, dpnt[3];
         mju_addScl3(dpnt, pnt, nudge, eps);
-        mjtNum dr = mju_rayGeomNormal(pos, mat, size, dpnt, vec, type, nullptr);
+        switch (type) {
+          case mjGEOM_MESH:
+            dr = mj_rayMeshNormal(m, d, 0, dpnt, vec, nullptr);
+            break;
+          default:
+            dr = mju_rayGeomNormal(pos, mat, size, dpnt, vec, type, nullptr);
+        }
         mju_addScl3(ds[i], dpnt, vec, dr);
       }
 

test/engine/testdata/ray/mesh.xml

@@ -0,0 +1,13 @@
+<mujoco>
+  <asset>
+    <mesh file="stanford_bunny.obj" scale="10 10 10"/>
+  </asset>
+
+  <include file="ray_swing.xml"/>
+
+  <worldbody>
+    <body pos="-.5 .002 -1" euler="90 0 -40" mocap="true">
+      <geom type="mesh" mesh="stanford_bunny"/>
+    </body>
+  </worldbody>
+</mujoco>