From 0b69d6fb6c0afcb95ad8a9f37f9beede8e422601 Mon Sep 17 00:00:00 2001
From: rootkiller6788 <c8688rickowens@outlook.com>
Date: Fri, 21 Aug 2026 16:12:08 +0800
Subject: [PATCH] Fix flex contact filtering compaction

The farthest-point sampling loop in filterFlexContacts swapped contacts in
place while the selection bookkeeping (selected[]/min_dist[]) was still
indexed against the pre-swap array. This corrupted the selection (re-selecting
already-picked contacts and skipping valid candidates) and, because the swap
was skipped for the final selected contact, left a wrong contact in the kept
prefix.

Collect the selected contacts in a temporary buffer and compact them into the
contact prefix only after the selection loop finishes, so the retained prefix
is exactly the set selected by farthest-point sampling.
---
 src/engine/engine_collision_driver.c | 22 ++++++++++++----------
 1 file changed, 12 insertions(+), 10 deletions(-)

diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c
index 8c586d995a..dd89b41b80 100644
--- a/src/engine/engine_collision_driver.c
+++ b/src/engine/engine_collision_driver.c
@@ -455,6 +455,7 @@ static void filterFlexContacts(mjData* d, int ncon_before) {
   mj_markStack(d);
   mjtByte* selected = mjSTACKALLOC(d, n, mjtByte);
   mjtNum* min_dist = mjSTACKALLOC(d, n, mjtNum);
+  mjContact* kept = mjSTACKALLOC(d, mjMAXCONPAIR, mjContact);
   memset(selected, 0, n);
 
   for (int i = 0; i < n; i++) {
@@ -472,8 +473,15 @@ static void filterFlexContacts(mjData* d, int ncon_before) {
     }
   }
 
+  // Select contacts by farthest point sampling.  The selected contacts are
+  // collected in `kept` and compacted into the contact prefix only after the
+  // selection loop finishes: swapping contacts in place during the loop would
+  // leave `selected`/`min_dist` indexed against a permuted array and corrupt
+  // the selection (and also drop the last selected contact when its index is
+  // not moved into the prefix).
   while (nselected < mjMAXCONPAIR && best >= 0) {
     selected[best] = 1;
+    kept[nselected] = contacts[best];
     mjtNum* bestpos = contacts[best].pos;
 
     int nextbest = -1;
@@ -494,20 +502,14 @@ static void filterFlexContacts(mjData* d, int ncon_before) {
       }
     }
 
-    if (nselected < mjMAXCONPAIR - 1) {
-      mjContact temp = contacts[nselected];
-      contacts[nselected] = contacts[best];
-      contacts[best] = temp;
-
-      if (nextbest == nselected) {
-        nextbest = best;
-      }
-    }
-
     nselected++;
     best = nextbest;
   }
 
+  for (int i = 0; i < nselected; i++) {
+    contacts[i] = kept[i];
+  }
+
   mj_freeStack(d);
 
   d->ncon = ncon_before + nselected;