Skip to content

Commit

Permalink
Check the metric extend of the occupied part of the octomap during br…
Browse files Browse the repository at this point in the history
…oadphase
  • Loading branch information
simonschmeisser committed Mar 25, 2021
1 parent f4018b4 commit 780152e
Show file tree
Hide file tree
Showing 3 changed files with 19 additions and 0 deletions.
3 changes: 3 additions & 0 deletions include/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
9 changes: 9 additions & 0 deletions include/fcl/geometry/octree/octree-inl.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
7 changes: 7 additions & 0 deletions include/fcl/geometry/octree/octree.h
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down

0 comments on commit 780152e

Please sign in to comment.