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

Fix address bug in meshEdgeNormals in doing edge multiccd with meshes.

Browse Source 
PiperOrigin-RevId: 790731924
Change-Id: Ia475b1f01731518166f229c25ebffd1fbf7c7939
This commit is contained in:
Kyle Bayes
2025-08-04 07:02:16 -07:00
committed by Copybara-Service
parent 7baaa39846
commit f2a2afeb03
2 changed files with 66 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+17 -12
View File
@@ -1833,21 +1833,26 @@ static int meshEdgeNormals(mjtNum* res, mjtNum* endverts, int dim, mjCCDObj* obj
int v1_num = m->mesh_polymapnum[vertadr + v1i];
if (v1_num > mjMAX_POLYVERT) v1_num = mjMAX_POLYVERT;
const int* polymap = m->mesh_polymap + v1_adr;
const int* polyvert = m->mesh_polyvert;
const int* polyvertadr = m->mesh_polyvertadr + polyadr;
const int* polyvertnum = m->mesh_polyvertnum + polyadr;
const float* vert = m->mesh_vert + 3*vertadr;
// loop through all faces with vertex v1
for (int i = 0; i < v1_num; i++) {
int idx = m->mesh_polymap[v1_adr + i];
int adr = m->mesh_polyvertadr[polyadr + idx];
int nvert = m->mesh_polyvertnum[polyadr + idx];
int idx = polymap[i];
int adr = polyvertadr[idx];
int nvert = polyvertnum[idx];
// find previous vertex in polygon to form edge
for (int j = 0; j < nvert; j++) {
int v = m->mesh_polyvert[adr + j];
if (v == v1i) {
float* verts = m->mesh_vert + 3*vertadr;
if (polyvert[adr + j] == v1i) {
int k = (j == 0) ? nvert - 1 : j - 1;
float* vert = verts + 3*k;
globalcoord(endverts + 3*i, mat, pos, vert[0], vert[1], vert[2]);
const float* v = vert + 3*polyvert[adr + k];
globalcoord(endverts + 3*i, mat, pos, v[0], v[1], v[2]);
sub3(res + 3*i, endverts + 3*i, v1);
mju_normalize3(res + 3*i);
break;
}
}
}
@@ -2041,15 +2046,15 @@ static int meshFace(mjtNum* res, mjCCDObj* obj, int idx) {
const mjtNum* pos = obj->data->geom_xpos + g;
int polyadr = m->mesh_polyadr[m->geom_dataid[obj->geom]];
int vertadr = m->mesh_vertadr[m->geom_dataid[obj->geom]];
const float* vert = m->mesh_vert + 3*vertadr;
const int* polyvert = m->mesh_polyvert;
int adr = m->mesh_polyvertadr[polyadr + idx], j = 0;
int nvert = m->mesh_polyvertnum[polyadr + idx];
if (nvert > mjMAX_POLYVERT) nvert = mjMAX_POLYVERT;
for (int i = nvert - 1; i >= 0; i--) {
float* verts = m->mesh_vert + 3*vertadr;
int v = m->mesh_polyvert[adr + i];
float* vert = verts + 3*v;
globalcoord(res + 3*j++, mat, pos, vert[0], vert[1], vert[2]);
const float* v = vert + 3*polyvert[adr + i];
globalcoord(res + 3*j++, mat, pos, v[0], v[1], v[2]);
}
return nvert;
}
test/engine/engine_collision_gjk_test.cc
+49
View File
@@ -1763,6 +1763,55 @@ TEST_F(MjGjkTest, MeshEdge) {
mj_deleteModel(model);
}
TEST_F(MjGjkTest, MeshEdge2) {
static constexpr char xml[] = R"(
<mujoco>
<option>
<flag multiccd="enable"/>
</option>
<asset>
<mesh name="meshbox"
vertex="-1 -1 -1
1 -1 -1
1 1 -1
1 1 1
1 -1 1
-1 1 -1
-1 1 1
-1 -1 1"/>
</asset>
<worldbody>
<geom type="box" name="floor" size="6 6 2" pos="0 0 -1"/>
<body pos="2 0 1" euler="0 5 0">
<freejoint/>
<geom type="mesh" name="meshbox" mesh="meshbox"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1000> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
mj_forward(model, data);
int g1 = mj_name2id(model, mjOBJ_GEOM, "floor");
int g2 = mj_name2id(model, mjOBJ_GEOM, "meshbox");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 4);
EXPECT_EQ(ncons, 2);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, EllipsoidEllipsoidPenetrating) {
std::array<char, 1000> error;
mjModel* model = LoadModelFromString(kEllipoid, error.data(), error.size());