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

Fix a box-box bad collision in NativeCCD.

Browse Source 
PiperOrigin-RevId: 713683497
Change-Id: I0c4cfd55d00faa196d8759046c87ee4ef74f01ab
This commit is contained in:
Kyle Bayes
2025-01-09 08:34:49 -08:00
committed by Copybara-Service
parent 90049343b9
commit daba00ca9d
2 changed files with 68 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+4 -4
View File
@@ -1104,19 +1104,19 @@ static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
int v4 = newVertex(pt, status->simplex1 + 9, status->simplex2 + 9);
// if the origin is on a face, replace the 3-simplex with a 2-simplex
if (attachFace(pt, v1, v2, v3, 1, 3, 2) == 0.0) {
if (attachFace(pt, v1, v2, v3, 1, 3, 2) < mjMINVAL) {
replaceSimplex3(pt, status, v1, v2, v3);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v1, v4, v2, 2, 3, 0) == 0.0) {
if (attachFace(pt, v1, v4, v2, 2, 3, 0) < mjMINVAL) {
replaceSimplex3(pt, status, v1, v4, v2);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v1, v3, v4, 0, 3, 1) == 0.0) {
if (attachFace(pt, v1, v3, v4, 0, 3, 1) < mjMINVAL) {
replaceSimplex3(pt, status, v1, v3, v4);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v4, v3, v2, 2, 0, 1) == 0.0) {
if (attachFace(pt, v4, v3, v2, 2, 0, 1) < mjMINVAL) {
replaceSimplex3(pt, status, v4, v3, v2);
return polytope3(pt, status, obj1, obj2);
}
test/engine/engine_collision_gjk_test.cc
+64
View File
@@ -335,6 +335,70 @@ TEST_F(MjGjkTest, BoxBoxDepth2) {
mj_deleteModel(model);
}
TEST_F(MjGjkTest, BoxBoxDepth3) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" pos="0 0 0" size="0.25 0.25 0.05"/>
<geom name="geom2" type="box" pos="0 0 0" size="0.25 0.25 0.05"/>
</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* xmat = data->geom_xmat;
mjtNum* xpos = data->geom_xpos;
xmat[0] = 0.965925826289068201191412299522;
xmat[1] = -0.258819045102520739476403832668;
xmat[2] = 0.000000000000000006339100926609;
xmat[3] = 0.258819045102520739476403832668;
xmat[4] = 0.965925826289068201191412299522;
xmat[5] = -0.000000000000000214827792362716;
xmat[6] = 0.000000000000000049478422780336;
xmat[7] = 0.000000000000000209148392896446;
xmat[8] = 1.000000000000000000000000000000;
xpos[0] = -0.015346499999999199323474918799;
xpos[1] = -0.023505500000000002086553152481;
xpos[2] = -4.562296442400120888294168253196;
xmat = data->geom_xmat + 9;
xpos = data->geom_xpos + 3;
xmat[0] = 0.866025403784438707610604524234;
xmat[1] = -0.499999999999999944488848768742;
xmat[2] = 0.000000000000000018716705841316;
xmat[3] = 0.499999999999999944488848768742;
xmat[4] = 0.866025403784438707610604524234;
xmat[5] = -0.000000000000000263161736875730;
xmat[6] = 0.000000000000000115371725704125;
xmat[7] = 0.000000000000000237263102359077;
xmat[8] = 1.000000000000000000000000000000;
xpos[0] = -0.015346499999999797803074130798;
xpos[1] = -0.023505499999999998617106200527;
xpos[2] = -4.659230360891631228525966434972;
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjtNum dir[3], pos[3];
mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos);
EXPECT_NEAR(dist, -0.003066, kTolerance);
EXPECT_NEAR(dir[0], 0, kTolerance);
EXPECT_NEAR(dir[1], 0, kTolerance);
EXPECT_NEAR(dir[2], -1, kTolerance);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, BoxBoxTouching) {
static constexpr char xml[] = R"(
<mujoco>