Fix early termination in GJK. It's possible for x_k to be the origin, so we should run the previous test to conclude that the objects are colliding.

PiperOrigin-RevId: 673834101
Change-Id: I596b30d2847670261f8067840b34dc13de401ecb

src/engine/engine_collision_gjk.c

@@ -141,11 +141,6 @@ 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];
@@ -154,6 +149,12 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
       break;
     }
 
+    // check if hyperplane is separating the Minkowski difference and the origin;
+    // if so the objects don't collide, so return early if geom distance isn't needed
+    if (!get_dist && mju_dot3(x_k, s_k) > 0) {
+      return mjMAXVAL;
+    }
+
     // TODO(kylebayes): signedVolume has been written to assume the first vertex is the latest
     // support to be added. Once the logic has been updated, then this hack should be removed.
     for (int i = n; i > 0; i--) {
@@ -1165,8 +1166,8 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m
     } else {
       dist = 0;
     }
-    mju_free(pt.faces);
-    mju_free(pt.verts);
+    free(pt.faces);
+    free(pt.verts);
   }
   return dist;
 }