Commit 3d5d0e93 authored by Steve Tonneau's avatar Steve Tonneau Committed by Florent Lamiraux
Browse files

fix build with octomap

parent 0ea409f8
......@@ -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})
......
......@@ -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);
......
......@@ -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();
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment