Skip to content

Commit b1f8d44

Browse files
kbayescopybara-github
kbayes
authored and
copybara-github
committed
Fix bug in replaceSimplex3 in nativeccd.
PiperOrigin-RevId: 746014344 Change-Id: I93b32ab5f67ee48ab72238de53f7cda07638cb79
1 parent 25126e8 commit b1f8d44

File tree

4 files changed

+123
-14
lines changed

4 files changed

+123
-14
lines changed

‎src/engine/engine_collision_gjk.c‎

Lines changed: 6 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -834,22 +834,16 @@ static void S1D(mjtNum lambda[2], const mjtNum s1[3], const mjtNum s2[3]) {
834834

835835
// replace a 3-simplex with one of its faces
836836
static inline void replaceSimplex3(Polytope* pt, mjCCDStatus* status, int v1, int v2, int v3) {
837+
// reset status simplex
837838
status->nsimplex = 3;
838-
Vertex* v = pt->verts;
839-
copy3(status->simplex[0].vert1, v[v1].vert1);
840-
copy3(status->simplex[1].vert1, v[v2].vert1);
841-
copy3(status->simplex[2].vert1, v[v3].vert1);
842-
843-
copy3(status->simplex[0].vert2, v[v1].vert2);
844-
copy3(status->simplex[1].vert2, v[v2].vert2);
845-
copy3(status->simplex[2].vert2, v[v3].vert2);
846-
847-
copy3(status->simplex[0].vert, v[v1].vert);
848-
copy3(status->simplex[1].vert, v[v2].vert);
849-
copy3(status->simplex[2].vert, v[v3].vert);
839+
status->simplex[0] = pt->verts[v1];
840+
status->simplex[1] = pt->verts[v2];
841+
status->simplex[2] = pt->verts[v3];
850842

843+
// reset polytope
851844
pt->nfaces = 0;
852845
pt->nverts = 0;
846+
pt->nmap = 0;
853847
}
854848

855849

‎test/engine/engine_collision_gjk_test.cc‎

Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1152,6 +1152,74 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD12) {
11521152
mj_deleteModel(model);
11531153
}
11541154

1155+
TEST_F(MjGjkTest, BoxBoxMultiCCD13) {
1156+
static constexpr char xml[] = R"(
1157+
<mujoco>
1158+
<worldbody>
1159+
<geom name="geom1" type="box" pos="0 0 0" size="0.02 0.02 0.02"/>
1160+
<geom name="geom2" type="box" pos="0 0 0" size="0.02 0.02 0.02"/>
1161+
</worldbody>
1162+
</mujoco>)";
1163+
1164+
std::array<char, 1000> error;
1165+
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
1166+
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
1167+
1168+
mjData* data = mj_makeData(model);
1169+
mj_forward(model, data);
1170+
1171+
mjtNum* xpos = data->geom_xpos;
1172+
mjtNum* xmat = data->geom_xmat;
1173+
1174+
xmat[0] = 1.0000000000000000000000000000000000000000;
1175+
xmat[1] = -0.0000000000000000000000000000001366192847;
1176+
xmat[2] = -0.0000000000000002451235402041944571528177;
1177+
xmat[3] = 0.0000000000000000000000000000001366707863;
1178+
xmat[4] = 1.0000000000000000000000000000000000000000;
1179+
xmat[5] = 0.0000000000000000002101047324941978855126;
1180+
xmat[6] = 0.0000000000000002451235402041944571528177;
1181+
xmat[7] = -0.0000000000000000002101047324941978855126;
1182+
xmat[8] = 1.0000000000000000000000000000000000000000;
1183+
1184+
xpos[0] = -0.1000000000000000055511151231257827021182;
1185+
xpos[1] = -0.2000000000000000111022302462515654042363;
1186+
xpos[2] = -0.0809921810760001470441693527391180396080;
1187+
1188+
xpos = data->geom_xpos + 3;
1189+
xmat = data->geom_xmat + 9;
1190+
1191+
xmat[0] = 1.0000000000000000000000000000000000000000;
1192+
xmat[1] = -0.0000000000000000000000000000000740327228;
1193+
xmat[2] = -0.0000000000000002557259745463766308177658;
1194+
xmat[3] = 0.0000000000000000000000000000000775428823;
1195+
xmat[4] = 1.0000000000000000000000000000000000000000;
1196+
xmat[5] = 0.0000000000000000137262533997081760161613;
1197+
xmat[6] = 0.0000000000000002557259745463766308177658;
1198+
xmat[7] = -0.0000000000000000137262533997081760161613;
1199+
xmat[8] = 1.0000000000000000000000000000000000000000;
1200+
1201+
xpos[0] = -0.1000000000000000055511151231257827021182;
1202+
xpos[1] = -0.2000000000000000111022302462515654042363;
1203+
xpos[2] = -0.0418396695286432432348000531874276930466;
1204+
1205+
int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
1206+
int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
1207+
1208+
mjCCDStatus status;
1209+
std::vector<mjtNum> dir, pos;
1210+
mjtNum dist;
1211+
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 8);
1212+
1213+
EXPECT_EQ(ncons, 4);
1214+
1215+
EXPECT_NEAR(dir[0], 0, kTolerance);
1216+
EXPECT_NEAR(dir[1], 0, kTolerance);
1217+
EXPECT_NEAR(dir[2], 1, kTolerance);
1218+
1219+
mj_deleteData(data);
1220+
mj_deleteModel(model);
1221+
}
1222+
11551223
TEST_F(MjGjkTest, SmallBoxMesh) {
11561224
static constexpr char xml[] = R"(
11571225
<mujoco>

‎test/engine/testdata/collision_convex/dice.xml‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@
2525
<worldbody>
2626
<light pos="0 0 1" dir="0 0 -1" castshadow="false"/>
2727
<body name="container" pos="0 0 -0.11">
28-
<geom class="floor" size="0.27 0.27 0.01"/>>
28+
<geom class="floor" size="0.27 0.27 0.01"/>
2929
<geom class="wall" size="0.26 0.01 0.10" pos="0.01 0.26 0.11"/>
3030
<geom class="wall" size="0.26 0.01 0.10" pos="-0.01 -0.26 0.11"/>
3131
<geom class="wall" size="0.01 0.26 0.10" pos="0.26 -0.01 0.11"/>
@@ -37,7 +37,7 @@
3737
<replicate count="4" offset="0 0 .1">
3838
<body pos="-.2 -.2 0.15">
3939
<freejoint/>
40-
<geom size=".02 .02 .02" type="mesh" mesh="die"/>
40+
<geom type="mesh" mesh="die"/>
4141
</body>
4242
</replicate>
4343
</replicate>

‎test/engine/testdata/collision_convex/dice_boxmesh.xml‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
<mujoco>
2+
<option>
3+
<flag multiccd="enable"/>
4+
</option>
5+
6+
<visual>
7+
<scale forcewidth="0.01" contactwidth="0.05" contactheight="0.05"/>
8+
<map force="0.1"/>
9+
</visual>
10+
11+
<asset>
12+
<mesh name="boxmesh" scale=".02 .02 .02"
13+
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"/>
14+
</asset>
15+
16+
<default>
17+
<geom condim="1" type="mesh" solref="2e-5 1"/>
18+
<default class="wall">
19+
<geom type="box" rgba="0.1 0.2 1 .2"/>
20+
</default>
21+
<default class="floor">
22+
<geom type="box" rgba="0.1 0.2 1 1"/>
23+
</default>
24+
</default>
25+
26+
<worldbody>
27+
<light pos="0 0 1" dir="0 0 -1" castshadow="false"/>
28+
<body name="container" pos="0 0 -0.11">
29+
<geom class="floor" size="0.27 0.27 0.01"/>
30+
<geom class="wall" size="0.26 0.01 0.10" pos="0.01 0.26 0.11"/>
31+
<geom class="wall" size="0.26 0.01 0.10" pos="-0.01 -0.26 0.11"/>
32+
<geom class="wall" size="0.01 0.26 0.10" pos="0.26 -0.01 0.11"/>
33+
<geom class="wall" size="0.01 0.26 0.10" pos="-0.26 0.01 0.11"/>
34+
</body>
35+
36+
<replicate count="5" offset=".1 0 0">
37+
<replicate count="5" offset="0 .1 0">
38+
<replicate count="4" offset="0 0 .1">
39+
<body pos="-.2 -.2 0.15">
40+
<freejoint/>
41+
<geom type="mesh" mesh="boxmesh"/>
42+
</body>
43+
</replicate>
44+
</replicate>
45+
</replicate>
46+
</worldbody>
47+
</mujoco>

0 commit comments

Comments
 (0)