diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c
index 06da687f..2dc048e6 100644
--- a/src/engine/engine_collision_convex.c
+++ b/src/engine/engine_collision_convex.c
@@ -889,6 +889,28 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9],
+// return true if multiccd can run in a single pass
+static int singlePass(const mjCCDObj* obj1, const mjCCDObj* obj2) {
+ const mjModel* m = obj1->model;
+
+ // single pass not supported for margins
+ if (obj1->margin > 0 || obj2->margin > 0) {
+ return 0;
+ }
+
+ // supported geoms for single pass
+ int type1 = m->geom_type[obj1->geom];
+ int type2 = m->geom_type[obj2->geom];
+ if (type1 == mjGEOM_BOX || type1 == mjGEOM_MESH) {
+ if (type2 == mjGEOM_BOX || type2 == mjGEOM_MESH) {
+ return 1;
+ }
+ }
+ return 0;
+}
+
+
+
// multi-point convex-convex collision, using libccd
int mjc_Convex(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
@@ -897,7 +919,8 @@ int mjc_Convex(const mjModel* m, const mjData* d,
mjc_initCCDObj(&obj1, m, d, g1, margin);
mjc_initCCDObj(&obj2, m, d, g2, margin);
int max_contacts = 1;
- if (mjENABLED(mjENBL_MULTICCD)) {
+
+ if (mjENABLED(mjENBL_MULTICCD) && singlePass(&obj1, &obj2)) {
max_contacts = 4;
}
@@ -905,9 +928,7 @@ int mjc_Convex(const mjModel* m, const mjData* d,
int ncon = mjc_CCDIteration(m, d, &obj1, &obj2, con, max_contacts, margin);
// nativeccd supports multi Box-Box collision directly
- if (mjENABLED(mjENBL_NATIVECCD) &&
- (m->geom_type[g1] == mjGEOM_BOX || m->geom_type[g1] == mjGEOM_MESH) &&
- (m->geom_type[g2] == mjGEOM_BOX || m->geom_type[g2] == mjGEOM_MESH)) {
+ if (mjENABLED(mjENBL_NATIVECCD) && singlePass(&obj1, &obj2)) {
return ncon;
}
diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c
index 1603746c..8e57d12c 100644
--- a/src/engine/engine_collision_gjk.c
+++ b/src/engine/engine_collision_gjk.c
@@ -2222,39 +2222,46 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m
obj1->geom_type == mjGEOM_CAPSULE || obj2->geom_type == mjGEOM_CAPSULE) {
void (*support1)(mjtNum*, struct _mjCCDObj*, const mjtNum*) = obj1->support;
void (*support2)(mjtNum*, struct _mjCCDObj*, const mjtNum*) = obj2->support;
- mjtNum margin1 = 0, margin2 = 0;
+ mjtNum full_margin1 = 0, full_margin2 = 0;
+ mjtNum margin1 = obj1->margin, margin2 = obj2->margin;
if (obj1->geom_type == mjGEOM_SPHERE) {
const mjModel* m = obj1->model;
- margin1 = m->geom_size[3*obj1->geom];
- support1 = obj1->support;
+ full_margin1 = m->geom_size[3*obj1->geom] + 0.5*margin1;
obj1->support = mjc_pointSupport;
+ obj1->margin = 0;
} else if (obj1->geom_type == mjGEOM_CAPSULE) {
const mjModel* m = obj1->model;
- margin1 = m->geom_size[3*obj1->geom];
- support1 = obj1->support;
+ full_margin1 = m->geom_size[3*obj1->geom] + 0.5*margin1;
obj1->support = mjc_lineSupport;
+ obj1->margin = 0;
}
if (obj2->geom_type == mjGEOM_SPHERE) {
const mjModel* m = obj2->model;
- margin2 = m->geom_size[3*obj2->geom];
- support2 = obj2->support;
+ full_margin2 = m->geom_size[3*obj2->geom] + 0.5*margin2;
obj2->support = mjc_pointSupport;
+ obj2->margin = 0;
} else if (obj2->geom_type == mjGEOM_CAPSULE) {
const mjModel* m = obj2->model;
- margin2 = m->geom_size[3*obj2->geom];
- support2 = obj2->support;
+ full_margin2 = m->geom_size[3*obj2->geom] + 0.5*margin2;
obj2->support = mjc_lineSupport;
+ obj2->margin = 0;
}
- status->dist_cutoff += margin1 + margin2;
+ status->dist_cutoff += full_margin1 + full_margin2;
gjk(status, obj1, obj2);
status->dist_cutoff = config->dist_cutoff;
+ // restore original margin and support
+ obj1->margin = margin1;
+ obj2->margin = margin2;
+ obj1->support = support1;
+ obj2->support = support2;
+
// shallow penetration, inflate contact
- if (status->dist > 0) {
- inflate(status, margin1, margin2);
+ if (status->dist > status->tolerance) {
+ inflate(status, full_margin1, full_margin2);
if (status->dist > status->dist_cutoff) {
status->dist = mjMAXVAL;
}
@@ -2268,14 +2275,11 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m
return 0;
}
- // deep penetration, reset everything and run GJK again
+ // deep penetration, reset initial conditions and rerun GJK + EPA
status->gjk_iterations = 0;
- obj1->support = support1;
- obj2->support = support2;
obj1->center(status->x1, obj1);
obj2->center(status->x2, obj2);
}
-
gjk(status, obj1, obj2);
// penetration recovery for contacts not needed
diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc
index f54e0ca2..333e4da0 100644
--- a/test/engine/engine_collision_gjk_test.cc
+++ b/test/engine/engine_collision_gjk_test.cc
@@ -1568,5 +1568,38 @@ TEST_F(MjGjkTest, CapsuleCapsule) {
mj_deleteModel(model);
}
+TEST_F(MjGjkTest, CylinderBoxMargin) {
+ 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);
+
+ EXPECT_EQ(data->ncon, 1);
+ EXPECT_LT(data->contact[0].efc_address, 0);
+
+ mj_deleteData(data);
+ mj_deleteModel(model);
+}
+
} // namespace
} // namespace mujoco