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

@@ -88,16 +88,23 @@ 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
 {
-  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);
+    S delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2;
 
-  // std::cout << "octree dimensions " << x_min << " " <<  x_max << " y: " << y_min << " " << y_max << " z: " << z_min << " " << z_max  << std::endl;
-  return AABB<S>(Vector3<S>(x_min, y_min, z_min), Vector3<S>(x_max, y_max, z_max));
+    // std::cout << "octree size " << delta << std::endl;
+    return AABB<S>(Vector3<S>(-delta, -delta, -delta), Vector3<S>(delta, delta, delta));
 }
 
 //==============================================================================

include/fcl/geometry/octree/octree.h

@@ -78,6 +78,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;