Fix caculation of normals when using GJK / EPA.

include/hpp/fcl/narrowphase/narrowphase.h

@@ -156,7 +156,7 @@ namespace fcl
           if (gjk.hasPenetrationInformation(shape)) {
             gjk.getClosestPoints (shape, w0, w1);
             distance = gjk.distance;
-            normal = tf1.getRotation() * (w1 - w0).normalized();
+            normal = tf1.getRotation() * (w0 - w1).normalized();
             p1 = p2 = tf1.transform((w0 + w1) / 2);
           } else {
             details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
@@ -168,7 +168,7 @@ namespace fcl
             {
               epa.getClosestPoints (shape, w0, w1);
               distance = -epa.depth;
-              normal = -epa.normal;
+              normal = tf1.getRotation() * epa.normal;
               p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
               assert (distance <= 1e-6);
             } else {
@@ -261,7 +261,7 @@ namespace fcl
             // Return contact points in case of collision
             //p1 = tf1.transform (p1);
             //p2 = tf1.transform (p2);
-            normal = (tf1.getRotation() * (p2 - p1)).normalized();
+            normal = (tf1.getRotation() * (p1 - p2)).normalized();
             p1 = tf1.transform(p1);
             p2 = tf1.transform(p2);
           } else {

src/narrowphase/gjk.cpp

@@ -575,7 +575,10 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
     alpha = std::max(alpha, omega);
     FCL_REAL diff (rl - alpha);
     if (iterations == 0) diff = std::abs(diff);
-    if(diff - tolerance * rl <= 0)
+    // TODO here, we can stop at iteration 0 if this condition is met.
+    // We stopping at iteration 0, the closest point will not be valid.
+    // if(diff - tolerance * rl <= 0)
+    if(iterations > 0 && diff - tolerance * rl <= 0)
     {
       if (iterations > 0)
         removeVertex(simplices[current]);