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
void deleteFace(Polytope* pt, Face* face) {
// delete face from map (return non-zero on error)
static int deleteFace(Polytope* pt, Face* face) {
// SHOULD NOT OCCUR
if (!pt->nmap) {
mju_warning("EPA: trying to delete face from empty polytope");
return;
if (pt->nmap < 2) {
pt->nmap = 0;
return 1;
}
face->dist = -1;
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] = pt->map[--pt->nmap];
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
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
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
static void horizon(Horizon* h, Face* face) {
deleteFace(h->pt, face);
if (deleteFace(h->pt, face)) return;
// first edge
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;
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
int wi = newVertex(pt, w1, w2), nfaces = pt->nfaces, nedges = h.nedges;