Provide better dedupe logic in multiccd in edge collision case.

PiperOrigin-RevId: 928519426
Change-Id: I67531d1c346b1b38c098eaf32c3b13fa7d0c8eaf

src/engine/engine_collision_gjk.c

@@ -1663,24 +1663,37 @@ static void polygonClip(mjCCDStatus* status, const mjtNum* face1, int nface1,
     return;
   }
 
-  // no pruning needed
+  // if the face is an edge, remove potential duplicates
-  int k = 0;
+  if (nface2 == 2 && npolygon > 2) {
-  for (int i = 0; i < 3*npolygon; i += 3) {
+    // find the two most distant vertices in the polygon
-    int skip = 0;
+    int best1 = 0, best2 = 1;
-
+    mjtNum d = 0;
-    // find possible duplicate vertices
+    for (int i = 0; i < npolygon; i++) {
-    for (int j = 0; j < k; j += 3) {
+      for (int j = i + 1; j < npolygon; j++) {
-      if (equal3(status->x2 + j, polygon + i)) {
+        mjtNum diff[3];
-        skip = 1;
+        sub3(diff, polygon + 3*j, polygon + 3*i);
-        break;
+        mjtNum d2 = dot3(diff, diff);
+        if (d2 > d) {
+          d = d2;
+          best1 = i;
+          best2 = j;
+        }
       }
     }
-    if (skip) continue;
+    copy3(status->x2, polygon + 3*best1);
-    copy3(status->x2 + k, polygon + i);
+    sub3(status->x1, status->x2, dir);
-    sub3(status->x1 + k, status->x2 + k, dir);
+    copy3(status->x2 + 3, polygon + 3*best2);
-    k += 3;
+    sub3(status->x1 + 3, status->x2 + 3, dir);
+    status->nx = 2;
+    return;
   }
-  status->nx = k/3;
+
+  // no pruning needed
+  for (int i = 0; i < 3*npolygon; i += 3) {
+    copy3(status->x2 + i, polygon + i);
+    sub3(status->x1 + i, status->x2 + i, dir);
+  }
+  status->nx = npolygon;
 }