Optimize the broadphase for performance

git-svn-id: https://kforge.ros.org/fcl/fcl_ros@111 253336fb-580f-4252-a368-f3cef5a2a82b

trunk/fcl/CMakeLists.txt

@@ -37,7 +37,7 @@ link_directories(${CCD_LIBRARY_DIRS})
 
 add_definitions(-DUSE_SVMLIGHT=0)
 
-add_library(${PROJECT_NAME} SHARED src/BV/AABB.cpp src/BV/OBB.cpp src/BV/RSS.cpp src/BV/kIOS.cpp src/BV/OBBRSS.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/gjk_libccd.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp src/matrix_3f.cpp src/interval.cpp src/interval_vector.cpp src/interval_matrix.cpp src/taylor_model.cpp src/taylor_vector.cpp src/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/gjk.cpp src/narrowphase.cpp)
+add_library(${PROJECT_NAME} SHARED src/BV/AABB.cpp src/BV/OBB.cpp src/BV/RSS.cpp src/BV/kIOS.cpp src/BV/OBBRSS.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/gjk_libccd.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp src/matrix_3f.cpp src/interval.cpp src/interval_vector.cpp src/interval_matrix.cpp src/taylor_model.cpp src/taylor_vector.cpp src/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/gjk.cpp src/narrowphase.cpp src/hierarchy_tree.cpp)
 
 target_link_libraries(${PROJECT_NAME} ${FLANN_LIBRARIES} ${CCD_LIBRARIES})
 

trunk/fcl/include/fcl/BVH_model.h

@@ -108,7 +108,6 @@ public:
     bv_splitter.reset(new BVSplitter<BV>(SPLIT_METHOD_MEAN));
     bv_fitter.reset(new BVFitter<BV>());
   }
-
   BVHModel(const BVHModel& other);
 
 

trunk/fcl/src/BV/AABB.cpp

@@ -52,7 +52,7 @@ AABB::AABB()
 BVH_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const
 {
   BVH_REAL result = 0;
-  for(unsigned int i = 0; i < 3; ++i)
+  for(size_t i = 0; i < 3; ++i)
   {
     BVH_REAL amin = min_[i];
     BVH_REAL amax = max_[i];

trunk/fcl/src/interval_tree.cpp

@@ -50,7 +50,7 @@ IntervalTreeNode::IntervalTreeNode(SimpleInterval* new_interval) :
     high(new_interval->high),
     max_high(high) {}
 
-IntervalTreeNode::~IntervalTreeNode(){}
+IntervalTreeNode::~IntervalTreeNode() {}
 
 
 IntervalTree::IntervalTree()

trunk/fcl/src/traversal_recurse.cpp

@@ -184,8 +184,10 @@ void selfCollisionRecurse(CollisionTraversalNodeBase* node, int b, BVHFrontList*
   int c2 = node->getFirstRightChild(b);
 
   selfCollisionRecurse(node, c1, front_list);
+  if(node->canStop() && !front_list) return;
 
   selfCollisionRecurse(node, c2, front_list);
+  if(node->canStop() && !front_list) return;
 
   collisionRecurse(node, c1, c2, front_list);
 }