From 223ba99ec2c515e75854f0c193a03625540d53f8 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 22 Dec 2025 06:48:06 -0800 Subject: [PATCH] Add `mju_rayFlexNormal` (not exposed in public header) PiperOrigin-RevId: 847746174 Change-Id: Ia8f597d296bf6a3eed9bd48b1c39b2ff2b5fcf90 --- doc/APIreference/functions.rst | 16 +++--- doc/APIreference/functions_override.rst | 16 +++--- doc/changelog.rst | 2 + src/engine/engine_ray.c | 68 ++++++++++++++++--------- src/engine/engine_ray.h | 7 +++ test/engine/engine_ray_test.cc | 47 +++++++++++------ test/engine/testdata/ray/flex.xml | 12 +++++ 7 files changed, 111 insertions(+), 57 deletions(-) create mode 100644 test/engine/testdata/ray/flex.xml diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index ed61b908..b9820381 100644 --- a/doc/APIreference/functions.rst +++ b/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. - -geomgroup is an array of length mjNGROUP, where 1 means the group should be included. Pass geomgroup=NULL to skip -group exclusion. - -If flg_static is 0, static geoms will be excluded. - -bodyexclude=-1 can be used to indicate that all bodies are included. +- 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. +- Exclude geoms in body with id ``bodyexclude``, use -1 to include all bodies. +- ``geomgroup`` is an array of length :ref:`mjNGROUP`, 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. *Nullable:* ``geomgroup``, ``geomid`` diff --git a/doc/APIreference/functions_override.rst b/doc/APIreference/functions_override.rst index d6acd726..1ca53297 100644 --- a/doc/APIreference/functions_override.rst +++ b/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. - -geomgroup is an array of length mjNGROUP, where 1 means the group should be included. Pass geomgroup=NULL to skip -group exclusion. - -If flg_static is 0, static geoms will be excluded. - -bodyexclude=-1 can be used to indicate that all bodies are included. +- 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. +- Exclude geoms in body with id ``bodyexclude``, use -1 to include all bodies. +- ``geomgroup`` is an array of length :ref:`mjNGROUP`, 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. *Nullable:* ``geomgroup``, ``geomid`` diff --git a/doc/changelog.rst b/doc/changelog.rst index 479b3ebc..990afd33 100644 --- a/doc/changelog.rst +++ b/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) -------------------------------- diff --git a/src/engine/engine_ray.c b/src/engine/engine_ray.c index 2078ec96..f7dc0ae7 100644 --- a/src/engine/engine_ray.c +++ b/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], - mjtNum normal[3]) { + const mjtNum pnt[3], const mjtNum vec[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 -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]) { +// intersect ray with flex, return nearest vertex id, compute normal if given +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]) { 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)) { - *vertid = m->flex_edge[2*e]; - } - else { - *vertid = m->flex_edge[2*e+1]; + if (vertid) { + if (mju_dist3(v1, intersect) < mju_dist3(v2, intersect)) { + *vertid = m->flex_edge[2*e]; + } else { + *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]) { - *vertid = edata[vid[i][0]]; - } else if (dist[1] <= dist[2]){ - *vertid = edata[vid[i][1]]; - } else { - *vertid = edata[vid[i][2]]; + if (vertid) { + if (dist[0] <= dist[1] && dist[0] <= dist[2]) { + *vertid = edata[vid[i][0]]; + } else if (dist[1] <= dist[2]){ + *vertid = edata[vid[i][1]]; + } 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]; } } } diff --git a/src/engine/engine_ray.h b/src/engine/engine_ray.h index 1d4e066c..00e38705 100644 --- a/src/engine/engine_ray.h +++ b/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, diff --git a/test/engine/engine_ray_test.cc b/test/engine/engine_ray_test.cc index efdd88ee..9b3172f2 100644 --- a/test/engine/engine_ray_test.cc +++ b/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) { - for (const char* path : - {kPlaneModel, kSphereModel, kCapsuleModel, kEllipsoidModel, - kCylinderModel, kBoxModel, kMeshModel, kSdfModel, kHfieldModel}) { +TEST_F(RayTest, RayNormal) { + for (const char* path : {kPlaneModel, kSphereModel, kCapsuleModel, + kEllipsoidModel, kCylinderModel, kBoxModel, + 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 - ASSERT_EQ(m->ngeom, 1) << path; + // exactly one geom or one flex, and one site in each model 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; - r = mj_rayNormal(m, d, pnt, vec, NULL, 1, -1, &geomid, normal); + if (!is_flex) { + int geomid; + r = mj_rayNormal(m, d, pnt, vec, nullptr, 1, -1, &geomid, normal); - // compare with sensor - EXPECT_EQ(r, d->sensordata[0]) << path << ", time " << d->time; + // compare with sensor, expect geomid to be 0 + 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( - Pointwise(DoubleNear(10*eps), expected), - Pointwise(DoubleNear(10*eps), expected_neg) - )) << path << ", time " << d->time; + EXPECT_THAT(normal, AnyOf(Pointwise(DoubleNear(100 * eps), expected), + Pointwise(DoubleNear(100 * eps), expected_neg))) + << path << ", time " << d->time; // increment count ntest++; diff --git a/test/engine/testdata/ray/flex.xml b/test/engine/testdata/ray/flex.xml new file mode 100644 index 00000000..70781da1 --- /dev/null +++ b/test/engine/testdata/ray/flex.xml @@ -0,0 +1,12 @@ + + + + + + + + + + +