Correction of a bug in EPA algorithm
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@425 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
parent
867f940102
commit
436bdaf255
|
@ -252,20 +252,18 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
|
|||
suppPointsA[3] = boundingVolume1->getSupportPoint(n, OBJECT_MARGIN);
|
||||
suppPointsB[3] = boundingVolume2->getSupportPoint(n.getOpposite(), OBJECT_MARGIN);
|
||||
points[3] = suppPointsA[3] - suppPointsB[3];
|
||||
suppPointsA[4] = boundingVolume1->getSupportPoint(n.getOpposite(), OBJECT_MARGIN); // TODO : Uncomment this
|
||||
suppPointsB[4] = boundingVolume2->getSupportPoint(n, OBJECT_MARGIN); // TODO : Uncomment this
|
||||
points[4] = suppPointsA[4] - suppPointsB[4]; // TODO : Uncomment this
|
||||
suppPointsA[4] = boundingVolume1->getSupportPoint(n.getOpposite(), OBJECT_MARGIN);
|
||||
suppPointsB[4] = boundingVolume2->getSupportPoint(n, OBJECT_MARGIN);
|
||||
points[4] = suppPointsA[4] - suppPointsB[4];
|
||||
|
||||
// Construct the triangle faces
|
||||
TriangleEPA* face0 = triangleStore.newTriangle(points, 0, 1, 3);
|
||||
TriangleEPA* face1 = triangleStore.newTriangle(points, 1, 2, 3);
|
||||
TriangleEPA* face2 = triangleStore.newTriangle(points, 2, 0, 3);
|
||||
//TriangleEPA* face3 = triangleStore.newTriangle(points, 0, 1, 2); // TODO : Remove this
|
||||
TriangleEPA* face3 = triangleStore.newTriangle(points, 0, 2, 4);// TODO : Uncomment this
|
||||
TriangleEPA* face4 = triangleStore.newTriangle(points, 2, 1, 4);// TODO : Uncomment this
|
||||
TriangleEPA* face5 = triangleStore.newTriangle(points, 1, 0, 4);// TODO : Uncomment this
|
||||
TriangleEPA* face3 = triangleStore.newTriangle(points, 0, 2, 4);
|
||||
TriangleEPA* face4 = triangleStore.newTriangle(points, 2, 1, 4);
|
||||
TriangleEPA* face5 = triangleStore.newTriangle(points, 1, 0, 4);
|
||||
|
||||
// TODO : Uncomment this
|
||||
// If the polytope hasn't been correctly constructed
|
||||
if (!(face0 && face1 && face2 && face3 && face4 && face5 &&
|
||||
face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0 &&
|
||||
|
@ -273,16 +271,6 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
|
|||
face4->getDistSquare() > 0.0 && face5->getDistSquare() > 0.0)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
// TODO : Remove this
|
||||
/*
|
||||
if (!(face0 && face1 && face2 && face3 &&
|
||||
face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0 &&
|
||||
face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) {
|
||||
return false;
|
||||
}
|
||||
*/
|
||||
|
||||
// Associate the edges of neighbouring faces
|
||||
link(EdgeEPA(face0, 1), EdgeEPA(face1, 2));
|
||||
|
@ -300,11 +288,10 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
|
|||
addFaceCandidate(face1, triangleHeap, nbTriangles, DBL_MAX);
|
||||
addFaceCandidate(face2, triangleHeap, nbTriangles, DBL_MAX);
|
||||
addFaceCandidate(face3, triangleHeap, nbTriangles, DBL_MAX);
|
||||
addFaceCandidate(face4, triangleHeap, nbTriangles, DBL_MAX); // TODO : Uncomment this
|
||||
addFaceCandidate(face5, triangleHeap, nbTriangles, DBL_MAX); // TODO : Uncomment this
|
||||
addFaceCandidate(face4, triangleHeap, nbTriangles, DBL_MAX);
|
||||
addFaceCandidate(face5, triangleHeap, nbTriangles, DBL_MAX);
|
||||
|
||||
nbVertices = 5; // TODO : Uncomment this
|
||||
//nbVertices = 4; // TODO : Remove this
|
||||
nbVertices = 5;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
|
|
@ -74,39 +74,39 @@ bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesS
|
|||
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else {
|
||||
// The current triangle is visible and therefore obsolete
|
||||
ownerTriangle->setIsObsolete(true);
|
||||
else {
|
||||
// The current triangle is visible and therefore obsolete
|
||||
ownerTriangle->setIsObsolete(true);
|
||||
|
||||
int backup = triangleStore.getNbTriangles();
|
||||
int backup = triangleStore.getNbTriangles();
|
||||
|
||||
if(!ownerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge(this->index)).computeSilhouette(vertices, index, triangleStore)) {
|
||||
ownerTriangle->setIsObsolete(false);
|
||||
if(!ownerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge(this->index)).computeSilhouette(vertices, index, triangleStore)) {
|
||||
ownerTriangle->setIsObsolete(false);
|
||||
|
||||
TriangleEPA* triangle = triangleStore.newTriangle(vertices, index, getTarget(), getSource());
|
||||
TriangleEPA* triangle = triangleStore.newTriangle(vertices, index, getTarget(), getSource());
|
||||
|
||||
// If the triangle has been created
|
||||
if (triangle) {
|
||||
halfLink(EdgeEPA(triangle, 1), *this);
|
||||
return true;
|
||||
// If the triangle has been created
|
||||
if (triangle) {
|
||||
halfLink(EdgeEPA(triangle, 1), *this);
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
else if (!ownerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(this->index)).computeSilhouette(vertices, index, triangleStore)) {
|
||||
ownerTriangle->setIsObsolete(false);
|
||||
|
||||
return false;
|
||||
}
|
||||
else if (!ownerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(this->index)).computeSilhouette(vertices, index, triangleStore)) {
|
||||
ownerTriangle->setIsObsolete(false);
|
||||
triangleStore.setNbTriangles(backup);
|
||||
|
||||
triangleStore.setNbTriangles(backup);
|
||||
TriangleEPA* triangle = triangleStore.newTriangle(vertices, index, getTarget(), getSource());
|
||||
|
||||
TriangleEPA* triangle = triangleStore.newTriangle(vertices, index, getTarget(), getSource());
|
||||
if (triangle) {
|
||||
halfLink(EdgeEPA(triangle, 1), *this);
|
||||
return true;
|
||||
}
|
||||
|
||||
if (triangle) {
|
||||
halfLink(EdgeEPA(triangle, 1), *this);
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -94,6 +94,7 @@ inline TriangleEPA* TrianglesStore::newTriangle(const Vector3D* vertices, uint v
|
|||
new (newTriangle) TriangleEPA(v0, v1, v2);
|
||||
if (!newTriangle->computeClosestPoint(vertices)) {
|
||||
nbTriangles--;
|
||||
// TODO : DO WE HAVE TO add "delete newTriangle;" here ??
|
||||
newTriangle = 0;
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue
Block a user