Check pair margin AABB before narrow phase check

Levi-Armstrong · Levi-Armstrong · commit 838f23d6fb7e · 2025-09-11T13:22:25.000-05:00
diff --git a/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_utils.h b/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_utils.h
@@ -126,6 +126,15 @@ class CollisionObjectWrapper : public btCollisionObject
    */
   void getAABB(btVector3& aabb_min, btVector3& aabb_max) const;
 
+  /**
+   * @brief Get the collision object's axis aligned bounding box
+   * This AABB is extended by the provided margin
+   * @param aabb_min The minimum point
+   * @param aabb_max The maximum point
+   * @margin margin The value to extend the aabb
+   */
+  void getAABB(btVector3& aabb_min, btVector3& aabb_max, btScalar margin) const;
+
   /**
    * @brief This clones the collision objects but not the collision shape wich is const.
    * @return Shared Pointer to the cloned collision object
diff --git a/tesseract_collision/bullet/src/bullet_cast_simple_manager.cpp b/tesseract_collision/bullet/src/bullet_cast_simple_manager.cpp
@@ -404,7 +404,7 @@ void BulletCastSimpleManager::contactTest(ContactResultMap& collisions, const Co
       continue;
 
     btVector3 min_aabb[2], max_aabb[2];  // NOLINT
-    cow1->getAABB(min_aabb[0], max_aabb[0]);
+    cow1->getAABB(min_aabb[0], max_aabb[0], 0.0);
 
     btCollisionObjectWrapper obA(nullptr, cow1->getCollisionShape(), cow1.get(), cow1->getWorldTransform(), -1, -1);
 
@@ -414,7 +414,10 @@ void BulletCastSimpleManager::contactTest(ContactResultMap& collisions, const Co
       assert(!contact_test_data_.done);
 
       const COW::Ptr& cow2 = *cow2_iter;
-      cow2->getAABB(min_aabb[1], max_aabb[1]);
+
+      const double contact_threshold =
+          contact_test_data_.collision_margin_data.getCollisionMargin(cow1->getName(), cow2->getName());
+      cow2->getAABB(min_aabb[1], max_aabb[1], contact_threshold);
 
       bool aabb_check = (min_aabb[0][0] <= max_aabb[1][0] && max_aabb[0][0] >= min_aabb[1][0]) &&
                         (min_aabb[0][1] <= max_aabb[1][1] && max_aabb[0][1] >= min_aabb[1][1]) &&
@@ -435,8 +438,7 @@ void BulletCastSimpleManager::contactTest(ContactResultMap& collisions, const Co
           if (algorithm != nullptr)
           {
             // Update the contact threshold to be pair specific
-            cc.m_closestDistanceThreshold =
-                contact_test_data_.collision_margin_data.getCollisionMargin(cow1->getName(), cow2->getName());
+            cc.m_closestDistanceThreshold = contact_threshold;
             TesseractBridgedManifoldResult contactPointResult(&obA, &obB, cc);
 
             // discrete collision detection query
diff --git a/tesseract_collision/bullet/src/bullet_discrete_simple_manager.cpp b/tesseract_collision/bullet/src/bullet_discrete_simple_manager.cpp
@@ -290,7 +290,7 @@ void BulletDiscreteSimpleManager::contactTest(ContactResultMap& collisions, cons
       continue;
 
     btVector3 min_aabb[2], max_aabb[2];  // NOLINT
-    cow1->getAABB(min_aabb[0], max_aabb[0]);
+    cow1->getAABB(min_aabb[0], max_aabb[0], 0.0);
 
     btCollisionObjectWrapper obA(nullptr, cow1->getCollisionShape(), cow1.get(), cow1->getWorldTransform(), -1, -1);
 
@@ -300,7 +300,9 @@ void BulletDiscreteSimpleManager::contactTest(ContactResultMap& collisions, cons
       assert(!contact_test_data_.done);
 
       const COW::Ptr& cow2 = *cow2_iter;
-      cow2->getAABB(min_aabb[1], max_aabb[1]);
+      const double contact_threshold =
+          contact_test_data_.collision_margin_data.getCollisionMargin(cow1->getName(), cow2->getName());
+      cow2->getAABB(min_aabb[1], max_aabb[1], contact_threshold);
 
       bool aabb_check = (min_aabb[0][0] <= max_aabb[1][0] && max_aabb[0][0] >= min_aabb[1][0]) &&
                         (min_aabb[0][1] <= max_aabb[1][1] && max_aabb[0][1] >= min_aabb[1][1]) &&
@@ -321,8 +323,7 @@ void BulletDiscreteSimpleManager::contactTest(ContactResultMap& collisions, cons
           if (algorithm != nullptr)
           {
             // Update the contact threshold to be pair specific
-            cc.m_closestDistanceThreshold =
-                contact_test_data_.collision_margin_data.getCollisionMargin(cow1->getName(), cow2->getName());
+            cc.m_closestDistanceThreshold = contact_threshold;
             TesseractBridgedManifoldResult contactPointResult(&obA, &obB, cc);
 
             // discrete collision detection query
diff --git a/tesseract_collision/bullet/src/bullet_utils.cpp b/tesseract_collision/bullet/src/bullet_utils.cpp
@@ -528,12 +528,17 @@ const tesseract_common::VectorIsometry3d& CollisionObjectWrapper::getCollisionGe
 }
 
 void CollisionObjectWrapper::getAABB(btVector3& aabb_min, btVector3& aabb_max) const
+{
+  const btScalar d = getContactProcessingThreshold() / 2.0;
+  getAABB(aabb_min, aabb_max, d);
+}
+
+void CollisionObjectWrapper::getAABB(btVector3& aabb_min, btVector3& aabb_max, btScalar margin) const
 {
   getCollisionShape()->getAabb(getWorldTransform(), aabb_min, aabb_max);
-  const btScalar& d = getContactProcessingThreshold() / 2.0;
-  btVector3 contactThreshold(d, d, d);
-  aabb_min -= contactThreshold;
-  aabb_max += contactThreshold;
+  btVector3 contact_threshold(margin, margin, margin);
+  aabb_min -= contact_threshold;
+  aabb_max += contact_threshold;
 }
 
 std::shared_ptr<CollisionObjectWrapper> CollisionObjectWrapper::clone()
@@ -1131,27 +1136,42 @@ bool TesseractCollisionPairCallback::processOverlap(btBroadphasePair& pair)
   const auto* cow0 = static_cast<const CollisionObjectWrapper*>(pair.m_pProxy0->m_clientObject);
   const auto* cow1 = static_cast<const CollisionObjectWrapper*>(pair.m_pProxy1->m_clientObject);
 
-  if (results_callback_.needsCollision(cow0, cow1))
-  {
-    btCollisionObjectWrapper obj0Wrap(nullptr, cow0->getCollisionShape(), cow0, cow0->getWorldTransform(), -1, -1);
-    btCollisionObjectWrapper obj1Wrap(nullptr, cow1->getCollisionShape(), cow1, cow1->getWorldTransform(), -1, -1);
+  if (!results_callback_.needsCollision(cow0, cow1))
+    return false;
 
-    // dispatcher will keep algorithms persistent in the collision pair
-    if (pair.m_algorithm == nullptr)
-    {
-      pair.m_algorithm = dispatcher_->findAlgorithm(&obj0Wrap, &obj1Wrap, nullptr, BT_CLOSEST_POINT_ALGORITHMS);
-    }
+  const double contact_threshold =
+      results_callback_.collisions_.collision_margin_data.getCollisionMargin(cow0->getName(), cow1->getName());
+  btVector3 min_aabb[2], max_aabb[2];  // NOLINT
+  cow0->getAABB(min_aabb[0], max_aabb[0], contact_threshold / 2.0);
+  cow1->getAABB(min_aabb[1], max_aabb[1], contact_threshold / 2.0);
 
-    if (pair.m_algorithm != nullptr)
-    {
-      TesseractBroadphaseBridgedManifoldResult contactPointResult(&obj0Wrap, &obj1Wrap, results_callback_);
-      contactPointResult.m_closestPointDistanceThreshold =
-          results_callback_.collisions_.collision_margin_data.getCollisionMargin(cow0->getName(), cow1->getName());
+  const bool aabb_check = (min_aabb[0][0] <= max_aabb[1][0] && max_aabb[0][0] >= min_aabb[1][0]) &&
+                          (min_aabb[0][1] <= max_aabb[1][1] && max_aabb[0][1] >= min_aabb[1][1]) &&
+                          (min_aabb[0][2] <= max_aabb[1][2] && max_aabb[0][2] >= min_aabb[1][2]);
 
-      // discrete collision detection query
-      pair.m_algorithm->processCollision(&obj0Wrap, &obj1Wrap, dispatch_info_, &contactPointResult);
-    }
+  // Check if ABB are overlapping
+  if (!aabb_check)
+    return false;
+
+  btCollisionObjectWrapper obj0Wrap(nullptr, cow0->getCollisionShape(), cow0, cow0->getWorldTransform(), -1, -1);
+  btCollisionObjectWrapper obj1Wrap(nullptr, cow1->getCollisionShape(), cow1, cow1->getWorldTransform(), -1, -1);
+
+  // dispatcher will keep algorithms persistent in the collision pair
+  if (pair.m_algorithm == nullptr)
+  {
+    pair.m_algorithm = dispatcher_->findAlgorithm(&obj0Wrap, &obj1Wrap, nullptr, BT_CLOSEST_POINT_ALGORITHMS);
   }
+
+  if (pair.m_algorithm != nullptr)
+  {
+    TesseractBroadphaseBridgedManifoldResult contactPointResult(&obj0Wrap, &obj1Wrap, results_callback_);
+    contactPointResult.m_closestPointDistanceThreshold = contact_threshold;
+
+    // discrete collision detection query
+    pair.m_algorithm->processCollision(&obj0Wrap, &obj1Wrap, dispatch_info_, &contactPointResult);
+  }
+
+  // Should this return true?
   return false;
 }
 
diff --git a/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_collision_object_wrapper.h b/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_collision_object_wrapper.h
@@ -57,6 +57,13 @@ class FCLCollisionObjectWrapper : public fcl::CollisionObject<double>
    */
   double getContactDistanceThreshold() const;
 
+  /**
+   * @brief Get AABB with provided margin
+   * @param margin The margin to apply to AABB
+   * @return The AABB
+   */
+  fcl::AABB<double> getAABB(double margin) const;
+
   /**
    * @brief Update the internal AABB. This must be called instead of the base class computeAABB().
    *
@@ -81,6 +88,9 @@ class FCLCollisionObjectWrapper : public fcl::CollisionObject<double>
 
   /** @brief The shape index, which is the geometries index in the urdf. */
   int shape_index_{ -1 };
+
+  /** @brief AABB which does not include margin */
+  fcl::AABB<double> aabb_without_margin_;
 };
 
 }  // namespace tesseract_collision::tesseract_collision_fcl
diff --git a/tesseract_collision/fcl/src/fcl_collision_object_wrapper.cpp b/tesseract_collision/fcl/src/fcl_collision_object_wrapper.cpp
@@ -36,22 +36,33 @@ void FCLCollisionObjectWrapper::setContactDistanceThreshold(double contact_dista
 
 double FCLCollisionObjectWrapper::getContactDistanceThreshold() const { return contact_distance_; }
 
+fcl::AABB<double> FCLCollisionObjectWrapper::getAABB(double margin) const
+{
+  fcl::AABB<double> aabb_with_margin{ aabb_without_margin_ };
+  fcl::Vector3<double> delta = fcl::Vector3<double>::Constant(margin);
+  aabb_with_margin.min_ -= delta;
+  aabb_with_margin.max_ += delta;
+  return aabb_with_margin;
+}
+
 void FCLCollisionObjectWrapper::updateAABB()
 {
   if (t.linear().isIdentity())
   {
-    aabb = translate(cgeom->aabb_local, t.translation());
-    fcl::Vector3<double> delta = fcl::Vector3<double>::Constant(contact_distance_ / 2.0);
-    aabb.min_ -= delta;
-    aabb.max_ += delta;
+    aabb_without_margin_ = translate(cgeom->aabb_local, t.translation());
   }
   else
   {
     fcl::Vector3<double> center = t * cgeom->aabb_center;
-    fcl::Vector3<double> delta = fcl::Vector3<double>::Constant(cgeom->aabb_radius + (contact_distance_ / 2.0));
-    aabb.min_ = center - delta;
-    aabb.max_ = center + delta;
+    fcl::Vector3<double> aabb_radius = fcl::Vector3<double>::Constant(cgeom->aabb_radius);
+    aabb_without_margin_.min_ = center - aabb_radius;
+    aabb_without_margin_.max_ = center + aabb_radius;
   }
+
+  aabb = aabb_without_margin_;
+  fcl::Vector3<double> delta = fcl::Vector3<double>::Constant(contact_distance_ / 2.0);
+  aabb.min_ -= delta;
+  aabb.max_ += delta;
 }
 
 void FCLCollisionObjectWrapper::setShapeIndex(int index) { shape_index_ = index; }
diff --git a/tesseract_collision/fcl/src/fcl_utils.cpp b/tesseract_collision/fcl/src/fcl_utils.cpp
@@ -237,6 +237,19 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi
   if (!needs_collision)
     return false;
 
+  const double margin = cdata->collision_margin_data.getCollisionMargin(cd1->getName(), cd2->getName()) / 2.0;
+
+  fcl::AABB<double> aabb1 = static_cast<const FCLCollisionObjectWrapper*>(o1)->getAABB(margin);
+  fcl::AABB<double> aabb2 = static_cast<const FCLCollisionObjectWrapper*>(o2)->getAABB(margin);
+
+  const bool aabb_check = (aabb1.min_[0] <= aabb2.max_[0] && aabb1.max_[0] >= aabb2.min_[0]) &&
+                          (aabb1.min_[1] <= aabb2.max_[1] && aabb1.max_[1] >= aabb2.min_[1]) &&
+                          (aabb1.min_[2] <= aabb2.max_[2] && aabb1.max_[2] >= aabb2.min_[2]);
+
+  // Check if ABB are overlapping
+  if (!aabb_check)
+    return false;
+
   std::size_t num_contacts = (cdata->req.contact_limit > 0) ? static_cast<std::size_t>(cdata->req.contact_limit) :
                                                               std::numeric_limits<std::size_t>::max();
   if (cdata->req.type == ContactTestType::FIRST)
