Fix some bugs in epa algorithm
This commit is contained in:
parent
877eb9ea24
commit
00fb8fb739
|
@ -46,7 +46,7 @@ void calculateBarycentricCoords(struct Vector3* a, struct Vector3* b, struct Vec
|
||||||
float denom = 1.0f / (d00 * d11 - d01 * d01);
|
float denom = 1.0f / (d00 * d11 - d01 * d01);
|
||||||
output->y = (d11 * d20 - d01 * d21) * denom;
|
output->y = (d11 * d20 - d01 * d21) * denom;
|
||||||
output->z = (d00 * d21 - d01 * d20) * 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) {
|
void evaluateBarycentricCoords(struct Vector3* a, struct Vector3* b, struct Vector3* c, struct Vector3* bary, struct Vector3* output) {
|
||||||
|
|
|
@ -38,6 +38,23 @@ struct ExpandingSimplex {
|
||||||
unsigned char triangleHeap[MAX_SIMPLEX_TRIANGLES];
|
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) {
|
void expandingSimplexAddPoint(struct ExpandingSimplex* simplex, struct Vector3* aPoint, struct Vector3* pointDiff) {
|
||||||
int result = simplex->pointCount;
|
int result = simplex->pointCount;
|
||||||
simplex->aPoints[result] = *aPoint;
|
simplex->aPoints[result] = *aPoint;
|
||||||
|
@ -45,10 +62,6 @@ void expandingSimplexAddPoint(struct ExpandingSimplex* simplex, struct Vector3*
|
||||||
++simplex->pointCount;
|
++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 expandingSimplexSiftDownHeap(struct ExpandingSimplex* simplex, int heapIndex) {
|
||||||
int parentHeapIndex = GET_PARENT_INDEX(heapIndex);
|
int parentHeapIndex = GET_PARENT_INDEX(heapIndex);
|
||||||
float currentDistance = EXPANDING_SIMPLEX_GET_DISTANCE(simplex, simplex->triangleHeap[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]);
|
float currentDistance = EXPANDING_SIMPLEX_GET_DISTANCE(simplex, simplex->triangleHeap[heapIndex]);
|
||||||
|
|
||||||
while (heapIndex < simplex->triangleCount) {
|
while (heapIndex < simplex->triangleCount) {
|
||||||
int swapWithChild;
|
int swapWithChild = -1;
|
||||||
|
|
||||||
for (swapWithChild = 0; swapWithChild < 2; ++swapWithChild) {
|
int childHeapIndex = GET_CHILD_INDEX(heapIndex, 0);
|
||||||
int childHeapIndex = GET_CHILD_INDEX(heapIndex, swapWithChild);
|
|
||||||
|
|
||||||
// check that we don't run off the end of the heap
|
// reached the end of the heap
|
||||||
if (childHeapIndex >= simplex->triangleCount) {
|
if (childHeapIndex >= simplex->triangleCount) {
|
||||||
break;
|
break;
|
||||||
}
|
|
||||||
|
|
||||||
if (EXPANDING_SIMPLEX_GET_DISTANCE(simplex, simplex->triangleHeap[heapIndex]) < currentDistance) {
|
|
||||||
swapWithChild = childHeapIndex;
|
|
||||||
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
|
// no child out of order
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
int childHeapIndex = GET_CHILD_INDEX(heapIndex, swapWithChild);
|
|
||||||
|
|
||||||
// swap child with the current node
|
// swap child with the current node
|
||||||
int tmp = simplex->triangleHeap[heapIndex];
|
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;
|
return heapIndex;
|
||||||
|
@ -179,7 +197,7 @@ void expandingSimplexTriangleDetermineDistance(struct ExpandingSimplex* simplex,
|
||||||
|
|
||||||
for (int i = 0; i < 3; ++i) {
|
for (int i = 0; i < 3; ++i) {
|
||||||
if (expandingSimplexTriangleCheckEdge(simplex, triangle, i)) {
|
if (expandingSimplexTriangleCheckEdge(simplex, triangle, i)) {
|
||||||
return ;
|
return;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -232,6 +250,7 @@ void expandingSimplexTriangleCheckRotate(struct ExpandingSimplex* simplex, int t
|
||||||
expandingSimplexRotateEdge(simplex, triangle, triangleIndex, heapIndex);
|
expandingSimplexRotateEdge(simplex, triangle, triangleIndex, heapIndex);
|
||||||
} else {
|
} else {
|
||||||
expandingSimplexTriangleDetermineDistance(simplex, triangle);
|
expandingSimplexTriangleDetermineDistance(simplex, triangle);
|
||||||
|
expandingSimplexFixHeap(simplex, heapIndex);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -301,7 +320,7 @@ void expandingSimplexExpand(struct ExpandingSimplex* expandingSimplex, int newPo
|
||||||
int nextNextFace = NEXT_FACE(nextFace);
|
int nextNextFace = NEXT_FACE(nextFace);
|
||||||
next.indices[0] = existing.indices[i];
|
next.indices[0] = existing.indices[i];
|
||||||
next.indices[1] = existing.indices[nextFace];
|
next.indices[1] = existing.indices[nextFace];
|
||||||
next.indices[3] = newPointIndex;
|
next.indices[2] = newPointIndex;
|
||||||
|
|
||||||
next.adjacentFaces[0] = existing.adjacentFaces[i];
|
next.adjacentFaces[0] = existing.adjacentFaces[i];
|
||||||
next.adjacentFaces[1] = triangleIndices[nextFace];
|
next.adjacentFaces[1] = triangleIndices[nextFace];
|
||||||
|
@ -355,7 +374,7 @@ void epaCalculateContact(struct ExpandingSimplex* simplex, struct SimplexTriangl
|
||||||
&result->contactA
|
&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) {
|
void epaSolve(struct Simplex* startingSimplex, void* objectA, MinkowsiSum objectASum, void* objectB, MinkowsiSum objectBSum, struct EpaResult* result) {
|
||||||
|
|
|
@ -6,6 +6,7 @@
|
||||||
struct EpaResult {
|
struct EpaResult {
|
||||||
struct Vector3 contactA;
|
struct Vector3 contactA;
|
||||||
struct Vector3 contactB;
|
struct Vector3 contactB;
|
||||||
|
// points from A to B
|
||||||
struct Vector3 normal;
|
struct Vector3 normal;
|
||||||
float penetration;
|
float penetration;
|
||||||
};
|
};
|
||||||
|
|
Loading…
Reference in a new issue