fix build with octomap

CMakeLists.txt

@@ -53,6 +53,14 @@ set(BOOST_COMPONENTS
 search_for_boost()
 # Optional dependencies
 add_optional_dependency("octomap >= 1.6")
+if (OCTOMAP_INCLUDE_DIRS AND OCTOMAP_LIBRARY_DIRS)
+	include_directories(${OCTOMAP_INCLUDE_DIRS})
+	link_directories(${OCTOMAP_LIBRARY_DIRS})
+	set(FCL_HAVE_OCTOMAP 1)
+	message(STATUS "FCL uses Octomap")
+else()
+	message(STATUS "FCL does not use Octomap")
+endif()
 # flann package ill defined: comment.
 # add_optional_dependency("flann >= 1.7")
 # if (${FLANN_FOUND})

include/hpp/fcl/traversal/traversal_node_octree.h

@@ -1041,6 +1041,11 @@ public:
     return false;
   }
 
+  bool BVTesting(int, int, FCL_REAL&) const
+  {
+    return false;
+  }
+
   void leafTesting(int, int, FCL_REAL&) const
   {
     otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result);
@@ -1073,6 +1078,11 @@ public:
     return -1;
   }
 
+  bool BVTesting(int, int, FCL_REAL&) const
+  {
+    return false;
+  }
+
   void leafTesting(int, int) const
   {
     otsolver->OcTreeDistance(model1, model2, tf1, tf2, request, *result);
@@ -1102,6 +1112,11 @@ public:
     return false;
   }
 
+  bool BVTesting(int, int, FCL_REAL&) const
+  {
+    return false;
+  }
+
   void leafTesting(int, int, FCL_REAL&) const
   {
     otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result);
@@ -1133,6 +1148,11 @@ public:
     return false;
   }
 
+  bool BVTesting(int, int, fcl::FCL_REAL&) const
+  {
+    return false;
+  }
+
   void leafTesting(int, int, FCL_REAL&) const
   {
     otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result);
@@ -1222,6 +1242,11 @@ public:
     return false;
   }
 
+  bool BVTesting(int, int, FCL_REAL&) const
+  {
+    return false;
+  }
+
   void leafTesting(int, int, FCL_REAL&) const
   {
     otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result);
@@ -1253,6 +1278,11 @@ public:
     return false;
   }
 
+  bool BVTesting(int, int, FCL_REAL&) const
+  {
+    return false;
+  }
+
   void leafTesting(int, int, FCL_REAL&) const
   {
     otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result);

src/collision_func_matrix.cpp

@@ -60,7 +60,8 @@ std::size_t ShapeOcTreeCollide(const CollisionGeometry* o1, const Transform3f& t
   OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
 
   initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
-  collide(&node);
+  FCL_REAL sqrDistance = 0;
+  collide(&node, sqrDistance);
 
   return result.numContacts();
 }
@@ -78,12 +79,13 @@ std::size_t OcTreeShapeCollide(const CollisionGeometry* o1, const Transform3f& t
   OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
 
   initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
-  collide(&node);
+  FCL_REAL sqrDistance = 0;
+  collide(&node, sqrDistance);
 
   return result.numContacts();
 }
 
-etemplate<typename NarrowPhaseSolver>
+template<typename NarrowPhaseSolver>
 std::size_t OcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
                           const NarrowPhaseSolver* nsolver,
                           const CollisionRequest& request, CollisionResult& result)
@@ -96,7 +98,8 @@ std::size_t OcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, c
   OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
 
   initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
-  collide(&node);
+  FCL_REAL sqrDistance = 0;
+  collide(&node, sqrDistance);
 
   return result.numContacts();
 }
@@ -119,7 +122,8 @@ std::size_t OcTreeBVHCollide(const CollisionGeometry* o1, const Transform3f& tf1
     OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
 
     initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, no_cost_request, result);
-    collide(&node);
+    FCL_REAL sqrDistance = 0;
+    collide(&node, sqrDistance);
 
     Box box;
     Transform3f box_tf;
@@ -140,7 +144,8 @@ std::size_t OcTreeBVHCollide(const CollisionGeometry* o1, const Transform3f& tf1
     OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
 
     initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
-    collide(&node);
+    FCL_REAL sqrDistance = 0;
+    collide(&node, sqrDistance);
   }
   
   return result.numContacts();
@@ -164,7 +169,8 @@ std::size_t BVHOcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1
     OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
 
     initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, no_cost_request, result);
-    collide(&node);
+    FCL_REAL sqrDistance = 0;
+    collide(&node, sqrDistance);
 
     Box box;
     Transform3f box_tf;
@@ -185,7 +191,8 @@ std::size_t BVHOcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1
     OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
 
     initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
-    collide(&node);
+    FCL_REAL sqrDistance = 0;
+    collide(&node, sqrDistance);
   }
   
   return result.numContacts();