Fix incorrect group-group collision checking for BulletCollisionDetec…

…tor (#1585)
dartsim · Jul 2, 2021 · e2bd9d1 · e2bd9d1
1 parent 3fbc45d
commit e2bd9d1
Show file tree

Hide file tree

Showing 5 changed files with 40 additions and 31 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -6,6 +6,10 @@
 
   * Added `Mesh`, `TriMesh`, and `Icosphere` classes: [#1579](https://github.com/dartsim/dart/pull/1579)
 
+* Collision
+
+  * Fixed incorrect group-group collision checking for BulletCollisionDetector: [#1585](https://github.com/dartsim/dart/pull/1585), [#717](https://github.com/dartsim/dart/issues/717)
+
 * GUI
 
   * Fixed incorrect MultiSphereConvexHull rendering: [#1579](https://github.com/dartsim/dart/pull/1579)

diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp
@@ -284,24 +284,12 @@ bool BulletCollisionDetector::collide(
   if (!checkGroupValidity(this, group2))
     return false;
 
-  static bool warned = false;
-  if (!warned)
-  {
-    dtwarn
-        << "[BulletCollisionDetector::collide] collide(group1, group2) "
-        << "supposed to check collisions of the objects in group1 against the "
-        << "objects in group2. However, the current implementation of this "
-        << "function checks for all the objects against each other of both "
-        << "group1 and group2, which is an incorrect behavior. This bug will "
-        << "be fixed in the next patch release. (see #717 for the details)\n";
-    warned = true;
-  }
-
+  // Create a new collision group, merging the two groups into
   mGroupForFiltering.reset(new BulletCollisionGroup(shared_from_this()));
   auto bulletCollisionWorld = mGroupForFiltering->getBulletCollisionWorld();
   auto bulletPairCache = bulletCollisionWorld->getPairCache();
-  auto filterCallback
-      = new detail::BulletOverlapFilterCallback(option.collisionFilter);
+  auto filterCallback = new detail::BulletOverlapFilterCallback(
+      option.collisionFilter, group1, group2);
   bulletPairCache->setOverlapFilterCallback(filterCallback);
 
   mGroupForFiltering->addShapeFramesOf(group1, group2);

diff --git a/dart/collision/bullet/detail/BulletOverlapFilterCallback.cpp b/dart/collision/bullet/detail/BulletOverlapFilterCallback.cpp
@@ -33,6 +33,7 @@
 #include "dart/collision/bullet/detail/BulletOverlapFilterCallback.hpp"
 
 #include "dart/collision/CollisionFilter.hpp"
+#include "dart/collision/bullet/BulletCollisionGroup.hpp"
 #include "dart/collision/bullet/BulletCollisionObject.hpp"
 
 namespace dart {
@@ -41,8 +42,14 @@ namespace detail {
 
 //==============================================================================
 BulletOverlapFilterCallback::BulletOverlapFilterCallback(
-    const std::shared_ptr<CollisionFilter>& filter)
-  : foundCollision(false), done(false), filter(filter)
+    const std::shared_ptr<CollisionFilter>& filter,
+    CollisionGroup* group1,
+    CollisionGroup* group2)
+  : foundCollision(false),
+    done(false),
+    filter(filter),
+    group1(group1),
+    group2(group2)
 {
   // Do nothing
 }
@@ -65,7 +72,7 @@ bool BulletOverlapFilterCallback::needBroadphaseCollision(
 
   bool collide = collide1 & collide2;
 
-  if (collide && filter)
+  if (collide)
   {
     auto object0 = static_cast<btCollisionObject*>(proxy0->m_clientObject);
     auto object1 = static_cast<btCollisionObject*>(proxy1->m_clientObject);
@@ -76,7 +83,23 @@ bool BulletOverlapFilterCallback::needBroadphaseCollision(
     const auto collObj0 = static_cast<BulletCollisionObject*>(userPtr0);
     const auto collObj1 = static_cast<BulletCollisionObject*>(userPtr1);
 
-    return !filter->ignoresCollision(collObj0, collObj1);
+    // Filter out if the two ShapeFrames are in the same group
+    if (group1 && group2)
+    {
+      const dynamics::ShapeFrame* shapeFrame0 = collObj0->getShapeFrame();
+      const dynamics::ShapeFrame* shapeFrame1 = collObj1->getShapeFrame();
+      if (group1->hasShapeFrame(shapeFrame0)
+          && group1->hasShapeFrame(shapeFrame1))
+        return false;
+      if (group2->hasShapeFrame(shapeFrame0)
+          && group2->hasShapeFrame(shapeFrame1))
+        return false;
+    }
+
+    if (filter)
+    {
+      return !filter->ignoresCollision(collObj0, collObj1);
+    }
   }
 
   return collide;

diff --git a/dart/collision/bullet/detail/BulletOverlapFilterCallback.hpp b/dart/collision/bullet/detail/BulletOverlapFilterCallback.hpp
@@ -49,7 +49,9 @@ struct BulletOverlapFilterCallback : public btOverlapFilterCallback
 {
   // Constructor
   explicit BulletOverlapFilterCallback(
-      const std::shared_ptr<CollisionFilter>& filter = nullptr);
+      const std::shared_ptr<CollisionFilter>& filter = nullptr,
+      CollisionGroup* group1 = nullptr,
+      CollisionGroup* group2 = nullptr);
 
   /// Returns true when pairs need collision
   bool needBroadphaseCollision(
@@ -63,6 +65,8 @@ struct BulletOverlapFilterCallback : public btOverlapFilterCallback
   mutable bool done;
 
   std::shared_ptr<CollisionFilter> filter;
+  const CollisionGroup* group1;
+  const CollisionGroup* group2;
 };
 
 } // namespace detail

diff --git a/unittests/comprehensive/test_Collision.cpp b/unittests/comprehensive/test_Collision.cpp
@@ -375,17 +375,7 @@ void testSimpleFrames(const std::shared_ptr<CollisionDetector>& cd)
   EXPECT_FALSE(group1->collide(group3.get()));
   EXPECT_TRUE(group2->collide(group3.get()));
   EXPECT_TRUE(group23->collide());
-#if HAVE_BULLET
-  if (cd->getType() == BulletCollisionDetector::getStaticType())
-  {
-    dtwarn << "Skipping group-group test for 'bullet' collision detector. "
-           << "Please see Issue #717 for the detail.\n";
-  }
-  else
-#endif
-  {
-    EXPECT_FALSE(group1->collide(group23.get()));
-  }
+  EXPECT_FALSE(group1->collide(group23.get()));
 }
 
 //==============================================================================