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

Exit recursion gracefully when horizon reaches empty polytope in EPA.

Browse Source 
PiperOrigin-RevId: 682050023
Change-Id: I8e04f00e6ab3e15e177d1902889441cdbf11cafd
This commit is contained in:
Kyle Bayes
2024-10-03 15:16:33 -07:00
committed by Copybara-Service
parent 9d91703871
commit dba32e827c
1 changed files with 14 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+14 -17
View File
@@ -918,25 +918,17 @@ static int newVertex(Polytope* pt, const mjtNum v1[3], const mjtNum v2[3]) {
// delete face from map // delete face from map (return non-zero on error)
void deleteFace(Polytope* pt, Face* face) { static int deleteFace(Polytope* pt, Face* face) {
// SHOULD NOT OCCUR // SHOULD NOT OCCUR
if (!pt->nmap) { if (pt->nmap < 2) {
mju_warning("EPA: trying to delete face from empty polytope"); pt->nmap = 0;
return; return 1;
} }
face->dist = -1; face->dist = -1;
pt->nmap--; pt->map[face->index] = pt->map[--pt->nmap];
// SHOULD NOT OCCUR
// last face; nothing to do
if (!pt->nmap) {
return; // EPA will flag a warning
}
pt->map[face->index] = pt->map[pt->nmap];
pt->map[face->index]->index = face->index; pt->map[face->index]->index = face->index;
return 0;
} }
@@ -1010,7 +1002,7 @@ static int horizonRec(Horizon* h, Face* face, int e) {
// v is visible from w so it is deleted and adjacent faces are checked // v is visible from w so it is deleted and adjacent faces are checked
if (mju_dot3(face->v, h->w) >= dist2) { if (mju_dot3(face->v, h->w) >= dist2) {
deleteFace(h->pt, face); if (deleteFace(h->pt, face)) return 1; // escape recursion on error
// recursively search the adjacent faces on the next two edges // recursively search the adjacent faces on the next two edges
for (int k = 1; k < 3; k++) { for (int k = 1; k < 3; k++) {
@@ -1032,7 +1024,7 @@ static int horizonRec(Horizon* h, Face* face, int e) {
// creates horizon given the face as starting point // creates horizon given the face as starting point
static void horizon(Horizon* h, Face* face) { static void horizon(Horizon* h, Face* face) {
deleteFace(h->pt, face); if (deleteFace(h->pt, face)) return;
// first edge // first edge
Face* adjFace = &h->pt->faces[face->adj[0]]; Face* adjFace = &h->pt->faces[face->adj[0]];
@@ -1133,6 +1125,11 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o
h.w = w; h.w = w;
horizon(&h, face); horizon(&h, face);
if (!pt->nmap) {
h.nedges = 0;
// next iteration will clean up and error out
continue;
}
// insert w as new vertex and attach faces along the horizon // insert w as new vertex and attach faces along the horizon
int wi = newVertex(pt, w1, w2), nfaces = pt->nfaces, nedges = h.nedges; int wi = newVertex(pt, w1, w2), nfaces = pt->nfaces, nedges = h.nedges;