diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 56d9672d..29b3530e 100644 --- a/src/user/user_objects.cc +++ b/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; - axis = edges[axis] > edges[2] ? axis : 2; + if (edges[1] >= edges[0] + mjEPS) axis = 1; + if (edges[2] >= edges[axis] + mjEPS) axis = 2; // find median along the axis // note: nth_element performs a partial sort of elements diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 1166fb83..f6ed8613 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -15,6 +15,7 @@ #ifndef MUJOCO_SRC_USER_USER_OBJECTS_H_ #define MUJOCO_SRC_USER_USER_OBJECTS_H_ +#include #include #include #include @@ -27,6 +28,7 @@ #include #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 diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index 634a70bb..4e1a7ea1 100644 --- a/test/user/user_objects_test.cc +++ b/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"( + + + + + + + + + + + + + )"; + + static constexpr char xml2[] = R"( + + + + + + + + + + + + + )"; + + std::array 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