Support arbitrary large meshes in multiccd by reusing EPA memory.

PiperOrigin-RevId: 947709621
Change-Id: Idc4f168434b9d0a8555c0bed989adb0e99770a78

Author: kbayes

doc/changelog.rst

@@ -21,6 +21,7 @@ General
   so assets contained in it failed to load. Failures in the ``mjz`` decoder now emit a warning with the underlying
   error instead of the generic "could not decode content" message.
 - Added support for resource writing via :ref:`mju_writeResource` and the ``write`` callback in :ref:`mjpResourceProvider`.
+- Added support for :ref:`multiccd <coMultiCCD>` with arbitrarily large meshes.
 
 .. admonition:: Breaking API changes
    :class: attention

doc/includes/references.h

@@ -643,6 +643,8 @@ typedef struct mjModel_ {
   mjtSize nemax;                  // number of potential equality-constraint rows
   mjtSize njmax;                  // number of available rows in constraint Jacobian (legacy)
   mjtSize nconmax;                // number of potential contacts in contact list (legacy)
+  mjtSize npolygonmax;            // maximum number of vertices in a mesh polygon
+  mjtSize nmeshdegmax;            // maximum number of edges adjacent to a mesh vertex
   mjtSize nuserdata;              // number of mjtNums reserved for the user
   mjtSize nsensordata;            // number of mjtNums in sensor data vector
   mjtSize npluginstate;           // number of mjtNums in plugin state vector

include/mujoco/mjmodel.h

@@ -331,6 +331,8 @@ typedef struct mjModel_ {
   mjtSize nemax;                  // number of potential equality-constraint rows
   mjtSize njmax;                  // number of available rows in constraint Jacobian (legacy)
   mjtSize nconmax;                // number of potential contacts in contact list (legacy)
+  mjtSize npolygonmax;            // maximum number of vertices in a mesh polygon
+  mjtSize nmeshdegmax;            // maximum number of edges adjacent to a mesh vertex
   mjtSize nuserdata;              // number of mjtNums reserved for the user
   mjtSize nsensordata;            // number of mjtNums in sensor data vector
   mjtSize npluginstate;           // number of mjtNums in plugin state vector

include/mujoco/mjxmacro.h

@@ -246,6 +246,8 @@
     X( nemax )              \
     X( njmax )              \
     X( nconmax )            \
+    X( npolygonmax )        \
+    X( nmeshdegmax )        \
     X( nuserdata )          \
     X( nsensordata )        \
     X( npluginstate )       \

python/mujoco/introspect/structs.py

@@ -1363,6 +1363,16 @@
                  type=ValueType(name='mjtSize'),
                  doc='number of potential contacts in contact list (legacy)',
              ),
+             StructFieldDecl(
+                 name='npolygonmax',
+                 type=ValueType(name='mjtSize'),
+                 doc='maximum number of vertices in a mesh polygon',
+             ),
+             StructFieldDecl(
+                 name='nmeshdegmax',
+                 type=ValueType(name='mjtSize'),
+                 doc='maximum number of edges adjacent to a mesh vertex',
+             ),
              StructFieldDecl(
                  name='nuserdata',
                  type=ValueType(name='mjtSize'),

src/engine/engine_collision_convex.c

@@ -102,11 +102,16 @@ static int mjc_penetration(const mjModel* m, mjData* d, mjCCDObj* obj1, mjCCDObj
   config.tolerance = m->opt.ccd_tolerance;
   config.max_contacts = ncon;
   config.dist_cutoff = 0;  // no geom distances needed
+  config.npolygonmax = m->npolygonmax;
+  config.nmeshdegmax = m->nmeshdegmax;
   if (buffer) {
     config.buffer = buffer;
   } else {
     mj_markStack(d);
-    config.buffer = mj_stackAllocByte(d, mjc_ccdSize(config.max_iterations), sizeof(mjtNum));
+    int npolygonmax = mjDISABLED(mjDSBL_MULTICCD) ? 0 : m->npolygonmax;
+    int nmeshdegmax = mjDISABLED(mjDSBL_MULTICCD) ? 0 : m->nmeshdegmax;
+    config.buffer = mj_stackAllocByte(d, mjc_ccdSize(npolygonmax, nmeshdegmax,
+                                                     config.max_iterations), sizeof(mjtNum));
   }
 
   if ((dist = mjc_ccd(&config, &status, obj1, obj2)) < 0) {

src/engine/engine_collision_driver.c

@@ -1887,7 +1887,9 @@ static void collisionTask(const mjModel* m, mjData* d, void* arg, int thread_id,
 static void mj_narrowphase(const mjModel* m, mjData* d, const mjcPair* buffer, int npair,
                            size_t parena) {
   int nthread = mju_numThread(d);
-  int ccd_size = mjc_ccdSize(m->opt.ccd_iterations);
+  int npolygonmax = mjDISABLED(mjDSBL_MULTICCD) ? 0 : m->npolygonmax;
+  int nmeshdegmax = mjDISABLED(mjDSBL_MULTICCD) ? 0 : m->nmeshdegmax;
+  int ccd_size = mjc_ccdSize(npolygonmax, nmeshdegmax, m->opt.ccd_iterations);
   mjtNum margin, gap;
 
   // try to balance load of 5 chunks per thread (chunksize should be divisible by 16)