Merge pull request #169 from jmirabel/devel

Add collide and distance prototype that update the GJK guess.

include/hpp/fcl/collision.h

@@ -59,6 +59,30 @@ std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
 std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
                     const CollisionGeometry* o2, const Transform3f& tf2,
                     const CollisionRequest& request, CollisionResult& result);
+
+/// @copydoc collide(const CollisionObject*, const CollisionObject*, const CollisionRequest&, CollisionResult&)
+/// \note this function update the initial guess of \c request if requested.
+///       See QueryRequest::updateGuess
+inline std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
+                           CollisionRequest& request, CollisionResult& result)
+{
+  std::size_t res = collide(o1, o2, (const CollisionRequest&) request, result);
+  request.updateGuess (result);
+  return res;
+}
+
+/// @copydoc collide(const CollisionObject*, const CollisionObject*, const CollisionRequest&, CollisionResult&)
+/// \note this function update the initial guess of \c request if requested.
+///       See QueryRequest::updateGuess
+inline std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
+                           const CollisionGeometry* o2, const Transform3f& tf2,
+                           CollisionRequest& request, CollisionResult& result)
+{
+  std::size_t res = collide(o1, tf1, o2, tf2,
+      (const CollisionRequest&) request, result);
+  request.updateGuess (result);
+  return res;
+}
 }
 
 } // namespace hpp

include/hpp/fcl/distance.h

@@ -54,6 +54,30 @@ FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const Di
 FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
                   const CollisionGeometry* o2, const Transform3f& tf2,
                   const DistanceRequest& request, DistanceResult& result);
+
+/// @copydoc distance(const CollisionObject*, const CollisionObject*, const DistanceRequest&, DistanceResult&)
+/// \note this function update the initial guess of \c request if requested.
+///       See QueryRequest::updateGuess
+inline FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2,
+    DistanceRequest& request, DistanceResult& result)
+{
+  FCL_REAL res = distance(o1, o2, (const DistanceRequest&) request, result);
+  request.updateGuess (result);
+  return res;
+}
+
+/// @copydoc distance(const CollisionObject*, const CollisionObject*, const DistanceRequest&, DistanceResult&)
+/// \note this function update the initial guess of \c request if requested.
+///       See QueryRequest::updateGuess
+inline FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
+                         const CollisionGeometry* o2, const Transform3f& tf2,
+                         DistanceRequest& request, DistanceResult& result)
+{
+  FCL_REAL res = distance(o1, tf1, o2, tf2,
+      (const DistanceRequest&) request, result);
+  request.updateGuess (result);
+  return res;
+}
 }
 
 } // namespace hpp

python/collision.cc

@@ -78,17 +78,26 @@ void exposeCollisionAPI ()
       ;
   }
 
+  if(!eigenpy::register_symbolic_link_to_registered_type<QueryRequest>())
+  {
+    class_ <QueryRequest> ("QueryRequest",
+        doxygen::class_doc<QueryRequest>(), no_init)
+      .DEF_RW_CLASS_ATTRIB (QueryRequest, enable_cached_gjk_guess    )
+      .DEF_RW_CLASS_ATTRIB (QueryRequest, cached_gjk_guess           )
+      .DEF_RW_CLASS_ATTRIB (QueryRequest, cached_support_func_guess  )
+      .DEF_CLASS_FUNC (QueryRequest, updateGuess)
+      ;
+  }
+
   if(!eigenpy::register_symbolic_link_to_registered_type<CollisionRequest>())
   {
-    class_ <CollisionRequest> ("CollisionRequest",
+    class_ <CollisionRequest, bases<QueryRequest> > ("CollisionRequest",
         doxygen::class_doc<CollisionRequest>(), no_init)
       .def (dv::init<CollisionRequest>())
       .def (dv::init<CollisionRequest, const CollisionRequestFlag, size_t>())
       .DEF_RW_CLASS_ATTRIB (CollisionRequest, num_max_contacts           )
       .DEF_RW_CLASS_ATTRIB (CollisionRequest, enable_contact             )
       .DEF_RW_CLASS_ATTRIB (CollisionRequest, enable_distance_lower_bound)
-      .DEF_RW_CLASS_ATTRIB (CollisionRequest, enable_cached_gjk_guess    )
-      .DEF_RW_CLASS_ATTRIB (CollisionRequest, cached_gjk_guess           )
       .DEF_RW_CLASS_ATTRIB (CollisionRequest, security_margin            )
       .DEF_RW_CLASS_ATTRIB (CollisionRequest, break_distance             )
       ;
@@ -119,9 +128,18 @@ void exposeCollisionAPI ()
       ;
   }
 
+  if(!eigenpy::register_symbolic_link_to_registered_type<QueryResult>())
+  {
+    class_ <QueryResult> ("QueryResult",
+        doxygen::class_doc<QueryResult>(), no_init)
+      .DEF_RW_CLASS_ATTRIB (QueryResult, cached_gjk_guess         )
+      .DEF_RW_CLASS_ATTRIB (QueryResult, cached_support_func_guess)
+      ;
+  }
+
   if(!eigenpy::register_symbolic_link_to_registered_type< CollisionResult >())
   {
-    class_ <CollisionResult> ("CollisionResult",
+    class_ <CollisionResult, bases<QueryResult> > ("CollisionResult",
         doxygen::class_doc<CollisionResult>(), no_init)
       .def (dv::init<CollisionResult>())
       .DEF_CLASS_FUNC (CollisionResult, isCollision)
@@ -147,5 +165,5 @@ void exposeCollisionAPI ()
   doxygen::def ("collide", static_cast< std::size_t (*)(
         const CollisionGeometry*, const Transform3f&,
         const CollisionGeometry*, const Transform3f&,
-        const CollisionRequest&, CollisionResult&) > (&collide));
+        CollisionRequest&, CollisionResult&) > (&collide));
 }

python/distance.cc

@@ -78,7 +78,7 @@ void exposeDistanceAPI ()
 {
   if(!eigenpy::register_symbolic_link_to_registered_type<DistanceRequest>())
   {
-    class_ <DistanceRequest> ("DistanceRequest",
+    class_ <DistanceRequest, bases<QueryRequest> > ("DistanceRequest",
         doxygen::class_doc<DistanceRequest>(),
         init<optional<bool,FCL_REAL,FCL_REAL> >((arg("self"),
                                                  arg("enable_nearest_points"),
@@ -92,7 +92,7 @@ void exposeDistanceAPI ()
 
   if(!eigenpy::register_symbolic_link_to_registered_type<DistanceResult>())
   {
-    class_ <DistanceResult> ("DistanceResult",
+    class_ <DistanceResult, bases<QueryResult> > ("DistanceResult",
                              doxygen::class_doc<DistanceResult>(),
                              no_init)
       .def (dv::init<DistanceResult>())
@@ -125,5 +125,5 @@ void exposeDistanceAPI ()
   doxygen::def ("distance", static_cast< FCL_REAL (*)(
         const CollisionGeometry*, const Transform3f&,
         const CollisionGeometry*, const Transform3f&,
-        const DistanceRequest&, DistanceResult&) > (&distance));
+        DistanceRequest&, DistanceResult&) > (&distance));
 }

src/shape/convex.cpp

@@ -42,8 +42,8 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, int num_points,
     throw std::logic_error ("Qhull failed");
   }
 
+  typedef std::size_t index_type;
   typedef int size_type;
-  typedef int index_type;
 
   // Map index in pts to index in vertices. -1 means not used
   std::vector<int> pts_to_vertices (num_points, -1);