Minor fixes to mj_multiRay

PiperOrigin-RevId: 847780959
Change-Id: I3588415f215bd5f1592bec6678a5bae45d5f0980

Author: yuvaltassa

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],

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);
 

python/mujoco/introspect/functions.py

@@ -3755,9 +3755,8 @@
              ),
              FunctionParameterDecl(
                  name='vec',
-                 type=ArrayType(
+                 type=PointerType(
                      inner_type=ValueType(name='mjtNum', is_const=True),
-                     extents=(3,),
                  ),
              ),
              FunctionParameterDecl(

src/engine/engine_ray.c

@@ -1466,15 +1466,20 @@ 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;
         AABB[2] =  mjPI;
         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);

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);
 

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