Exit GJK early if geom distance doesn't need to be recovered.

PiperOrigin-RevId: 672334991
Change-Id: I4af582c799140ccf3ac4d51952e547b08ab5d55e

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);
 

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

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);