Add blacklist to BodyNodeCollisionFilter

dart/collision/CollisionFilter.cpp

@@ -38,44 +38,164 @@ namespace dart {
 namespace collision {
 
 //==============================================================================
-bool BodyNodeCollisionFilter::needCollision(
+bool CollisionFilter::needCollision(
+    const CollisionObject* object1, const CollisionObject* object2) const
+{
+  return !ignoresCollision(object1, object2);
+}
+
+//==============================================================================
+void CompositeCollisionFilter::addCollisionFilter(const CollisionFilter* filter)
+{
+  if (!filter)
+    return;
+
+  const auto result = std::find(mFilters.begin(), mFilters.end(), filter);
+  const bool found = (result != mFilters.end());
+  if (found)
+    return;
+
+  mFilters.insert(filter);
+}
+
+//==============================================================================
+void CompositeCollisionFilter::removeCollisionFilter(
+    const CollisionFilter* filter)
+{
+  mFilters.erase(filter);
+}
+
+//==============================================================================
+void CompositeCollisionFilter::removeAllCollisionFilters()
+{
+  mFilters.clear();
+}
+
+//==============================================================================
+bool CompositeCollisionFilter::ignoresCollision(
+    const CollisionObject* object1, const CollisionObject* object2) const
+{
+  for (const auto* filter : mFilters)
+  {
+    const bool collisionIgnored = filter->ignoresCollision(object1, object2);
+    if (collisionIgnored)
+      return true;
+  }
+
+  return false;
+}
+
+//==============================================================================
+void BodyNodeCollisionFilter::addBodyNodePairToBlackList(
+    const dynamics::BodyNode* bodyNode1, const dynamics::BodyNode* bodyNode2)
+{
+  if (!bodyNode1 || !bodyNode2)
+    return;
+
+  const auto* bodyNodeLess = bodyNode1;
+  const auto* bodyNodeGreater = bodyNode2;
+
+  if (bodyNodeLess > bodyNodeGreater)
+    std::swap(bodyNodeLess, bodyNodeGreater);
+
+  // Call insert in case an entry for bodyNodeLess doesn't exist. If it doesn't
+  // exist, it will be initialized with an empty set. If it does already exist,
+  // we will just get an iterator to the existing entry.
+  const auto itLess = mBlackList.insert(
+      std::make_pair(bodyNodeLess,
+                     std::unordered_set<const dynamics::BodyNode*>())).first;
+
+  // Insert bodyNodeGreater into the set corresponding to bodyNodeLess. If the
+  // pair already existed, this will do nothing.
+  itLess->second.insert(bodyNodeGreater);
+}
+
+//==============================================================================
+void BodyNodeCollisionFilter::removeBodyNodePairFromBlackList(
+    const dynamics::BodyNode* bodyNode1, const dynamics::BodyNode* bodyNode2)
+{
+  if (!bodyNode1 || !bodyNode2)
+    return;
+
+  const auto* bodyNodeLess = bodyNode1;
+  const auto* bodyNodeGreater = bodyNode2;
+
+  if (bodyNodeLess > bodyNodeGreater)
+    std::swap(bodyNodeLess, bodyNodeGreater);
+
+  // Remove the pair only when it already exists
+  const auto resultLeft = mBlackList.find(bodyNodeLess);
+  const bool foundLeft = (resultLeft != mBlackList.end());
+  if (foundLeft)
+  {
+    auto& associatedRights = resultLeft->second;
+    associatedRights.erase(bodyNodeGreater);
+
+    if (associatedRights.empty())
+      mBlackList.erase(resultLeft);
+  }
+}
+
+//==============================================================================
+void BodyNodeCollisionFilter::removeAllBodyNodePairsFromBlackList()
+{
+  mBlackList.clear();
+}
+
+//==============================================================================
+bool BodyNodeCollisionFilter::ignoresCollision(
     const collision::CollisionObject* object1,
     const collision::CollisionObject* object2) const
 {
   if (object1 == object2)
-    return false;
+    return true;
 
   auto shapeNode1 = object1->getShapeFrame()->asShapeNode();
   auto shapeNode2 = object2->getShapeFrame()->asShapeNode();
 
-  if (!shapeNode1 || !shapeNode2)
-    return true;
-  // We assume that non-ShapeNode is always being checked collision.
+  if (!shapeNode1)
+  {
+    dterr << "[BodyNodeCollisionFilter::ignoresCollision] Inappropriate "
+          << "collision filter is used for shapeNode1. This collision filter "
+          << "shouldn't be for non-ShapeNodes.\n";
+    assert(false);
+  }
+
+  if (!shapeNode2)
+  {
+    dterr << "[BodyNodeCollisionFilter::ignoresCollision] Inappropriate "
+          << "collision filter is used for shapeNode2. This collision filter "
+          << "shouldn't be for non-ShapeNodes.\n";
+    assert(false);
+  }
 
   auto bodyNode1 = shapeNode1->getBodyNodePtr();
   auto bodyNode2 = shapeNode2->getBodyNodePtr();
 
   if (bodyNode1 == bodyNode2)
-    return false;
+    return true;
 
   if (!bodyNode1->isCollidable() || !bodyNode2->isCollidable())
-    return false;
+    return true;
 
   if (bodyNode1->getSkeleton() == bodyNode2->getSkeleton())
   {
     auto skeleton = bodyNode1->getSkeleton();
 
     if (!skeleton->isEnabledSelfCollisionCheck())
-      return false;
+      return true;
 
     if (!skeleton->isEnabledAdjacentBodyCheck())
     {
       if (areAdjacentBodies(bodyNode1, bodyNode2))
-        return false;
+        return true;
     }
   }
 
-  return true;
+  if (hasBodyNodePairInBlacklist(bodyNode1, bodyNode2))
+    return true;
+
+  return false;
 }
 
 //==============================================================================
@@ -93,5 +213,31 @@ bool BodyNodeCollisionFilter::areAdjacentBodies(
   return false;
 }
 
-}  // namespace collision
-}  // namespace dart
+//==============================================================================
+bool BodyNodeCollisionFilter::hasBodyNodePairInBlacklist(
+    const dynamics::BodyNode* bodyNode1,
+    const dynamics::BodyNode* bodyNode2) const
+{
+  const auto* bodyNodeLess = bodyNode1;
+  const auto* bodyNodeGreater = bodyNode2;
+
+  if (bodyNodeLess > bodyNodeGreater)
+    std::swap(bodyNodeLess, bodyNodeGreater);
+
+  const auto resultLeft = mBlackList.find(bodyNodeLess);
+  const bool foundLeft = (resultLeft != mBlackList.end());
+  if (foundLeft)
+  {
+    auto& key = resultLeft->second;
+
+    const auto resultRight = key.find(bodyNodeGreater);
+    const bool foundRight = (resultRight != key.end());
+    if (foundRight)
+      return true;
+  }
+
+  return false;
+}
+
+} // namespace collision
+} // namespace dart

dart/collision/CollisionFilter.hpp

@@ -32,6 +32,11 @@
 #ifndef DART_COLLISION_COLLISIONFILTER_HPP_
 #define DART_COLLISION_COLLISIONFILTER_HPP_
 
+#include <unordered_map>
+#include <unordered_set>
+
+#include "dart/common/Deprecated.hpp"
+
 namespace dart {
 
 namespace dynamics {
@@ -42,22 +47,88 @@ namespace collision {
 
 class CollisionObject;
 
-struct CollisionFilter
+class CollisionFilter
 {
-  virtual bool needCollision(const CollisionObject* object1,
-                             const CollisionObject* object2) const = 0;
+public:
+  /// Returns true if the given two CollisionObjects should be checked by the
+  /// collision detector, false otherwise.
+  /// \deprecated Deprecated in 6.3.0. Please use ignoreCollision instead. Note
+  /// that ignoreCollision returns logically opposite to what needCollision
+  /// returns.
+  DART_DEPRECATED(6.3)
+  bool needCollision(
+      const CollisionObject* object1, const CollisionObject* object2) const;
+
+  /// Returns true if the given two CollisionObjects should be checked by the
+  /// collision detector, false otherwise.
+  virtual bool ignoresCollision(
+      const CollisionObject* object1, const CollisionObject* object2) const = 0;
 };
 
-struct BodyNodeCollisionFilter : CollisionFilter
+class CompositeCollisionFilter : public CollisionFilter
 {
-  bool needCollision(const CollisionObject* object1,
-                     const CollisionObject* object2) const override;
+public:
+  /// Adds a collision filter to this CompositeCollisionFilter.
+  void addCollisionFilter(const CollisionFilter* filter);
+
+  /// Removes a collision filter from this CompositeCollisionFilter.
+  void removeCollisionFilter(const CollisionFilter* filter);
+
+  /// Removes all the collision filters from this CompositeCollisionFilter.
+  void removeAllCollisionFilters();
+
+  // Documentation inherited
+  bool ignoresCollision(
+      const CollisionObject* object1,
+      const CollisionObject* object2) const override;
+
+protected:
+  /// Collision filters
+  std::unordered_set<const CollisionFilter*> mFilters;
+};
 
+class BodyNodeCollisionFilter : public CollisionFilter
+{
+public:
+  /// Add a BodyNode pair to the blacklist.
+  void addBodyNodePairToBlackList(
+      const dynamics::BodyNode* bodyNode1,
+      const dynamics::BodyNode* bodyNode2);
+
+  /// Remove a BodyNode pair from the blacklist.
+  void removeBodyNodePairFromBlackList(
+      const dynamics::BodyNode* bodyNode1,
+      const dynamics::BodyNode* bodyNode2);
+
+  /// Remove all the BodyNode pairs from the blacklist.
+  void removeAllBodyNodePairsFromBlackList();
+
+  // Documentation inherited
+  bool ignoresCollision(
+      const CollisionObject* object1,
+      const CollisionObject* object2) const override;
+
+private:
+  /// Returns true if the two BodyNodes are adjacent BodyNodes (i.e., the two
+  /// BodyNodes are connected by a Joint).
   bool areAdjacentBodies(const dynamics::BodyNode* bodyNode1,
                          const dynamics::BodyNode* bodyNode2) const;
+
+  /// Returns true if the BodyNode pair is in the blacklist.
+  bool hasBodyNodePairInBlacklist(
+      const dynamics::BodyNode* bodyNode1,
+      const dynamics::BodyNode* bodyNode2) const;
+
+  /// List of pairs to be excluded in the collision detection.
+  ///
+  /// Each pair is stored so that the key of the std::unordered_map always has
+  /// a value less than every element in the set that is associated with it.
+  std::unordered_map<
+      const dynamics::BodyNode*,
+      std::unordered_set<const dynamics::BodyNode*>> mBlackList;
 };
 
-}  // namespace collision
-}  // namespace dart
+} // namespace collision
+} // namespace dart
 
 #endif  // DART_COLLISION_COLLISIONFILTER_HPP_

dart/collision/bullet/BulletCollisionDetector.cpp

@@ -195,7 +195,7 @@ void filterOutCollisions(btCollisionWorld* world)
     const auto collObj0 = static_cast<BulletCollisionObject*>(userPtr0);
     const auto collObj1 = static_cast<BulletCollisionObject*>(userPtr1);
 
-    if (!filter->needCollision(collObj0, collObj1))
+    if (filter->ignoresCollision(collObj0, collObj1))
       manifoldsToRelease.push_back(contactManifold);
   }
 

dart/collision/bullet/detail/BulletCollisionDispatcher.cpp

@@ -78,7 +78,7 @@ bool BulletCollisionDispatcher::needsCollision(
   const auto collObj1
       = static_cast<BulletCollisionObject*>(body1->getUserPointer());
 
-  if (mFilter && !mFilter->needCollision(collObj0, collObj1))
+  if (mFilter && mFilter->ignoresCollision(collObj0, collObj1))
     return false;
 
   return btCollisionDispatcher::needsCollision(body0, body1);

dart/collision/bullet/detail/BulletOverlapFilterCallback.cpp

@@ -75,7 +75,7 @@ bool BulletOverlapFilterCallback::needBroadphaseCollision(
     const auto collObj0 = static_cast<BulletCollisionObject*>(userPtr0);
     const auto collObj1 = static_cast<BulletCollisionObject*>(userPtr1);
 
-    return filter->needCollision(collObj0, collObj1);
+    return !filter->ignoresCollision(collObj0, collObj1);
   }
 
   return collide;

dart/collision/dart/DARTCollisionDetector.cpp

@@ -145,7 +145,7 @@ bool DARTCollisionDetector::collide(
     {
       auto* collObj2 = objects[j];
 
-      if (filter && !filter->needCollision(collObj1, collObj2))
+      if (filter && filter->ignoresCollision(collObj1, collObj2))
         continue;
 
       collisionFound = checkPair(collObj1, collObj2, option, result);
@@ -207,7 +207,7 @@ bool DARTCollisionDetector::collide(
     {
       auto* collObj2 = objects2[j];
 
-      if (filter && !filter->needCollision(collObj1, collObj2))
+      if (filter && filter->ignoresCollision(collObj1, collObj2))
         continue;
 
       collisionFound = checkPair(collObj1, collObj2, option, result);

dart/collision/fcl/FCLCollisionDetector.cpp

@@ -1099,7 +1099,7 @@ bool collisionCallback(
     assert(collisionObject1);
     assert(collisionObject2);
 
-    if (!filter->needCollision(collisionObject2, collisionObject1))
+    if (filter->ignoresCollision(collisionObject2, collisionObject1))
       return collData->done;
   }
 

dart/collision/ode/OdeCollisionDetector.cpp

@@ -267,7 +267,7 @@ void CollisionCallback(void* data, dGeomID o1, dGeomID o2)
   assert(collObj1);
   assert(collObj2);
 
-  if (filter && !filter->needCollision(collObj1, collObj2))
+  if (filter && filter->ignoresCollision(collObj1, collObj2))
       return;
 
   // Perform narrow-phase collision detection

dart/constraint/ConstraintSolver.cpp

@@ -259,6 +259,18 @@ collision::ConstCollisionGroupPtr ConstraintSolver::getCollisionGroup() const
   return mCollisionGroup;
 }
 
+//==============================================================================
+collision::CollisionOption& ConstraintSolver::getCollisionOption()
+{
+  return mCollisionOption;
+}
+
+//==============================================================================
+const collision::CollisionOption& ConstraintSolver::getCollisionOption() const
+{
+  return mCollisionOption;
+}
+
 //==============================================================================
 collision::CollisionResult& ConstraintSolver::getLastCollisionResult()
 {