diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index a9b657c2..db94e99f 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -125,6 +125,7 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { mjtNum* x2_k = status->x2; mju_sub3(x_k, x1_k, x2_k); mjtNum epsilon = status->tolerance * status->tolerance; + int get_dist = status->has_distances; // if both geoms are discrete, finite convergence is guaranteed; set tolerance to 0 if (discreteGeoms(obj1, obj2)) { @@ -141,6 +142,11 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { gjkSupport(s1, s2, obj1, obj2, x_k); mju_sub3(s_k, s1, s2); + // return early if geom distance isn't needed + if (!get_dist && mju_dot3(x_k, s_k) > 0) { + return mjMAXVAL; + } + // the stopping criteria relies on the Frank-Wolfe duality gap given by // |f(x_k) - f(x_min)|^2 <= < grad f(x_k), (x_k - s_k) > mjtNum diff[3]; @@ -1116,6 +1122,8 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m status->epa_iterations = -1; status->tolerance = config->tolerance; status->max_iterations = config->max_iterations; + status->has_contacts = config->contacts; + status->has_distances = config->distances; mjtNum dist = gjk(status, obj1, obj2); diff --git a/src/engine/engine_collision_gjk.h b/src/engine/engine_collision_gjk.h index 47e1e539..fcb11ace 100644 --- a/src/engine/engine_collision_gjk.h +++ b/src/engine/engine_collision_gjk.h @@ -40,6 +40,8 @@ struct _mjCCDStatus { // configurations used int max_iterations; // the maximum number of iterations for GJK and EPA mjtNum tolerance; // tolerance used by GJK and EPA + int has_contacts; // set to true if attempted to recover contact (pendetration) info + int has_distances; // set to true if attempted to recover distance info // statistics for debugging purposes int gjk_iterations; // number of iterations that GJK ran diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 970405b0..e350ee10 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -49,12 +49,19 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { mjtNum GeomDist(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3], mjtNum x2[3]) { - mjCCDConfig config = {kMaxIterations, kTolerance}; + mjCCDConfig config; + mjCCDStatus status; + + // set config + config.max_iterations = kMaxIterations, + config.tolerance = kTolerance, + config.contacts = 0; // no geom contacts needed + config.distances = 1; // no geom distances needed + mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, mjc_center, mjc_support}; mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, mjc_center, mjc_support}; - mjCCDStatus status; mjtNum dist = mjc_ccd(&config, &status, &obj1, &obj2); if (x1 != nullptr) mju_copy3(x1, status.x1); if (x2 != nullptr) mju_copy3(x2, status.x2);