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

Fix bug in horizon code in nativeccd where parallel faces are included in horizon.

Browse Source 
PiperOrigin-RevId: 742687622
Change-Id: I4a180cb9c7451e0a2c22ffd77b7b6f6fd68836a4
This commit is contained in:
Kyle Bayes
2025-04-01 07:20:17 -07:00
committed by Copybara-Service
parent 305b68ecbf
commit 08d22baea1
2 changed files with 67 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+1 -1
View File
@@ -1271,7 +1271,7 @@ static int horizonRec(Polytope* pt, Face* face, int e) {
mjtNum dist2 = face->dist * face->dist;
// v is visible from w so it is deleted and adjacent faces are checked
if (dot3(face->v, pt->horizon.w) >= dist2) {
if (dot3(face->v, pt->horizon.w) > dist2) {
deleteFace(pt, face);
// recursively search the adjacent faces on the next two edges
test/engine/engine_collision_gjk_test.cc
+66
View File
@@ -1022,6 +1022,72 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD10) {
mj_deleteModel(model);
}
TEST_F(MjGjkTest, BoxBoxMultiCCD11) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" pos="0 0 0" size="0.025 0.025 0.025"/>
<geom name="geom2" type="box" pos="0 0 0" size="0.025 0.025 0.025"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
mj_forward(model, data);
mjtNum* xpos = data->geom_xpos;
mjtNum* xmat = data->geom_xmat;
xmat[0] = 1.0000000000000000000000000000000000000000;
xmat[1] = 0.0000000000000000000000000000000000000000;
xmat[2] = 0.0000000000000000000000000000000000000000;
xmat[3] = 0.0000000000000000000000000000000000000000;
xmat[4] = 1.0000000000000000000000000000000000000000;
xmat[5] = -0.0000000000000000013928437397151766790940;
xmat[6] = 0.0000000000000000000000000000000000000000;
xmat[7] = 0.0000000000000000013928437397151766790940;
xmat[8] = 1.0000000000000000000000000000000000000000;
xpos[0] = -0.1036549999999999971400654885655967518687;
xpos[1] = -0.1963450000000000195132798808117513544858;
xpos[2] = 0.1247685038468368534658736734854755923152;
xpos = data->geom_xpos + 3;
xmat = data->geom_xmat + 9;
xmat[0] = 1.0000000000000000000000000000000000000000;
xmat[1] = 0.0000000000000000000000000000000000000000;
xmat[2] = 0.0000000000000000000000000000000000000000;
xmat[3] = 0.0000000000000000000000000000000000000000;
xmat[4] = 1.0000000000000000000000000000000000000000;
xmat[5] = -0.0000000000000000018885268354605779111974;
xmat[6] = 0.0000000000000000000000000000000000000000;
xmat[7] = 0.0000000000000000018885268354605779111974;
xmat[8] = 1.0000000000000000000000000000000000000000;
xpos[0] = -0.1036549999999999971400654885655967518687;
xpos[1] = -0.1963450000000000195132798808117513544858;
xpos[2] = 0.1745248497897437800485676007156143896282;
int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 8);
EXPECT_EQ(ncons, 4);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, SmallBoxMesh) {
static constexpr char xml[] = R"(
<mujoco>