Use mesh BVH for speeding up ray mesh intersection.

PiperOrigin-RevId: 534141245
Change-Id: I9f5a71ea059a1a232fd2910d1efc0506ab6dbef3

src/user/user_objects.cc

@@ -34,6 +34,7 @@
 #include "engine/engine_macro.h"
 #include "engine/engine_passive.h"
 #include "engine/engine_plugin.h"
+#include "engine/engine_util_blas.h"
 #include "engine/engine_util_errmem.h"
 #include "engine/engine_util_misc.h"
 #include "engine/engine_util_solve.h"
@@ -338,6 +339,14 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements
     }
   }
 
+  // inflate flat AABBs
+  for (int i=0; i<3; i++) {
+    if (mju_abs(AABB[i]-AABB[i+3])<mjEPS) {
+      AABB[i+0] -= mjEPS;
+      AABB[i+3] += mjEPS;
+    }
+  }
+
   // store current index
   int index = nbvh++;
   child.push_back(-1);
@@ -432,6 +441,10 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements
               name_.c_str(), nelements);
   }
 
+  if (lev>mjMAXTREEDEPTH) {
+    mju_warning("max tree depth exceeded in body=%s", name_.c_str());
+  }
+
   return index;
 }