From 6a6f55b7639ff44ae48fe6a3c815de0a043dc068 Mon Sep 17 00:00:00 2001
From: "chappuis.daniel" <chappuis.daniel@92aac97c-a6ce-11dd-a772-7fcde58d38e6>
Date: Sat, 19 Feb 2011 10:19:59 +0000
Subject: [PATCH] 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      |  1 -
 src/collision/GJK/GJKAlgorithm.cpp |  3 +++
 src/collision/GJK/Simplex.cpp      | 16 ++++++++--------
 src/collision/GJK/Simplex.h        |  2 +-
 4 files changed, 12 insertions(+), 10 deletions(-)

diff --git a/src/collision/EPA/EdgeEPA.cpp b/src/collision/EPA/EdgeEPA.cpp
index c6545ff1..ee8904a7 100644
--- a/src/collision/EPA/EdgeEPA.cpp
+++ b/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
diff --git a/src/collision/GJK/GJKAlgorithm.cpp b/src/collision/GJK/GJKAlgorithm.cpp
index 10cd5386..7b0c461a 100644
--- a/src/collision/GJK/GJKAlgorithm.cpp
+++ b/src/collision/GJK/GJKAlgorithm.cpp
@@ -204,6 +204,9 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
             // There is an intersection, therefore we return true
             return true;
         }
+
+        double test = simplex.getMaxLengthSquareOfAPoint(); // TODO : Remove this
+        test = 4.5;
         
     } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint());
 
diff --git a/src/collision/GJK/Simplex.cpp b/src/collision/GJK/Simplex.cpp
index 41bce576..db6971b0 100644
--- a/src/collision/GJK/Simplex.cpp
+++ b/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
-    // Johnson's algorithm test
+    // If the simplex that contains only the last added point is valid for the Johnson's algorithm test
     if (isValidSubset(lastFoundBit)) {
-        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
+        bitsCurrentSimplex = lastFoundBit;                  // Set the current simplex to the set that contains only 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]) {
-                maxLenSquare = pointsLengthSquare[i];
+            if (maxLengthSquare < pointsLengthSquare[i]) {
+                maxLengthSquare = pointsLengthSquare[i];
             }
 
             // Closest point v = sum(lambda_i * points[i])
diff --git a/src/collision/GJK/Simplex.h b/src/collision/GJK/Simplex.h
index eb23b23a..4807cb72 100644
--- a/src/collision/GJK/Simplex.h
+++ b/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