diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index e3e41069..81a0f155 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -447,6 +447,8 @@ static void makeFlexSparse(mjModel* m, mjData* d) { mju_zeroInt(m->flex_vertedge, 2 * m->nflexedge); mju_zeroInt(m->flex_vertedge, 2 * m->nflexedge); mju_zero(m->flex_vertmetric, 4 * m->nflexvert); + mju_zeroInt(m->flexedge_J_colind, m->nJfe); + mju_zeroInt(m->flexvert_J_colind, 2 * m->nJfv); int current_adj_offset = 0; // compute lengths and Jacobians of edges diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 85b900ae..7030768c 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -13,14 +13,12 @@ // limitations under the License. #include -#include #include #include #include #include #include #include -#include #include #include #include @@ -107,116 +105,11 @@ void mjCFlexcomp::MarkEmptyCells(mjCFlex* flex, const double* points, int cx = flex->spec.cellcount[0]; int cy = flex->spec.cellcount[1]; int cz = flex->spec.cellcount[2]; - int ncells = cx * cy * cz; int order = flex->spec.order; - // determine which cells contain mesh elements (not just vertices) - // for each element, compute its AABB and mark all overlapping cells - std::vector has_element(ncells, false); - - double dx = minmax[3] - minmax[0]; - double dy = minmax[4] - minmax[1]; - double dz = minmax[5] - minmax[2]; - - // vertices per element: dim+1 (edges=2, triangles=3, tets=4) - int nvpe = flex->spec.dim + 1; - - if (nvpe > 0 && !element.empty()) { - int nelem = element.size() / nvpe; - for (int e = 0; e < nelem; e++) { - // compute element AABB - double elo[3] = {1e30, 1e30, 1e30}; - double ehi[3] = {-1e30, -1e30, -1e30}; - for (int v = 0; v < nvpe; v++) { - int vid = element[nvpe * e + v]; - for (int j = 0; j < 3; j++) { - elo[j] = std::min(elo[j], points[3 * vid + j]); - ehi[j] = std::max(ehi[j], points[3 * vid + j]); - } - } - - // map element AABB to cell range - auto cellIdx = [](double coord, double lo, double d, int nc) { - if (d <= 0) return 0; - int c = (int)((coord - lo) / d * nc); - return std::max(0, std::min(nc - 1, c)); - }; - - int ci0 = cellIdx(elo[0], minmax[0], dx, cx); - int ci1 = cellIdx(ehi[0], minmax[0], dx, cx); - int cj0 = cellIdx(elo[1], minmax[1], dy, cy); - int cj1 = cellIdx(ehi[1], minmax[1], dy, cy); - int ck0 = cellIdx(elo[2], minmax[2], dz, cz); - int ck1 = cellIdx(ehi[2], minmax[2], dz, cz); - - // mark all overlapping cells as containing elements - for (int ci = ci0; ci <= ci1; ci++) { - for (int cj = cj0; cj <= cj1; cj++) { - for (int ck = ck0; ck <= ck1; ck++) { - has_element[ci * cy * cz + cj * cz + ck] = true; - } - } - } - } - } - - // default: all cells non-empty (only exterior cells will be empty) - flex->cell_empty.assign(ncells, false); - - // for dim=2 (surface mesh): check watertightness and flood-fill - if (flex->spec.dim == 2 && nvpe == 3 && !element.empty()) { - // flood-fill from grid boundary to find exterior cells - // cells reachable from the boundary through non-element cells - // are outside the mesh volume; cells NOT reachable are interior - std::vector visited(ncells, false); - std::queue> bfs; - - // seed BFS from boundary cells that have no elements - for (int ci = 0; ci < cx; ci++) { - for (int cj = 0; cj < cy; cj++) { - for (int ck = 0; ck < cz; ck++) { - if (ci == 0 || ci == cx - 1 || - cj == 0 || cj == cy - 1 || - ck == 0 || ck == cz - 1) { - int idx = ci * cy * cz + cj * cz + ck; - if (!has_element[idx] && !visited[idx]) { - visited[idx] = true; - flex->cell_empty[idx] = true; - bfs.push({ci, cj, ck}); - } - } - } - } - } - - // BFS: spread through non-element cells - const int dirs[6][3] = { - {-1, 0, 0}, {1, 0, 0}, {0, -1, 0}, - {0, 1, 0}, {0, 0, -1}, {0, 0, 1}}; - while (!bfs.empty()) { - auto [ci, cj, ck] = bfs.front(); - bfs.pop(); - for (auto& d : dirs) { - int ni = ci + d[0], nj = cj + d[1], nk = ck + d[2]; - if (ni < 0 || ni >= cx || - nj < 0 || nj >= cy || - nk < 0 || nk >= cz) { - continue; - } - int nidx = ni * cy * cz + nj * cz + nk; - if (!visited[nidx] && !has_element[nidx]) { - visited[nidx] = true; - flex->cell_empty[nidx] = true; - bfs.push({ni, nj, nk}); - } - } - } - } else { - // dim!=2 (e.g., tet mesh): cells without element overlap are empty - for (int c = 0; c < ncells; c++) { - flex->cell_empty[c] = !has_element[c]; - } - } + // delegate cell_empty computation to mjCFlex + int nelem = element.size() / (flex->spec.dim + 1); + flex->ComputeCellEmpty(points, element.data(), npnt, nelem, flex->spec.dim, minmax); // pin nodes that belong exclusively to empty cells for (int gi = 0; gi < nx; gi++) { diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 23d71d90..d9b454f5 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -4698,8 +4699,8 @@ void mjCFlex::Compile(const mjVFS* vfs) { // no elemtexcoord: copy from faces if (elemtexcoord_.empty() && !texcoord_.empty()) { - elemtexcoord_.assign(3*nelem, 0); - memcpy(elemtexcoord_.data(), elem_.data(), 3*nelem*sizeof(int)); + elemtexcoord_.assign((dim + 1) * nelem, 0); + memcpy(elemtexcoord_.data(), elem_.data(), (dim + 1) * nelem * sizeof(int)); } // resolve material name @@ -4914,6 +4915,15 @@ void mjCFlex::Compile(const mjVFS* vfs) { // create shell fragments and element-vertex collision pairs CreateShellPair(); + // recompute cell_empty from vertex/element geometry + // (survives XML round-trips where flexcomp data is lost) + if (interpolated && cell_empty.empty()) { + int cx = spec.cellcount[0], cy = spec.cellcount[1], cz = spec.cellcount[2]; + if (cx * cy * cz > 1) { + ComputeCellEmpty(vertxpos.data(), elem_.data(), nvert, nelem, dim); + } + } + // compute linear stiffness for interpolated elements (cached) bool stiffness_cached = false; if (young > 0 && interpolated) { @@ -5223,6 +5233,131 @@ std::vector mjCFlex::ComputeUnrotatedNodePositions( } +// identify cells with no mesh content from vertex/element geometry +void mjCFlex::ComputeCellEmpty(const double* vpos, const int* elems, + int nv, int ne, int fdim, + const double* bbox) { + int cx = spec.cellcount[0]; + int cy = spec.cellcount[1]; + int cz = spec.cellcount[2]; + int ncells = cx * cy * cz; + + // use precomputed bounding box if provided, otherwise compute from vertices + double minmax[6]; + if (bbox) { + for (int j = 0; j < 6; j++) minmax[j] = bbox[j]; + } else { + minmax[0] = minmax[1] = minmax[2] = 1e30; + minmax[3] = minmax[4] = minmax[5] = -1e30; + for (int i = 0; i < nv; i++) { + for (int j = 0; j < 3; j++) { + minmax[j+0] = std::min(minmax[j+0], vpos[3*i+j]); + minmax[j+3] = std::max(minmax[j+3], vpos[3*i+j]); + } + } + } + + double dx = minmax[3] - minmax[0]; + double dy = minmax[4] - minmax[1]; + double dz = minmax[5] - minmax[2]; + + // determine which cells contain mesh elements + std::vector has_element(ncells, false); + int nvpe = fdim + 1; + + if (nvpe > 0 && ne > 0) { + for (int e = 0; e < ne; e++) { + // compute element AABB + double elo[3] = {1e30, 1e30, 1e30}; + double ehi[3] = {-1e30, -1e30, -1e30}; + for (int v = 0; v < nvpe; v++) { + int vid = elems[nvpe * e + v]; + for (int j = 0; j < 3; j++) { + elo[j] = std::min(elo[j], vpos[3 * vid + j]); + ehi[j] = std::max(ehi[j], vpos[3 * vid + j]); + } + } + + // map element AABB to cell range + auto cellIdx = [](double coord, double lo, double d, int nc) { + if (d <= 0) return 0; + int c = (int)((coord - lo) / d * nc); + return std::max(0, std::min(nc - 1, c)); + }; + + int ci0 = cellIdx(elo[0], minmax[0], dx, cx); + int ci1 = cellIdx(ehi[0], minmax[0], dx, cx); + int cj0 = cellIdx(elo[1], minmax[1], dy, cy); + int cj1 = cellIdx(ehi[1], minmax[1], dy, cy); + int ck0 = cellIdx(elo[2], minmax[2], dz, cz); + int ck1 = cellIdx(ehi[2], minmax[2], dz, cz); + + for (int ci = ci0; ci <= ci1; ci++) { + for (int cj = cj0; cj <= cj1; cj++) { + for (int ck = ck0; ck <= ck1; ck++) { + has_element[ci * cy * cz + cj * cz + ck] = true; + } + } + } + } + } + + cell_empty.assign(ncells, false); + + // for dim=2 (surface mesh): flood-fill from boundary to find exterior cells + if (fdim == 2 && nvpe == 3 && ne > 0) { + std::vector visited(ncells, false); + std::queue> bfs; + + // seed BFS from boundary cells that have no elements + for (int ci = 0; ci < cx; ci++) { + for (int cj = 0; cj < cy; cj++) { + for (int ck = 0; ck < cz; ck++) { + if (ci == 0 || ci == cx - 1 || + cj == 0 || cj == cy - 1 || + ck == 0 || ck == cz - 1) { + int idx = ci * cy * cz + cj * cz + ck; + if (!has_element[idx] && !visited[idx]) { + visited[idx] = true; + cell_empty[idx] = true; + bfs.push({ci, cj, ck}); + } + } + } + } + } + + // BFS: spread through non-element cells + const int dirs[6][3] = { + {-1, 0, 0}, {1, 0, 0}, {0, -1, 0}, + {0, 1, 0}, {0, 0, -1}, {0, 0, 1}}; + while (!bfs.empty()) { + auto [ci, cj, ck] = bfs.front(); + bfs.pop(); + for (auto& d : dirs) { + int ni = ci + d[0], nj = cj + d[1], nk = ck + d[2]; + if (ni < 0 || ni >= cx || + nj < 0 || nj >= cy || + nk < 0 || nk >= cz) { + continue; + } + int nidx = ni * cy * cz + nj * cz + nk; + if (!visited[nidx] && !has_element[nidx]) { + visited[nidx] = true; + cell_empty[nidx] = true; + bfs.push({ni, nj, nk}); + } + } + } + } else { + // dim!=2: cells without element overlap are empty + for (int c = 0; c < ncells; c++) { + cell_empty[c] = !has_element[c]; + } + } +} + + // create flex BVH void mjCFlex::CreateBVH() { int nbvh = 0; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index a2566b4d..ac58c810 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1055,6 +1055,9 @@ class mjCFlex: public mjCFlex_, private mjsFlex { void Compile(const mjVFS* vfs); // compiler void CreateBVH(void); // create flex BVH void CreateShellPair(void); // create shells and evpairs + void ComputeCellEmpty(const double* vpos, const int* elems, // identify cells + int nv, int ne, int fdim, // with no mesh content + const double* bbox = nullptr); // optional precomputed bbox std::vector vert0_; // vertex positions in [0, 1]^d in the bounding box std::vector node0_; // node Cartesian positions diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index c31b1bf2..f249df3f 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -1553,7 +1553,7 @@ void mjXReader::OneFlex(XMLElement* elem, mjsFlex* flex) { flex->internal = (n == 1); } MapValue(cont, "selfcollide", &flex->selfcollide, flexself_map, 5); - if (MapValue(cont, "passive", &flex->passive, bool_map, 2)) { + if (MapValue(cont, "passive", &n, bool_map, 2)) { flex->passive = (n == 1); } ReadAttrInt(cont, "activelayers", &flex->activelayers); diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 0bc4c07a..ff17f141 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -190,6 +190,7 @@ void mjXWriter::OneFlex(XMLElement* elem, const mjCFlex* flex) { WriteAttrKey(cont, "internal", bool_map, 2, flex->internal, defflex.internal); WriteAttrKey(cont, "selfcollide", flexself_map, 5, flex->selfcollide, defflex.selfcollide); WriteAttrInt(cont, "activelayers", flex->activelayers, defflex.activelayers); + WriteAttrKey(cont, "passive", bool_map, 2, flex->passive, defflex.passive); // remove contact is no attributes if (!cont->FirstAttribute()) { @@ -202,7 +203,7 @@ void mjXWriter::OneFlex(XMLElement* elem, const mjCFlex* flex) { WriteAttr(elastic, "poisson", 1, &flex->poisson, &defflex.poisson); WriteAttr(elastic, "thickness", 1, &flex->thickness, &defflex.thickness); WriteAttr(elastic, "damping", 1, &flex->damping, &defflex.damping); - WriteAttrKey(elastic, "elastic2d", elastic2d_map, 2, flex->elastic2d, defflex.elastic2d); + WriteAttrKey(elastic, "elastic2d", elastic2d_map, 4, flex->elastic2d, defflex.elastic2d); // edge subelement XMLElement* edge = InsertEnd(elem, "edge"); diff --git a/test/fixture.cc b/test/fixture.cc index f621330e..6a6b1732 100644 --- a/test/fixture.cc +++ b/test/fixture.cc @@ -251,10 +251,17 @@ mjtNum CompareModel(const mjModel* m1, const mjModel* m2, // compare arrays, apart from bvh-related ones (which includes flex_vert0), as // those are sensitive to numerical differences when meshes are perfectly - // symmetric. + // symmetric. Also skip flex fields derived from node local positions and + // cell geometry that are not fully serialized to XML. #define X(type, name, nr, nc) \ - if (strncmp(#name, "bvh_", 4) && strncmp(#name, "flex_vert0", 4) && \ - strncmp(#name, "mesh_poly", 4)) { \ + if (strncmp(#name, "bvh_", 4) && \ + strncmp(#name, "flex_vert", 9) && \ + strncmp(#name, "mesh_poly", 9) && \ + strcmp(#name, "flex_centered") && \ + strcmp(#name, "flex_size") && \ + strcmp(#name, "flexedge_length0") && \ + strcmp(#name, "flexedge_invweight0") && \ + strncmp(#name, "flex_node", 9)) { \ for (int r = 0; r < m1->nr; r++) { \ for (int c = 0; c < nc; c++) { \ dif = Compare(m1->name[r * nc + c], m2->name[r * nc + c]); \