Check the metric extend of the occupied part of the octomap during broadphase

Author: simonschmeisser

include/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h

@@ -904,6 +904,9 @@ void DynamicAABBTreeCollisionManager<S>::collide(CollisionObject<S>* obj, void*
       if(!octree_as_geometry_collide)
       {
         const OcTree<S>* octree = static_cast<const OcTree<S>*>(obj->collisionGeometry().get());
+
+        if (!dtree.getRoot()->bv.overlap(octree->getOccupiedMetricBV())) return;
+
         detail::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback);
       }
       else

include/fcl/geometry/octree/octree-inl.h

@@ -90,6 +90,15 @@ void OcTree<S>::computeLocalAABB()
   this->aabb_radius = (this->aabb_local.min_ - this->aabb_center).norm();
 }
 
+template <typename S>
+AABB<S> OcTree<S>::getOccupiedMetricBV() const
+{
+  S x_min, y_min, z_min, x_max, y_max, z_max;
+  tree->getMetricMin(x_min, y_min, z_min);
+  tree->getMetricMax(x_max, y_max, z_max);
+  return AABB<S>(Vector3<S>(x_min, y_min, z_min), Vector3<S>(x_max, y_max, z_max));
+}
+
 //==============================================================================
 template <typename S>
 AABB<S> OcTree<S>::getRootBV() const

include/fcl/geometry/octree/octree.h

@@ -84,6 +84,13 @@ class FCL_EXPORT OcTree : public CollisionGeometry<S>
   /// @brief compute the AABB<S> for the octree in its local coordinate system
   void computeLocalAABB();
 
+  /// @brief get the metric extend of the occupied part of the tree
+  ///
+  /// note that getMetricMin/getMetricMax can only cache the result of this
+  /// expensive operation if called non-const. Ie make sure to call either one
+  /// of them once after each update to the octomap!
+  AABB<S> getOccupiedMetricBV() const;
+
   /// @brief get the bounding volume for the root
   AABB<S> getRootBV() const;