Implement mesh extrema in a 3x3x3 grid corresponding to each feature of a unit cube. These are used as seeds for a better initial point in mesh hill climbing with up to a 2x speedup in mjc_Convex.

PiperOrigin-RevId: 959645299
Change-Id: I032ae534704e0cc440ddb7691fd21a29a359f521

src/engine/engine_collision_convex.c

@@ -407,7 +407,24 @@ static void mjc_hillclimbSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[
   mulMatTVec3(local_dir, mat, dir);
 
   int prev = -1;
-  int imax = obj->meshindex >= 0 ? obj->meshindex : 0;
+  int imax;
+
+  // map continuous direction to discrete 3x3x3 grid (-1, 0, 1) indices
+  int cx = (local_dir[0] > 0.4) - (local_dir[0] < -0.4) + 1;
+  int cy = (local_dir[1] > 0.4) - (local_dir[1] < -0.4) + 1;
+  int cz = (local_dir[2] > 0.4) - (local_dir[2] < -0.4) + 1;
+  int grid_idx = obj->data.mesh.extrema[cx*9 + cy*3 + cz];
+
+  if (obj->meshindex >= 0) {
+    // warm start: pick the better of cached vertex vs grid seed
+    mjtNum cached_dot = dot3f(local_dir, verts + 3*vert_globalid[obj->meshindex]);
+    mjtNum seed_dot = dot3f(local_dir, verts + 3*vert_globalid[grid_idx]);
+    imax = (seed_dot > cached_dot) ? grid_idx : obj->meshindex;
+  } else {
+    // cold start: use grid seed
+    imax = grid_idx;
+  }
+
   mjtNum max = dot3f(local_dir, verts + 3*vert_globalid[imax]);
 
   // hillclimb until no change
@@ -734,9 +751,11 @@ void mjc_initCCDObj(mjCCDObj* obj, const mjModel* m, const mjData* d, int g, mjt
       polyadr = m->mesh_polyadr[m->geom_dataid[g]];
       if (graphadr < 0 || m->mesh_vertnum[m->geom_dataid[g]] < mjMESH_HILLCLIMB_MIN) {
         obj->data.mesh.graph = NULL;
+        obj->data.mesh.extrema = NULL;
         obj->support = mjc_meshSupport;
       } else {
         obj->data.mesh.graph = m->mesh_graph + graphadr;
+        obj->data.mesh.extrema = m->mesh_extrema + 27 * m->geom_dataid[g];
         obj->support = mjc_hillclimbSupport;
       }
       obj->data.mesh.vert = m->mesh_vert + 3*vertadr;

src/engine/engine_collision_convex.h

@@ -62,7 +62,8 @@ struct _mjCCDObj {
       const int* polyvertnum;
       const int* polyvert;
       const mjtNum* polynormal;
-      const int*graph;
+      const int* graph;
+      const int* extrema;
     } mesh;
 
     // hfield prism data

src/user/user_model.cc

@@ -26,6 +26,7 @@
 #include <exception>
 #include <filesystem>  // NOLINT(build/c++17)
 #include <functional>
+#include <limits>
 #include <mutex>
 #include <set>
 #include <sstream>
@@ -3534,9 +3535,45 @@ void mjCModel::CopyObjects(mjModel* m) {
     } else {
       memset(m->mesh_facetexcoord + 3*face_adr, 0, 3*pme->nface()*sizeof(int));
     }
+    memset(m->mesh_extrema + 27*i, 0, 27*sizeof(int));
     if (pme->szgraph()) {
       pme->CopyGraph(m->mesh_graph + graph_adr);
+
+      // compute grid extrema (local indices in graph)
+      float max_val[27];
+      for (int k = 0; k < 27; k++) {
+        max_val[k] = std::numeric_limits<float>::lowest();
+      }
+
+      const int* graph = m->mesh_graph + graph_adr;
+      int numgraphvert = graph[0];
+      const int* vert_globalid = graph + 2 + numgraphvert;
+      const float* verts = m->mesh_vert + 3*vert_adr;
+
+      // map the 27 features (8 vertices, 6 faces, 12 edges) of a unit cube to the farthest
+      // vertex in the mesh
+      for (int local_id = 0; local_id < numgraphvert; local_id++) {
+        int global_id = vert_globalid[local_id];
+        float x = verts[3 * global_id + 0];
+        float y = verts[3 * global_id + 1];
+        float z = verts[3 * global_id + 2];
+
+        int k = 0;
+        for (int cx = -1; cx <= 1; cx++) {
+          for (int cy = -1; cy <= 1; cy++) {
+            for (int cz = -1; cz <= 1; cz++) {
+              float dot = x * cx + y * cy + z * cz;
+              if (dot > max_val[k]) {
+                max_val[k] = dot;
+                m->mesh_extrema[27*i + k] = local_id;
+              }
+              k++;
+            }
+          }
+        }
+      }
     }
+
     pme->CopyPolygonNormals(m->mesh_polynormal + 3*poly_adr);
     pme->CopyPolygons(m->mesh_polyvert + polyvert_adr, m->mesh_polyvertadr + poly_adr,
                       m->mesh_polyvertnum + poly_adr, polyvert_adr);