implementation of GJK and EPA collision detection algorithm continued

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

src/collision/EPA/EdgeEPA.cpp

@@ -81,7 +81,6 @@ void EdgeEPA::halfLink(EdgeEPA edge) {
     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

src/collision/GJK/GJKAlgorithm.cpp

@@ -205,6 +205,9 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
             return true;
         }
 
+        double test = simplex.getMaxLengthSquareOfAPoint(); // TODO : Remove this
+        test = 4.5;
+        
     } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint());
 
     // The objects (without margins) intersect. Therefore, we run the GJK algorithm again but on the

src/collision/GJK/Simplex.cpp

@@ -312,11 +312,11 @@ bool Simplex::computeClosestPoint(Vector3D& v) {
         }
     }
 
-    // If the simplex that contains only the last added point is valid for the
+    // If the simplex that contains only the last added point is valid for the Johnson's algorithm test
-    // Johnson's algorithm test
     if (isValidSubset(lastFoundBit)) {
-        bitsCurrentSimplex = lastFoundBit;      // Set the current simplex to the set that contains only the last added point
+        bitsCurrentSimplex = lastFoundBit;                  // Set the current simplex to the set that contains only the last added point
-        v = points[lastFound];                  // The closest point of the simplex "v" is the last added point
+        maxLengthSquare = pointsLengthSquare[lastFound];    // Update the maximum square length
+        v = points[lastFound];                              // The closest point of the simplex "v" is the last added point
         return true;
     }
 
@@ -344,9 +344,9 @@ void Simplex::backupClosestPointInSimplex(Vector3D& v) {
 
 // Return the closest point "v" in the convex hull of the points in the subset
 // represented by the bits "subset"
-Vector3D Simplex::computeClosestPointForSubset(Bits subset) const {
+Vector3D Simplex::computeClosestPointForSubset(Bits subset) {
     Vector3D v(0.0, 0.0, 0.0);      // Closet point v = sum(lambda_i * points[i])
-    double maxLenSquare = 0.0;
+    maxLengthSquare = 0.0;
     double deltaX = 0.0;            // deltaX = sum of all det[subset][i]
     int i;
     Bits bit;
@@ -358,8 +358,8 @@ Vector3D Simplex::computeClosestPointForSubset(Bits subset) const {
             // deltaX = sum of all det[subset][i]
             deltaX += det[subset][i];
 
-            if (maxLenSquare < pointsLengthSquare[i]) {
+            if (maxLengthSquare < pointsLengthSquare[i]) {
-                maxLenSquare = pointsLengthSquare[i];
+                maxLengthSquare = pointsLengthSquare[i];
             }
 
             // Closest point v = sum(lambda_i * points[i])

src/collision/GJK/Simplex.h

@@ -67,7 +67,7 @@ class Simplex {
         bool isProperSubset(Bits subset) const;                     // Return true if the subset is a proper subset
         void updateCache();                                         // Update the cached values used during the GJK algorithm
         void computeDeterminants();                                 // Compute the cached determinant values
-        Vector3D computeClosestPointForSubset(Bits subset) const;   // Return the closest point "v" in the convex hull of a subset of points
+        Vector3D computeClosestPointForSubset(Bits subset);         // Return the closest point "v" in the convex hull of a subset of points
 
     public:
         Simplex();                          // Constructor