From af9d36e8331e7e994319a4adc5b6e6e1a97cbc93 Mon Sep 17 00:00:00 2001
From: Jeongseok Lee <jslee02@gmail.com>
Date: Mon, 28 Aug 2017 16:37:47 +0900
Subject: [PATCH 01/19] Add blacklist to BodyNodeCollisionFilter

---
 dart/collision/CollisionFilter.cpp            | 153 +++++++++++++++++-
 dart/collision/CollisionFilter.hpp            |  87 +++++++++-
 .../bullet/BulletCollisionDetector.cpp        |   2 +-
 .../detail/BulletCollisionDispatcher.cpp      |   2 +-
 .../detail/BulletOverlapFilterCallback.cpp    |   2 +-
 dart/collision/dart/DARTCollisionDetector.cpp |   4 +-
 dart/collision/fcl/FCLCollisionDetector.cpp   |   2 +-
 dart/collision/ode/OdeCollisionDetector.cpp   |   2 +-
 dart/constraint/ConstraintSolver.cpp          |  12 ++
 dart/constraint/ConstraintSolver.hpp          |  11 +-
 unittests/comprehensive/test_Collision.cpp    |   8 +-
 11 files changed, 263 insertions(+), 22 deletions(-)

diff --git a/dart/collision/CollisionFilter.cpp b/dart/collision/CollisionFilter.cpp
index 1732727c4df6b..097c4b2dfc64a 100644
--- a/dart/collision/CollisionFilter.cpp
+++ b/dart/collision/CollisionFilter.cpp
@@ -38,7 +38,125 @@ namespace dart {
 namespace collision {
 
 //==============================================================================
-bool BodyNodeCollisionFilter::needCollision(
+bool CollisionFilter::needCollision(
+    const CollisionObject* object1, const CollisionObject* object2) const
+{
+  return needsCollisionCheck(object1, object2);
+}
+
+//==============================================================================
+void CompositeCollisionFilter::addCollisionFilter(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.push_back(filter);
+}
+
+//==============================================================================
+void CompositeCollisionFilter::removeCollisionFilter(
+    const CollisionFilter* filter)
+{
+  mFilters.erase(
+        std::remove(mFilters.begin(), mFilters.end(), filter), mFilters.end());
+}
+
+//==============================================================================
+void CompositeCollisionFilter::removeAllCollisionFilters()
+{
+  mFilters.clear();
+}
+
+//==============================================================================
+bool CompositeCollisionFilter::needsCollisionCheck(
+    const CollisionObject* object1, const CollisionObject* object2) const
+{
+  for (const auto* filter : mFilters)
+  {
+    const bool needCollision = filter->needsCollisionCheck(object1, object2);
+    if (!needCollision)
+      return false;
+  }
+
+  return true;
+}
+
+//==============================================================================
+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);
+
+  // Add the pair only when it doesn't already exist
+  const auto resultLeft = mBlackList.find(bodyNodeLess);
+  const bool foundLeft = (resultLeft != mBlackList.end());
+  if (!foundLeft)
+  {
+    mBlackList[bodyNodeLess] = {bodyNodeGreater};
+  }
+  else
+  {
+    auto& key = resultLeft->second;
+
+    const auto resultRight = key.find(bodyNodeGreater);
+    const bool foundRight = (resultRight != key.end());
+    if (!foundRight)
+      key.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& key = resultLeft->second;
+
+    const auto resultRight = key.find(bodyNodeGreater);
+    const bool foundRight = (resultRight != key.end());
+    if (foundRight)
+    {
+      key.erase(bodyNodeGreater);
+
+      if (key.empty())
+        mBlackList.erase(resultLeft);
+    }
+  }
+}
+
+//==============================================================================
+void BodyNodeCollisionFilter::removeAllBodyNodePairsFromBlackList()
+{
+  mBlackList.clear();
+}
+
+//==============================================================================
+bool BodyNodeCollisionFilter::needsCollisionCheck(
     const collision::CollisionObject* object1,
     const collision::CollisionObject* object2) const
 {
@@ -75,6 +193,9 @@ bool BodyNodeCollisionFilter::needCollision(
     }
   }
 
+  if (existsBodyNodePairInBlacklist(bodyNode1, bodyNode2))
+    return false;
+
   return true;
 }
 
@@ -93,5 +214,31 @@ bool BodyNodeCollisionFilter::areAdjacentBodies(
   return false;
 }
 
-}  // namespace collision
-}  // namespace dart
+//==============================================================================
+bool BodyNodeCollisionFilter::existsBodyNodePairInBlacklist(
+    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
diff --git a/dart/collision/CollisionFilter.hpp b/dart/collision/CollisionFilter.hpp
index a507d23e13df2..dc6cc64fda8d3 100644
--- a/dart/collision/CollisionFilter.hpp
+++ b/dart/collision/CollisionFilter.hpp
@@ -32,6 +32,12 @@
 #ifndef DART_COLLISION_COLLISIONFILTER_HPP_
 #define DART_COLLISION_COLLISIONFILTER_HPP_
 
+#include <set>
+#include <unordered_map>
+#include <vector>
+
+#include "dart/common/Deprecated.hpp"
+
 namespace dart {
 
 namespace dynamics {
@@ -42,22 +48,87 @@ 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 needsCollisionCheck instead.
+  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 needsCollisionCheck(
+      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(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 needsCollisionCheck(
+      const CollisionObject* object1,
+      const CollisionObject* object2) const override;
+
+protected:
+  /// Collision filters
+  std::vector<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 needsCollisionCheck(
+      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 existsBodyNodePairInBlacklist(
+      const dynamics::BodyNode* bodyNode1,
+      const dynamics::BodyNode* bodyNode2) const;
+
+  /// List of pairs to be excluded in the collision detection.
+  ///
+  /// Each pair is stored as which the key of the std::unordered_map is not
+  /// always greater than any of elements of the associated value that is a
+  /// std::set.
+  std::unordered_map<
+      const dynamics::BodyNode*,
+      std::set<const dynamics::BodyNode*>> mBlackList;
 };
 
-}  // namespace collision
-}  // namespace dart
+} // namespace collision
+} // namespace dart
 
 #endif  // DART_COLLISION_COLLISIONFILTER_HPP_
diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp
index 0a8fb66e650eb..ca16bb6bd4cd0 100644
--- a/dart/collision/bullet/BulletCollisionDetector.cpp
+++ b/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->needsCollisionCheck(collObj0, collObj1))
       manifoldsToRelease.push_back(contactManifold);
   }
 
diff --git a/dart/collision/bullet/detail/BulletCollisionDispatcher.cpp b/dart/collision/bullet/detail/BulletCollisionDispatcher.cpp
index dc9c66f66cf1a..8985959be3a35 100644
--- a/dart/collision/bullet/detail/BulletCollisionDispatcher.cpp
+++ b/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->needsCollisionCheck(collObj0, collObj1))
     return false;
 
   return btCollisionDispatcher::needsCollision(body0, body1);
diff --git a/dart/collision/bullet/detail/BulletOverlapFilterCallback.cpp b/dart/collision/bullet/detail/BulletOverlapFilterCallback.cpp
index 4ed67f7d76e42..6f879d4253c9d 100644
--- a/dart/collision/bullet/detail/BulletOverlapFilterCallback.cpp
+++ b/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->needsCollisionCheck(collObj0, collObj1);
   }
 
   return collide;
diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp
index 268157a959859..fc39596fe60c8 100644
--- a/dart/collision/dart/DARTCollisionDetector.cpp
+++ b/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->needsCollisionCheck(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->needsCollisionCheck(collObj1, collObj2))
         continue;
 
       collisionFound = checkPair(collObj1, collObj2, option, result);
diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp
index 67000e7f671b8..cd6f116abaa6f 100644
--- a/dart/collision/fcl/FCLCollisionDetector.cpp
+++ b/dart/collision/fcl/FCLCollisionDetector.cpp
@@ -1099,7 +1099,7 @@ bool collisionCallback(
     assert(collisionObject1);
     assert(collisionObject2);
 
-    if (!filter->needCollision(collisionObject2, collisionObject1))
+    if (!filter->needsCollisionCheck(collisionObject2, collisionObject1))
       return collData->done;
   }
 
diff --git a/dart/collision/ode/OdeCollisionDetector.cpp b/dart/collision/ode/OdeCollisionDetector.cpp
index 379c8f6452ed5..3debd389b6e7d 100644
--- a/dart/collision/ode/OdeCollisionDetector.cpp
+++ b/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->needsCollisionCheck(collObj1, collObj2))
       return;
 
   // Perform narrow-phase collision detection
diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp
index 04079739790db..76d5b5b6b776b 100644
--- a/dart/constraint/ConstraintSolver.cpp
+++ b/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()
 {
diff --git a/dart/constraint/ConstraintSolver.hpp b/dart/constraint/ConstraintSolver.hpp
index c41f2e7e49940..0ee970a35965c 100644
--- a/dart/constraint/ConstraintSolver.hpp
+++ b/dart/constraint/ConstraintSolver.hpp
@@ -50,9 +50,6 @@ class ShapeNodeCollisionObject;
 
 namespace constraint {
 
-// TODO:
-//   - RootSkeleton concept
-
 /// ConstraintSolver manages constraints and computes constraint impulses
 class ConstraintSolver
 {
@@ -125,6 +122,14 @@ class ConstraintSolver
   /// ConstraintSolver
   collision::ConstCollisionGroupPtr getCollisionGroup() const;
 
+  /// Returns collision option that is used for collision checkings in this
+  /// ConstraintSolver to generate contact constraints.
+  collision::CollisionOption& getCollisionOption();
+
+  /// Returns collision option that is used for collision checkings in this
+  /// ConstraintSolver to generate contact constraints.
+  const collision::CollisionOption& getCollisionOption() const;
+
   /// Return the last collision checking result
   collision::CollisionResult& getLastCollisionResult();
 
diff --git a/unittests/comprehensive/test_Collision.cpp b/unittests/comprehensive/test_Collision.cpp
index acbee7afa4c17..17a769a9bdb40 100644
--- a/unittests/comprehensive/test_Collision.cpp
+++ b/unittests/comprehensive/test_Collision.cpp
@@ -1164,7 +1164,8 @@ void testFilter(const std::shared_ptr<CollisionDetector>& cd)
 
   // Default collision filter for Skeleton
   CollisionOption option;
-  option.collisionFilter = std::make_shared<BodyNodeCollisionFilter>();
+  auto bodyNodeFilter = std::make_shared<BodyNodeCollisionFilter>();
+  option.collisionFilter = bodyNodeFilter;
 
   skel->enableSelfCollisionCheck();
   skel->enableAdjacentBodyCheck();
@@ -1193,6 +1194,11 @@ void testFilter(const std::shared_ptr<CollisionDetector>& cd)
   EXPECT_FALSE(skel->isEnabledAdjacentBodyCheck());
   EXPECT_TRUE(group->collide());
   EXPECT_FALSE(group->collide(option));
+
+  skel->enableSelfCollisionCheck();
+  skel->enableAdjacentBodyCheck();
+  bodyNodeFilter->addBodyNodePairToBlackList(body0, body1);
+  EXPECT_FALSE(group->collide(option));
 }
 
 //==============================================================================

From a3f01ff256565c815aecb4d02cb7a9137352864a Mon Sep 17 00:00:00 2001
From: Jeongseok Lee <jslee02@gmail.com>
Date: Mon, 28 Aug 2017 16:40:40 +0900
Subject: [PATCH 02/19] Add more test for blacklist of collision filter

---
 unittests/comprehensive/test_Collision.cpp | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/unittests/comprehensive/test_Collision.cpp b/unittests/comprehensive/test_Collision.cpp
index 17a769a9bdb40..8e2f82d1dd082 100644
--- a/unittests/comprehensive/test_Collision.cpp
+++ b/unittests/comprehensive/test_Collision.cpp
@@ -1195,10 +1195,17 @@ void testFilter(const std::shared_ptr<CollisionDetector>& cd)
   EXPECT_TRUE(group->collide());
   EXPECT_FALSE(group->collide(option));
 
+  // Test blacklist
   skel->enableSelfCollisionCheck();
   skel->enableAdjacentBodyCheck();
   bodyNodeFilter->addBodyNodePairToBlackList(body0, body1);
   EXPECT_FALSE(group->collide(option));
+  bodyNodeFilter->removeBodyNodePairFromBlackList(body0, body1);
+  EXPECT_TRUE(group->collide(option));
+  bodyNodeFilter->addBodyNodePairToBlackList(body0, body1);
+  EXPECT_FALSE(group->collide(option));
+  bodyNodeFilter->removeAllBodyNodePairsFromBlackList();
+  EXPECT_TRUE(group->collide(option));
 }
 
 //==============================================================================

From 43820d54383a7d77f98b3dbb40e98da1af924b94 Mon Sep 17 00:00:00 2001
From: Jeongseok Lee <jslee02@gmail.com>
Date: Mon, 28 Aug 2017 17:47:56 +0900
Subject: [PATCH 03/19] Update collision filter test to use world

---
 unittests/comprehensive/test_Collision.cpp | 13 ++++++++++---
 1 file changed, 10 insertions(+), 3 deletions(-)

diff --git a/unittests/comprehensive/test_Collision.cpp b/unittests/comprehensive/test_Collision.cpp
index 8e2f82d1dd082..3e5b75d1f9045 100644
--- a/unittests/comprehensive/test_Collision.cpp
+++ b/unittests/comprehensive/test_Collision.cpp
@@ -1158,12 +1158,19 @@ void testFilter(const std::shared_ptr<CollisionDetector>& cd)
   auto* body1 = pair1.second;
   body1->createShapeNodeWith<VisualAspect, CollisionAspect>(shape);
 
-  // Create a collision group from the skeleton
-  auto group = cd->createCollisionGroup(skel.get());
+  // Create a world and add the created skeleton
+  auto world = std::make_shared<simulation::World>();
+  world->addSkeleton(skel);
+
+  // Get the constraint solver from the world
+  auto constraintSolver = world->getConstraintSolver();
+
+  // Get the collision group from the constraint solver
+  auto group = constraintSolver->getCollisionGroup();
   EXPECT_EQ(group->getNumShapeFrames(), 2u);
 
   // Default collision filter for Skeleton
-  CollisionOption option;
+  auto& option = constraintSolver->getCollisionOption();
   auto bodyNodeFilter = std::make_shared<BodyNodeCollisionFilter>();
   option.collisionFilter = bodyNodeFilter;
 

From 68373c6e5d6644e556d00af8c607be1bd20f2d7f Mon Sep 17 00:00:00 2001
From: Jeongseok Lee <jslee02@gmail.com>
Date: Tue, 29 Aug 2017 08:05:43 +0900
Subject: [PATCH 04/19] Fix collision filter test to use the given collision
 detector

---
 unittests/comprehensive/test_Collision.cpp | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/unittests/comprehensive/test_Collision.cpp b/unittests/comprehensive/test_Collision.cpp
index 3e5b75d1f9045..5433b132a27d1 100644
--- a/unittests/comprehensive/test_Collision.cpp
+++ b/unittests/comprehensive/test_Collision.cpp
@@ -1160,10 +1160,9 @@ void testFilter(const std::shared_ptr<CollisionDetector>& cd)
 
   // Create a world and add the created skeleton
   auto world = std::make_shared<simulation::World>();
-  world->addSkeleton(skel);
-
-  // Get the constraint solver from the world
   auto constraintSolver = world->getConstraintSolver();
+  constraintSolver->setCollisionDetector(cd);
+  world->addSkeleton(skel);
 
   // Get the collision group from the constraint solver
   auto group = constraintSolver->getCollisionGroup();

From 305f47ee7bc858c530a9d766423a488b1b766ca4 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@osrfoundation.org>
Date: Tue, 29 Aug 2017 14:43:59 -0700
Subject: [PATCH 05/19] Using insert instead of manual logic

---
 dart/collision/CollisionFilter.cpp | 26 ++++++++++----------------
 1 file changed, 10 insertions(+), 16 deletions(-)

diff --git a/dart/collision/CollisionFilter.cpp b/dart/collision/CollisionFilter.cpp
index 097c4b2dfc64a..14e6d39a8ce1b 100644
--- a/dart/collision/CollisionFilter.cpp
+++ b/dart/collision/CollisionFilter.cpp
@@ -99,22 +99,16 @@ void BodyNodeCollisionFilter::addBodyNodePairToBlackList(
   if (bodyNodeLess > bodyNodeGreater)
     std::swap(bodyNodeLess, bodyNodeGreater);
 
-  // Add the pair only when it doesn't already exist
-  const auto resultLeft = mBlackList.find(bodyNodeLess);
-  const bool foundLeft = (resultLeft != mBlackList.end());
-  if (!foundLeft)
-  {
-    mBlackList[bodyNodeLess] = {bodyNodeGreater};
-  }
-  else
-  {
-    auto& key = resultLeft->second;
-
-    const auto resultRight = key.find(bodyNodeGreater);
-    const bool foundRight = (resultRight != key.end());
-    if (!foundRight)
-      key.insert(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 it_less = mBlackList.insert(
+        std::make_pair(bodyNodeLess,
+                       std::set<const dynamics::BodyNode*>())).first;
+
+  // Insert bodyNodeGreater into the set corresponding to bodyNodeLess. If the
+  // pair already existed, this will do nothing.
+  it_less->second.insert(bodyNodeGreater);
 }
 
 //==============================================================================

From 2418f76a55ddbe60e6ccc0bb4b317a7033cb2b54 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@osrfoundation.org>
Date: Tue, 29 Aug 2017 16:17:47 -0700
Subject: [PATCH 06/19] Language tweaks in comments

---
 dart/collision/CollisionFilter.cpp | 2 +-
 dart/collision/CollisionFilter.hpp | 5 ++---
 2 files changed, 3 insertions(+), 4 deletions(-)

diff --git a/dart/collision/CollisionFilter.cpp b/dart/collision/CollisionFilter.cpp
index 14e6d39a8ce1b..c51816ff18467 100644
--- a/dart/collision/CollisionFilter.cpp
+++ b/dart/collision/CollisionFilter.cpp
@@ -162,7 +162,7 @@ bool BodyNodeCollisionFilter::needsCollisionCheck(
 
   if (!shapeNode1 || !shapeNode2)
     return true;
-  // We assume that non-ShapeNode is always being checked collision.
+  // We assume that non-ShapeNodes are always being checked for collisions.
 
   auto bodyNode1 = shapeNode1->getBodyNodePtr();
   auto bodyNode2 = shapeNode2->getBodyNodePtr();
diff --git a/dart/collision/CollisionFilter.hpp b/dart/collision/CollisionFilter.hpp
index dc6cc64fda8d3..12afea52d8c97 100644
--- a/dart/collision/CollisionFilter.hpp
+++ b/dart/collision/CollisionFilter.hpp
@@ -120,9 +120,8 @@ class BodyNodeCollisionFilter : public CollisionFilter
 
   /// List of pairs to be excluded in the collision detection.
   ///
-  /// Each pair is stored as which the key of the std::unordered_map is not
-  /// always greater than any of elements of the associated value that is a
-  /// std::set.
+  /// 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::set<const dynamics::BodyNode*>> mBlackList;

From 33f46faf95ae40e6cb509c4254a69904b8cc0ed9 Mon Sep 17 00:00:00 2001
From: Jeongseok Lee <jslee02@gmail.com>
Date: Wed, 30 Aug 2017 21:45:47 +0900
Subject: [PATCH 07/19] Rename needsCollisionCheck to ignoreCollision per this
 comment: https://github.com/dartsim/dart/pull/911#issuecomment-325805602

---
 dart/collision/CollisionFilter.cpp            | 30 +++++++++----------
 dart/collision/CollisionFilter.hpp            | 10 ++++---
 .../bullet/BulletCollisionDetector.cpp        |  2 +-
 .../detail/BulletCollisionDispatcher.cpp      |  2 +-
 .../detail/BulletOverlapFilterCallback.cpp    |  2 +-
 dart/collision/dart/DARTCollisionDetector.cpp |  4 +--
 dart/collision/fcl/FCLCollisionDetector.cpp   |  2 +-
 dart/collision/ode/OdeCollisionDetector.cpp   |  2 +-
 8 files changed, 28 insertions(+), 26 deletions(-)

diff --git a/dart/collision/CollisionFilter.cpp b/dart/collision/CollisionFilter.cpp
index c51816ff18467..320a554ea00ca 100644
--- a/dart/collision/CollisionFilter.cpp
+++ b/dart/collision/CollisionFilter.cpp
@@ -41,7 +41,7 @@ namespace collision {
 bool CollisionFilter::needCollision(
     const CollisionObject* object1, const CollisionObject* object2) const
 {
-  return needsCollisionCheck(object1, object2);
+  return !ignoresCollision(object1, object2);
 }
 
 //==============================================================================
@@ -73,17 +73,17 @@ void CompositeCollisionFilter::removeAllCollisionFilters()
 }
 
 //==============================================================================
-bool CompositeCollisionFilter::needsCollisionCheck(
+bool CompositeCollisionFilter::ignoresCollision(
     const CollisionObject* object1, const CollisionObject* object2) const
 {
   for (const auto* filter : mFilters)
   {
-    const bool needCollision = filter->needsCollisionCheck(object1, object2);
-    if (!needCollision)
-      return false;
+    const bool collisionIgnored = filter->ignoresCollision(object1, object2);
+    if (collisionIgnored)
+      return true;
   }
 
-  return true;
+  return false;
 }
 
 //==============================================================================
@@ -150,47 +150,47 @@ void BodyNodeCollisionFilter::removeAllBodyNodePairsFromBlackList()
 }
 
 //==============================================================================
-bool BodyNodeCollisionFilter::needsCollisionCheck(
+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;
+    return false;
   // We assume that non-ShapeNodes are always being checked for collisions.
 
   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;
     }
   }
 
   if (existsBodyNodePairInBlacklist(bodyNode1, bodyNode2))
-    return false;
+    return true;
 
-  return true;
+  return false;
 }
 
 //==============================================================================
diff --git a/dart/collision/CollisionFilter.hpp b/dart/collision/CollisionFilter.hpp
index 12afea52d8c97..dac1a0e4c34fe 100644
--- a/dart/collision/CollisionFilter.hpp
+++ b/dart/collision/CollisionFilter.hpp
@@ -53,14 +53,16 @@ class CollisionFilter
 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 needsCollisionCheck instead.
+  /// \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 needsCollisionCheck(
+  virtual bool ignoresCollision(
       const CollisionObject* object1, const CollisionObject* object2) const = 0;
 };
 
@@ -77,7 +79,7 @@ class CompositeCollisionFilter : public CollisionFilter
   void removeAllCollisionFilters();
 
   // Documentation inherited
-  bool needsCollisionCheck(
+  bool ignoresCollision(
       const CollisionObject* object1,
       const CollisionObject* object2) const override;
 
@@ -103,7 +105,7 @@ class BodyNodeCollisionFilter : public CollisionFilter
   void removeAllBodyNodePairsFromBlackList();
 
   // Documentation inherited
-  bool needsCollisionCheck(
+  bool ignoresCollision(
       const CollisionObject* object1,
       const CollisionObject* object2) const override;
 
diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp
index ca16bb6bd4cd0..eb182f8149691 100644
--- a/dart/collision/bullet/BulletCollisionDetector.cpp
+++ b/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->needsCollisionCheck(collObj0, collObj1))
+    if (filter->ignoresCollision(collObj0, collObj1))
       manifoldsToRelease.push_back(contactManifold);
   }
 
diff --git a/dart/collision/bullet/detail/BulletCollisionDispatcher.cpp b/dart/collision/bullet/detail/BulletCollisionDispatcher.cpp
index 8985959be3a35..c3227ca3a1482 100644
--- a/dart/collision/bullet/detail/BulletCollisionDispatcher.cpp
+++ b/dart/collision/bullet/detail/BulletCollisionDispatcher.cpp
@@ -78,7 +78,7 @@ bool BulletCollisionDispatcher::needsCollision(
   const auto collObj1
       = static_cast<BulletCollisionObject*>(body1->getUserPointer());
 
-  if (mFilter && !mFilter->needsCollisionCheck(collObj0, collObj1))
+  if (mFilter && mFilter->ignoresCollision(collObj0, collObj1))
     return false;
 
   return btCollisionDispatcher::needsCollision(body0, body1);
diff --git a/dart/collision/bullet/detail/BulletOverlapFilterCallback.cpp b/dart/collision/bullet/detail/BulletOverlapFilterCallback.cpp
index 6f879d4253c9d..98147d9b618eb 100644
--- a/dart/collision/bullet/detail/BulletOverlapFilterCallback.cpp
+++ b/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->needsCollisionCheck(collObj0, collObj1);
+    return !filter->ignoresCollision(collObj0, collObj1);
   }
 
   return collide;
diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp
index fc39596fe60c8..4125b0985040f 100644
--- a/dart/collision/dart/DARTCollisionDetector.cpp
+++ b/dart/collision/dart/DARTCollisionDetector.cpp
@@ -145,7 +145,7 @@ bool DARTCollisionDetector::collide(
     {
       auto* collObj2 = objects[j];
 
-      if (filter && !filter->needsCollisionCheck(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->needsCollisionCheck(collObj1, collObj2))
+      if (filter && filter->ignoresCollision(collObj1, collObj2))
         continue;
 
       collisionFound = checkPair(collObj1, collObj2, option, result);
diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp
index cd6f116abaa6f..f522b0ee7e33a 100644
--- a/dart/collision/fcl/FCLCollisionDetector.cpp
+++ b/dart/collision/fcl/FCLCollisionDetector.cpp
@@ -1099,7 +1099,7 @@ bool collisionCallback(
     assert(collisionObject1);
     assert(collisionObject2);
 
-    if (!filter->needsCollisionCheck(collisionObject2, collisionObject1))
+    if (filter->ignoresCollision(collisionObject2, collisionObject1))
       return collData->done;
   }
 
diff --git a/dart/collision/ode/OdeCollisionDetector.cpp b/dart/collision/ode/OdeCollisionDetector.cpp
index 3debd389b6e7d..ec5fda5ace4fb 100644
--- a/dart/collision/ode/OdeCollisionDetector.cpp
+++ b/dart/collision/ode/OdeCollisionDetector.cpp
@@ -267,7 +267,7 @@ void CollisionCallback(void* data, dGeomID o1, dGeomID o2)
   assert(collObj1);
   assert(collObj2);
 
-  if (filter && !filter->needsCollisionCheck(collObj1, collObj2))
+  if (filter && filter->ignoresCollision(collObj1, collObj2))
       return;
 
   // Perform narrow-phase collision detection

From 8cc365a350e17e0ecd9df6343cd22f4fdac209ac Mon Sep 17 00:00:00 2001
From: Jeongseok Lee <jslee02@gmail.com>
Date: Wed, 30 Aug 2017 21:52:41 +0900
Subject: [PATCH 08/19] Use std::unordered_set instead of std::vector per this
 comment: https://github.com/dartsim/dart/pull/911#discussion_r135910973

---
 dart/collision/CollisionFilter.cpp | 7 +++----
 dart/collision/CollisionFilter.hpp | 6 +++---
 2 files changed, 6 insertions(+), 7 deletions(-)

diff --git a/dart/collision/CollisionFilter.cpp b/dart/collision/CollisionFilter.cpp
index 320a554ea00ca..597c5b6680f33 100644
--- a/dart/collision/CollisionFilter.cpp
+++ b/dart/collision/CollisionFilter.cpp
@@ -45,7 +45,7 @@ bool CollisionFilter::needCollision(
 }
 
 //==============================================================================
-void CompositeCollisionFilter::addCollisionFilter(CollisionFilter* filter)
+void CompositeCollisionFilter::addCollisionFilter(const CollisionFilter* filter)
 {
   if (!filter)
     return;
@@ -55,15 +55,14 @@ void CompositeCollisionFilter::addCollisionFilter(CollisionFilter* filter)
   if (found)
     return;
 
-  mFilters.push_back(filter);
+  mFilters.insert(filter);
 }
 
 //==============================================================================
 void CompositeCollisionFilter::removeCollisionFilter(
     const CollisionFilter* filter)
 {
-  mFilters.erase(
-        std::remove(mFilters.begin(), mFilters.end(), filter), mFilters.end());
+  mFilters.erase(filter);
 }
 
 //==============================================================================
diff --git a/dart/collision/CollisionFilter.hpp b/dart/collision/CollisionFilter.hpp
index dac1a0e4c34fe..2a3070af991d8 100644
--- a/dart/collision/CollisionFilter.hpp
+++ b/dart/collision/CollisionFilter.hpp
@@ -34,7 +34,7 @@
 
 #include <set>
 #include <unordered_map>
-#include <vector>
+#include <unordered_set>
 
 #include "dart/common/Deprecated.hpp"
 
@@ -70,7 +70,7 @@ class CompositeCollisionFilter : public CollisionFilter
 {
 public:
   /// Adds a collision filter to this CompositeCollisionFilter.
-  void addCollisionFilter(CollisionFilter* filter);
+  void addCollisionFilter(const CollisionFilter* filter);
 
   /// Removes a collision filter from this CompositeCollisionFilter.
   void removeCollisionFilter(const CollisionFilter* filter);
@@ -85,7 +85,7 @@ class CompositeCollisionFilter : public CollisionFilter
 
 protected:
   /// Collision filters
-  std::vector<CollisionFilter*> mFilters;
+  std::unordered_set<const CollisionFilter*> mFilters;
 };
 
 class BodyNodeCollisionFilter : public CollisionFilter

From 78b7e42018f73bfa5d8ebbb1bbf2f5a6ec70bcd1 Mon Sep 17 00:00:00 2001
From: Jeongseok Lee <jslee02@gmail.com>
Date: Wed, 30 Aug 2017 21:55:07 +0900
Subject: [PATCH 09/19] Rename existsBodyNodePairInBlacklist to
 hasBodyNodePairInBlacklist per this comment:
 https://github.com/dartsim/dart/pull/911#discussion_r135932951

---
 dart/collision/CollisionFilter.cpp | 4 ++--
 dart/collision/CollisionFilter.hpp | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/dart/collision/CollisionFilter.cpp b/dart/collision/CollisionFilter.cpp
index 597c5b6680f33..0bdb3b298f92b 100644
--- a/dart/collision/CollisionFilter.cpp
+++ b/dart/collision/CollisionFilter.cpp
@@ -186,7 +186,7 @@ bool BodyNodeCollisionFilter::ignoresCollision(
     }
   }
 
-  if (existsBodyNodePairInBlacklist(bodyNode1, bodyNode2))
+  if (hasBodyNodePairInBlacklist(bodyNode1, bodyNode2))
     return true;
 
   return false;
@@ -208,7 +208,7 @@ bool BodyNodeCollisionFilter::areAdjacentBodies(
 }
 
 //==============================================================================
-bool BodyNodeCollisionFilter::existsBodyNodePairInBlacklist(
+bool BodyNodeCollisionFilter::hasBodyNodePairInBlacklist(
     const dynamics::BodyNode* bodyNode1,
     const dynamics::BodyNode* bodyNode2) const
 {
diff --git a/dart/collision/CollisionFilter.hpp b/dart/collision/CollisionFilter.hpp
index 2a3070af991d8..33d24de984cf7 100644
--- a/dart/collision/CollisionFilter.hpp
+++ b/dart/collision/CollisionFilter.hpp
@@ -116,7 +116,7 @@ class BodyNodeCollisionFilter : public CollisionFilter
                          const dynamics::BodyNode* bodyNode2) const;
 
   /// Returns true if the BodyNode pair is in the blacklist.
-  bool existsBodyNodePairInBlacklist(
+  bool hasBodyNodePairInBlacklist(
       const dynamics::BodyNode* bodyNode1,
       const dynamics::BodyNode* bodyNode2) const;
 

From e6f2f00b236a5da1791db7d9ce0a44a1052e020e Mon Sep 17 00:00:00 2001
From: Jeongseok Lee <jslee02@gmail.com>
Date: Wed, 30 Aug 2017 21:57:51 +0900
Subject: [PATCH 10/19] Use std::unordered_set instead of std::set per this
 comment: https://github.com/dartsim/dart/pull/911#discussion_r135934539

---
 dart/collision/CollisionFilter.cpp | 8 ++++----
 dart/collision/CollisionFilter.hpp | 2 +-
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/dart/collision/CollisionFilter.cpp b/dart/collision/CollisionFilter.cpp
index 0bdb3b298f92b..4070e155afabd 100644
--- a/dart/collision/CollisionFilter.cpp
+++ b/dart/collision/CollisionFilter.cpp
@@ -101,13 +101,13 @@ void BodyNodeCollisionFilter::addBodyNodePairToBlackList(
   // 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 it_less = mBlackList.insert(
-        std::make_pair(bodyNodeLess,
-                       std::set<const dynamics::BodyNode*>())).first;
+  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.
-  it_less->second.insert(bodyNodeGreater);
+  itLess->second.insert(bodyNodeGreater);
 }
 
 //==============================================================================
diff --git a/dart/collision/CollisionFilter.hpp b/dart/collision/CollisionFilter.hpp
index 33d24de984cf7..acff5638df8e4 100644
--- a/dart/collision/CollisionFilter.hpp
+++ b/dart/collision/CollisionFilter.hpp
@@ -126,7 +126,7 @@ class BodyNodeCollisionFilter : public CollisionFilter
   /// a value less than every element in the set that is associated with it.
   std::unordered_map<
       const dynamics::BodyNode*,
-      std::set<const dynamics::BodyNode*>> mBlackList;
+      std::unordered_set<const dynamics::BodyNode*>> mBlackList;
 };
 
 } // namespace collision

From 96596fdae1a08c7de38f6e517cf62e4f3d34e826 Mon Sep 17 00:00:00 2001
From: Jeongseok Lee <jslee02@gmail.com>
Date: Wed, 30 Aug 2017 22:00:07 +0900
Subject: [PATCH 11/19] Use more informative variable name: key ->
 associatedRights

---
 dart/collision/CollisionFilter.cpp | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/dart/collision/CollisionFilter.cpp b/dart/collision/CollisionFilter.cpp
index 4070e155afabd..d148b2167cbd5 100644
--- a/dart/collision/CollisionFilter.cpp
+++ b/dart/collision/CollisionFilter.cpp
@@ -128,15 +128,15 @@ void BodyNodeCollisionFilter::removeBodyNodePairFromBlackList(
   const bool foundLeft = (resultLeft != mBlackList.end());
   if (foundLeft)
   {
-    auto& key = resultLeft->second;
+    auto& associatedRights = resultLeft->second;
 
-    const auto resultRight = key.find(bodyNodeGreater);
-    const bool foundRight = (resultRight != key.end());
+    const auto resultRight = associatedRights.find(bodyNodeGreater);
+    const bool foundRight = (resultRight != associatedRights.end());
     if (foundRight)
     {
-      key.erase(bodyNodeGreater);
+      associatedRights.erase(bodyNodeGreater);
 
-      if (key.empty())
+      if (associatedRights.empty())
         mBlackList.erase(resultLeft);
     }
   }

From 72a2b28d0ac91a6fe4f878589ec1173ceb699ce4 Mon Sep 17 00:00:00 2001
From: Jeongseok Lee <jslee02@gmail.com>
Date: Wed, 30 Aug 2017 22:03:47 +0900
Subject: [PATCH 12/19] Remove redundant operations per this comment:
 https://github.com/dartsim/dart/pull/911#discussion_r135939398

---
 dart/collision/CollisionFilter.cpp | 12 +++---------
 1 file changed, 3 insertions(+), 9 deletions(-)

diff --git a/dart/collision/CollisionFilter.cpp b/dart/collision/CollisionFilter.cpp
index d148b2167cbd5..b39fac7bc193f 100644
--- a/dart/collision/CollisionFilter.cpp
+++ b/dart/collision/CollisionFilter.cpp
@@ -129,16 +129,10 @@ void BodyNodeCollisionFilter::removeBodyNodePairFromBlackList(
   if (foundLeft)
   {
     auto& associatedRights = resultLeft->second;
+    associatedRights.erase(bodyNodeGreater);
 
-    const auto resultRight = associatedRights.find(bodyNodeGreater);
-    const bool foundRight = (resultRight != associatedRights.end());
-    if (foundRight)
-    {
-      associatedRights.erase(bodyNodeGreater);
-
-      if (associatedRights.empty())
-        mBlackList.erase(resultLeft);
-    }
+    if (associatedRights.empty())
+      mBlackList.erase(resultLeft);
   }
 }
 

From 6a8e5ac2cdeed4e6b38dfb89c1d74a38298d6992 Mon Sep 17 00:00:00 2001
From: Jeongseok Lee <jslee02@gmail.com>
Date: Wed, 30 Aug 2017 22:17:24 +0900
Subject: [PATCH 13/19] Assert when BodyNodeCollisionFilter is used for
 non-ShapeNodes per this comment:
 https://github.com/dartsim/dart/pull/911#discussion_r135932252

---
 dart/collision/CollisionFilter.cpp | 18 +++++++++++++++---
 1 file changed, 15 insertions(+), 3 deletions(-)

diff --git a/dart/collision/CollisionFilter.cpp b/dart/collision/CollisionFilter.cpp
index b39fac7bc193f..d8b8646c058b9 100644
--- a/dart/collision/CollisionFilter.cpp
+++ b/dart/collision/CollisionFilter.cpp
@@ -153,9 +153,21 @@ bool BodyNodeCollisionFilter::ignoresCollision(
   auto shapeNode1 = object1->getShapeFrame()->asShapeNode();
   auto shapeNode2 = object2->getShapeFrame()->asShapeNode();
 
-  if (!shapeNode1 || !shapeNode2)
-    return false;
-  // We assume that non-ShapeNodes are always being checked for collisions.
+  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();

From 866947c9da378e2b1b77be7db48c00aa0bc0e602 Mon Sep 17 00:00:00 2001
From: Jeongseok Lee <jslee02@gmail.com>
Date: Wed, 30 Aug 2017 22:22:53 +0900
Subject: [PATCH 14/19] Remove unnecessary header inclusion

---
 dart/collision/CollisionFilter.hpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/dart/collision/CollisionFilter.hpp b/dart/collision/CollisionFilter.hpp
index acff5638df8e4..50421a5d217df 100644
--- a/dart/collision/CollisionFilter.hpp
+++ b/dart/collision/CollisionFilter.hpp
@@ -32,7 +32,6 @@
 #ifndef DART_COLLISION_COLLISIONFILTER_HPP_
 #define DART_COLLISION_COLLISIONFILTER_HPP_
 
-#include <set>
 #include <unordered_map>
 #include <unordered_set>
 

From f46337b22f7ce86de4a45078d6b55979bba5c747 Mon Sep 17 00:00:00 2001
From: Jeongseok Lee <jslee02@gmail.com>
Date: Thu, 31 Aug 2017 11:46:13 +0900
Subject: [PATCH 15/19] Use more informative variable name: key ->
 associatedRights

---
 dart/collision/CollisionFilter.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/dart/collision/CollisionFilter.cpp b/dart/collision/CollisionFilter.cpp
index d8b8646c058b9..3452601830429 100644
--- a/dart/collision/CollisionFilter.cpp
+++ b/dart/collision/CollisionFilter.cpp
@@ -228,10 +228,10 @@ bool BodyNodeCollisionFilter::hasBodyNodePairInBlacklist(
   const bool foundLeft = (resultLeft != mBlackList.end());
   if (foundLeft)
   {
-    auto& key = resultLeft->second;
+    auto& associatedRights = resultLeft->second;
 
-    const auto resultRight = key.find(bodyNodeGreater);
-    const bool foundRight = (resultRight != key.end());
+    const auto resultRight = associatedRights.find(bodyNodeGreater);
+    const bool foundRight = (resultRight != associatedRights.end());
     if (foundRight)
       return true;
   }

From 8c7611abc2e3a286f6200c000b26933cc2ffe977 Mon Sep 17 00:00:00 2001
From: Jeongseok Lee <jslee02@gmail.com>
Date: Thu, 31 Aug 2017 11:47:50 +0900
Subject: [PATCH 16/19] Remove unnecessary operations

---
 dart/collision/CollisionFilter.cpp | 6 +-----
 1 file changed, 1 insertion(+), 5 deletions(-)

diff --git a/dart/collision/CollisionFilter.cpp b/dart/collision/CollisionFilter.cpp
index 3452601830429..be1e9902537d6 100644
--- a/dart/collision/CollisionFilter.cpp
+++ b/dart/collision/CollisionFilter.cpp
@@ -47,14 +47,10 @@ bool CollisionFilter::needCollision(
 //==============================================================================
 void CompositeCollisionFilter::addCollisionFilter(const CollisionFilter* filter)
 {
+  // nullptr is not an allowed 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);
 }
 

From b5879bc41d30791e82e4006988f9939f2cbcbd14 Mon Sep 17 00:00:00 2001
From: Jeongseok Lee <jslee02@gmail.com>
Date: Thu, 31 Aug 2017 12:22:36 +0900
Subject: [PATCH 17/19] Allow BodyNodeCollisionFilter is used for
 non-ShapeNode; return false in this case

---
 dart/collision/CollisionFilter.cpp | 22 +++++++---------------
 1 file changed, 7 insertions(+), 15 deletions(-)

diff --git a/dart/collision/CollisionFilter.cpp b/dart/collision/CollisionFilter.cpp
index be1e9902537d6..e9e0e98892137 100644
--- a/dart/collision/CollisionFilter.cpp
+++ b/dart/collision/CollisionFilter.cpp
@@ -149,21 +149,13 @@ bool BodyNodeCollisionFilter::ignoresCollision(
   auto shapeNode1 = object1->getShapeFrame()->asShapeNode();
   auto shapeNode2 = object2->getShapeFrame()->asShapeNode();
 
-  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);
-  }
+  // We don't filter out for non-ShapeNode because this class shouldn't have the
+  // authority to make decisions about filtering any ShapeFrames that aren't
+  // attached to a BodyNode. So here we just return false. In order to decide
+  // whether the non-ShapeNode should be ignored, please use other collision
+  // filters.
+  if (!shapeNode1 || !shapeNode2)
+    return false;
 
   auto bodyNode1 = shapeNode1->getBodyNodePtr();
   auto bodyNode2 = shapeNode2->getBodyNodePtr();

From 23f10bd957b0323e89ca80a8bf1cb854f54b0ad0 Mon Sep 17 00:00:00 2001
From: Jeongseok Lee <jslee02@gmail.com>
Date: Thu, 31 Aug 2017 13:03:31 +0900
Subject: [PATCH 18/19] Generalize blacklist functionality from
 BodyNodeCollisionFilter

---
 dart/collision/CollisionFilter.cpp       |  71 +----------
 dart/collision/CollisionFilter.hpp       |  15 +--
 dart/collision/detail/UnorderedPairs.hpp | 150 +++++++++++++++++++++++
 3 files changed, 157 insertions(+), 79 deletions(-)
 create mode 100644 dart/collision/detail/UnorderedPairs.hpp

diff --git a/dart/collision/CollisionFilter.cpp b/dart/collision/CollisionFilter.cpp
index e9e0e98892137..255b33d802120 100644
--- a/dart/collision/CollisionFilter.cpp
+++ b/dart/collision/CollisionFilter.cpp
@@ -85,57 +85,20 @@ bool CompositeCollisionFilter::ignoresCollision(
 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);
+  mBodyNodeBlackList.addPair(bodyNode1, bodyNode2);
 }
 
 //==============================================================================
 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);
-  }
+  mBodyNodeBlackList.removePair(bodyNode1, bodyNode2);
 }
 
 //==============================================================================
 void BodyNodeCollisionFilter::removeAllBodyNodePairsFromBlackList()
 {
-  mBlackList.clear();
+  mBodyNodeBlackList.removeAllPairs();
 }
 
 //==============================================================================
@@ -180,7 +143,7 @@ bool BodyNodeCollisionFilter::ignoresCollision(
     }
   }
 
-  if (hasBodyNodePairInBlacklist(bodyNode1, bodyNode2))
+  if (mBodyNodeBlackList.contains(bodyNode1, bodyNode2))
     return true;
 
   return false;
@@ -201,31 +164,5 @@ bool BodyNodeCollisionFilter::areAdjacentBodies(
   return false;
 }
 
-//==============================================================================
-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& associatedRights = resultLeft->second;
-
-    const auto resultRight = associatedRights.find(bodyNodeGreater);
-    const bool foundRight = (resultRight != associatedRights.end());
-    if (foundRight)
-      return true;
-  }
-
-  return false;
-}
-
 } // namespace collision
 } // namespace dart
diff --git a/dart/collision/CollisionFilter.hpp b/dart/collision/CollisionFilter.hpp
index 50421a5d217df..86450df93e93b 100644
--- a/dart/collision/CollisionFilter.hpp
+++ b/dart/collision/CollisionFilter.hpp
@@ -36,6 +36,7 @@
 #include <unordered_set>
 
 #include "dart/common/Deprecated.hpp"
+#include "dart/collision/detail/UnorderedPairs.hpp"
 
 namespace dart {
 
@@ -114,18 +115,8 @@ class BodyNodeCollisionFilter : public CollisionFilter
   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;
+  /// List of pairs to be ignored in the collision detection.
+  detail::UnorderedPairs<dynamics::BodyNode> mBodyNodeBlackList;
 };
 
 } // namespace collision
diff --git a/dart/collision/detail/UnorderedPairs.hpp b/dart/collision/detail/UnorderedPairs.hpp
new file mode 100644
index 0000000000000..92449b012cee6
--- /dev/null
+++ b/dart/collision/detail/UnorderedPairs.hpp
@@ -0,0 +1,150 @@
+/*
+ * Copyright (c) 2017, Graphics Lab, Georgia Tech Research Corporation
+ * Copyright (c) 2017, Personal Robotics Lab, Carnegie Mellon University
+ * All rights reserved.
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DART_COLLISION_DETAIL_UNORDEREDPAIRS_HPP_
+#define DART_COLLISION_DETAIL_UNORDEREDPAIRS_HPP_
+
+#include <unordered_map>
+#include <unordered_set>
+
+namespace dart {
+namespace collision {
+namespace detail {
+
+template <class T>
+class UnorderedPairs
+{
+public:
+  /// Add a pair to this container.
+  void addPair(const T* left, const T* right);
+
+  /// Remove a pair from this container.
+  void removePair(const T* left, const T* right);
+
+  /// Remove all the pairs from this container.
+  void removeAllPairs();
+
+  /// Returns true if this container contains the pair.
+  bool contains(const T* left, const T* right) const;
+
+private:
+  /// Each pair is stored so that the key of the std::unordered_map always has
+  /// a value less than every element in the std::unordered_set that is
+  /// associated with it.
+  std::unordered_map<const T*, std::unordered_set<const T*>> mList;
+};
+
+//==============================================================================
+template <class T>
+void UnorderedPairs<T>::addPair(const T* left, const T* right)
+{
+  if (!left || !right)
+    return;
+
+  const auto* less = left;
+  const auto* greater = right;
+
+  if (less > greater)
+    std::swap(less, greater);
+
+  // 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 = mList.insert(
+      std::make_pair(less, std::unordered_set<const T*>())).first;
+
+  // Insert bodyNodeGreater into the set corresponding to bodyNodeLess. If the
+  // pair already existed, this will do nothing.
+  itLess->second.insert(greater);
+}
+
+//==============================================================================
+template<class T>
+void UnorderedPairs<T>::removePair(const T* left, const T* right)
+{
+  if (!left || !right)
+    return;
+
+  const auto* bodyNodeLess = left;
+  const auto* bodyNodeGreater = right;
+
+  if (bodyNodeLess > bodyNodeGreater)
+    std::swap(bodyNodeLess, bodyNodeGreater);
+
+  // Remove the pair only when it already exists
+  const auto resultLeft = mList.find(bodyNodeLess);
+  const bool foundLeft = (resultLeft != mList.end());
+  if (foundLeft)
+  {
+    auto& associatedRights = resultLeft->second;
+    associatedRights.erase(bodyNodeGreater);
+
+    if (associatedRights.empty())
+      mList.erase(resultLeft);
+  }
+}
+
+//==============================================================================
+template<class T>
+void UnorderedPairs<T>::removeAllPairs()
+{
+  mList.clear();
+}
+
+//==============================================================================
+template<class T>
+bool UnorderedPairs<T>::contains(const T* left, const T* right) const
+{
+  const auto* less = left;
+  const auto* greater = right;
+
+  if (less > greater)
+    std::swap(less, greater);
+
+  const auto resultLeft = mList.find(less);
+  const bool foundLeft = (resultLeft != mList.end());
+  if (foundLeft)
+  {
+    auto& associatedRights = resultLeft->second;
+
+    const auto resultRight = associatedRights.find(greater);
+    const bool foundRight = (resultRight != associatedRights.end());
+    if (foundRight)
+      return true;
+  }
+
+  return false;
+}
+
+} // namespace detail
+} // namespace collision
+} // namespace dart
+
+#endif // DART_COLLISION_DETAIL_UNORDEREDPAIRS_HPP_

From 42ad25e820333f89f00fff5e7fa436bd0e64565a Mon Sep 17 00:00:00 2001
From: Jeongseok Lee <jslee02@gmail.com>
Date: Thu, 31 Aug 2017 13:09:14 +0900
Subject: [PATCH 19/19] Clean up code

---
 dart/collision/CollisionFilter.cpp       | 5 ++---
 dart/collision/CollisionFilter.hpp       | 3 ---
 dart/collision/detail/UnorderedPairs.hpp | 8 +++++---
 3 files changed, 7 insertions(+), 9 deletions(-)

diff --git a/dart/collision/CollisionFilter.cpp b/dart/collision/CollisionFilter.cpp
index 255b33d802120..0e72412d2d56a 100644
--- a/dart/collision/CollisionFilter.cpp
+++ b/dart/collision/CollisionFilter.cpp
@@ -56,7 +56,7 @@ void CompositeCollisionFilter::addCollisionFilter(const CollisionFilter* filter)
 
 //==============================================================================
 void CompositeCollisionFilter::removeCollisionFilter(
-    const CollisionFilter* filter)
+const CollisionFilter* filter)
 {
   mFilters.erase(filter);
 }
@@ -73,8 +73,7 @@ bool CompositeCollisionFilter::ignoresCollision(
 {
   for (const auto* filter : mFilters)
   {
-    const bool collisionIgnored = filter->ignoresCollision(object1, object2);
-    if (collisionIgnored)
+    if (filter->ignoresCollision(object1, object2))
       return true;
   }
 
diff --git a/dart/collision/CollisionFilter.hpp b/dart/collision/CollisionFilter.hpp
index 86450df93e93b..b578473b12acc 100644
--- a/dart/collision/CollisionFilter.hpp
+++ b/dart/collision/CollisionFilter.hpp
@@ -32,9 +32,6 @@
 #ifndef DART_COLLISION_COLLISIONFILTER_HPP_
 #define DART_COLLISION_COLLISIONFILTER_HPP_
 
-#include <unordered_map>
-#include <unordered_set>
-
 #include "dart/common/Deprecated.hpp"
 #include "dart/collision/detail/UnorderedPairs.hpp"
 
diff --git a/dart/collision/detail/UnorderedPairs.hpp b/dart/collision/detail/UnorderedPairs.hpp
index 92449b012cee6..7d8b5b9a76b5b 100644
--- a/dart/collision/detail/UnorderedPairs.hpp
+++ b/dart/collision/detail/UnorderedPairs.hpp
@@ -42,19 +42,21 @@ template <class T>
 class UnorderedPairs
 {
 public:
-  /// Add a pair to this container.
+  /// Adds a pair to this container.
   void addPair(const T* left, const T* right);
 
-  /// Remove a pair from this container.
+  /// Removes a pair from this container.
   void removePair(const T* left, const T* right);
 
-  /// Remove all the pairs from this container.
+  /// Removes all the pairs from this container.
   void removeAllPairs();
 
   /// Returns true if this container contains the pair.
   bool contains(const T* left, const T* right) const;
 
 private:
+  /// The actual container to store pairs.
+  ///
   /// Each pair is stored so that the key of the std::unordered_map always has
   /// a value less than every element in the std::unordered_set that is
   /// associated with it.