From 436bdaf2551885d1b34048af4e1942e222d51f9a Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Wed, 25 May 2011 23:04:48 +0000 Subject: [PATCH] Correction of a bug in EPA algorithm git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@425 92aac97c-a6ce-11dd-a772-7fcde58d38e6 --- src/collision/EPA/EPAAlgorithm.cpp | 31 ++++++-------------- src/collision/EPA/EdgeEPA.cpp | 46 +++++++++++++++--------------- src/collision/EPA/TrianglesStore.h | 1 + 3 files changed, 33 insertions(+), 45 deletions(-) diff --git a/src/collision/EPA/EPAAlgorithm.cpp b/src/collision/EPA/EPAAlgorithm.cpp index f19562fd..045a3421 100644 --- a/src/collision/EPA/EPAAlgorithm.cpp +++ b/src/collision/EPA/EPAAlgorithm.cpp @@ -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; } diff --git a/src/collision/EPA/EdgeEPA.cpp b/src/collision/EPA/EdgeEPA.cpp index 3f421985..2a66386e 100644 --- a/src/collision/EPA/EdgeEPA.cpp +++ b/src/collision/EPA/EdgeEPA.cpp @@ -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; } } diff --git a/src/collision/EPA/TrianglesStore.h b/src/collision/EPA/TrianglesStore.h index d684f1ed..ac5984c3 100644 --- a/src/collision/EPA/TrianglesStore.h +++ b/src/collision/EPA/TrianglesStore.h @@ -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; } }