chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix flexcomp empty cell detection that was causing missing cells.

Browse Source 
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
This commit is contained in:
Alessio Quaglino
2026-04-20 09:27:03 -07:00
committed by Copybara-Service
parent 476e2e909e
commit 188196603d
2 changed files with 164 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_flexcomp.cc
+106 -19
View File
@@ -13,12 +13,14 @@
// limitations under the License.
#include <algorithm>
#include <array>
#include <climits>
#include <cmath>
#include <cstddef>
#include <cstdio>
#include <cstring>
#include <iostream>
#include <queue>
#include <sstream>
#include <stdexcept>
#include <string>
@@ -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<bool> 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<bool> visited(ncells, false);
std::queue<std::array<int, 3>> 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
test/user/user_flex_test.cc
+58
View File
@@ -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"(
<mujoco>
<worldbody>
<body name="parent">
<freejoint/>
<inertial mass="0.01" pos="0.5 0.5 0.5"
diaginertia="0.001 0.001 0.001"/>
<flexcomp name="test" type="direct" dim="2"
dof="trilinear" mass="1" cellcount="6 6 6"
point="0 0 0 1 0 0 1 1 0 0 1 0
0 0 1 1 0 1 1 1 1 0 1 1"
element="0 1 2 0 2 3 4 6 5 4 7 6
0 5 1 0 4 5 2 7 3 2 6 7
0 3 7 0 7 4 1 5 6 1 6 2">
<contact selfcollide="none"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>