Fix some bugs in epa algorithm

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) {

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;
+        int swapWithChild = -1;
-        
-        for (swapWithChild = 0; swapWithChild < 2; ++swapWithChild) {
-            int childHeapIndex = GET_CHILD_INDEX(heapIndex, swapWithChild);
 
-            // check that we don't run off the end of the heap
+        int childHeapIndex = GET_CHILD_INDEX(heapIndex, 0);
-            if (childHeapIndex >= simplex->triangleCount) {
-                break;
-            }
 
-            if (EXPANDING_SIMPLEX_GET_DISTANCE(simplex, simplex->triangleHeap[heapIndex]) < currentDistance) {
+        // reached the end of the heap
-                swapWithChild = childHeapIndex;
+        if (childHeapIndex >= simplex->triangleCount) {
-                break;
+            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[heapIndex] = simplex->triangleHeap[swapWithChild];
-        simplex->triangleHeap[childHeapIndex] = tmp;
+        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) {

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;
 };