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

Fix bug in DeleteFace in NativeCCD.

Browse Source 
PiperOrigin-RevId: 675948504
Change-Id: If9d967c8e75580a3a002fce9c10a2e1bfd0b5d46
This commit is contained in:
Kyle Bayes
2024-09-18 05:19:00 -07:00
committed by Copybara-Service
parent bfda9f4ace
commit 0657e3e871
1 changed files with 11 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+11 -3
View File
@@ -898,7 +898,7 @@ static int newVertex(Polytope* pt, const mjtNum v1[3], const mjtNum v2[3]) {
// swap two nodes in heap // swap two nodes in heap
inline void swap(Polytope* pt, int i, int j) { static inline void swap(Polytope* pt, int i, int j) {
Face* tmp = pt->heap[i]; Face* tmp = pt->heap[i];
pt->heap[i] = pt->heap[j]; pt->heap[i] = pt->heap[j];
pt->heap[j] = tmp; pt->heap[j] = tmp;
@@ -927,11 +927,19 @@ void heapify(Polytope* pt, int i) {
// delete face from heap // delete face from heap
void deleteFace(Polytope* pt, Face* face) { void deleteFace(Polytope* pt, Face* face) {
if (!pt->nheap) {
return;
}
face->dist = -1;
pt->nheap--; pt->nheap--;
if (pt->nheap < 1) return;
// last face; nothing to do
if (!pt->nheap) {
return;
}
// bubble up face to top of heap // bubble up face to top of heap
face->dist = -1;
int i = face->index; int i = face->index;
while (i != 0) { while (i != 0) {
int parent = (i - 1) >> 1; int parent = (i - 1) >> 1;