implementation of GJK and EPA collision detection algorithm continued

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

src/body/BoundingSphere.h

@@ -92,14 +92,14 @@ inline Vector3D BoundingSphere::getSupportPoint(const Vector3D& direction, doubl
     double length = direction.length();
 
     // If the direction vector is not the zero vector
-    if (length > EPSILON) {
+    if (length > 0.0) {
         // Return the support point of the sphere in the given direction
         return center + (radius + margin) * direction.getUnit();
     }
 
     // If the direction vector is the zero vector we return a point on the
     // boundary of the sphere
-    return center + Vector3D(radius + margin, 0.0, 0.0);   
+    return center + Vector3D(0, radius + margin, 0);
 }
 
 }; // End of the ReactPhysics3D namespace

src/collision/EPA/EPAAlgorithm.cpp

@@ -210,12 +210,12 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
                 }
 
                 // Associate the edges of neighbouring triangle faces
-                EdgeEPA(face0, 0).link(EdgeEPA(face1, 2));
+                link(EdgeEPA(face0, 0), EdgeEPA(face1, 2));
-                EdgeEPA(face0, 1).link(EdgeEPA(face3, 2));
+                link(EdgeEPA(face0, 1), EdgeEPA(face3, 2));
-                EdgeEPA(face0, 2).link(EdgeEPA(face2, 0));
+                link(EdgeEPA(face0, 2), EdgeEPA(face2, 0));
-                EdgeEPA(face1, 0).link(EdgeEPA(face2, 2));
+                link(EdgeEPA(face1, 0), EdgeEPA(face2, 2));
-                EdgeEPA(face1, 1).link(EdgeEPA(face3, 0));
+                link(EdgeEPA(face1, 1), EdgeEPA(face3, 0));
-                EdgeEPA(face2, 1).link(EdgeEPA(face3, 1));
+                link(EdgeEPA(face2, 1), EdgeEPA(face3, 1));
 
                 // Add the triangle faces in the candidate heap
                 addFaceCandidate(face0, triangleHeap, nbTriangles, DBL_MAX);
@@ -252,18 +252,20 @@ 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);
+            suppPointsA[4] = boundingVolume1->getSupportPoint(n.getOpposite(), OBJECT_MARGIN);    // TODO : Uncomment this
-            suppPointsB[4] = boundingVolume2->getSupportPoint(n, OBJECT_MARGIN);
+            suppPointsB[4] = boundingVolume2->getSupportPoint(n, OBJECT_MARGIN);                  // TODO : Uncomment this
-            points[4] = suppPointsA[4] - suppPointsB[4];
+            points[4] = suppPointsA[4] - suppPointsB[4];                                          // TODO : Uncomment this
 
             // 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, 2, 4);
+            //TriangleEPA* face3 = triangleStore.newTriangle(points, 0, 1, 2);    // TODO : Remove this
-            TriangleEPA* face4 = triangleStore.newTriangle(points, 2, 1, 4);
+            TriangleEPA* face3 = triangleStore.newTriangle(points, 0, 2, 4);// TODO : Uncomment this
-            TriangleEPA* face5 = triangleStore.newTriangle(points, 1, 0, 4);
+            TriangleEPA* face4 = triangleStore.newTriangle(points, 2, 1, 4);// TODO : Uncomment this
+            TriangleEPA* face5 = triangleStore.newTriangle(points, 1, 0, 4);// TODO : Uncomment this
 
+            // 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 &&
@@ -272,26 +274,37 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
                 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
-            EdgeEPA(face0, 1).link(EdgeEPA(face1, 2));
+            link(EdgeEPA(face0, 1), EdgeEPA(face1, 2));
-            EdgeEPA(face1, 1).link(EdgeEPA(face2, 2));
+            link(EdgeEPA(face1, 1), EdgeEPA(face2, 2));
-            EdgeEPA(face2, 1).link(EdgeEPA(face0, 2));
+            link(EdgeEPA(face2, 1), EdgeEPA(face0, 2));
-            EdgeEPA(face0, 0).link(EdgeEPA(face5, 0));
+            link(EdgeEPA(face0, 0), EdgeEPA(face5, 0));
-            EdgeEPA(face1, 0).link(EdgeEPA(face4, 0));
+            link(EdgeEPA(face1, 0), EdgeEPA(face4, 0));
-            EdgeEPA(face2, 0).link(EdgeEPA(face3, 0));
+            link(EdgeEPA(face2, 0), EdgeEPA(face3, 0));
-            EdgeEPA(face3, 1).link(EdgeEPA(face4, 2));
+            link(EdgeEPA(face3, 1), EdgeEPA(face4, 2));
-            EdgeEPA(face4, 1).link(EdgeEPA(face5, 2));
+            link(EdgeEPA(face4, 1), EdgeEPA(face5, 2));
-            EdgeEPA(face5, 1).link(EdgeEPA(face3, 2));
+            link(EdgeEPA(face5, 1), EdgeEPA(face3, 2));
 
             // Add the candidate faces in the heap
             addFaceCandidate(face0, triangleHeap, nbTriangles, DBL_MAX);
             addFaceCandidate(face1, triangleHeap, nbTriangles, DBL_MAX);
             addFaceCandidate(face2, triangleHeap, nbTriangles, DBL_MAX);
             addFaceCandidate(face3, triangleHeap, nbTriangles, DBL_MAX);
-            addFaceCandidate(face4, triangleHeap, nbTriangles, DBL_MAX);
+            addFaceCandidate(face4, triangleHeap, nbTriangles, DBL_MAX);  // TODO : Uncomment this
-            addFaceCandidate(face5, triangleHeap, nbTriangles, DBL_MAX);
+            addFaceCandidate(face5, triangleHeap, nbTriangles, DBL_MAX);  // TODO : Uncomment this
 
-            nbVertices = 5;
+            nbVertices = 5;   // TODO : Uncomment this
+            //nbVertices = 4;     // TODO : Remove this
         }
         break;
     }
@@ -338,6 +351,7 @@ 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]] ||
@@ -368,7 +382,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
     } while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
 
     // Compute the contact info
-    v = triangle->getClosestPoint();
+    v = triangle->getClosestPoint();    // TODO : Check if we have to compute this vector here
     Vector3D pA = triangle->computeClosestPointOfObject(suppPointsA);
     Vector3D pB = triangle->computeClosestPointOfObject(suppPointsB);
     Vector3D diff = pB - pA;

src/collision/EPA/EdgeEPA.cpp

@@ -31,6 +31,7 @@
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;
 
+
 // Constructor
 EdgeEPA::EdgeEPA() {
     
@@ -57,30 +58,6 @@ uint EdgeEPA::getTarget() const {
     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());
-
-    // If the link is possible
-    if (isPossible) {
-        this->getOwnerTriangle()->setAdjacentEdge(index, edge);
-        edge.getOwnerTriangle()->setAdjacentEdge(edge.getIndex(), *this);
-    }
-
-    // Return if the link has been made
-    return isPossible;
-}
-
-// 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& triangleStore) {
     // If the edge has not already been visited
@@ -91,7 +68,7 @@ bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesS
 
             // If the triangle has been created
             if (triangle) {
-                EdgeEPA(triangle, 1).halfLink(*this);
+                halfLink(EdgeEPA(triangle, 1), *this);
                 return true;
             }
 
@@ -111,7 +88,7 @@ bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesS
 
             // If the triangle has been created
             if (triangle) {
-                EdgeEPA(triangle, 1).halfLink(*this);
+                halfLink(EdgeEPA(triangle, 1), *this);
                 return true;
             }
 
@@ -125,7 +102,7 @@ bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesS
             TriangleEPA* triangle = triangleStore.newTriangle(vertices, index, getTarget(), getSource());
 
             if (triangle) {
-                EdgeEPA(triangle, 1).halfLink(*this);
+                halfLink(EdgeEPA(triangle, 1), *this);
                 return true;
             }
 

src/collision/EPA/EdgeEPA.h

@@ -56,13 +56,10 @@ class EdgeEPA {
         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);                                    // Half link the edge with another one
         bool computeSilhouette(const Vector3D* vertices, uint index,
                                TrianglesStore& triangleStore);          // Compute the recursive silhouette algorithm
 };
 
-
 // Return the pointer to the owner triangle
 inline TriangleEPA* EdgeEPA::getOwnerTriangle() const {
     return ownerTriangle;

src/collision/EPA/TriangleEPA.cpp

@@ -82,6 +82,28 @@ bool TriangleEPA::computeClosestPoint(const Vector3D* vertices) {
     return false;
 }
 
+// Link an edge with another one (meaning that the current edge of a triangle will
+// 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());
+
+    if (isPossible) {
+        edge0.getOwnerTriangle()->adjacentEdges[edge0.getIndex()] = edge1;
+        edge1.getOwnerTriangle()->adjacentEdges[edge1.getIndex()] = edge0;
+    }
+
+    return isPossible;
+}
+
+// Make an half link of an edge with another one from another triangle
+void reactphysics3d::halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1) {
+    assert(edge0.getSource() == edge1.getTarget() && edge0.getTarget() == edge1.getSource());
+
+    // Link
+    edge0.getOwnerTriangle()->adjacentEdges[edge0.getIndex()] = edge1;
+}
+
 // Compute recursive silhouette algorithm for that triangle
 bool TriangleEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesStore& triangleStore) {
     
@@ -100,9 +122,9 @@ bool TriangleEPA::computeSilhouette(const Vector3D* vertices, uint index, Triang
         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));
+            halfLink(triangle->getAdjacentEdge(1), EdgeEPA(triangle, 1));
 
-            if (!EdgeEPA(triangle, 0).link(EdgeEPA(&triangleStore[j], 2))) {
+            if (!link(EdgeEPA(triangle, 0), EdgeEPA(&triangleStore[j], 2))) {
                 return false;
             }
         }

src/collision/EPA/TriangleEPA.h

@@ -34,6 +34,11 @@
 // ReactPhysics3D namespace
 namespace reactphysics3d {
 
+// Prototypes
+bool link(const EdgeEPA& edge0, const EdgeEPA& edge1);
+void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1);
+
+
 /*  -------------------------------------------------------------------
     Class TriangleEPA :
         This class represents a triangle face of the current polytope in the EPA
@@ -70,6 +75,8 @@ class TriangleEPA {
                                TrianglesStore& triangleStore);                              // Compute recursive silhouette algorithm for that triangle
 
         uint operator[](int i) const;                                                       // Access operator
+        friend bool link(const EdgeEPA& edge0, const EdgeEPA& edge1);                       // Associate two edges
+        friend void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1);                   // Make a half-link between two edges
 };
 
 // Return an edge of the triangle
@@ -106,7 +113,7 @@ inline const Vector3D& TriangleEPA::getClosestPoint() const {
 
 // Return true if the closest point on affine hull is inside the triangle
 inline bool TriangleEPA::isClosestPointInternalToTriangle() const {
-    return (lambda1 >= 0.0 && lambda2 >= 0.0 && lambda1 + lambda2 <= det);
+    return (lambda1 >= 0.0 && lambda2 >= 0.0 && (lambda1 + lambda2) <= det);
 }
 
 // Return true if the triangle is visible from a given vertex

src/collision/EPA/TrianglesStore.h

@@ -90,8 +90,7 @@ inline TriangleEPA* TrianglesStore::newTriangle(const Vector3D* vertices, uint v
 
     // If we have not reach the maximum number of triangles
     if (nbTriangles != MAX_TRIANGLES) {
-        newTriangle = &triangles[nbTriangles];
+        newTriangle = &triangles[nbTriangles++];
-        nbTriangles++;
         new (newTriangle) TriangleEPA(v0, v1, v2);
         if (!newTriangle->computeClosestPoint(vertices)) {
             nbTriangles--;