From 00fb8fb739513ae034045cc13eee5b867b1c4bd1 Mon Sep 17 00:00:00 2001 From: James Lambert Date: Wed, 1 Jun 2022 16:53:24 -0600 Subject: [PATCH] Fix some bugs in epa algorithm --- src/math/plane.c | 2 +- src/physics/epa.c | 69 ++++++++++++++++++++++++++++++----------------- src/physics/epa.h | 1 + 3 files changed, 46 insertions(+), 26 deletions(-) diff --git a/src/math/plane.c b/src/math/plane.c index c81353b..922652f 100644 --- a/src/math/plane.c +++ b/src/math/plane.c @@ -46,7 +46,7 @@ void calculateBarycentricCoords(struct Vector3* a, struct Vector3* b, struct Vec float denom = 1.0f / (d00 * d11 - d01 * d01); output->y = (d11 * d20 - d01 * d21) * denom; output->z = (d00 * d21 - d01 * d20) * denom; - output->x = 1.0f - output->x - output->y; + output->x = 1.0f - output->y - output->z; } void evaluateBarycentricCoords(struct Vector3* a, struct Vector3* b, struct Vector3* c, struct Vector3* bary, struct Vector3* output) { diff --git a/src/physics/epa.c b/src/physics/epa.c index 6d11c30..c7f7c7b 100644 --- a/src/physics/epa.c +++ b/src/physics/epa.c @@ -38,6 +38,23 @@ struct ExpandingSimplex { unsigned char triangleHeap[MAX_SIMPLEX_TRIANGLES]; }; + +#define GET_PARENT_INDEX(heapIndex) (((heapIndex) - 1) >> 1) +#define GET_CHILD_INDEX(heapIndex, childHeapIndex) (((heapIndex) << 1) + 1 + (childHeapIndex)) +#define EXPANDING_SIMPLEX_GET_DISTANCE(simplex, triangleIndex) ((simplex)->triangles[triangleIndex].distanceToOrigin) + +int validateHeap(struct ExpandingSimplex* simplex) { + for (int i = 1; i < simplex->triangleCount; ++i) { + int parentIndex = GET_PARENT_INDEX(i); + + if (simplex->triangles[simplex->triangleHeap[i]].distanceToOrigin < simplex->triangles[simplex->triangleHeap[parentIndex]].distanceToOrigin) { + return 0; + } + } + + return 1; +} + void expandingSimplexAddPoint(struct ExpandingSimplex* simplex, struct Vector3* aPoint, struct Vector3* pointDiff) { int result = simplex->pointCount; simplex->aPoints[result] = *aPoint; @@ -45,10 +62,6 @@ void expandingSimplexAddPoint(struct ExpandingSimplex* simplex, struct Vector3* ++simplex->pointCount; } -#define GET_PARENT_INDEX(heapIndex) (((heapIndex) - 1) >> 1) -#define GET_CHILD_INDEX(heapIndex, childHeapIndex) (((heapIndex) << 1) + 1 + (childHeapIndex)) -#define EXPANDING_SIMPLEX_GET_DISTANCE(simplex, triangleIndex) ((simplex)->triangles[triangleIndex].distanceToOrigin) - int expandingSimplexSiftDownHeap(struct ExpandingSimplex* simplex, int heapIndex) { int parentHeapIndex = GET_PARENT_INDEX(heapIndex); float currentDistance = EXPANDING_SIMPLEX_GET_DISTANCE(simplex, simplex->triangleHeap[heapIndex]); @@ -76,35 +89,40 @@ int expandingSimplexSiftUpHeap(struct ExpandingSimplex* simplex, int heapIndex) float currentDistance = EXPANDING_SIMPLEX_GET_DISTANCE(simplex, simplex->triangleHeap[heapIndex]); while (heapIndex < simplex->triangleCount) { - int swapWithChild; - - for (swapWithChild = 0; swapWithChild < 2; ++swapWithChild) { - int childHeapIndex = GET_CHILD_INDEX(heapIndex, swapWithChild); + int swapWithChild = -1; - // check that we don't run off the end of the heap - if (childHeapIndex >= simplex->triangleCount) { - break; - } + int childHeapIndex = GET_CHILD_INDEX(heapIndex, 0); - if (EXPANDING_SIMPLEX_GET_DISTANCE(simplex, simplex->triangleHeap[heapIndex]) < currentDistance) { - swapWithChild = childHeapIndex; - break; - } + // reached the end of the heap + if (childHeapIndex >= simplex->triangleCount) { + break; } - if (swapWithChild == 2) { + float childDistance = EXPANDING_SIMPLEX_GET_DISTANCE(simplex, simplex->triangleHeap[childHeapIndex]); + + // check that we don't run off the end of the heap + if (childDistance < currentDistance) { + swapWithChild = childHeapIndex; + } + + float otherChildDistance = EXPANDING_SIMPLEX_GET_DISTANCE(simplex, simplex->triangleHeap[childHeapIndex + 1]); + + // grab the smallest child + if (childHeapIndex + 1 < simplex->triangleCount && otherChildDistance < currentDistance && otherChildDistance < childDistance) { + swapWithChild = childHeapIndex + 1; + } + + if (swapWithChild == -1) { // no child out of order break; } - int childHeapIndex = GET_CHILD_INDEX(heapIndex, swapWithChild); - // swap child with the current node int tmp = simplex->triangleHeap[heapIndex]; - simplex->triangleHeap[heapIndex] = simplex->triangleHeap[childHeapIndex]; - simplex->triangleHeap[childHeapIndex] = tmp; + simplex->triangleHeap[heapIndex] = simplex->triangleHeap[swapWithChild]; + simplex->triangleHeap[swapWithChild] = tmp; - heapIndex = childHeapIndex; + heapIndex = swapWithChild; } return heapIndex; @@ -179,7 +197,7 @@ void expandingSimplexTriangleDetermineDistance(struct ExpandingSimplex* simplex, for (int i = 0; i < 3; ++i) { if (expandingSimplexTriangleCheckEdge(simplex, triangle, i)) { - return ; + return; } } @@ -232,6 +250,7 @@ void expandingSimplexTriangleCheckRotate(struct ExpandingSimplex* simplex, int t expandingSimplexRotateEdge(simplex, triangle, triangleIndex, heapIndex); } else { expandingSimplexTriangleDetermineDistance(simplex, triangle); + expandingSimplexFixHeap(simplex, heapIndex); } } @@ -301,7 +320,7 @@ void expandingSimplexExpand(struct ExpandingSimplex* expandingSimplex, int newPo int nextNextFace = NEXT_FACE(nextFace); next.indices[0] = existing.indices[i]; next.indices[1] = existing.indices[nextFace]; - next.indices[3] = newPointIndex; + next.indices[2] = newPointIndex; next.adjacentFaces[0] = existing.adjacentFaces[i]; next.adjacentFaces[1] = triangleIndices[nextFace]; @@ -355,7 +374,7 @@ void epaCalculateContact(struct ExpandingSimplex* simplex, struct SimplexTriangl &result->contactA ); - vector3AddScaled(&result->contactA, &result->normal, result->penetration, &result->contactB); + vector3AddScaled(&result->contactA, &result->normal, -result->penetration, &result->contactB); } void epaSolve(struct Simplex* startingSimplex, void* objectA, MinkowsiSum objectASum, void* objectB, MinkowsiSum objectBSum, struct EpaResult* result) { diff --git a/src/physics/epa.h b/src/physics/epa.h index 98edce7..24067bb 100644 --- a/src/physics/epa.h +++ b/src/physics/epa.h @@ -6,6 +6,7 @@ struct EpaResult { struct Vector3 contactA; struct Vector3 contactB; + // points from A to B struct Vector3 normal; float penetration; };