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

@@ -165,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

@@ -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));
 }