From 0657e3e871a4fbf0f447a273ce80d015b6734385 Mon Sep 17 00:00:00 2001
From: Kyle Bayes <kylebayes@google.com>
Date: Wed, 18 Sep 2024 05:19:00 -0700
Subject: [PATCH] Fix bug in DeleteFace in NativeCCD.

PiperOrigin-RevId: 675948504
Change-Id: If9d967c8e75580a3a002fce9c10a2e1bfd0b5d46
---
 src/engine/engine_collision_gjk.c | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c
index 20c2f6d7..b1b36e8f 100644
--- a/src/engine/engine_collision_gjk.c
+++ b/src/engine/engine_collision_gjk.c
@@ -898,7 +898,7 @@ static int newVertex(Polytope* pt, const mjtNum v1[3], const mjtNum v2[3]) {
 
 
 // swap two nodes in heap
-inline void swap(Polytope* pt, int i, int j) {
+static inline void swap(Polytope* pt, int i, int j) {
   Face* tmp = pt->heap[i];
   pt->heap[i] = pt->heap[j];
   pt->heap[j] = tmp;
@@ -927,11 +927,19 @@ void heapify(Polytope* pt, int i) {
 
 // delete face from heap
 void deleteFace(Polytope* pt, Face* face) {
+  if (!pt->nheap) {
+    return;
+  }
+
+  face->dist = -1;
   pt->nheap--;
-  if (pt->nheap < 1) return;
+
+  // last face; nothing to do
+  if (!pt->nheap) {
+    return;
+  }
 
   // bubble up face to top of heap
-  face->dist = -1;
   int i = face->index;
   while (i != 0) {
     int parent = (i - 1) >> 1;