implementation of GJK and EPA collision detection algorithm continued

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

src/collision/EPA/EdgeEPA.cpp

@@ -54,15 +54,14 @@ uint EdgeEPA::getSource() const {
 
 // Return the index of the target vertex of the edge (vertex ending the edge)
 uint EdgeEPA::getTarget() const {
-    return (*ownerTriangle)[IndexOfNextCounterClockwiseEdge(index)];
+    return (*ownerTriangle)[indexOfNextCounterClockwiseEdge(index)];
 }
 
 // Link the edge with another one (meaning that the current edge of a triangle will
 // is associated with the edge of another triangle in order that both triangles
 // are neighbour along both edges)
-bool EdgeEPA::link(EdgeEPA& edge) {
-    bool isPossible = (this->getSource() == edge.getTarget() &&
-                       this->getTarget() == edge.getSource());
+bool EdgeEPA::link(EdgeEPA edge) {
+    bool isPossible = (this->getSource() == edge.getTarget() && this->getTarget() == edge.getSource());
 
     // If the link is possible
     if (isPossible) {
@@ -74,13 +73,66 @@ bool EdgeEPA::link(EdgeEPA& edge) {
     return isPossible;
 }
 
-// Half link the edge with another one
-void EdgeEPA::halfLink(EdgeEPA& edge) const {
+// Half link the edge with another one from another triangle
+void EdgeEPA::halfLink(EdgeEPA edge) {
+    assert(this->getSource() == edge.getTarget() && this->getTarget() == edge.getSource());
 
+    // Link
+    this->getOwnerTriangle()->setAdjacentEdge(index, edge);
 }
 
 
 // Compute the silhouette
-bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesStore trianglesStore) {
-    // TODO : Implement this
+bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, 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)) {
+            TriangleEPA* triangle = triangleStore.newTriangle(vertices, index, getTarget(), getSource());
+
+            // If the triangle has been created
+            if (triangle) {
+                EdgeEPA(triangle, 1).halfLink(*this);
+                return true;
+            }
+
+            return false;
+        }
+    }
+    else {
+        // The current triangle is visible and therefore obsolete
+        ownerTriangle->setIsObsolete(true);
+
+        int backup = triangleStore.getNbTriangles();
+
+        if(!ownerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge(index)).computeSilhouette(vertices, index, triangleStore)) {
+            ownerTriangle->setIsObsolete(false);
+
+            TriangleEPA* triangle = triangleStore.newTriangle(vertices, index, getTarget(), getSource());
+
+            // If the triangle has been created
+            if (triangle) {
+                EdgeEPA(triangle, 1).halfLink(*this);
+                return true;
+            }
+
+            return false;
+        }
+        else if (!ownerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(index)).computeSilhouette(vertices, index, triangleStore)) {
+            ownerTriangle->setIsObsolete(false);
+
+            triangleStore.setNbTriangles(backup);
+
+            TriangleEPA* triangle = triangleStore.newTriangle(vertices, index, getTarget(), getSource());
+
+            if (triangle) {
+                EdgeEPA(triangle, 1).halfLink(*this);
+                return true;
+            }
+
+            return false;
+        }
+    }
+
+    return true;
 }

src/collision/EPA/EdgeEPA.h

@@ -52,15 +52,14 @@ class EdgeEPA {
         EdgeEPA();                                          // Constructor
         EdgeEPA(TriangleEPA* ownerTriangle, int index);     // Constructor
         ~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 getTarget() const;                                         // Return the index of the target vertex of the edge
-        bool link(EdgeEPA& edge);                                       // Link the edge with another one
-        void halfLink(EdgeEPA& edge) const;                             // Half link the edge with another one
+        bool link(EdgeEPA edge);                                        // Link the edge with another one
+        void halfLink(EdgeEPA edge);                                    // Half link the edge with another one
         bool computeSilhouette(const Vector3D* vertices, uint index,
-                               TrianglesStore trianglesStore);          // Compute the recursive silhouette algorithm
+                               TrianglesStore triangleStore);           // Compute the recursive silhouette algorithm
 };
 
 
@@ -75,12 +74,12 @@ inline int EdgeEPA::getIndex() const {
 }
 
 // Return the index of the next counter-clockwise edge of the ownver triangle
-inline int IndexOfNextCounterClockwiseEdge(int i) {
+inline int indexOfNextCounterClockwiseEdge(int i) {
     return (i + 1) % 3;
 }
 
 // Return the index of the previous counter-clockwise edge of the ownver triangle
-inline int IndexOfPreviousCounterClockwiseEdge(int i) {
+inline int indexOfPreviousCounterClockwiseEdge(int i) {
     return (i + 2) % 3;
 }
 

src/collision/EPA/TriangleEPA.cpp

@@ -96,4 +96,18 @@ bool TriangleEPA::computeSilhouette(const Vector3D* vertices, uint index, Triang
                   adjacentEdges[1].computeSilhouette(vertices, index, triangleStore) &&
                   adjacentEdges[2].computeSilhouette(vertices, index, triangleStore);
 
+    if (result) {
+        int i,j;
+        for (i=first, j=triangleStore.getNbTriangles()-1; i != triangleStore.getNbTriangles(); j = i++) {
+            TriangleEPA* triangle = &triangleStore[i];
+            triangle->getAdjacentEdge(1).halfLink(EdgeEPA(triangle, 1));
+
+            if (!EdgeEPA(triangle, 0).link(EdgeEPA(&triangleStore[j], 2))) {
+                return false;
+            }
+        }
+
+    }
+
+    return result;
 }

src/collision/EPA/TriangleEPA.h

@@ -56,7 +56,7 @@ class TriangleEPA {
         TriangleEPA(uint v1, uint v2, uint v3);     // Constructor
         ~TriangleEPA();                             // Destructor
 
-        const EdgeEPA& getAdjacentEdge(int index) const;                                    // Return an adjacent edge of the triangle
+        EdgeEPA& getAdjacentEdge(int index);                                                // Return an adjacent edge of the triangle
         void setAdjacentEdge(int index, EdgeEPA& edge);                                     // Set an adjacent edge of the triangle
         double getDistSquare() const;                                                       // Return the square distance  of the closest point to origin
         void setIsObsolete(bool isObsolete);                                                // Set the isObsolete value
@@ -73,7 +73,7 @@ class TriangleEPA {
 };
 
 // Return an edge of the triangle
-inline const EdgeEPA& TriangleEPA::getAdjacentEdge(int index) const {
+inline EdgeEPA& TriangleEPA::getAdjacentEdge(int index) {
     assert(index >= 0 && index < 3);
     return adjacentEdges[index];
 }