From 2444defc636689dc5d859bb9ed4083ea25865d06 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Tue, 14 Jul 2026 08:47:52 -0700 Subject: [PATCH] Support arbitrary large meshes in multiccd by reusing EPA memory. PiperOrigin-RevId: 947709621 Change-Id: Idc4f168434b9d0a8555c0bed989adb0e99770a78 --- doc/changelog.rst | 1 + doc/includes/references.h | 2 + include/mujoco/mjmodel.h | 2 + include/mujoco/mjxmacro.h | 2 + python/mujoco/introspect/structs.py | 10 + src/engine/engine_collision_convex.c | 7 +- src/engine/engine_collision_driver.c | 4 +- src/engine/engine_collision_gjk.c | 170 +++--- src/engine/engine_collision_gjk.h | 7 +- src/engine/engine_io.c | 2 + src/engine/engine_setconst.c | 11 + src/engine/engine_support.c | 4 +- test/engine/engine_collision_gjk_test.cc | 5 +- .../engine/testdata/collision_convex/cone.obj | 502 ++++++++++++++++++ .../collision_convex/conemesh_box.xml | 22 + unity/Runtime/Bindings/MjBindings.cs | 2 + wasm/codegen/generated/bindings.cc | 2 + wasm/codegen/generated/bindings.h | 12 + 18 files changed, 680 insertions(+), 87 deletions(-) create mode 100644 test/engine/testdata/collision_convex/cone.obj create mode 100644 test/engine/testdata/collision_convex/conemesh_box.xml diff --git a/doc/changelog.rst b/doc/changelog.rst index de44a0cb..e4a3e361 100644 --- a/doc/changelog.rst +++ b/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 ` with arbitrarily large meshes. .. admonition:: Breaking API changes :class: attention diff --git a/doc/includes/references.h b/doc/includes/references.h index faed9cd1..94d31210 100644 --- a/doc/includes/references.h +++ b/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 diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 1467e39e..efd76ba3 100644 --- a/include/mujoco/mjmodel.h +++ b/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 diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 620dcf92..5667a17f 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -246,6 +246,8 @@ X( nemax ) \ X( njmax ) \ X( nconmax ) \ + X( npolygonmax ) \ + X( nmeshdegmax ) \ X( nuserdata ) \ X( nsensordata ) \ X( npluginstate ) \ diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index a9dfe2fc..a2495ce4 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -1363,6 +1363,16 @@ STRUCTS: Mapping[str, StructDecl] = dict([ 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'), diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 6fe17b4e..f823ec21 100644 --- a/src/engine/engine_collision_convex.c +++ b/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) { diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index dc8f8da8..233a75d6 100644 --- a/src/engine/engine_collision_driver.c +++ b/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) diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 06f447f1..e451b3d0 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -1619,24 +1619,25 @@ static mjtNum planeIntersect(mjtNum res[3], const mjtNum pn[3], mjtNum pd, // clip a polygon against another polygon static void polygonClip(mjCCDStatus* status, const mjtNum* face1, int nface1, const mjtNum* face2, int nface2, const mjtNum n[3], - const mjtNum dir[3]) { + const mjtNum dir[3], mjtNum* buffer, int npolygonmax) { // clipping face needs to be at least a triangle if (nface1 < 3) { return; } + mjtNum* polygon = buffer; + mjtNum* clipped = polygon + 6 * npolygonmax; + mjtNum* pn = clipped + 6 * npolygonmax; + mjtNum* pd = pn + 3 * npolygonmax; + // compute plane normal and distance to plane for each vertex - mjtNum pn[3 * mjMAX_POLYVERT], pd[mjMAX_POLYVERT]; for (int i = 0; i < nface1 - 1; i++) { pd[i] = planeNormal(&pn[3*i], &face1[3*i], &face1[3*i + 3], n); } pd[nface1 - 1] = planeNormal(&pn[3*(nface1 - 1)], &face1[3*(nface1 - 1)], &face1[0], n); // reserve 2 * max_sides as max sides for a clipped polygon - mjtNum polygon1[6 * mjMAX_POLYVERT], polygon2[6 * mjMAX_POLYVERT], *polygon, *clipped; int npolygon = nface2, nclipped = 0; - polygon = polygon1; - clipped = polygon2; for (int i = 0; i < nface2; i++) { copy3(polygon + 3*i, face2 + 3*i); @@ -1700,16 +1701,6 @@ static void polygonClip(mjCCDStatus* status, const mjtNum* face1, int nface1, return; } - // TODO(kylebayes): Consider using a heristic to prune the polygon. - if (npolygon > mjMAXCONPAIR) { - status->nx = mjMAXCONPAIR; - for (int i = 0; i < 3*mjMAXCONPAIR; i += 3) { - copy3(status->x2 + i, polygon + i); - sub3(status->x1 + i, status->x2 + i, dir); - } - return; - } - // if the face is an edge, remove potential duplicates if (nface2 == 2 && npolygon > 2) { // find the two most distant vertices in the polygon @@ -1775,8 +1766,8 @@ static int intersect(int res[2], const int* arr1, const int* arr2, int n, int m) // compute possible polygon normals of a mesh given up to 3 vertices -static int meshNormals(mjtNum* res, int resind[3], int dim, mjCCDObj* obj, - int v1, int v2, int v3) { +static int meshNormals(mjtNum* res, int resind[3], int dim, mjCCDObj* obj, const int vi[3]) { + int v1 = vi[0], v2 = vi[1], v3 = vi[2]; const int* polymap = obj->data.mesh.polymap; const mjtNum* polynormal = obj->data.mesh.polynormal; @@ -1827,7 +1818,6 @@ static int meshNormals(mjtNum* res, int resind[3], int dim, mjCCDObj* obj, int v1_num = obj->data.mesh.mpolymapnum[v1]; // cap number of possible faces intersecting at a vertex - if (v1_num > mjMAX_POLYVERT) v1_num = mjMAX_POLYVERT; for (int i = 0; i < v1_num; i++) { int index = polymap[v1_adr + i]; const mjtNum* normal = polynormal + 3*index; @@ -1842,7 +1832,9 @@ static int meshNormals(mjtNum* res, int resind[3], int dim, mjCCDObj* obj, // compute normal directional vectors along possible edges given by up to two vertices static int meshEdgeNormals(mjtNum* res, mjtNum* endverts, int dim, mjCCDObj* obj, - const mjtNum v1[3], const mjtNum v2[3], int v1i, int v2i) { + const mjtNum v[9], int v1i) { + const mjtNum* v1 = v; + const mjtNum* v2 = v + 3; // mesh data const mjtNum* mat = obj->mat; const mjtNum* pos = obj->pos; @@ -1860,7 +1852,6 @@ static int meshEdgeNormals(mjtNum* res, mjtNum* endverts, int dim, mjCCDObj* obj if (dim == 1) { int v1_adr = obj->data.mesh.mpolymapadr[v1i]; int v1_num = obj->data.mesh.mpolymapnum[v1i]; - if (v1_num > mjMAX_POLYVERT) v1_num = mjMAX_POLYVERT; // loop through all faces with vertex v1 for (int i = 0; i < v1_num; i++) { @@ -1871,8 +1862,8 @@ static int meshEdgeNormals(mjtNum* res, mjtNum* endverts, int dim, mjCCDObj* obj for (int j = 0; j < nvert; j++) { if (polyvert[adr + j] == v1i) { int k = (j == 0) ? nvert - 1 : j - 1; - const float* v = vert + 3*polyvert[adr + k]; - globalcoord(endverts + 3*i, mat, pos, v[0], v[1], v[2]); + const float* vert_k = vert + 3*polyvert[adr + k]; + globalcoord(endverts + 3*i, mat, pos, vert_k[0], vert_k[1], vert_k[2]); sub3(res + 3*i, endverts + 3*i, v1); mju_normalize3(res + 3*i); break; @@ -1913,7 +1904,8 @@ static int boxNormals2(mjtNum res[9], int resind[3], const mjtNum mat[9], const // compute possible face normals of a box given up to 3 vertices static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj, - int v1, int v2, int v3, const mjtNum dir[3]) { + const int vi[3], const mjtNum dir[3]) { + int v1 = vi[0], v2 = vi[1], v3 = vi[2]; const mjtNum* mat = obj->mat; if (dim == 3) { int c = 0; @@ -1967,7 +1959,9 @@ static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj, // compute possible edge normals for box for edge collisions static int boxEdgeNormals(mjtNum res[9], mjtNum endverts[9], int dim, mjCCDObj* obj, - const mjtNum v1[3], const mjtNum v2[3], int v1i, int v2i) { + const mjtNum v[9], int v1i) { + const mjtNum* v1 = v; + const mjtNum* v2 = v + 3; // box data const mjtNum* mat = obj->mat; const mjtNum* pos = obj->pos; @@ -2059,9 +2053,6 @@ static int meshFace(mjtNum* res, mjCCDObj* obj, int idx) { int adr = obj->data.mesh.polyvertadr[idx], j = 0; int nvert = obj->data.mesh.polyvertnum[idx]; - if (nvert > mjMAX_POLYVERT) { - nvert = mjMAX_POLYVERT; - } const float* vert = obj->data.mesh.vert; const int* polyvert = obj->data.mesh.polyvert + adr; for (int i = nvert - 1; i >= 0; i--) { @@ -2105,55 +2096,58 @@ static inline int alignedFaceEdge(int res[2], const mjtNum* edge, int nedge, // return number (1, 2 or 3) of dimensions of a simplex; reorder vertices if necessary -static inline int simplexDim(int* v1i, int* v2i, int* v3i, mjtNum** v1, mjtNum** v2, mjtNum** v3) { - int val1 = *v1i; - int val2 = *v2i; - int val3 = *v3i; - - if (val1 != val2) { - return (val3 == val1 || val3 == val2) ? 2 : 3; - } - if (val1 != val3) { - *v2i = *v3i; - *v2 = *v3; +static inline int simplexDim(int vi[3], mjtNum v[9]) { + if (vi[0] == vi[1]) { + if (vi[0] == vi[2]) return 1; + vi[1] = vi[2]; + copy3(v + 3, v + 6); return 2; } - return 1; + return (vi[2] == vi[0] || vi[2] == vi[1]) ? 2 : 3; } // recover multiple contacts from EPA polytope -static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status, - mjCCDObj* obj1, mjCCDObj* obj2) { +static void multicontact(int nmeshdegmax, int npolygonmax, uint8_t* buffer, Polytope* pt, + Face* face, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { if (obj1->geom_type == mjGEOM_MESH && !obj1->data.mesh.mesh_polynum) { return; } if (obj2->geom_type == mjGEOM_MESH && !obj2->data.mesh.mesh_polynum) { return; } + if (obj1->geom_type == mjGEOM_BOX || obj2->geom_type == mjGEOM_BOX) { + nmeshdegmax = nmeshdegmax < 3 ? 3 : nmeshdegmax; + npolygonmax = npolygonmax < 4 ? 4 : npolygonmax; + } - mjtNum face1[mjMAX_POLYVERT * 3], face2[mjMAX_POLYVERT * 3], endverts[mjMAX_POLYVERT * 3]; - // get vertices of faces from EPA int verts[3] = EPA_VERT_EXPAND(face->verts); - int v11i = pt->verts[verts[0]].index1; - int v12i = pt->verts[verts[1]].index1; - int v13i = pt->verts[verts[2]].index1; - int v21i = pt->verts[verts[0]].index2; - int v22i = pt->verts[verts[1]].index2; - int v23i = pt->verts[verts[2]].index2; - mjtNum* v11 = pt->verts[verts[0]].vert1; - mjtNum* v12 = pt->verts[verts[1]].vert1; - mjtNum* v13 = pt->verts[verts[2]].vert1; - mjtNum* v21 = pt->verts[verts[0]].vert2; - mjtNum* v22 = pt->verts[verts[1]].vert2; - mjtNum* v23 = pt->verts[verts[2]].vert2; + int v1i[3] = {pt->verts[verts[0]].index1, pt->verts[verts[1]].index1, pt->verts[verts[2]].index1}; + int v2i[3] = {pt->verts[verts[0]].index2, pt->verts[verts[1]].index2, pt->verts[verts[2]].index2}; + + /// save relevant polytope data before overwriting buffer space + mjtNum v1[9], v2[9]; + copy3(v1 + 0, pt->verts[verts[0]].vert1); + copy3(v1 + 3, pt->verts[verts[1]].vert1); + copy3(v1 + 6, pt->verts[verts[2]].vert1); + copy3(v2 + 0, pt->verts[verts[0]].vert2); + copy3(v2 + 3, pt->verts[verts[1]].vert2); + copy3(v2 + 6, pt->verts[verts[2]].vert2); + + uint8_t* ptr = buffer; + int* idx1 = (int*)ptr; ptr += align8(sizeof(int) * nmeshdegmax); + int* idx2 = (int*)ptr; ptr += align8(sizeof(int) * nmeshdegmax); + mjtNum* n1 = (mjtNum*)ptr; ptr += align8(sizeof(mjtNum) * 3 * nmeshdegmax); + mjtNum* n2 = (mjtNum*)ptr; ptr += align8(sizeof(mjtNum) * 3 * nmeshdegmax); + mjtNum* endverts = (mjtNum*)ptr; ptr += align8(sizeof(mjtNum) * 3 * nmeshdegmax); + mjtNum* face1 = (mjtNum*)ptr; ptr += align8(sizeof(mjtNum) * 3 * npolygonmax); + mjtNum* face2 = (mjtNum*)ptr; ptr += align8(sizeof(mjtNum) * 3 * npolygonmax); + mjtNum* polygon = (mjtNum*)ptr; // buffer for polygonClip // get dimensions of features of geoms 1 and 2 - int nface1 = simplexDim(&v11i, &v12i, &v13i, &v11, &v12, &v13); - int nface2 = simplexDim(&v21i, &v22i, &v23i, &v21, &v22, &v23); + int nface1 = simplexDim(v1i, v1); + int nface2 = simplexDim(v2i, v2); int nnorms1 = 0, nnorms2 = 0; - mjtNum n1[3 * mjMAX_POLYVERT], n2[3 * mjMAX_POLYVERT]; // normals of possible face collisions - int idx1[mjMAX_POLYVERT], idx2[mjMAX_POLYVERT]; // indices of faces mjtNum dir[3], dir_neg[3]; sub3(dir, status->x2, status->x1); @@ -2161,14 +2155,14 @@ static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status, // get all possible face normals for each geom if (obj1->geom_type == mjGEOM_BOX) { - nnorms1 = boxNormals(n1, idx1, nface1, obj1, v11i, v12i, v13i, dir_neg); + nnorms1 = boxNormals(n1, idx1, nface1, obj1, v1i, dir_neg); } else if (obj1->geom_type == mjGEOM_MESH) { - nnorms1 = meshNormals(n1, idx1, nface1, obj1, v11i, v12i, v13i); + nnorms1 = meshNormals(n1, idx1, nface1, obj1, v1i); } if (obj2->geom_type == mjGEOM_BOX) { - nnorms2 = boxNormals(n2, idx2, nface2, obj2, v21i, v22i, v23i, dir); + nnorms2 = boxNormals(n2, idx2, nface2, obj2, v2i, dir); } else if (obj2->geom_type == mjGEOM_MESH) { - nnorms2 = meshNormals(n2, idx2, nface2, obj2, v21i, v22i, v23i); + nnorms2 = meshNormals(n2, idx2, nface2, obj2, v2i); } // determine if any two face normals match @@ -2178,9 +2172,9 @@ static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status, if (nface1 < 3 && nface1 <= nface2) { nnorms1 = 0; if (obj1->geom_type == mjGEOM_BOX) { - nnorms1 = boxEdgeNormals(n1, endverts, nface1, obj1, v11, v12, v11i, v12i); + nnorms1 = boxEdgeNormals(n1, endverts, nface1, obj1, v1, v1i[0]); } else if (obj1->geom_type == mjGEOM_MESH) { - nnorms1 = meshEdgeNormals(n1, endverts, nface1, obj1, v11, v12, v11i, v12i); + nnorms1 = meshEdgeNormals(n1, endverts, nface1, obj1, v1, v1i[0]); } if (!alignedFaceEdge(res, n1, nnorms1, n2, nnorms2)) return; edgecon1 = 1; @@ -2189,9 +2183,9 @@ static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status, } else if (nface2 < 3) { nnorms2 = 0; if (obj2->geom_type == mjGEOM_BOX) { - nnorms2 = boxEdgeNormals(n2, endverts, nface2, obj2, v21, v22, v21i, v22i); + nnorms2 = boxEdgeNormals(n2, endverts, nface2, obj2, v2, v2i[0]); } else if (obj2->geom_type == mjGEOM_MESH) { - nnorms2 = meshEdgeNormals(n2, endverts, nface2, obj2, v21, v22, v21i, v22i); + nnorms2 = meshEdgeNormals(n2, endverts, nface2, obj2, v2, v2i[0]); } if (!alignedFaceEdge(res, n2, nnorms2, n1, nnorms1)) return; edgecon2 = 1; @@ -2204,7 +2198,7 @@ static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status, // recover geom1 matching edge or face if (edgecon1) { - copy3(face1, pt->verts[verts[0]].vert1); + copy3(face1, v1); copy3(face1 + 3, endverts + 3*i); nface1 = 2; } else { @@ -2219,7 +2213,7 @@ static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status, // recover geom2 matching edge or face if (edgecon2) { - copy3(face2, pt->verts[verts[0]].vert2); + copy3(face2, v2); copy3(face2 + 3, endverts + 3*i); nface2 = 2; } else { @@ -2238,7 +2232,7 @@ static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status, // face1 is an edge; clip face1 against face2 if (edgecon1) { scl3(approx_dir, n2 + 3*j, -norm3(dir)); - polygonClip(status, face2, nface2, face1, nface1, n2 + 3*j, approx_dir); + polygonClip(status, face2, nface2, face1, nface1, n2 + 3*j, approx_dir, polygon, npolygonmax); // x1 and x2 must be flipped as we flipped the faces in polygonClip int nx = status->nx; for (int k = 0; k < nx; k++) { @@ -2253,13 +2247,13 @@ static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status, // face2 is an edge; clip face2 against face1 if (edgecon2) { scl3(approx_dir, n1 + 3*j, -norm3(dir)); - polygonClip(status, face1, nface1, face2, nface2, n1 + 3*j, approx_dir); + polygonClip(status, face1, nface1, face2, nface2, n1 + 3*j, approx_dir, polygon, npolygonmax); return; } // face-face collision scl3(approx_dir, n2 + 3*j, norm3(dir)); - polygonClip(status, face1, nface1, face2, nface2, n1 + 3*i, approx_dir); + polygonClip(status, face1, nface1, face2, nface2, n1 + 3*i, approx_dir, polygon, npolygonmax); } @@ -2284,12 +2278,29 @@ static inline void inflate(mjCCDStatus* status, mjtNum margin1, mjtNum margin2) // return size in bytes of the buffer needed for mjc_ccd for a given number of iterations -size_t mjc_ccdSize(int iterations) { - return align8(sizeof(Vertex) * (5 + iterations)) // vertices in polytope - + align8(sizeof(Face) * 6 * iterations) // faces in polytope - + align8(sizeof(Face*) * 6 * iterations) // map in polytope - + align8(sizeof(int) * 24) // horizon indices - + align8(sizeof(int) * 24); // horizon edges +size_t mjc_ccdSize(int npolygonmax, int nmeshdegmax, int iterations) { + size_t epa_size = align8(sizeof(Vertex) * (5 + iterations)) // vertices in polytope + + align8(sizeof(Face) * 6 * iterations) // faces in polytope + + align8(sizeof(Face*) * 6 * iterations) // map in polytope + + align8(sizeof(int) * 24) // horizon indices + + align8(sizeof(int) * 24); // horizon edges + + // allocate room for box geoms (multicontact is hardwired for box-box collisions) + npolygonmax = npolygonmax < 4 ? 4 : npolygonmax; + nmeshdegmax = nmeshdegmax < 3 ? 3 : nmeshdegmax; + size_t multiccd_size = align8(sizeof(mjtNum) * 3 * 2 * npolygonmax) + + align8(sizeof(mjtNum) * 3 * 2 * npolygonmax) + + align8(sizeof(mjtNum) * 3 * npolygonmax) + + align8(sizeof(mjtNum) * npolygonmax) + + align8(sizeof(int) * nmeshdegmax) + + align8(sizeof(int) * nmeshdegmax) + + align8(sizeof(mjtNum) * 3 * nmeshdegmax) + + align8(sizeof(mjtNum) * 3 * nmeshdegmax) + + align8(sizeof(mjtNum) * 3 * nmeshdegmax) + + align8(sizeof(mjtNum) * 3 * npolygonmax) + + align8(sizeof(mjtNum) * 3 * npolygonmax); + + return epa_size > multiccd_size ? epa_size : multiccd_size; } @@ -2405,7 +2416,8 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m if (!ret) { Face* face = epa(status, &pt, obj1, obj2); if (config->max_contacts > 1 && face) { - multicontact(&pt, face, status, obj1, obj2); + multicontact(config->nmeshdegmax, config->npolygonmax, config->buffer, &pt, face, status, + obj1, obj2); } } } diff --git a/src/engine/engine_collision_gjk.h b/src/engine/engine_collision_gjk.h index db16277d..df5a121c 100644 --- a/src/engine/engine_collision_gjk.h +++ b/src/engine/engine_collision_gjk.h @@ -19,7 +19,6 @@ #include #include -#include #include #include "engine/engine_collision_convex.h" @@ -43,8 +42,6 @@ extern "C" { #define mjEDGE_TOL 0.00159999931 #endif -// max number of supported vertices in a polygon face of a mesh -#define mjMAX_POLYVERT 150 // Status of an EPA run typedef enum { @@ -77,6 +74,8 @@ typedef struct { int max_contacts; // set to max number of contact points to recover mjtNum dist_cutoff; // set to max geom distance to recover void* buffer; // buffer memory for polytope (should be sized given by mjc_ccdSize) + int npolygonmax; // max number of vertices in a mesh polygon + int nmeshdegmax; // max number of edges adjacent to a mesh vertex } mjCCDConfig; // data produced from running GJK and EPA @@ -102,7 +101,7 @@ typedef struct { } mjCCDStatus; // return size in bytes of the buffer needed for mjc_ccd for a given number of iterations -MJAPI size_t mjc_ccdSize(int iterations); +MJAPI size_t mjc_ccdSize(int npolygonmax, int nmeshdegmax, int iterations); // run general convex collision detection, returns positive for distance, negative for penetration MJAPI mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2); diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 897b0aee..c312a424 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -249,6 +249,7 @@ void mj_makeModel(mjModel** dest, { // dummy variables for MJMODEL_SIZES set after mjModel construction int nnames_map = 0, nJmom = 0, ngravcomp = 0, nemax = 0, njmax = 0, nconmax=0; + int npolygonmax = 0, nmeshdegmax = 0; int nuserdata=0, nsensordata=0, npluginstate=0, nhistory=0, narena=0, nbuffer=0; // sizes must be non-negative and fit in int, except for the byte arrays texdata and textdata @@ -268,6 +269,7 @@ void mj_makeModel(mjModel** dest, // suppress unused variable warnings (void)nnames_map; (void)nJmom; (void)ngravcomp; (void)nemax; (void)njmax; (void)nconmax; + (void)npolygonmax; (void)nmeshdegmax; (void)nuserdata; (void)nsensordata; (void)npluginstate; (void)nhistory; (void)narena; (void)nbuffer; } diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 1925ad7a..096f7651 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -1321,6 +1321,17 @@ static void setSpring(mjModel* m, mjData* d) { // entry point: set all remaining constant fields of mjModel, except for lengthrange void mj_setConst(mjModel* m, mjData* d) { + // compute npolygonmax and nmeshdegmax + m->npolygonmax = 0; + for (int i=0; i < m->nmeshpoly; i++) { + m->npolygonmax = mjMAX(m->npolygonmax, m->mesh_polyvertnum[i]); + } + + m->nmeshdegmax = 0; + for (int i=0; i < m->nmeshvert; i++) { + m->nmeshdegmax = mjMAX(m->nmeshdegmax, m->mesh_polymapnum[i]); + } + // recompute sameframe flags from current model geometry setSameframe(m); diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 81872fbf..b999c7b5 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -525,9 +525,11 @@ static mjtNum mj_geomDistanceCCD(const mjModel* m, mjData* d, int g1, int g2, // set config config.max_iterations = m->opt.ccd_iterations; config.tolerance = m->opt.ccd_tolerance; + config.npolygonmax = 0; + config.nmeshdegmax = 0; config.max_contacts = 1; // want contacts config.dist_cutoff = distmax; // want geom distances - config.buffer = mj_stackAllocByte(d, mjc_ccdSize(config.max_iterations), sizeof(mjtNum)); + config.buffer = mj_stackAllocByte(d, mjc_ccdSize(0, 0, config.max_iterations), sizeof(mjtNum)); mjCCDObj obj1, obj2; mjc_initCCDObj(&obj1, m, d, g1, 0); diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 6a4260cf..d9c6f9fb 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -125,11 +125,14 @@ int Penetration(mjCCDStatus& status, mjtNum& depth, std::vector& dir, mjCCDConfig config; // set config - auto buffer = std::vector(mjc_ccdSize(kMaxIterations)); + auto buffer = std::vector( + mjc_ccdSize(model->npolygonmax, model->nmeshdegmax, kMaxIterations)); config.max_iterations = kMaxIterations; config.tolerance = kTolerance; config.max_contacts = max_contacts; config.dist_cutoff = 0; // no geom distances needed + config.npolygonmax = model->npolygonmax; + config.nmeshdegmax = model->nmeshdegmax; config.max_contacts = max_contacts; config.buffer = buffer.data(); diff --git a/test/engine/testdata/collision_convex/cone.obj b/test/engine/testdata/collision_convex/cone.obj new file mode 100644 index 00000000..688a6d11 --- /dev/null +++ b/test/engine/testdata/collision_convex/cone.obj @@ -0,0 +1,502 @@ +# Unit cone with 500 base vertices +v 0.0 0.0 1.0 +v 1.000000 0.000000 -1.0 +v 0.999921 0.012566 -1.0 +v 0.999684 0.025130 -1.0 +v 0.999289 0.037690 -1.0 +v 0.998737 0.050244 -1.0 +v 0.998027 0.062791 -1.0 +v 0.997159 0.075327 -1.0 +v 0.996134 0.087851 -1.0 +v 0.994951 0.100362 -1.0 +v 0.993611 0.112856 -1.0 +v 0.992115 0.125333 -1.0 +v 0.990461 0.137790 -1.0 +v 0.988652 0.150226 -1.0 +v 0.986686 0.162637 -1.0 +v 0.984564 0.175023 -1.0 +v 0.982287 0.187381 -1.0 +v 0.979855 0.199710 -1.0 +v 0.977268 0.212007 -1.0 +v 0.974527 0.224271 -1.0 +v 0.971632 0.236499 -1.0 +v 0.968583 0.248690 -1.0 +v 0.965382 0.260842 -1.0 +v 0.962028 0.272952 -1.0 +v 0.958522 0.285019 -1.0 +v 0.954865 0.297042 -1.0 +v 0.951057 0.309017 -1.0 +v 0.947098 0.320944 -1.0 +v 0.942991 0.332820 -1.0 +v 0.938734 0.344643 -1.0 +v 0.934329 0.356412 -1.0 +v 0.929776 0.368125 -1.0 +v 0.925077 0.379779 -1.0 +v 0.920232 0.391374 -1.0 +v 0.915241 0.402906 -1.0 +v 0.910106 0.414376 -1.0 +v 0.904827 0.425779 -1.0 +v 0.899405 0.437116 -1.0 +v 0.893841 0.448383 -1.0 +v 0.888136 0.459580 -1.0 +v 0.882291 0.470704 -1.0 +v 0.876307 0.481754 -1.0 +v 0.870184 0.492727 -1.0 +v 0.863923 0.503623 -1.0 +v 0.857527 0.514440 -1.0 +v 0.850994 0.525175 -1.0 +v 0.844328 0.535827 -1.0 +v 0.837528 0.546394 -1.0 +v 0.830596 0.556876 -1.0 +v 0.823533 0.567269 -1.0 +v 0.816339 0.577573 -1.0 +v 0.809017 0.587785 -1.0 +v 0.801567 0.597905 -1.0 +v 0.793990 0.607930 -1.0 +v 0.786288 0.617860 -1.0 +v 0.778462 0.627691 -1.0 +v 0.770513 0.637424 -1.0 +v 0.762443 0.647056 -1.0 +v 0.754251 0.656586 -1.0 +v 0.745941 0.666012 -1.0 +v 0.737513 0.675333 -1.0 +v 0.728969 0.684547 -1.0 +v 0.720309 0.693653 -1.0 +v 0.711536 0.702650 -1.0 +v 0.702650 0.711536 -1.0 +v 0.693653 0.720309 -1.0 +v 0.684547 0.728969 -1.0 +v 0.675333 0.737513 -1.0 +v 0.666012 0.745941 -1.0 +v 0.656586 0.754251 -1.0 +v 0.647056 0.762443 -1.0 +v 0.637424 0.770513 -1.0 +v 0.627691 0.778462 -1.0 +v 0.617860 0.786288 -1.0 +v 0.607930 0.793990 -1.0 +v 0.597905 0.801567 -1.0 +v 0.587785 0.809017 -1.0 +v 0.577573 0.816339 -1.0 +v 0.567269 0.823533 -1.0 +v 0.556876 0.830596 -1.0 +v 0.546394 0.837528 -1.0 +v 0.535827 0.844328 -1.0 +v 0.525175 0.850994 -1.0 +v 0.514440 0.857527 -1.0 +v 0.503623 0.863923 -1.0 +v 0.492727 0.870184 -1.0 +v 0.481754 0.876307 -1.0 +v 0.470704 0.882291 -1.0 +v 0.459580 0.888136 -1.0 +v 0.448383 0.893841 -1.0 +v 0.437116 0.899405 -1.0 +v 0.425779 0.904827 -1.0 +v 0.414376 0.910106 -1.0 +v 0.402906 0.915241 -1.0 +v 0.391374 0.920232 -1.0 +v 0.379779 0.925077 -1.0 +v 0.368125 0.929776 -1.0 +v 0.356412 0.934329 -1.0 +v 0.344643 0.938734 -1.0 +v 0.332820 0.942991 -1.0 +v 0.320944 0.947098 -1.0 +v 0.309017 0.951057 -1.0 +v 0.297042 0.954865 -1.0 +v 0.285019 0.958522 -1.0 +v 0.272952 0.962028 -1.0 +v 0.260842 0.965382 -1.0 +v 0.248690 0.968583 -1.0 +v 0.236499 0.971632 -1.0 +v 0.224271 0.974527 -1.0 +v 0.212007 0.977268 -1.0 +v 0.199710 0.979855 -1.0 +v 0.187381 0.982287 -1.0 +v 0.175023 0.984564 -1.0 +v 0.162637 0.986686 -1.0 +v 0.150226 0.988652 -1.0 +v 0.137790 0.990461 -1.0 +v 0.125333 0.992115 -1.0 +v 0.112856 0.993611 -1.0 +v 0.100362 0.994951 -1.0 +v 0.087851 0.996134 -1.0 +v 0.075327 0.997159 -1.0 +v 0.062791 0.998027 -1.0 +v 0.050244 0.998737 -1.0 +v 0.037690 0.999289 -1.0 +v 0.025130 0.999684 -1.0 +v 0.012566 0.999921 -1.0 +v 0.000000 1.000000 -1.0 +v -0.012566 0.999921 -1.0 +v -0.025130 0.999684 -1.0 +v -0.037690 0.999289 -1.0 +v -0.050244 0.998737 -1.0 +v -0.062791 0.998027 -1.0 +v -0.075327 0.997159 -1.0 +v -0.087851 0.996134 -1.0 +v -0.100362 0.994951 -1.0 +v -0.112856 0.993611 -1.0 +v -0.125333 0.992115 -1.0 +v -0.137790 0.990461 -1.0 +v -0.150226 0.988652 -1.0 +v -0.162637 0.986686 -1.0 +v -0.175023 0.984564 -1.0 +v -0.187381 0.982287 -1.0 +v -0.199710 0.979855 -1.0 +v -0.212007 0.977268 -1.0 +v -0.224271 0.974527 -1.0 +v -0.236499 0.971632 -1.0 +v -0.248690 0.968583 -1.0 +v -0.260842 0.965382 -1.0 +v -0.272952 0.962028 -1.0 +v -0.285019 0.958522 -1.0 +v -0.297042 0.954865 -1.0 +v -0.309017 0.951057 -1.0 +v -0.320944 0.947098 -1.0 +v -0.332820 0.942991 -1.0 +v -0.344643 0.938734 -1.0 +v -0.356412 0.934329 -1.0 +v -0.368125 0.929776 -1.0 +v -0.379779 0.925077 -1.0 +v -0.391374 0.920232 -1.0 +v -0.402906 0.915241 -1.0 +v -0.414376 0.910106 -1.0 +v -0.425779 0.904827 -1.0 +v -0.437116 0.899405 -1.0 +v -0.448383 0.893841 -1.0 +v -0.459580 0.888136 -1.0 +v -0.470704 0.882291 -1.0 +v -0.481754 0.876307 -1.0 +v -0.492727 0.870184 -1.0 +v -0.503623 0.863923 -1.0 +v -0.514440 0.857527 -1.0 +v -0.525175 0.850994 -1.0 +v -0.535827 0.844328 -1.0 +v -0.546394 0.837528 -1.0 +v -0.556876 0.830596 -1.0 +v -0.567269 0.823533 -1.0 +v -0.577573 0.816339 -1.0 +v -0.587785 0.809017 -1.0 +v -0.597905 0.801567 -1.0 +v -0.607930 0.793990 -1.0 +v -0.617860 0.786288 -1.0 +v -0.627691 0.778462 -1.0 +v -0.637424 0.770513 -1.0 +v -0.647056 0.762443 -1.0 +v -0.656586 0.754251 -1.0 +v -0.666012 0.745941 -1.0 +v -0.675333 0.737513 -1.0 +v -0.684547 0.728969 -1.0 +v -0.693653 0.720309 -1.0 +v -0.702650 0.711536 -1.0 +v -0.711536 0.702650 -1.0 +v -0.720309 0.693653 -1.0 +v -0.728969 0.684547 -1.0 +v -0.737513 0.675333 -1.0 +v -0.745941 0.666012 -1.0 +v -0.754251 0.656586 -1.0 +v -0.762443 0.647056 -1.0 +v -0.770513 0.637424 -1.0 +v -0.778462 0.627691 -1.0 +v -0.786288 0.617860 -1.0 +v -0.793990 0.607930 -1.0 +v -0.801567 0.597905 -1.0 +v -0.809017 0.587785 -1.0 +v -0.816339 0.577573 -1.0 +v -0.823533 0.567269 -1.0 +v -0.830596 0.556876 -1.0 +v -0.837528 0.546394 -1.0 +v -0.844328 0.535827 -1.0 +v -0.850994 0.525175 -1.0 +v -0.857527 0.514440 -1.0 +v -0.863923 0.503623 -1.0 +v -0.870184 0.492727 -1.0 +v -0.876307 0.481754 -1.0 +v -0.882291 0.470704 -1.0 +v -0.888136 0.459580 -1.0 +v -0.893841 0.448383 -1.0 +v -0.899405 0.437116 -1.0 +v -0.904827 0.425779 -1.0 +v -0.910106 0.414376 -1.0 +v -0.915241 0.402906 -1.0 +v -0.920232 0.391374 -1.0 +v -0.925077 0.379779 -1.0 +v -0.929776 0.368125 -1.0 +v -0.934329 0.356412 -1.0 +v -0.938734 0.344643 -1.0 +v -0.942991 0.332820 -1.0 +v -0.947098 0.320944 -1.0 +v -0.951057 0.309017 -1.0 +v -0.954865 0.297042 -1.0 +v -0.958522 0.285019 -1.0 +v -0.962028 0.272952 -1.0 +v -0.965382 0.260842 -1.0 +v -0.968583 0.248690 -1.0 +v -0.971632 0.236499 -1.0 +v -0.974527 0.224271 -1.0 +v -0.977268 0.212007 -1.0 +v -0.979855 0.199710 -1.0 +v -0.982287 0.187381 -1.0 +v -0.984564 0.175023 -1.0 +v -0.986686 0.162637 -1.0 +v -0.988652 0.150226 -1.0 +v -0.990461 0.137790 -1.0 +v -0.992115 0.125333 -1.0 +v -0.993611 0.112856 -1.0 +v -0.994951 0.100362 -1.0 +v -0.996134 0.087851 -1.0 +v -0.997159 0.075327 -1.0 +v -0.998027 0.062791 -1.0 +v -0.998737 0.050244 -1.0 +v -0.999289 0.037690 -1.0 +v -0.999684 0.025130 -1.0 +v -0.999921 0.012566 -1.0 +v -1.000000 0.000000 -1.0 +v -0.999921 -0.012566 -1.0 +v -0.999684 -0.025130 -1.0 +v -0.999289 -0.037690 -1.0 +v -0.998737 -0.050244 -1.0 +v -0.998027 -0.062791 -1.0 +v -0.997159 -0.075327 -1.0 +v -0.996134 -0.087851 -1.0 +v -0.994951 -0.100362 -1.0 +v -0.993611 -0.112856 -1.0 +v -0.992115 -0.125333 -1.0 +v -0.990461 -0.137790 -1.0 +v -0.988652 -0.150226 -1.0 +v -0.986686 -0.162637 -1.0 +v -0.984564 -0.175023 -1.0 +v -0.982287 -0.187381 -1.0 +v -0.979855 -0.199710 -1.0 +v -0.977268 -0.212007 -1.0 +v -0.974527 -0.224271 -1.0 +v -0.971632 -0.236499 -1.0 +v -0.968583 -0.248690 -1.0 +v -0.965382 -0.260842 -1.0 +v -0.962028 -0.272952 -1.0 +v -0.958522 -0.285019 -1.0 +v -0.954865 -0.297042 -1.0 +v -0.951057 -0.309017 -1.0 +v -0.947098 -0.320944 -1.0 +v -0.942991 -0.332820 -1.0 +v -0.938734 -0.344643 -1.0 +v -0.934329 -0.356412 -1.0 +v -0.929776 -0.368125 -1.0 +v -0.925077 -0.379779 -1.0 +v -0.920232 -0.391374 -1.0 +v -0.915241 -0.402906 -1.0 +v -0.910106 -0.414376 -1.0 +v -0.904827 -0.425779 -1.0 +v -0.899405 -0.437116 -1.0 +v -0.893841 -0.448383 -1.0 +v -0.888136 -0.459580 -1.0 +v -0.882291 -0.470704 -1.0 +v -0.876307 -0.481754 -1.0 +v -0.870184 -0.492727 -1.0 +v -0.863923 -0.503623 -1.0 +v -0.857527 -0.514440 -1.0 +v -0.850994 -0.525175 -1.0 +v -0.844328 -0.535827 -1.0 +v -0.837528 -0.546394 -1.0 +v -0.830596 -0.556876 -1.0 +v -0.823533 -0.567269 -1.0 +v -0.816339 -0.577573 -1.0 +v -0.809017 -0.587785 -1.0 +v -0.801567 -0.597905 -1.0 +v -0.793990 -0.607930 -1.0 +v -0.786288 -0.617860 -1.0 +v -0.778462 -0.627691 -1.0 +v -0.770513 -0.637424 -1.0 +v -0.762443 -0.647056 -1.0 +v -0.754251 -0.656586 -1.0 +v -0.745941 -0.666012 -1.0 +v -0.737513 -0.675333 -1.0 +v -0.728969 -0.684547 -1.0 +v -0.720309 -0.693653 -1.0 +v -0.711536 -0.702650 -1.0 +v -0.702650 -0.711536 -1.0 +v -0.693653 -0.720309 -1.0 +v -0.684547 -0.728969 -1.0 +v -0.675333 -0.737513 -1.0 +v -0.666012 -0.745941 -1.0 +v -0.656586 -0.754251 -1.0 +v -0.647056 -0.762443 -1.0 +v -0.637424 -0.770513 -1.0 +v -0.627691 -0.778462 -1.0 +v -0.617860 -0.786288 -1.0 +v -0.607930 -0.793990 -1.0 +v -0.597905 -0.801567 -1.0 +v -0.587785 -0.809017 -1.0 +v -0.577573 -0.816339 -1.0 +v -0.567269 -0.823533 -1.0 +v -0.556876 -0.830596 -1.0 +v -0.546394 -0.837528 -1.0 +v -0.535827 -0.844328 -1.0 +v -0.525175 -0.850994 -1.0 +v -0.514440 -0.857527 -1.0 +v -0.503623 -0.863923 -1.0 +v -0.492727 -0.870184 -1.0 +v -0.481754 -0.876307 -1.0 +v -0.470704 -0.882291 -1.0 +v -0.459580 -0.888136 -1.0 +v -0.448383 -0.893841 -1.0 +v -0.437116 -0.899405 -1.0 +v -0.425779 -0.904827 -1.0 +v -0.414376 -0.910106 -1.0 +v -0.402906 -0.915241 -1.0 +v -0.391374 -0.920232 -1.0 +v -0.379779 -0.925077 -1.0 +v -0.368125 -0.929776 -1.0 +v -0.356412 -0.934329 -1.0 +v -0.344643 -0.938734 -1.0 +v -0.332820 -0.942991 -1.0 +v -0.320944 -0.947098 -1.0 +v -0.309017 -0.951057 -1.0 +v -0.297042 -0.954865 -1.0 +v -0.285019 -0.958522 -1.0 +v -0.272952 -0.962028 -1.0 +v -0.260842 -0.965382 -1.0 +v -0.248690 -0.968583 -1.0 +v -0.236499 -0.971632 -1.0 +v -0.224271 -0.974527 -1.0 +v -0.212007 -0.977268 -1.0 +v -0.199710 -0.979855 -1.0 +v -0.187381 -0.982287 -1.0 +v -0.175023 -0.984564 -1.0 +v -0.162637 -0.986686 -1.0 +v -0.150226 -0.988652 -1.0 +v -0.137790 -0.990461 -1.0 +v -0.125333 -0.992115 -1.0 +v -0.112856 -0.993611 -1.0 +v -0.100362 -0.994951 -1.0 +v -0.087851 -0.996134 -1.0 +v -0.075327 -0.997159 -1.0 +v -0.062791 -0.998027 -1.0 +v -0.050244 -0.998737 -1.0 +v -0.037690 -0.999289 -1.0 +v -0.025130 -0.999684 -1.0 +v -0.012566 -0.999921 -1.0 +v -0.000000 -1.000000 -1.0 +v 0.012566 -0.999921 -1.0 +v 0.025130 -0.999684 -1.0 +v 0.037690 -0.999289 -1.0 +v 0.050244 -0.998737 -1.0 +v 0.062791 -0.998027 -1.0 +v 0.075327 -0.997159 -1.0 +v 0.087851 -0.996134 -1.0 +v 0.100362 -0.994951 -1.0 +v 0.112856 -0.993611 -1.0 +v 0.125333 -0.992115 -1.0 +v 0.137790 -0.990461 -1.0 +v 0.150226 -0.988652 -1.0 +v 0.162637 -0.986686 -1.0 +v 0.175023 -0.984564 -1.0 +v 0.187381 -0.982287 -1.0 +v 0.199710 -0.979855 -1.0 +v 0.212007 -0.977268 -1.0 +v 0.224271 -0.974527 -1.0 +v 0.236499 -0.971632 -1.0 +v 0.248690 -0.968583 -1.0 +v 0.260842 -0.965382 -1.0 +v 0.272952 -0.962028 -1.0 +v 0.285019 -0.958522 -1.0 +v 0.297042 -0.954865 -1.0 +v 0.309017 -0.951057 -1.0 +v 0.320944 -0.947098 -1.0 +v 0.332820 -0.942991 -1.0 +v 0.344643 -0.938734 -1.0 +v 0.356412 -0.934329 -1.0 +v 0.368125 -0.929776 -1.0 +v 0.379779 -0.925077 -1.0 +v 0.391374 -0.920232 -1.0 +v 0.402906 -0.915241 -1.0 +v 0.414376 -0.910106 -1.0 +v 0.425779 -0.904827 -1.0 +v 0.437116 -0.899405 -1.0 +v 0.448383 -0.893841 -1.0 +v 0.459580 -0.888136 -1.0 +v 0.470704 -0.882291 -1.0 +v 0.481754 -0.876307 -1.0 +v 0.492727 -0.870184 -1.0 +v 0.503623 -0.863923 -1.0 +v 0.514440 -0.857527 -1.0 +v 0.525175 -0.850994 -1.0 +v 0.535827 -0.844328 -1.0 +v 0.546394 -0.837528 -1.0 +v 0.556876 -0.830596 -1.0 +v 0.567269 -0.823533 -1.0 +v 0.577573 -0.816339 -1.0 +v 0.587785 -0.809017 -1.0 +v 0.597905 -0.801567 -1.0 +v 0.607930 -0.793990 -1.0 +v 0.617860 -0.786288 -1.0 +v 0.627691 -0.778462 -1.0 +v 0.637424 -0.770513 -1.0 +v 0.647056 -0.762443 -1.0 +v 0.656586 -0.754251 -1.0 +v 0.666012 -0.745941 -1.0 +v 0.675333 -0.737513 -1.0 +v 0.684547 -0.728969 -1.0 +v 0.693653 -0.720309 -1.0 +v 0.702650 -0.711536 -1.0 +v 0.711536 -0.702650 -1.0 +v 0.720309 -0.693653 -1.0 +v 0.728969 -0.684547 -1.0 +v 0.737513 -0.675333 -1.0 +v 0.745941 -0.666012 -1.0 +v 0.754251 -0.656586 -1.0 +v 0.762443 -0.647056 -1.0 +v 0.770513 -0.637424 -1.0 +v 0.778462 -0.627691 -1.0 +v 0.786288 -0.617860 -1.0 +v 0.793990 -0.607930 -1.0 +v 0.801567 -0.597905 -1.0 +v 0.809017 -0.587785 -1.0 +v 0.816339 -0.577573 -1.0 +v 0.823533 -0.567269 -1.0 +v 0.830596 -0.556876 -1.0 +v 0.837528 -0.546394 -1.0 +v 0.844328 -0.535827 -1.0 +v 0.850994 -0.525175 -1.0 +v 0.857527 -0.514440 -1.0 +v 0.863923 -0.503623 -1.0 +v 0.870184 -0.492727 -1.0 +v 0.876307 -0.481754 -1.0 +v 0.882291 -0.470704 -1.0 +v 0.888136 -0.459580 -1.0 +v 0.893841 -0.448383 -1.0 +v 0.899405 -0.437116 -1.0 +v 0.904827 -0.425779 -1.0 +v 0.910106 -0.414376 -1.0 +v 0.915241 -0.402906 -1.0 +v 0.920232 -0.391374 -1.0 +v 0.925077 -0.379779 -1.0 +v 0.929776 -0.368125 -1.0 +v 0.934329 -0.356412 -1.0 +v 0.938734 -0.344643 -1.0 +v 0.942991 -0.332820 -1.0 +v 0.947098 -0.320944 -1.0 +v 0.951057 -0.309017 -1.0 +v 0.954865 -0.297042 -1.0 +v 0.958522 -0.285019 -1.0 +v 0.962028 -0.272952 -1.0 +v 0.965382 -0.260842 -1.0 +v 0.968583 -0.248690 -1.0 +v 0.971632 -0.236499 -1.0 +v 0.974527 -0.224271 -1.0 +v 0.977268 -0.212007 -1.0 +v 0.979855 -0.199710 -1.0 +v 0.982287 -0.187381 -1.0 +v 0.984564 -0.175023 -1.0 +v 0.986686 -0.162637 -1.0 +v 0.988652 -0.150226 -1.0 +v 0.990461 -0.137790 -1.0 +v 0.992115 -0.125333 -1.0 +v 0.993611 -0.112856 -1.0 +v 0.994951 -0.100362 -1.0 +v 0.996134 -0.087851 -1.0 +v 0.997159 -0.075327 -1.0 +v 0.998027 -0.062791 -1.0 +v 0.998737 -0.050244 -1.0 +v 0.999289 -0.037690 -1.0 +v 0.999684 -0.025130 -1.0 +v 0.999921 -0.012566 -1.0 diff --git a/test/engine/testdata/collision_convex/conemesh_box.xml b/test/engine/testdata/collision_convex/conemesh_box.xml new file mode 100644 index 00000000..ae13ad00 --- /dev/null +++ b/test/engine/testdata/collision_convex/conemesh_box.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index e5afb973..ea5c30e3 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -1102,6 +1102,8 @@ public unsafe struct mjModel_ { public UInt64 nemax; public UInt64 njmax; public UInt64 nconmax; + public UInt64 npolygonmax; + public UInt64 nmeshdegmax; public UInt64 nuserdata; public UInt64 nsensordata; public UInt64 npluginstate; diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index cb62406d..79bfb7c9 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -5148,6 +5148,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("nlight", &MjModel::nlight, &MjModel::set_nlight, reference()) .property("nmat", &MjModel::nmat, &MjModel::set_nmat, reference()) .property("nmesh", &MjModel::nmesh, &MjModel::set_nmesh, reference()) + .property("nmeshdegmax", &MjModel::nmeshdegmax, &MjModel::set_nmeshdegmax, reference()) .property("nmeshface", &MjModel::nmeshface, &MjModel::set_nmeshface, reference()) .property("nmeshgraph", &MjModel::nmeshgraph, &MjModel::set_nmeshgraph, reference()) .property("nmeshnormal", &MjModel::nmeshnormal, &MjModel::set_nmeshnormal, reference()) @@ -5167,6 +5168,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("nplugin", &MjModel::nplugin, &MjModel::set_nplugin, reference()) .property("npluginattr", &MjModel::npluginattr, &MjModel::set_npluginattr, reference()) .property("npluginstate", &MjModel::npluginstate, &MjModel::set_npluginstate, reference()) + .property("npolygonmax", &MjModel::npolygonmax, &MjModel::set_npolygonmax, reference()) .property("nq", &MjModel::nq, &MjModel::set_nq, reference()) .property("nsensor", &MjModel::nsensor, &MjModel::set_nsensor, reference()) .property("nsensordata", &MjModel::nsensordata, &MjModel::set_nsensordata, reference()) diff --git a/wasm/codegen/generated/bindings.h b/wasm/codegen/generated/bindings.h index 9299567c..ff6e32e0 100644 --- a/wasm/codegen/generated/bindings.h +++ b/wasm/codegen/generated/bindings.h @@ -4221,6 +4221,18 @@ struct MjModel { void set_nconmax(int value) { ptr_->nconmax = static_cast(value); } + int npolygonmax() const { + return static_cast(ptr_->npolygonmax); + } + void set_npolygonmax(int value) { + ptr_->npolygonmax = static_cast(value); + } + int nmeshdegmax() const { + return static_cast(ptr_->nmeshdegmax); + } + void set_nmeshdegmax(int value) { + ptr_->nmeshdegmax = static_cast(value); + } int nuserdata() const { return static_cast(ptr_->nuserdata); }