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

Fix bug in nativeccd multiccd.

Browse Source 
PiperOrigin-RevId: 740495321
Change-Id: I7eaa0da8514e27c233d49567c276d1d238808d1f
This commit is contained in:
Kyle Bayes
2025-03-25 14:55:25 -07:00
committed by Copybara-Service
parent 69f8349203
commit d6943731b2
2 changed files with 41 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
@@ -1546,7 +1546,7 @@ static mjtNum planeNormal(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3],
// find what side of a plane a point p lies
static int halfspace(const mjtNum a[3], const mjtNum n[3], const mjtNum p[3]) {
mjtNum diff[3] = {p[0] - a[0], p[1] - a[1], p[2] - a[2]};
return dot3(diff, n) > 0;
return dot3(diff, n) >= 0.0;
}
test/engine/engine_collision_gjk_test.cc
+40
View File
@@ -951,6 +951,46 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD9) {
mj_deleteModel(model);
}
TEST_F(MjGjkTest, BoxBoxMultiCCD10) {
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;
xpos[0] = -0.1034859999999999946584949839234468527138;
xpos[1] = -0.0765140000000000264357424839545274153352;
xpos[2] = 0.1257628745456405572333835607423679903150;
xpos = data->geom_xpos + 3;
xpos[0] = -0.1034859999999999946584949839234468527138;
xpos[1] = -0.0765140000000000264357424839545274153352;
xpos[2] = 0.1751399999999999623767621415026951581240;
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 8);
EXPECT_EQ(ncons, 4);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, SmallBoxMesh) {
static constexpr char xml[] = R"(
<mujoco>