Add ComputeCollision

include/hpp/fcl/collision.h

@@ -42,6 +42,7 @@
 #include <hpp/fcl/data_types.h>
 #include <hpp/fcl/collision_object.h>
 #include <hpp/fcl/collision_data.h>
+#include <hpp/fcl/collision_func_matrix.h>
 
 namespace hpp
 {
@@ -83,8 +84,60 @@ inline std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
   request.updateGuess (result);
   return res;
 }
-}
 
+/// This class reduces the cost of identifying the geometry pair.
+/// This is mostly useful for repeated shape-shape queries.
+///
+/// \code
+///   ComputeCollision calc_collision (o1, o2);
+///   std::size_t ncontacts = calc_collision(tf1, tf2, request, result);
+/// \endcode
+class HPP_FCL_DLLAPI ComputeCollision {
+public:
+  ComputeCollision(const CollisionGeometry* o1, const CollisionGeometry* o2);
+
+  std::size_t operator()(const Transform3f& tf1, const Transform3f& tf2,
+      const CollisionRequest& request, CollisionResult& result)
+  {
+    bool cached = request.enable_cached_gjk_guess;
+    solver.enable_cached_guess = cached;
+    if (cached) {
+      solver.cached_guess = request.cached_gjk_guess;
+      solver.support_func_cached_guess = request.cached_support_func_guess;
+    }
+
+    std::size_t res;
+    if (swap_geoms) {
+      res = func(o2, tf2, o1, tf1, &solver, request, result);
+      invertResults(result);
+    } else {
+      res = func (o1, tf1, o2, tf2, &solver, request, result);
+    }
+
+    if (cached) {
+      result.cached_gjk_guess = solver.cached_guess;
+      result.cached_support_func_guess = solver.support_func_cached_guess;
+    }
+    return res;
+  }
+
+  inline std::size_t operator()(const Transform3f& tf1, const Transform3f& tf2,
+      CollisionRequest& request, CollisionResult& result)
+  {
+    std::size_t res = operator()(tf1, tf2, (const CollisionRequest&) request, result);
+    request.updateGuess (result);
+    return res;
+  }
+
+private:
+  CollisionGeometry const *o1, *o2;
+  GJKSolver solver;
+
+  CollisionFunctionMatrix::CollisionFunc func;
+  bool swap_geoms;
+};
+
+} // namespace fcl
 } // namespace hpp
 
 #endif

python/collision.cc

@@ -179,4 +179,12 @@ void exposeCollisionAPI ()
         const CollisionGeometry*, const Transform3f&,
         const CollisionGeometry*, const Transform3f&,
         CollisionRequest&, CollisionResult&) > (&collide));
+
+  class_<ComputeCollision> ("ComputeCollision",
+      doxygen::class_doc<ComputeCollision>(), no_init)
+    .def (dv::init<ComputeCollision, const CollisionGeometry*, const CollisionGeometry*>())
+    .def ("__call__", static_cast< std::size_t (ComputeCollision::*)(
+        const Transform3f&, const Transform3f&,
+        CollisionRequest&, CollisionResult&) > (&ComputeCollision::operator()));
+
 }

src/collision.cpp

@@ -56,17 +56,11 @@ CollisionFunctionMatrix& getCollisionFunctionLookTable()
 // reorder collision results in the order the call has been made.
 void invertResults(CollisionResult& result)
 {
-    const CollisionGeometry* otmp;
-    int btmp;
     for(std::vector<Contact>::iterator it = result.contacts.begin();
         it != result.contacts.end(); ++it)
     {
-        otmp = it->o1;
-        it->o1 = it->o2;
-        it->o2 = otmp;
-        btmp = it->b1;
-        it->b1 = it->b2;
-        it->b2 = btmp;
+        std::swap(it->o1, it->o2);
+        std::swap(it->b1, it->b2);
     }
 }
 
@@ -136,7 +130,32 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
   return res;
 }
 
-}
+ComputeCollision::ComputeCollision(const CollisionGeometry* o1,
+    const CollisionGeometry* o2)
+  : o1(o1), o2(o2)
+{
+  const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable();
+
+  OBJECT_TYPE object_type1 = o1->getObjectType();
+  NODE_TYPE node_type1 = o1->getNodeType();
+  OBJECT_TYPE object_type2 = o2->getObjectType();
+  NODE_TYPE node_type2 = o2->getNodeType();
+
+  swap_geoms = object_type1 == OT_GEOM && object_type2 == OT_BVH;
 
+  if(   ( swap_geoms && !looktable.collision_matrix[node_type2][node_type1])
+     || (!swap_geoms && !looktable.collision_matrix[node_type1][node_type2]))
+  {
+    std::ostringstream oss;
+    oss << "Warning: collision function between node type " << node_type1 <<
+      " and node type " << node_type2 << " is not supported";
+    throw std::invalid_argument(oss.str());
+  }
+  if (swap_geoms)
+    func = looktable.collision_matrix[node_type2][node_type1];
+  else
+    func = looktable.collision_matrix[node_type1][node_type2];
+}
 
+} // namespace fcl
 } // namespace hpp