Check pair margin AABB before narrow phase check #1208

Levi-Armstrong · 2025-09-11T18:23:10Z

codecov · 2025-09-11T18:45:35Z

Codecov Report

❌ Patch coverage is 92.98246% with 4 lines in your changes missing coverage. Please review.
✅ Project coverage is 93.53%. Comparing base (bf3ad4c) to head (838f23d).

Files with missing lines	Patch %	Lines
...collision/fcl/src/fcl_collision_object_wrapper.cpp	78.57%	3 Missing ⚠️
tesseract_collision/fcl/src/fcl_utils.cpp	87.50%	1 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1208      +/-   ##
==========================================
- Coverage   93.53%   93.53%   -0.01%     
==========================================
  Files         272      272              
  Lines       18796    18823      +27     
==========================================
+ Hits        17581    17606      +25     
- Misses       1215     1217       +2

Files with missing lines	Coverage Δ
.../include/tesseract_collision/bullet/bullet_utils.h	`100.00% <ø> (ø)`
...ollision/bullet/src/bullet_cast_simple_manager.cpp	`96.72% <100.00%> (ø)`
...sion/bullet/src/bullet_discrete_simple_manager.cpp	`97.19% <100.00%> (ø)`
tesseract_collision/bullet/src/bullet_utils.cpp	`94.07% <100.00%> (+0.11%)`	⬆️
tesseract_collision/fcl/src/fcl_utils.cpp	`89.08% <87.50%> (-0.06%)`	⬇️
...collision/fcl/src/fcl_collision_object_wrapper.cpp	`81.48% <78.57%> (+6.48%)`	⬆️

... and 1 file with indirect coverage changes

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

marrts · 2025-09-11T21:06:57Z

@marrts

I'll try and test this next week and report back with the differences compared to before any of the recent collision updates

rjoomen · 2025-09-12T10:14:34Z

tesseract_collision/bullet/src/bullet_utils.cpp

+      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);


Using no and full threshold instead of half twice would save one set of expansion calculations, but we'd need a getAABB_noexpand() function without expansion.

rjoomen · 2025-09-12T10:16:58Z

tesseract_collision/bullet/src/bullet_utils.cpp

-      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]) &&


Use Bullet function TestAabbAgainstAabb2(min_aabb[0], max_aabb[0], min_aabb[1], max_aabb[1])

rjoomen · 2025-09-12T10:22:40Z

tesseract_collision/bullet/src/bullet_discrete_simple_manager.cpp

+          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]) &&


Use Bullet function TestAabbAgainstAabb2(min_aabb[0], max_aabb[0], min_aabb[1], max_aabb[1])

rjoomen · 2025-09-12T10:22:46Z

tesseract_collision/bullet/src/bullet_cast_simple_manager.cpp

+          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]) &&


Use Bullet function TestAabbAgainstAabb2(min_aabb[0], max_aabb[0], min_aabb[1], max_aabb[1])

rjoomen · 2025-09-12T11:23:52Z

tesseract_collision/bullet/src/bullet_utils.cpp

+    pair.m_algorithm->processCollision(&obj0Wrap, &obj1Wrap, dispatch_info_, &contactPointResult);
+  }
+
+  // Should this return true?


No, it should not return true here. We might return true if needsCollision is false above, see Bullet source, where it mentions that returning true allows removal of the pair.

rjoomen · 2025-09-12T11:32:42Z

tesseract_collision/bullet/src/bullet_utils.cpp

-      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]) &&


We could add an additional check here: if (contact_threshold < (cow0->getContactProcessingThreshold() + cow1->getContactProcessingThreshold()) then do the extra aabb_check.

rjoomen · 2025-09-12T11:34:59Z

tesseract_collision/bullet/src/bullet_utils.cpp

-  {
-    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))


We're basically replicating the broadphase AABB check here, but with the actual margin between the objects. In the scenario where all margins are the same, this would lead to completely superfluous duplicate AABB checking. When margins vary, the cost of duplicate AABB checks is most likely compensated for by the reduced number of narrowphase collide calls. I just looked into the Bullet source, and maybe it is possible to override the original broadphase AABB check. Then we could insert the actual pair margin there, which would be ideal. I don't have time to dive into this right now, but I can look into it next week.

Maybe this callback.

rjoomen · 2025-09-12T11:36:06Z

tesseract_collision/fcl/src/fcl_utils.cpp

+                          (aabb1.min_[2] <= aabb2.max_[2] && aabb1.max_[2] >= aabb2.min_[2]);
+
+  // Check if ABB are overlapping
+  if (!aabb_check)


We can add the same check as suggested above: first check if the sum of the object margins is smaller than the pair margin before doing the extra aabb check.

rjoomen · 2025-09-12T11:36:13Z

tesseract_collision/fcl/src/fcl_utils.cpp

+                          (aabb1.min_[2] <= aabb2.max_[2] && aabb1.max_[2] >= aabb2.min_[2]);
+
+  // Check if ABB are overlapping
+  if (!aabb_check)


We can add the same check as suggested above: first check if the sum of the object margins is smaller than the pair margin before doing the extra aabb check.

marrts · 2025-09-25T20:07:36Z

I know this isn't ready to merge yet, but I ran some tests on my application that go through a bunch of various plans with several unique collision pairs, although none exceed the default collision margin, just reduce it.

On commit 53ad1b4 (before @rjoomen's recent changes): averaged 6 min 43 sec
On current master: averaged 6 min 15 sec
This PR rebased on master: averaged 6 min 23 sec

This was only 2 trials each, but this PR didn't show improvement for me, basically the same as the current master, which did for sure show improvement from the previous version

Levi-Armstrong requested a review from rjoomen September 11, 2025 18:23

Check pair margin AABB before narrow phase check

a30cd7c

Levi-Armstrong force-pushed the feature/check-pair-margin-aabb-before-narrow-phase branch from 838f23d to a30cd7c Compare September 11, 2025 21:23

rjoomen requested changes Sep 12, 2025

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

Check pair margin AABB before narrow phase check #1208

Check pair margin AABB before narrow phase check #1208

Uh oh!

Levi-Armstrong commented Sep 11, 2025 •

edited

Loading

Uh oh!

codecov bot commented Sep 11, 2025

Uh oh!

marrts commented Sep 11, 2025

Uh oh!

rjoomen Sep 12, 2025

Uh oh!

rjoomen Sep 12, 2025

Uh oh!

rjoomen Sep 12, 2025

Uh oh!

rjoomen Sep 12, 2025

Uh oh!

rjoomen Sep 12, 2025

Uh oh!

rjoomen Sep 12, 2025

Uh oh!

rjoomen Sep 12, 2025

Uh oh!

rjoomen Sep 12, 2025

Uh oh!

rjoomen Sep 12, 2025

Uh oh!

rjoomen Sep 12, 2025

Uh oh!

marrts commented Sep 25, 2025

Uh oh!

Uh oh!

Check pair margin AABB before narrow phase check #1208

Are you sure you want to change the base?

Check pair margin AABB before narrow phase check #1208

Uh oh!

Conversation

Levi-Armstrong commented Sep 11, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

codecov bot commented Sep 11, 2025

Codecov Report

Uh oh!

marrts commented Sep 11, 2025

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

marrts commented Sep 25, 2025

Uh oh!

Uh oh!

Levi-Armstrong commented Sep 11, 2025 •

edited

Loading