Replace mjQUICKSORT with faster, native sorting function. fixes #1638

PiperOrigin-RevId: 688519096
Change-Id: I05ab1576703c2458968ef81915ac682a8beac391

src/engine/engine_collision_sdf.c

@@ -306,11 +306,10 @@ static void undoTransformation(const mjModel* m, const mjData* d, int g,
 //---------------------------- narrow phase -----------------------------------------------
 
 // comparison function for contact sorting
-quicksortfunc(distcompare, dist, i1, i2) {
-  mjtNum d1 = ((mjtNum*)dist)[*(int*)i1];
-  mjtNum d2 = ((mjtNum*)dist)[*(int*)i2];
-
-  if (d1 < d2) {
+static inline int distcmp(int* i, int* j, void* context) {
+  mjtNum d1 = ((mjtNum*)context)[*i];
+  mjtNum d2 = ((mjtNum*)context)[*j];
+    if (d1 < d2) {
     return -1;
   } else if (d1 == d2) {
     return 0;
@@ -319,6 +318,9 @@ quicksortfunc(distcompare, dist, i1, i2) {
   }
 }
 
+// define distSort function for contact sorting
+mjSORT(distSort, int, distcmp)
+
 // check if the collision point already exists
 static int isknown(const mjtNum* points, const mjtNum x[3], int cnt) {
   for (int i = 0; i < cnt; i++) {
@@ -642,7 +644,10 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g
   }
 
   // sort contacts using depth
-  mjQUICKSORT(index, ncandidate, sizeof(int), distcompare, dist);
+  if (ncandidate > 1) {
+    int buf[MAXMESHPNT];
+    distSort(index, buf, ncandidate, dist);
+  }
 
   // add only the first mjMAXCONPAIR pairs
   for (int i=0; i < mju_min(ncandidate, mjMAXCONPAIR); i++) {