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

Terminate early without contact in EPA if upper < lower on first iteration.

Browse Source 
PiperOrigin-RevId: 929828707
Change-Id: Ide1106c14ae6eb003a9bbc238609b4bfdde221b0
This commit is contained in:
Kyle Bayes
2026-06-10 07:01:29 -07:00
committed by Copybara-Service
parent 711aa9fcd8
commit 6957966c7d
2 changed files with 62 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+4
View File
@@ -1393,6 +1393,10 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
upper2 = upper * upper;
}
if (upper - lower < tolerance) {
// terminate without contact when upper < lower on first iteration
if (k == 0 && upper < lower) {
face = NULL;
}
break;
}
test/engine/engine_collision_gjk_test.cc
+58
View File
@@ -1353,6 +1353,64 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD14) {
ASSERT_EQ(ncons, 4);
}
TEST_F(MjGjkTest, BoxBoxMultiCCD15) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" size="0.5 0.5 0.1"/>
<geom name="geom2" type="box" size="0.025 0.025 0.025"/>
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat;
mjtNum* xpos = data->geom_xpos;
xmat[0] = 1.0;
xmat[1] = 0.0;
xmat[2] = 0.0;
xmat[3] = 0.0;
xmat[4] = 1.0;
xmat[5] = 0.0;
xmat[6] = 0.0;
xmat[7] = 0.0;
xmat[8] = 1.0;
xpos[0] = 0.0;
xpos[1] = 0.0;
xpos[2] = 0.0;
xmat = data->geom_xmat + 9;
xpos = data->geom_xpos + 3;
xmat[0] = 1.0;
xmat[1] = 1.62423755001306e-10;
xmat[2] = -1.73500047822017e-05;
xmat[3] = 1.44241105171083e-10;
xmat[4] = 1.0;
xmat[5] = 1.76752037077677e-05;
xmat[6] = 1.73500047822017e-05;
xmat[7] = -1.76752037077677e-05;
xmat[8] = 1.0;
xpos[0] = 0.0520339831709862;
xpos[1] = -0.0520339831709862;
xpos[2] = 0.124986477196217;
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model.get(), mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 4);
ASSERT_EQ(ncons, sizeof(mjtNum) == 8 ? 4 : 0);
}
TEST_F(MjGjkTest, SmallBoxMesh) {
static constexpr char xml[] = R"(
<mujoco>