From 3d5d0e9351ebdd0c3f5372076f028e29cc097ec1 Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@laas.fr>
Date: Fri, 3 Jul 2015 15:15:40 +0200
Subject: [PATCH] fix build with octomap

---
 CMakeLists.txt                                |  8 +++++
 .../hpp/fcl/traversal/traversal_node_octree.h | 30 +++++++++++++++++++
 src/collision_func_matrix.cpp                 | 23 +++++++++-----
 3 files changed, 53 insertions(+), 8 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0d281a56..771ee5bf 100644
--- a/CMakeLists.txt
+++ b/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})
diff --git a/include/hpp/fcl/traversal/traversal_node_octree.h b/include/hpp/fcl/traversal/traversal_node_octree.h
index 331dc89f..6daa1b67 100644
--- a/include/hpp/fcl/traversal/traversal_node_octree.h
+++ b/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);
diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp
index 7e8dea63..c5e1e645 100644
--- a/src/collision_func_matrix.cpp
+++ b/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();
-- 
GitLab