Make BVH generation robust to very small perturbations.

PiperOrigin-RevId: 629070367
Change-Id: I62b91a572cd59a51ed85e4e97aea53fdb2da1986

src/user/user_objects.cc

@@ -432,10 +432,11 @@ int mjCBoundingVolumeHierarchy::MakeBVH(
     return index;
   }
 
-  // find longest axis for splitting the bounding box
+  // find longest axis, by a margin of at least mjEPS, default to 0
+  int axis = 0;
   mjtNum edges[3] = { AAMM[3]-AAMM[0], AAMM[4]-AAMM[1], AAMM[5]-AAMM[2] };
-  int axis = edges[0] > edges[1] ? 0 : 1;
+  if (edges[1] >= edges[0] + mjEPS) axis = 1;
-  axis = edges[axis] > edges[2] ? axis : 2;
+  if (edges[2] >= edges[axis] + mjEPS) axis = 2;
 
   // find median along the axis
   // note: nth_element performs a partial sort of elements

src/user/user_objects.h

@@ -15,6 +15,7 @@
 #ifndef MUJOCO_SRC_USER_USER_OBJECTS_H_
 #define MUJOCO_SRC_USER_USER_OBJECTS_H_
 
+#include <cstdlib>
 #include <functional>
 #include <map>
 #include <string>
@@ -27,6 +28,7 @@
 #include <mujoco/mjplugin.h>
 #include "user/user_api.h"
 #include "user/user_cache.h"
+#include "user/user_util.h"
 
 // forward declarations of all mjC/X classes
 class mjCError;
@@ -149,7 +151,7 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ {
     int axis = 0;
 
     bool operator()(const BVElement& e1, const BVElement& e2) const {
-      if (e1.lpos[axis] != e2.lpos[axis]) {
+      if (std::abs(e1.lpos[axis] - e2.lpos[axis]) > mjEPS) {
         return e1.lpos[axis] < e2.lpos[axis];
       }
       // comparing pointers gives a stable sort, because they both come from the same array

test/user/user_objects_test.cc

@@ -1714,5 +1714,53 @@ TEST_F(MujocoTest, Frame) {
   mj_deleteData(d);
 }
 
+// ------------- test bvh ------------------------------------------------------
+TEST_F(MujocoTest, RobustBVH) {
+  static constexpr char xml1[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <geom size=".1" pos="0 0 0"/>
+        <geom size=".1" pos="0 1 0"/>
+        <geom size=".1" pos="1 0 0"/>
+        <geom size=".1" pos="1 1 0"/>
+        <geom size=".1" pos="2 0 0"/>
+        <geom size=".1" pos="2 1 0"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+
+  static constexpr char xml2[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <geom size=".1" pos="0 0 0"/>
+        <geom size=".1" pos="0 1 0"/>
+        <geom size=".1" pos="1.00000000000001 0 0"/>
+        <geom size=".1" pos="1 1.00000000000001 0"/>
+        <geom size=".1" pos="2 0 0"/>
+        <geom size=".1" pos="2 1 0"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+
+  std::array<char, 1024> error;
+  mjModel* m1 = LoadModelFromString(xml1, error.data(), error.size());
+  EXPECT_THAT(m1, testing::NotNull()) << error.data();
+
+  mjModel* m2 = LoadModelFromString(xml2, error.data(), error.size());
+  EXPECT_THAT(m2, testing::NotNull()) << error.data();
+
+  EXPECT_EQ(m1->nbvh, m2->nbvh);
+  for (int i = 0; i < m1->nbvh; i++) {
+    EXPECT_EQ(m1->bvh_nodeid[i], m2->bvh_nodeid[i]);
+  }
+
+  mj_deleteModel(m1);
+  mj_deleteModel(m2);
+}
+
 }  // namespace
 }  // namespace mujoco