chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Make BVH generation robust to very small perturbations.

Browse Source 
PiperOrigin-RevId: 629070367
Change-Id: I62b91a572cd59a51ed85e4e97aea53fdb2da1986
This commit is contained in:
Alessio Quaglino
2024-04-29 08:05:22 -07:00
committed by Copybara-Service
parent d6553ed192
commit 618d879904
3 changed files with 55 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_objects.cc
+4 -3
View File
@@ -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
src/user/user_objects.h
+3 -1
View File
@@ -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
+48
View File
@@ -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