diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c
index cb58530d..3c2df6c3 100644
--- a/src/engine/engine_collision_gjk.c
+++ b/src/engine/engine_collision_gjk.c
@@ -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;
}
diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc
index 934a034e..5eb5f6df 100644
--- a/test/engine/engine_collision_gjk_test.cc
+++ b/test/engine/engine_collision_gjk_test.cc
@@ -1152,6 +1152,74 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD12) {
mj_deleteModel(model);
}
+TEST_F(MjGjkTest, BoxBoxMultiCCD13) {
+ static constexpr char xml[] = R"(
+
+
+
+
+
+ )";
+
+ std::array 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 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"(
diff --git a/test/engine/testdata/collision_convex/dice.xml b/test/engine/testdata/collision_convex/dice.xml
index 2a3b6880..5f077f4c 100644
--- a/test/engine/testdata/collision_convex/dice.xml
+++ b/test/engine/testdata/collision_convex/dice.xml
@@ -25,7 +25,7 @@
- >
+
@@ -37,7 +37,7 @@
-
+
diff --git a/test/engine/testdata/collision_convex/dice_boxmesh.xml b/test/engine/testdata/collision_convex/dice_boxmesh.xml
new file mode 100644
index 00000000..cfd3ae86
--- /dev/null
+++ b/test/engine/testdata/collision_convex/dice_boxmesh.xml
@@ -0,0 +1,47 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+