diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index c62b5d6f..081781ac 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -13,12 +13,14 @@ // limitations under the License. #include +#include #include #include #include #include #include #include +#include #include #include #include @@ -108,27 +110,112 @@ void mjCFlexcomp::MarkEmptyCells(mjCFlex* flex, const double* points, int ncells = cx * cy * cz; int order = flex->spec.order; - // determine which cells contain mesh vertices - flex->cell_empty.assign(ncells, true); - for (int i = 0; i < npnt; i++) { - // compute parametric coordinates of mesh vertex in [0, 1]^3 - // for flat meshes (zero extent along an axis), default to 0.5 - double dx = minmax[3] - minmax[0]; - double dy = minmax[4] - minmax[1]; - double dz = minmax[5] - minmax[2]; - double sx = dx > 0 ? (points[3*i+0] - minmax[0]) / dx : 0.5; - double sy = dy > 0 ? (points[3*i+1] - minmax[1]) / dy : 0.5; - double sz = dz > 0 ? (points[3*i+2] - minmax[2]) / dz : 0.5; + // 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); - // find containing cell - int ci = std::min((int)(sx * cx), cx - 1); - int cj = std::min((int)(sy * cy), cy - 1); - int ck = std::min((int)(sz * cz), cz - 1); - ci = std::max(ci, 0); - cj = std::max(cj, 0); - ck = std::max(ck, 0); + double dx = minmax[3] - minmax[0]; + double dy = minmax[4] - minmax[1]; + double dz = minmax[5] - minmax[2]; - flex->cell_empty[ci * cy * cz + cj * cz + ck] = false; + // 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]; + } } // pin nodes that belong exclusively to empty cells diff --git a/test/user/user_flex_test.cc b/test/user/user_flex_test.cc index f3df253c..c4d10e10 100644 --- a/test/user/user_flex_test.cc +++ b/test/user/user_flex_test.cc @@ -1179,6 +1179,64 @@ TEST_F(UserFlexTest, EmptyCellNodePinningQuadratic) { mj_deleteModel(m); } +TEST_F(UserFlexTest, EmptyCellDetectsElements) { + // A cube surface mesh (dim=2, 12 triangles) spanning [0,1]^3. + // With cellcount="6 6 6" (216 cells), only 8 corner cells contain + // mesh vertices. + // + // Bug: MarkEmptyCells only checked vertices, so 208/216 cells are + // marked empty, causing most interior nodes to be incorrectly pinned. + // Fix: check element AABBs to correctly identify occupied cells. + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + // 6x6x6 trilinear grid: (6+1)^3 = 343 nodes + int nadr = m->flex_nodeadr[0]; + int nnode = m->flex_nodenum[0]; + ASSERT_EQ(nnode, 343); + + // Count pinned nodes: those assigned to the parent body. + int parent_bid = mj_name2id(m, mjOBJ_BODY, "parent"); + ASSERT_GT(parent_bid, 0); + int pinned = 0; + for (int n = nadr; n < nadr + nnode; n++) { + if (m->flex_nodebodyid[n] == parent_bid) { + pinned++; + } + } + + // The cube surface fills the entire bounding box. The element-AABB + // marks all boundary cells as surface cells (152/216). The interior + // flood-fill finds no exterior seeds (all boundary cells are surface), + // so the remaining 64 cells are classified as interior (non-empty). + // No cells are empty → 0 nodes pinned. + EXPECT_EQ(pinned, 0); + + mj_deleteData(mj_makeData(m)); + mj_deleteModel(m); +} + TEST_F(UserFlexTest, TotalMassTrilinear) { static constexpr char xml[] = R"(