Commit 79303b38 authored by jpan's avatar jpan
Browse files

conditional compile for octomap


git-svn-id: https://kforge.ros.org/fcl/fcl_ros@170 253336fb-580f-4252-a368-f3cef5a2a82b
parent b0a935c9
......@@ -26,6 +26,18 @@ else()
message(STATUS "FCL does not use SSE")
endif()
# Check for Octomap flags
set(FCL_HAVE_OCTOMAP 0)
include(FCLCheckOctomap)
fcl_check_for_octomap()
if (OCTOMAP_FLAGS)
message(STATUS "FCL uses octomap")
add_definitions(${OCTOMAP_FLAGS})
set(FCL_HAVE_OCTOMAP 1)
else()
message(STATUS "FCL does not use octomap")
endif()
# make sure we know what flag we used for SSE
include_directories("include")
......
macro(fcl_check_for_octomap)
#check for Octomap
include(CheckCXXSourceRuns)
set(OCTOMAP_FLAGS)
check_cxx_source_runs("
#include <octomap/octomap.h>
int main()
{
octomap::OcTree* tree = new octomap::OcTree(0.1);
for(int x = -20; x < 20; ++x)
{
for(int y = -20; y < 20; ++y)
{
for(int z = -20; z < 20; ++z)
{
tree->updateNode(octomap::point3d(x * 0.05, y * 0.05, z * 0.05), true);
}
}
}
for(int x = -30; x < 30; ++x)
{
for(int y = -30; y < 30; ++y)
{
for(int z = -30; z < 30; ++z)
{
tree->updateNode(octomap::point3d(x * 0.02 - 1.0, y * 0.02 - 1.0, z * 0.02 - 1.0), false);
}
}
}
return 1;
}"
HAS_OCTOMAP)
set(CMAKE_REQUIRED_FLAGS)
if(HAS_OCTOMAP)
set(OCTOMAP_FLAGS "-loctomap")
message(STATUS " Found octomap, using flags: ${OCTOMAP_FLAGS}")
endif()
endmacro(fcl_check_for_octomap)
\ No newline at end of file
......@@ -40,7 +40,6 @@
#include "fcl/broadphase/broadphase.h"
#include "fcl/broadphase/hierarchy_tree.h"
#include "fcl/octree.h"
#include "fcl/BV/BV.h"
#include "fcl/shape/geometric_shapes_utility.h"
#include <boost/unordered_map.hpp>
......@@ -118,81 +117,22 @@ public:
}
/// @brief perform collision test between one object and all the objects belonging to the manager
void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
{
if(size() == 0) return;
switch(obj->getCollisionGeometry()->getNodeType())
{
case GEOM_OCTREE:
{
if(!octree_as_geometry_collide)
{
const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
collisionRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback);
}
else
collisionRecurse(dtree.getRoot(), obj, cdata, callback);
}
break;
default:
collisionRecurse(dtree.getRoot(), obj, cdata, callback);
}
}
void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
/// @brief perform distance computation between one object and all the objects belonging to the manager
void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
{
if(size() == 0) return;
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
switch(obj->getCollisionGeometry()->getNodeType())
{
case GEOM_OCTREE:
{
if(!octree_as_geometry_distance)
{
const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
distanceRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist);
}
else
distanceRecurse(dtree.getRoot(), obj, cdata, callback, min_dist);
}
break;
default:
distanceRecurse(dtree.getRoot(), obj, cdata, callback, min_dist);
}
}
void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
/// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision)
void collide(void* cdata, CollisionCallBack callback) const
{
if(size() == 0) return;
selfCollisionRecurse(dtree.getRoot(), cdata, callback);
}
void collide(void* cdata, CollisionCallBack callback) const;
/// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
void distance(void* cdata, DistanceCallBack callback) const
{
if(size() == 0) return;
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
selfDistanceRecurse(dtree.getRoot(), cdata, callback, min_dist);
}
void distance(void* cdata, DistanceCallBack callback) const;
/// @brief perform collision test with objects belonging to another manager
void collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
{
DynamicAABBTreeCollisionManager* other_manager = static_cast<DynamicAABBTreeCollisionManager*>(other_manager_);
if((size() == 0) || (other_manager->size() == 0)) return;
collisionRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), cdata, callback);
}
void collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const;
/// @brief perform distance test with objects belonging to another manager
void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
{
DynamicAABBTreeCollisionManager* other_manager = static_cast<DynamicAABBTreeCollisionManager*>(other_manager_);
if((size() == 0) || (other_manager->size() == 0)) return;
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
distanceRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), cdata, callback, min_dist);
}
void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const;
/// @brief whether the manager is empty
bool empty() const
......@@ -215,25 +155,7 @@ private:
bool setup_;
bool collisionRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, CollisionCallBack callback) const;
bool collisionRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, CollisionCallBack callback) const;
bool selfCollisionRecurse(DynamicAABBNode* root, void* cdata, CollisionCallBack callback) const;
bool distanceRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
bool distanceRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
bool selfDistanceRecurse(DynamicAABBNode* root, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
void update_(CollisionObject* updated_obj);
/// @brief special manager-obj collision for octree
bool collisionRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback) const;
bool distanceRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
};
......
......@@ -39,7 +39,6 @@
#include "fcl/broadphase/broadphase.h"
#include "fcl/broadphase/hierarchy_tree.h"
#include "fcl/octree.h"
#include "fcl/BV/BV.h"
#include "fcl/shape/geometric_shapes_utility.h"
#include <boost/unordered_map.hpp>
......@@ -116,81 +115,22 @@ public:
}
/// @brief perform collision test between one object and all the objects belonging to the manager
void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
{
if(size() == 0) return;
switch(obj->getCollisionGeometry()->getNodeType())
{
case GEOM_OCTREE:
{
if(!octree_as_geometry_collide)
{
const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
collisionRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback);
}
else
collisionRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback);
}
break;
default:
collisionRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback);
}
}
void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
/// @brief perform distance computation between one object and all the objects belonging to the manager
void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
{
if(size() == 0) return;
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
switch(obj->getCollisionGeometry()->getNodeType())
{
case GEOM_OCTREE:
{
if(!octree_as_geometry_distance)
{
const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
distanceRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist);
}
else
distanceRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback, min_dist);
}
break;
default:
distanceRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback, min_dist);
}
}
void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
/// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision)
void collide(void* cdata, CollisionCallBack callback) const
{
if(size() == 0) return;
selfCollisionRecurse(dtree.getNodes(), dtree.getRoot(), cdata, callback);
}
void collide(void* cdata, CollisionCallBack callback) const;
/// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
void distance(void* cdata, DistanceCallBack callback) const
{
if(size() == 0) return;
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
selfDistanceRecurse(dtree.getNodes(), dtree.getRoot(), cdata, callback, min_dist);
}
void distance(void* cdata, DistanceCallBack callback) const;
/// @brief perform collision test with objects belonging to another manager
void collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
{
DynamicAABBTreeCollisionManager_Array* other_manager = static_cast<DynamicAABBTreeCollisionManager_Array*>(other_manager_);
if((size() == 0) || (other_manager->size() == 0)) return;
collisionRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), cdata, callback);
}
void collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const;
/// @brief perform distance test with objects belonging to another manager
void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
{
DynamicAABBTreeCollisionManager_Array* other_manager = static_cast<DynamicAABBTreeCollisionManager_Array*>(other_manager_);
if((size() == 0) || (other_manager->size() == 0)) return;
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
distanceRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), cdata, callback, min_dist);
}
void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const;
/// @brief whether the manager is empty
bool empty() const
......@@ -213,26 +153,7 @@ private:
bool setup_;
bool collisionRecurse(DynamicAABBNode* nodes1, size_t root1, DynamicAABBNode* nodes2, size_t root2, void* cdata, CollisionCallBack callback) const;
bool collisionRecurse(DynamicAABBNode* nodes, size_t root, CollisionObject* query, void* cdata, CollisionCallBack callback) const;
bool selfCollisionRecurse(DynamicAABBNode* nodes, size_t root, void* cdata, CollisionCallBack callback) const;
bool distanceRecurse(DynamicAABBNode* nodes1, size_t root1, DynamicAABBNode* nodes2, size_t root2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
bool distanceRecurse(DynamicAABBNode* nodes, size_t root, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
bool selfDistanceRecurse(DynamicAABBNode* nodes, size_t root, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
void update_(CollisionObject* updated_obj);
/// @brief special manager-obj collision for octree
bool collisionRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback) const;
bool distanceRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
void update_(CollisionObject* updated_obj);
};
......
......@@ -37,5 +37,6 @@
#define FCL_VERSION "@FCL_VERSION@"
#cmakedefine01 FCL_HAVE_SSE
#cmakedefine01 FCL_HAVE_OCTOMAP
#endif
......@@ -57,6 +57,9 @@ private:
FCL_REAL default_occupancy;
FCL_REAL occupancy_threshold;
FCL_REAL free_threshold;
public:
/// @brief OcTreeNode must implement the following interfaces:
......@@ -69,12 +72,20 @@ public:
OcTree(FCL_REAL resolution) : tree(boost::shared_ptr<const octomap::OcTree>(new octomap::OcTree(resolution)))
{
default_occupancy = tree->getOccupancyThres();
// default occupancy/free threshold is consistent with default setting from octomap
occupancy_threshold = tree->getOccupancyThres();
free_threshold = 0;
}
/// @brief construct octree from octomap
OcTree(const boost::shared_ptr<const octomap::OcTree>& tree_) : tree(tree_)
{
default_occupancy = tree->getOccupancyThres();
// default occupancy/free threshold is consistent with default setting from octomap
occupancy_threshold = tree->getOccupancyThres();
free_threshold = 0;
}
/// @brief compute the AABB for the octree in its local coordinate system
......@@ -103,13 +114,15 @@ public:
/// @brief whether one node is completely occupied
inline bool isNodeOccupied(const OcTreeNode* node) const
{
return tree->isNodeOccupied(node);
// return tree->isNodeOccupied(node);
return node->getOccupancy() >= occupancy_threshold;
}
/// @brief whether one node is completely free
inline bool isNodeFree(const OcTreeNode* node) const
{
return false; // default no definitely free node
// return false; // default no definitely free node
return node->getOccupancy() <= free_threshold;
}
/// @brief whether one node is uncertain
......@@ -128,7 +141,8 @@ public:
it != end;
++it)
{
if(tree->isNodeOccupied(*it))
// if(tree->isNodeOccupied(*it))
if(isNodeOccupied(&*it))
{
FCL_REAL size = it.getSize();
FCL_REAL x = it.getX();
......@@ -147,13 +161,13 @@ public:
/// @brief the threshold used to decide whether one node is occupied, this is NOT the octree occupied_thresold
FCL_REAL getOccupancyThres() const
{
return tree->getOccupancyThres();
return occupancy_threshold;
}
/// @brief the threshold used to decide whether one node is occupied, this is NOT the octree free_threshold
FCL_REAL getFreeThres() const
{
return 0.0;
return free_threshold;
}
FCL_REAL getDefaultOccupancy() const
......@@ -166,6 +180,16 @@ public:
default_occupancy = d;
}
void setOccupancyThres(FCL_REAL d)
{
occupancy_threshold = d;
}
void setFreeThres(FCL_REAL d)
{
free_threshold = d;
}
/// @brief return object type, it is an octree
OBJECT_TYPE getObjectType() const { return OT_OCTREE; }
......
......@@ -41,11 +41,17 @@
#include "fcl/traversal/traversal_node_bvhs.h"
#include "fcl/traversal/traversal_node_shapes.h"
#include "fcl/traversal/traversal_node_bvh_shape.h"
#if FCL_HAVE_OCTOMAP
#include "fcl/traversal/traversal_node_octree.h"
#endif
#include "fcl/BVH/BVH_utility.h"
namespace fcl
{
#if FCL_HAVE_OCTOMAP
/// @brief Initialize traversal node for collision between two octrees, given current object transform
template<typename NarrowPhaseSolver>
bool initialize(OcTreeCollisionTraversalNode<NarrowPhaseSolver>& node,
......@@ -276,6 +282,8 @@ bool initialize(OcTreeMeshDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
return true;
}
#endif
/// @brief Initialize traversal node for collision between two geometric shapes, given current object transform
template<typename S1, typename S2, typename NarrowPhaseSolver>
......
......@@ -45,6 +45,7 @@
namespace fcl
{
#if FCL_HAVE_OCTOMAP
template<typename T_SH, typename NarrowPhaseSolver>
std::size_t ShapeOcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
......@@ -189,6 +190,7 @@ std::size_t BVHOcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1
return result.numContacts();
}
#endif
template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>
std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
......@@ -566,6 +568,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
collision_matrix[BV_kIOS][BV_kIOS] = &BVHCollide<kIOS, NarrowPhaseSolver>;
collision_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHCollide<OBBRSS, NarrowPhaseSolver>;
#if FCL_HAVE_OCTOMAP
collision_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeCollide<Box, NarrowPhaseSolver>;
collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeCollide<Sphere, NarrowPhaseSolver>;
collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeCollide<Capsule, NarrowPhaseSolver>;
......@@ -601,7 +604,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
collision_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<16>, NarrowPhaseSolver>;
collision_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<18>, NarrowPhaseSolver>;
collision_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<24>, NarrowPhaseSolver>;
#endif
}
......
......@@ -43,6 +43,7 @@
namespace fcl
{
#if FCL_HAVE_OCTOMAP
template<typename T_SH, typename NarrowPhaseSolver>
FCL_REAL ShapeOcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
......@@ -123,7 +124,7 @@ FCL_REAL OcTreeBVHDistance(const CollisionGeometry* o1, const Transform3f& tf1,
return result.min_distance;
}
#endif
template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>
FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
......@@ -422,6 +423,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance<kIOS, NarrowPhaseSolver>;
distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance<OBBRSS, NarrowPhaseSolver>;
#if FCL_HAVE_OCTOMAP
distance_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeDistance<Box, NarrowPhaseSolver>;
distance_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeDistance<Sphere, NarrowPhaseSolver>;
distance_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeDistance<Capsule, NarrowPhaseSolver>;
......@@ -457,6 +459,9 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<16>, NarrowPhaseSolver>;
distance_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<18>, NarrowPhaseSolver>;
distance_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<24>, NarrowPhaseSolver>;
#endif
}
template struct DistanceFunctionMatrix<GJKSolver_libccd>;
......
Supports Markdown
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