Terminate early without contact in EPA if upper < lower on first iteration.

PiperOrigin-RevId: 929828707 Change-Id: Ide1106c14ae6eb003a9bbc238609b4bfdde221b0
2026-06-10 07:01:29 -07:00
parent 711aa9fcd8
commit 6957966c7d
2 changed files with 62 additions and 0 deletions
@@ -1393,6 +1393,10 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
      upper2 = upper * upper;
    }
    if (upper - lower < tolerance) {
+      // terminate without contact when upper < lower on first iteration
+      if (k == 0 && upper < lower) {
+        face = NULL;
+      }
      break;
    }