Merge pull request #182 from jmirabel/devel

Fix calculation of normal and nearest points in GJKSolver::shapeDistance

include/hpp/fcl/narrowphase/narrowphase.h

@@ -234,7 +234,8 @@ namespace fcl
         Vec3f w0, w1;
         gjk.getClosestPoints (shape, w0, w1);
         distance = 0;
-        p1 = p2 = tf1.transform (.5* (w0 + w1));
+        p1 = tf1.transform (w0);
+        p2 = tf1.transform (w1);
         normal = Vec3f (0,0,0);
         return false;
       }
@@ -261,7 +262,8 @@ namespace fcl
             //p1 = tf1.transform (p1);
             //p2 = tf1.transform (p2);
             normal = (tf1.getRotation() * (p2 - p1)).normalized();
-            p1 = p2 = tf1.transform(p1);
+            p1 = tf1.transform(p1);
+            p2 = tf1.transform(p2);
           } else {
             details::EPA epa(epa_max_face_num, epa_max_vertex_num,
                              epa_max_iterations, epa_tolerance);
@@ -275,15 +277,15 @@ namespace fcl
               epa.getClosestPoints (shape, w0, w1);
               assert (epa.depth >= -eps);
               distance = (std::min) (0., -epa.depth);
-              // TODO should be
-              // normal = tf1.getRotation() * epa.normal;
-              normal = tf2.getRotation() * epa.normal;
-              p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
+              normal = tf1.getRotation() * epa.normal;
+              p1 = tf1.transform(w0);
+              p2 = tf1.transform(w1);
               return false;
             }
             distance = -(std::numeric_limits<FCL_REAL>::max)();
             gjk.getClosestPoints (shape, p1, p2);
-            p1 = p2 = tf1.transform (p1);
+            p1 = tf1.transform(p1);
+            p2 = tf1.transform(p2);
           }
           return false;
         }

python/CMakeLists.txt

@@ -42,7 +42,11 @@ SET(${LIBRARY_NAME}_HEADERS
   fcl.hh
   )
 
-IF(NOT DOXYGEN_FOUND OR (APPLE AND PYTHON_VERSION_MAJOR LESS 3))
+SET(ENABLE_PYTHON_DOXYGEN_AUTODOC TRUE CACHE BOOL "Enable automatic documentation of Python bindings from Doxygen documentation")
+
+IF(    NOT ENABLE_PYTHON_DOXYGEN_AUTODOC
+    OR NOT DOXYGEN_FOUND
+    OR (APPLE AND PYTHON_VERSION_MAJOR LESS 3)) # Disable documentation on APPLE for python 2.
   SET(ENABLE_DOXYGEN_AUTODOC FALSE)
 ELSE()
   SET(ENABLE_DOXYGEN_AUTODOC TRUE)

python/collision-geometries.cc

@@ -34,8 +34,7 @@
 
 #include <boost/python.hpp>
 
-#include <eigenpy/registration.hpp>
-#include <eigenpy/eigen-to-python.hpp>
+#include <eigenpy/eigenpy.hpp>
 
 #include "fcl.hh"
 

python/collision.cc

@@ -35,8 +35,7 @@
 #include <boost/python.hpp>
 #include <boost/python/suite/indexing/vector_indexing_suite.hpp>
 
-#include <eigenpy/eigen-to-python.hpp>
-#include <eigenpy/registration.hpp>
+#include <eigenpy/eigenpy.hpp>
 
 #include <hpp/fcl/fwd.hh>
 #include <hpp/fcl/collision.h>

python/distance.cc

@@ -35,8 +35,7 @@
 #include <boost/python.hpp>
 #include <boost/python/suite/indexing/vector_indexing_suite.hpp>
 
-#include <eigenpy/registration.hpp>
-#include <eigenpy/eigen-to-python.hpp>
+#include <eigenpy/eigenpy.hpp>
 
 #include "fcl.hh"
 

python/fcl.cc

@@ -34,7 +34,7 @@
 
 #include <boost/python.hpp>
 
-#include <eigenpy/registration.hpp>
+#include <eigenpy/eigenpy.hpp>
 
 #include "fcl.hh"
 

python/gjk.cc

@@ -35,7 +35,7 @@
 #include <boost/python.hpp>
 #include <boost/python/suite/indexing/vector_indexing_suite.hpp>
 
-#include <eigenpy/registration.hpp>
+#include <eigenpy/eigenpy.hpp>
 
 #include "fcl.hh"
 

src/collision_func_matrix.cpp

@@ -85,10 +85,10 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
   if (distance <= 0) {
     if (result.numContacts () < request.num_max_contacts) {
       const Vec3f& p1 = distanceResult.nearest_points [0];
-      assert (!request.enable_contact || p1 == distanceResult.nearest_points [1]);
+      const Vec3f& p2 = distanceResult.nearest_points [1];
 
       Contact contact (o1, o2, distanceResult.b1, distanceResult.b2,
-          p1,
+          (p1+p2)/2,
           distanceResult.normal,
           -distance+request.security_margin);