From 188196603d5c1bdfa36f4f6d9ba5b7395324a174 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 20 Apr 2026 09:27:03 -0700 Subject: [PATCH] Fix flexcomp empty cell detection that was causing missing cells. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The previous implementation only checked whether grid cells contained mesh vertices to determine occupancy. For coarse meshes with large faces, most cells were incorrectly marked empty and pruned—even cells fully inside the object volume. This change improves the algorithm by: - Marking cells that overlap with any mesh element's AABB as non-empty. - For surface meshes, running a flood-fill from the grid boundary through non-overlapping cells to identify truly exterior cells. This preserves empty interior cells, preventing incorrect pruning of the object's core. - For volumetric meshes, defaulting to element-AABB overlap detection directly. Limitations for non-watertight meshes: If the mesh contains holes larger than the grid cell size, the flood-fill will leak into the interior. In this case, all non-element cells (including interior ones) will be marked as empty. PiperOrigin-RevId: 902675331 Change-Id: I5a84303a33d5ca7436213e7ce9aca3806c9f5a0f --- src/user/user_flexcomp.cc | 125 ++++++++++++++++++++++++++++++------ test/user/user_flex_test.cc | 58 +++++++++++++++++ 2 files changed, 164 insertions(+), 19 deletions(-) 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"(