From 7fddeeaff69686b3c6ad64f32e2205113392c251 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 22 Dec 2025 08:51:33 -0800 Subject: [PATCH] Minor fixes to mj_multiRay PiperOrigin-RevId: 847780959 Change-Id: I3588415f215bd5f1592bec6678a5bae45d5f0980 --- doc/includes/references.h | 2 +- include/mujoco/mujoco.h | 2 +- python/mujoco/introspect/functions.py | 3 +-- src/engine/engine_ray.c | 22 +++++++++++++--------- src/engine/engine_ray.h | 2 +- src/user/user_model.cc | 2 ++ 6 files changed, 19 insertions(+), 14 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index f857b796..5135c431 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3236,7 +3236,7 @@ void mj_loadPluginLibrary(const char* path); void mj_loadAllPluginLibraries(const char* directory, mjfPluginLibraryLoadCallback callback); int mj_version(void); const char* mj_versionString(void); -void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum vec[3], +void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec, const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, int* geomid, mjtNum* dist, int nray, mjtNum cutoff); mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum pnt[3], const mjtNum vec[3], diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index d3a286fe..00d42673 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -629,7 +629,7 @@ MJAPI const char* mj_versionString(void); // Intersect multiple rays emanating from a single point. // Similar semantics to mj_ray, but vec is an array of (nray x 3) directions. // Nullable: geomgroup -MJAPI void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum vec[3], +MJAPI void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec, const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, int* geomid, mjtNum* dist, int nray, mjtNum cutoff); diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index f0a25c93..1726aa94 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -3755,9 +3755,8 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), FunctionParameterDecl( name='vec', - type=ArrayType( + type=PointerType( inner_type=ValueType(name='mjtNum', is_const=True), - extents=(3,), ), ), FunctionParameterDecl( diff --git a/src/engine/engine_ray.c b/src/engine/engine_ray.c index f7dc0ae7..28a1343a 100644 --- a/src/engine/engine_ray.c +++ b/src/engine/engine_ray.c @@ -1466,6 +1466,7 @@ void mju_multiRayPrepare(const mjModel* m, const mjData* d, const mjtNum pnt[3], AABB[3] = mju_max(AABB[3], elevation); } + // azimuth crosses discontinuity, fall back to no angular culling if (AABB[2]-AABB[0] > mjPI) { AABB[0] = -mjPI; AABB[1] = 0; @@ -1473,8 +1474,12 @@ void mju_multiRayPrepare(const mjModel* m, const mjData* d, const mjtNum pnt[3], AABB[3] = mjPI; } - if (AABB[3]-AABB[1] > mjPI) { // SHOULD NOT OCCUR - mjERROR("discontinuity in azimuth angle"); + // elevation overflow, fall back to no angular culling + if (AABB[3]-AABB[1] > mjPI) { + AABB[0] = -mjPI; + AABB[1] = 0; + AABB[2] = mjPI; + AABB[3] = mjPI; } mju_copy(geom_ba+4*g, AABB, 4); @@ -1488,11 +1493,6 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co int* ray_eliminate, mjtNum* geom_ba, int geomid[1]) { mjtNum dist, newdist; - // check vector length - if (mju_norm3(vec) < mjMINVAL) { - mjERROR("vector length is too small"); - } - // clear result dist = -1; *geomid = -1; @@ -1558,7 +1558,7 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co // 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], +void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec, const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, int* geomid, mjtNum* dist, int nray, mjtNum cutoff) { mj_markStack(d); @@ -1573,7 +1573,11 @@ void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum // loop over rays for (int i=0; i < nray; i++) { - dist[i] = mju_singleRay(m, d, pnt, vec+3*i, geom_eliminate, geom_ba, geomid+i); + if (mju_dot3(vec+3*i, vec+3*i) < mjMINVAL) { + dist[i] = -1; + } else { + dist[i] = mju_singleRay(m, d, pnt, vec+3*i, geom_eliminate, geom_ba, geomid+i); + } } mj_freeStack(d); diff --git a/src/engine/engine_ray.h b/src/engine/engine_ray.h index 00e38705..e80e5940 100644 --- a/src/engine/engine_ray.h +++ b/src/engine/engine_ray.h @@ -31,7 +31,7 @@ MJAPI void mju_multiRayPrepare(const mjModel* m, const mjData* d, // intersect multiple rays emanating from a single source // 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], +MJAPI void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec, const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, int* geomid, mjtNum* dist, int nray, mjtNum cutoff); diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 63e89e94..4001c5cb 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -4412,7 +4412,9 @@ mjModel* mjCModel::Compile(const mjVFS* vfs, mjModel** m) { } catch (mjCError err) { // deallocate everything allocated in Compile mj_deleteModel(model); + model = nullptr; mj_deleteData(data); + data = nullptr; Clear(); // save error info