chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 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;
}