Continuation of EPA algorithm

git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@426 92aac97c-a6ce-11dd-a772-7fcde58d38e6

src/collision/EPA/EPAAlgorithm.cpp

@@ -30,8 +30,6 @@
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;
 
-// TODO : Check that allocated memory is correctly deleted
-
 // Constructor
 EPAAlgorithm::EPAAlgorithm() {
 
@@ -90,8 +88,6 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
     TrianglesStore triangleStore;                   // Store the triangles
     TriangleEPA* triangleHeap[MAX_FACETS];          // Heap that contains the face candidate of the EPA algorithm
 
-    // TODO : Check that we call all the supportPoint() function with a margin
-
     // Get the simplex computed previously by the GJK algorithm
     unsigned int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
 
@@ -338,7 +334,6 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
             }
 
             // Compute the error
-            double distSquare = triangle->getDistSquare();  // TODO : REMOVE THIS
             double error = wDotv - triangle->getDistSquare();
             if (error <= std::max(tolerance, REL_ERROR_SQUARE * wDotv) ||
                 points[indexNewVertex] == points[(*triangle)[0]] ||
@@ -347,18 +342,16 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
                 break;
             }
 
-            // Now, we compute the silhouette cast by the new vertex.
+            // Now, we compute the silhouette cast by the new vertex. The current triangle
-            // The current triangle face will not be in the convex hull.
+            // face will not be in the convex hull. We start the local recursive silhouette
-            // We start the local recursive silhouette algorithm from
+            // algorithm from the current triangle face.
-            // the current triangle face.
             int i = triangleStore.getNbTriangles();
             if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) {
                 break;
             }
 
-            // Construct the new polytope by constructing triangle faces from the
+            // Add all the new triangle faces computed with the silhouette algorithm
-            // silhouette to the new vertex of the polytope in order that the new
+            // to the candidates list of faces of the current polytope
-            // polytope is always convex
             while(i != triangleStore.getNbTriangles()) {
                 TriangleEPA* newTriangle = &triangleStore[i];
                 addFaceCandidate(newTriangle, triangleHeap, nbTriangles, upperBoundSquarePenDepth);
@@ -369,12 +362,11 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
     } while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
 
     // Compute the contact info
-    v = triangle->getClosestPoint();    // TODO : Check if we have to compute this vector here
+    v = triangle->getClosestPoint();
     Vector3D pA = triangle->computeClosestPointOfObject(suppPointsA);
     Vector3D pB = triangle->computeClosestPointOfObject(suppPointsB);
-    Vector3D diff = pB - pA;
+    Vector3D normal = v.getUnit();
-    Vector3D normal = diff.getUnit();
+    double penetrationDepth = v.length();
-    double penetrationDepth = diff.length();
     assert(penetrationDepth > 0.0);
     contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
                                   normal, penetrationDepth, pA, pB);

src/collision/EPA/EPAAlgorithm.h

@@ -65,7 +65,8 @@ class TriangleComparison {
         that contains the origin and expend it in order to find the point on the boundary
         of (A-B) that is closest to the origin. An initial simplex that contains origin
         has been computed wit GJK algorithm. The EPA Algorithm will extend this simplex
-        polytope to find the correct penetration depth.
+        polytope to find the correct penetration depth. The implementation of the EPA
+        algorithm is based on the book "Collision Detection in 3D Environments".
     -------------------------------------------------------------------
 */
 class EPAAlgorithm {

src/collision/EPA/EdgeEPA.cpp

@@ -49,22 +49,22 @@ EdgeEPA::~EdgeEPA() {
 }
 
 // Return the index of the source vertex of the edge (vertex starting the edge)
-uint EdgeEPA::getSource() const {
+uint EdgeEPA::getSourceVertexIndex() const {
     return (*ownerTriangle)[index];
 }
 
 // Return the index of the target vertex of the edge (vertex ending the edge)
-uint EdgeEPA::getTarget() const {
+uint EdgeEPA::getTargetVertexIndex() const {
     return (*ownerTriangle)[indexOfNextCounterClockwiseEdge(index)];
 }
 
-// Compute the silhouette
+// Execute the recursive silhouette algorithm from this edge
-bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesStore& triangleStore) {
+bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint indexNewVertex, TrianglesStore& triangleStore) {
     // If the edge has not already been visited
     if (!ownerTriangle->getIsObsolete()) {
         // If the triangle of this edge is not visible from the given point
-        if (!ownerTriangle->isVisibleFromVertex(vertices, index)) {
+        if (!ownerTriangle->isVisibleFromVertex(vertices, indexNewVertex)) {
-            TriangleEPA* triangle = triangleStore.newTriangle(vertices, index, getTarget(), getSource());
+            TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex());
 
             // If the triangle has been created
             if (triangle) {
@@ -80,10 +80,10 @@ bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesS
 
             int backup = triangleStore.getNbTriangles();
 
-            if(!ownerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge(this->index)).computeSilhouette(vertices, index, triangleStore)) {
+            if(!ownerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge(this->index)).computeSilhouette(vertices, indexNewVertex, triangleStore)) {
                 ownerTriangle->setIsObsolete(false);
 
-                TriangleEPA* triangle = triangleStore.newTriangle(vertices, index, getTarget(), getSource());
+                TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex());
 
                 // If the triangle has been created
                 if (triangle) {
@@ -93,12 +93,12 @@ bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesS
 
                 return false;
             }
-            else if (!ownerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(this->index)).computeSilhouette(vertices, index, triangleStore)) {
+            else if (!ownerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(this->index)).computeSilhouette(vertices, indexNewVertex, triangleStore)) {
                 ownerTriangle->setIsObsolete(false);
 
                 triangleStore.setNbTriangles(backup);
 
-                TriangleEPA* triangle = triangleStore.newTriangle(vertices, index, getTarget(), getSource());
+                TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex());
 
                 if (triangle) {
                     halfLink(EdgeEPA(triangle, 1), *this);

src/collision/EPA/EdgeEPA.h

@@ -54,10 +54,10 @@ class EdgeEPA {
         ~EdgeEPA();                                                     // Destructor
         TriangleEPA* getOwnerTriangle() const;                          // Return the pointer to the owner triangle
         int getIndex() const;                                           // Return the index of the edge in the triangle
-        uint getSource() const;                                         // Return index of the source vertex of the edge
+        uint getSourceVertexIndex() const;                              // Return index of the source vertex of the edge
-        uint getTarget() const;                                         // Return the index of the target vertex of the edge
+        uint getTargetVertexIndex() const;                              // Return the index of the target vertex of the edge
         bool computeSilhouette(const Vector3D* vertices, uint index,
-                               TrianglesStore& triangleStore);          // Compute the recursive silhouette algorithm
+                               TrianglesStore& triangleStore);          // Execute the recursive silhouette algorithm from this edge
 };
 
 // Return the pointer to the owner triangle

src/collision/EPA/TriangleEPA.cpp

@@ -86,7 +86,8 @@ bool TriangleEPA::computeClosestPoint(const Vector3D* vertices) {
 // be associated with the edge of another triangle in order that both triangles
 // are neighbour along both edges)
 bool reactphysics3d::link(const EdgeEPA& edge0, const EdgeEPA& edge1) {
-    bool isPossible = (edge0.getSource() == edge1.getTarget() && edge0.getTarget() == edge1.getSource());
+    bool isPossible = (edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() &&
+                       edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex());
 
     if (isPossible) {
         edge0.getOwnerTriangle()->adjacentEdges[edge0.getIndex()] = edge1;
@@ -96,30 +97,44 @@ bool reactphysics3d::link(const EdgeEPA& edge0, const EdgeEPA& edge1) {
     return isPossible;
 }
 
-// Make an half link of an edge with another one from another triangle
+// Make an half link of an edge with another one from another triangle. An half-link
+// between an edge "edge0" and an edge "edge1" represents the fact that "edge1" is an
+// adjacent edge of "edge0" but not the opposite. The opposite edge connection will
+// be made later.
 void reactphysics3d::halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1) {
-    assert(edge0.getSource() == edge1.getTarget() && edge0.getTarget() == edge1.getSource());
+    assert(edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() &&
+           edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex());
 
     // Link
     edge0.getOwnerTriangle()->adjacentEdges[edge0.getIndex()] = edge1;
 }
 
-// Compute recursive silhouette algorithm for that triangle
+// Execute the recursive silhouette algorithm from this triangle face
-bool TriangleEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesStore& triangleStore) {
+// The parameter "vertices" is an array that contains the vertices of the current polytope and the
+// parameter "indexNewVertex" is the index of the new vertex in this array. The goal of the silhouette algorithm is
+// to add the new vertex in the polytope by keeping it convex. Therefore, the triangle faces that are visible from the
+// new vertex must be removed from the polytope and we need to add triangle faces where each face contains the new vertex
+// and an edge of the silhouette. The silhouette is the connected set of edges that are part of the border between faces that
+// are seen and faces that are not seen from the new vertex. This method starts from the nearest face from the new vertex,
+// computes the silhouette and create the new faces from the new vertex in order that we always have a convex polytope. The
+// faces visible from the new vertex are set obselete and will not be considered as being a candidate face in the future.
+bool TriangleEPA::computeSilhouette(const Vector3D* vertices, uint indexNewVertex, TrianglesStore& triangleStore) {
     
     uint first = triangleStore.getNbTriangles();
 
-    // Mark the current triangle as obsolete
+    // Mark the current triangle as obsolete because it
     setIsObsolete(true);
 
     // Execute recursively the silhouette algorithm for the ajdacent edges of neighbouring
     // triangles of the current triangle
-    bool result = adjacentEdges[0].computeSilhouette(vertices, index, triangleStore) &&
+    bool result = adjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) &&
-                  adjacentEdges[1].computeSilhouette(vertices, index, triangleStore) &&
+                  adjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore) &&
-                  adjacentEdges[2].computeSilhouette(vertices, index, triangleStore);
+                  adjacentEdges[2].computeSilhouette(vertices, indexNewVertex, triangleStore);
 
     if (result) {
         int i,j;
+
+        // For each triangle face that contains the new vertex and an edge of the silhouette
         for (i=first, j=triangleStore.getNbTriangles()-1; i != triangleStore.getNbTriangles(); j = i++) {
             TriangleEPA* triangle = &triangleStore[i];
             halfLink(triangle->getAdjacentEdge(1), EdgeEPA(triangle, 1));

src/collision/EPA/TriangleEPA.h

@@ -72,7 +72,7 @@ class TriangleEPA {
         bool computeClosestPoint(const Vector3D* vertices);                                 // Compute the point v closest to the origin of this triangle
         Vector3D computeClosestPointOfObject(const Vector3D* supportPointsOfObject) const;  // Compute the point of an object closest to the origin
         bool computeSilhouette(const Vector3D* vertices, uint index,
-                               TrianglesStore& triangleStore);                              // Compute recursive silhouette algorithm for that triangle
+                               TrianglesStore& triangleStore);                              // Execute the recursive silhouette algorithm from this triangle face
 
         uint operator[](int i) const;                                                       // Access operator
         friend bool link(const EdgeEPA& edge0, const EdgeEPA& edge1);                       // Associate two edges

src/collision/EPA/TrianglesStore.h

@@ -50,13 +50,13 @@ class TrianglesStore {
         int nbTriangles;                            // Number of triangles
         
     public:
-        TrianglesStore();               // Constructor
+        TrianglesStore();                           // Constructor
-        ~TrianglesStore();              // Destructor
+        ~TrianglesStore();                          // Destructor
 
-        void clear();                       // Clear all the storage
+        void clear();                               // Clear all the storage
-        int getNbTriangles() const;         // Return the number of triangles
+        int getNbTriangles() const;                 // Return the number of triangles
-        void setNbTriangles(int backup);    // Set the number of triangles
+        void setNbTriangles(int backup);            // Set the number of triangles
-        TriangleEPA& last();                // Return the last triangle
+        TriangleEPA& last();                        // Return the last triangle
 
         TriangleEPA* newTriangle(const Vector3D* vertices, uint v0, uint v1, uint v2);  // Create a new triangle
 
@@ -94,7 +94,6 @@ 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;
         }
     }