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

Fix bug in replaceSimplex3 in nativeccd.

Browse Source 
PiperOrigin-RevId: 746014344
Change-Id: I93b32ab5f67ee48ab72238de53f7cda07638cb79
This commit is contained in:
Kyle Bayes
2025-04-10 07:03:23 -07:00
committed by Copybara-Service
parent 25126e88c7
commit b1f8d444f2
4 changed files with 123 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+6 -12
View File
@@ -834,22 +834,16 @@ static void S1D(mjtNum lambda[2], const mjtNum s1[3], const mjtNum s2[3]) {
// replace a 3-simplex with one of its faces
static inline void replaceSimplex3(Polytope* pt, mjCCDStatus* status, int v1, int v2, int v3) {
// reset status simplex
status->nsimplex = 3;
Vertex* v = pt->verts;
copy3(status->simplex[0].vert1, v[v1].vert1);
copy3(status->simplex[1].vert1, v[v2].vert1);
copy3(status->simplex[2].vert1, v[v3].vert1);
copy3(status->simplex[0].vert2, v[v1].vert2);
copy3(status->simplex[1].vert2, v[v2].vert2);
copy3(status->simplex[2].vert2, v[v3].vert2);
copy3(status->simplex[0].vert, v[v1].vert);
copy3(status->simplex[1].vert, v[v2].vert);
copy3(status->simplex[2].vert, v[v3].vert);
status->simplex[0] = pt->verts[v1];
status->simplex[1] = pt->verts[v2];
status->simplex[2] = pt->verts[v3];
// reset polytope
pt->nfaces = 0;
pt->nverts = 0;
pt->nmap = 0;
}
test/engine/engine_collision_gjk_test.cc
+68
View File
@@ -1152,6 +1152,74 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD12) {
mj_deleteModel(model);
}
TEST_F(MjGjkTest, BoxBoxMultiCCD13) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" pos="0 0 0" size="0.02 0.02 0.02"/>
<geom name="geom2" type="box" pos="0 0 0" size="0.02 0.02 0.02"/>
</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.0000000000000000000000000000001366192847;
xmat[2] = -0.0000000000000002451235402041944571528177;
xmat[3] = 0.0000000000000000000000000000001366707863;
xmat[4] = 1.0000000000000000000000000000000000000000;
xmat[5] = 0.0000000000000000002101047324941978855126;
xmat[6] = 0.0000000000000002451235402041944571528177;
xmat[7] = -0.0000000000000000002101047324941978855126;
xmat[8] = 1.0000000000000000000000000000000000000000;
xpos[0] = -0.1000000000000000055511151231257827021182;
xpos[1] = -0.2000000000000000111022302462515654042363;
xpos[2] = -0.0809921810760001470441693527391180396080;
xpos = data->geom_xpos + 3;
xmat = data->geom_xmat + 9;
xmat[0] = 1.0000000000000000000000000000000000000000;
xmat[1] = -0.0000000000000000000000000000000740327228;
xmat[2] = -0.0000000000000002557259745463766308177658;
xmat[3] = 0.0000000000000000000000000000000775428823;
xmat[4] = 1.0000000000000000000000000000000000000000;
xmat[5] = 0.0000000000000000137262533997081760161613;
xmat[6] = 0.0000000000000002557259745463766308177658;
xmat[7] = -0.0000000000000000137262533997081760161613;
xmat[8] = 1.0000000000000000000000000000000000000000;
xpos[0] = -0.1000000000000000055511151231257827021182;
xpos[1] = -0.2000000000000000111022302462515654042363;
xpos[2] = -0.0418396695286432432348000531874276930466;
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);
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, SmallBoxMesh) {
static constexpr char xml[] = R"(
<mujoco>
test/engine/testdata/collision_convex/dice.xml
Vendored
+2 -2
View File
@@ -25,7 +25,7 @@
<worldbody>
<light pos="0 0 1" dir="0 0 -1" castshadow="false"/>
<body name="container" pos="0 0 -0.11">
<geom class="floor" size="0.27 0.27 0.01"/>>
<geom class="floor" size="0.27 0.27 0.01"/>
<geom class="wall" size="0.26 0.01 0.10" pos="0.01 0.26 0.11"/>
<geom class="wall" size="0.26 0.01 0.10" pos="-0.01 -0.26 0.11"/>
<geom class="wall" size="0.01 0.26 0.10" pos="0.26 -0.01 0.11"/>
@@ -37,7 +37,7 @@
<replicate count="4" offset="0 0 .1">
<body pos="-.2 -.2 0.15">
<freejoint/>
<geom size=".02 .02 .02" type="mesh" mesh="die"/>
<geom type="mesh" mesh="die"/>
</body>
</replicate>
</replicate>
test/engine/testdata/collision_convex/dice_boxmesh.xml
Vendored
+47
View File
@@ -0,0 +1,47 @@
<mujoco>
<option>
<flag multiccd="enable"/>
</option>
<visual>
<scale forcewidth="0.01" contactwidth="0.05" contactheight="0.05"/>
<map force="0.1"/>
</visual>
<asset>
<mesh name="boxmesh" scale=".02 .02 .02"
vertex="-1 -1 -1 1 -1 -1 1 1 -1 1 1 1 1 -1 1 -1 1 -1 -1 1 1 -1 -1 1"/>
</asset>
<default>
<geom condim="1" type="mesh" solref="2e-5 1"/>
<default class="wall">
<geom type="box" rgba="0.1 0.2 1 .2"/>
</default>
<default class="floor">
<geom type="box" rgba="0.1 0.2 1 1"/>
</default>
</default>
<worldbody>
<light pos="0 0 1" dir="0 0 -1" castshadow="false"/>
<body name="container" pos="0 0 -0.11">
<geom class="floor" size="0.27 0.27 0.01"/>
<geom class="wall" size="0.26 0.01 0.10" pos="0.01 0.26 0.11"/>
<geom class="wall" size="0.26 0.01 0.10" pos="-0.01 -0.26 0.11"/>
<geom class="wall" size="0.01 0.26 0.10" pos="0.26 -0.01 0.11"/>
<geom class="wall" size="0.01 0.26 0.10" pos="-0.26 0.01 0.11"/>
</body>
<replicate count="5" offset=".1 0 0">
<replicate count="5" offset="0 .1 0">
<replicate count="4" offset="0 0 .1">
<body pos="-.2 -.2 0.15">
<freejoint/>
<geom type="mesh" mesh="boxmesh"/>
</body>
</replicate>
</replicate>
</replicate>
</worldbody>
</mujoco>