From bd1bbfe4014657f7ae930822a16351668582898e Mon Sep 17 00:00:00 2001
From: Kyle Bayes <kylebayes@google.com>
Date: Tue, 21 Jan 2025 02:50:05 -0800
Subject: [PATCH] Fix bug where EPA horizon returns zero edges due to numerical
 inaccuracies in float32.

PiperOrigin-RevId: 717825833
Change-Id: I9fa440816765d4c3eaa0c6c26ec0bd90c774fa01
---
 src/engine/engine_collision_gjk.c | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c
index 48472675..ec81a722 100644
--- a/src/engine/engine_collision_gjk.c
+++ b/src/engine/engine_collision_gjk.c
@@ -1347,6 +1347,15 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
     h.w = w;
     horizon(&h, face);
 
+    // unrecoverable numerical issue; at least one face was deleted so nedges is 3 or more
+    if (h.nedges < 3) {
+      mj_freeStack(d);
+      status->epa_iterations = k;
+      status->nx = 0;
+      status->dist = 0;
+      return NULL;
+    }
+
     // insert w as new vertex and attach faces along the horizon
     int wi = newVertex(pt, w1, w2), nfaces = pt->nfaces, nedges = h.nedges;