Fix bug in mesh hillclimbing support function.

PiperOrigin-RevId: 695710616
Change-Id: I2127aee584c13efb44d70cc4e267354ca0b80bf0

src/engine/engine_collision_convex.c

@@ -14,6 +14,7 @@
 
 #include "engine/engine_collision_convex.h"
 
+#include <float.h>
 #include <stddef.h>
 
 #include <ccd/ccd.h>
@@ -284,6 +285,13 @@ static void mjc_boxSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
 
 
 
+// dot product between mjtNum and float
+static inline mjtNum dot3f(const mjtNum a[3], const float b[3]) {
+  return a[0]*(mjtNum)b[0] + a[1]*(mjtNum)b[1] + a[2]*(mjtNum)b[2];
+}
+
+
+
 // mesh support function via exhaustive search
 static void mjc_meshSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
   const mjModel* m = obj->model;
@@ -299,34 +307,32 @@ static void mjc_meshSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
   mjtNum local_dir[3];
   mulMatTVec3(local_dir, mat, dir);
 
-  mjtNum tmp = -1E+10;
+  mjtNum max = -FLT_MAX;
-  int ibest = -1;
+  int imax = 0;
+
+  // used cached results from previous search
   if (obj->meshindex >= 0) {
-    ibest = obj->meshindex;
+    imax = obj->meshindex;
-    tmp = local_dir[0] * (mjtNum)verts[3*ibest + 0] +
+    max = dot3f(local_dir, verts + 3*imax);
-          local_dir[1] * (mjtNum)verts[3*ibest + 1] +
-          local_dir[2] * (mjtNum)verts[3*ibest + 2];
   }
 
-  // search all vertices, find best
+  // search all vertices, find maximum dot product
   for (int i=0; i < nverts; i++) {
-    mjtNum vdot = local_dir[0] * (mjtNum)verts[3*i + 0] +
+    mjtNum vdot = dot3f(local_dir, verts + 3*i);
-                  local_dir[1] * (mjtNum)verts[3*i + 1] +
-                  local_dir[2] * (mjtNum)verts[3*i + 2];
 
-    // update best
+    // update max
-    if (vdot > tmp) {
+    if (vdot > max) {
-      tmp = vdot;
+      max = vdot;
-      ibest = i;
+      imax = i;
     }
   }
 
-  // record best vertex index
+  // record vertex index of maximum
-  obj->meshindex = ibest;
+  obj->meshindex = imax;
 
-  local_dir[0] = (mjtNum)verts[3*ibest + 0];
+  local_dir[0] = (mjtNum)verts[3*imax + 0];
-  local_dir[1] = (mjtNum)verts[3*ibest + 1];
+  local_dir[1] = (mjtNum)verts[3*imax + 1];
-  local_dir[2] = (mjtNum)verts[3*ibest + 2];
+  local_dir[2] = (mjtNum)verts[3*imax + 2];
 
   // transform result to global frame
   localToGlobal(res, mat, local_dir, pos);
@@ -354,38 +360,29 @@ static void mjc_hillclimbSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[
   mjtNum local_dir[3];
   mulMatTVec3(local_dir, mat, dir);
 
-  mjtNum tmp = -1E+10;
+  // hillclimb until no change
-  int ibest= -1, prev = -1;
+  mjtNum max = -FLT_MAX;
-  // hill-climb until no change
+  int prev = -1, imax = obj->meshindex < 0 ? 0 : obj->meshindex;
   do {
-    prev = ibest;
+    prev = imax;
-    for (int i = vert_edgeadr[ibest]; edge_localid[i] >= 0; i++) {
+    for (int i = vert_edgeadr[imax]; edge_localid[i] >= 0; i++) {
       int idx = 3*vert_globalid[edge_localid[i]];
-      mjtNum vdot = local_dir[0] * (mjtNum)verts[idx + 0] +
+      mjtNum vdot = dot3f(local_dir, verts + idx);
-                    local_dir[1] * (mjtNum)verts[idx + 1] +
+      if (vdot > max) {
-                    local_dir[2] * (mjtNum)verts[idx + 2];
+        max = vdot;
-      if (vdot > tmp) {
+        imax = edge_localid[i];  // update maximum vertex index
-        tmp = vdot;
-        ibest = edge_localid[i];  // update best
       }
     }
-  } while (ibest != prev);
+  } while (imax != prev);
 
-  // record best vertex index (local id)
+  // record vertex index of maximum (local id)
-  obj->meshindex = ibest;
+  obj->meshindex = imax;
 
-  // map best index to globalid
+  // get resulting support vertex
-  ibest = vert_globalid[ibest];
+  imax = 3*vert_globalid[imax];
-
+  local_dir[0] = (mjtNum)verts[imax + 0];
-  // sanity check, SHOULD NOT OCCUR
+  local_dir[1] = (mjtNum)verts[imax + 1];
-  if (ibest < 0) {
+  local_dir[2] = (mjtNum)verts[imax + 2];
-    mju_warning("mesh_support could not find support vertex");
-    mju_zero3(res);
-  } else {
-    local_dir[0] = (mjtNum)verts[3*ibest + 0];
-    local_dir[1] = (mjtNum)verts[3*ibest + 1];
-    local_dir[2] = (mjtNum)verts[3*ibest + 2];
-  }
 
   // transform result to global frame
   localToGlobal(res, mat, local_dir, pos);