diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp
index 76b4b0195dd53..f64d7b81f563d 100644
--- a/dart/collision/CollisionDetector.cpp
+++ b/dart/collision/CollisionDetector.cpp
@@ -215,7 +215,7 @@ void CollisionDetector::enablePair(dynamics::BodyNode* _node1,
   CollisionNode* collisionNode1 = getCollisionNode(_node1);
   CollisionNode* collisionNode2 = getCollisionNode(_node2);
   if (collisionNode1 && collisionNode2)
-    getPairCollidable(collisionNode1, collisionNode2) = true;
+    setPairCollidable(collisionNode1, collisionNode2, true);
 }
 
 void CollisionDetector::disablePair(dynamics::BodyNode* _node1,
@@ -223,7 +223,7 @@ void CollisionDetector::disablePair(dynamics::BodyNode* _node1,
   CollisionNode* collisionNode1 = getCollisionNode(_node1);
   CollisionNode* collisionNode2 = getCollisionNode(_node2);
   if (collisionNode1 && collisionNode2)
-    getPairCollidable(collisionNode1, collisionNode2) = false;
+    setPairCollidable(collisionNode1, collisionNode2, false);
 }
 
 //==============================================================================
@@ -271,16 +271,51 @@ bool CollisionDetector::containSkeleton(const dynamics::Skeleton* _skeleton)
   return false;
 }
 
-std::vector<bool>::reference CollisionDetector::getPairCollidable(
-    const CollisionNode* _node1, const CollisionNode* _node2) {
+bool CollisionDetector::getPairCollidable(const CollisionNode* _node1,
+                                          const CollisionNode* _node2)
+{
   assert(_node1 != _node2);
-  int index1 = _node1->getIndex();
-  int index2 = _node2->getIndex();
+
+  size_t index1 = _node1->getIndex();
+  size_t index2 = _node2->getIndex();
+
   if (index1 < index2)
     std::swap(index1, index2);
+
+  // TODO(JS): Workaround
+  // If this fuction is called before all the body nodes in the world are not
+  // added to the collision detector than it cause seg fault. We just return
+  // false in that case.
+  if (index1 > mCollidablePairs.size() - 1
+      || index2 > mCollidablePairs.size() - 1)
+    return true;
+
   return mCollidablePairs[index1][index2];
 }
 
+void CollisionDetector::setPairCollidable(const CollisionNode* _node1,
+                                          const CollisionNode* _node2,
+                                          bool _val)
+{
+  assert(_node1 != _node2);
+
+  size_t index1 = _node1->getIndex();
+  size_t index2 = _node2->getIndex();
+
+  if (index1 < index2)
+    std::swap(index1, index2);
+
+  // TODO(JS): Workaround
+  // If this fuction is called before all the body nodes in the world are not
+  // added to the collision detector than it cause seg fault. We just return in
+  // that case.
+  if (index1 > mCollidablePairs.size() - 1
+      || index2 > mCollidablePairs.size() - 1)
+    return;
+
+  mCollidablePairs[index1][index2] = _val;
+}
+
 bool CollisionDetector::isAdjacentBodies(const dynamics::BodyNode* _bodyNode1,
                                          const dynamics::BodyNode* _bodyNode2)
 {
diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h
index c931bdcfebbc5..d1c3d4c79bed9 100644
--- a/dart/collision/CollisionDetector.h
+++ b/dart/collision/CollisionDetector.h
@@ -162,14 +162,14 @@ class CollisionDetector
   /// \brief
   void setNumMaxContacs(int _num);
 
+  /// \brief
+  bool isCollidable(const CollisionNode* _node1, const CollisionNode* _node2);
+
 protected:
   /// \brief
   virtual bool detectCollision(CollisionNode* _node1, CollisionNode* _node2,
                                bool _calculateContactPoints) = 0;
 
-  /// \brief
-  bool isCollidable(const CollisionNode* _node1, const CollisionNode* _node2);
-
   /// \brief
   std::vector<Contact> mContacts;
 
@@ -187,8 +187,13 @@ class CollisionDetector
   bool containSkeleton(const dynamics::Skeleton* _skeleton);
 
   /// \brief
-  std::vector<bool>::reference getPairCollidable(const CollisionNode* _node1,
-                                                 const CollisionNode* _node2);
+  bool getPairCollidable(const CollisionNode* _node1,
+                         const CollisionNode* _node2);
+
+  /// \brief
+  void setPairCollidable(const CollisionNode* _node1,
+                         const CollisionNode* _node2,
+                         bool _val);
 
   /// \brief Return true if _bodyNode1 and _bodyNode2 are adjacent bodies
   bool isAdjacentBodies(const dynamics::BodyNode* _bodyNode1,
diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp
index 8ced062dcd57b..90caacf6a51db 100644
--- a/dart/collision/bullet/BulletCollisionDetector.cpp
+++ b/dart/collision/bullet/BulletCollisionDetector.cpp
@@ -78,35 +78,28 @@ struct CollisionFilter : public btOverlapFilterCallback
     collide = collide && (_proxy1->m_collisionFilterGroup &
                           _proxy0->m_collisionFilterMask);
 
-    btCollisionObject* collObj0 =
-        static_cast<btCollisionObject*>(_proxy0->m_clientObject);
-    btCollisionObject* collObj1 =
-        static_cast<btCollisionObject*>(_proxy1->m_clientObject);
-
-    BulletUserData* userData0 =
-        static_cast<BulletUserData*>(collObj0->getUserPointer());
-    BulletUserData* userData1 =
-        static_cast<BulletUserData*>(collObj1->getUserPointer());
-
-//    if (!userData0->btCollDet->isCollidable(userData0->btCollNode,
-//                                            userData1->btCollNode)) {
-//      std::cout<< "false.\n";
-//      return false;
-//    }
-
-    if (userData0->bodyNode == userData1->bodyNode)
-      return false;
-
-    if (!userData0->bodyNode->isCollidable())
-      return false;
-
-    if (!userData1->bodyNode->isCollidable())
-      return false;
-
-    if (userData0->bodyNode->getSkeleton() ==
-        userData1->bodyNode->getSkeleton())
-      if (!userData0->bodyNode->getSkeleton()->isEnabledSelfCollisionCheck())
-        return false;
+    if (collide)
+    {
+      btCollisionObject* collObj0 =
+          static_cast<btCollisionObject*>(_proxy0->m_clientObject);
+      btCollisionObject* collObj1 =
+          static_cast<btCollisionObject*>(_proxy1->m_clientObject);
+
+      BulletUserData* userData0 =
+          static_cast<BulletUserData*>(collObj0->getUserPointer());
+      BulletUserData* userData1 =
+          static_cast<BulletUserData*>(collObj1->getUserPointer());
+
+      // Assume single collision detector
+      assert(userData0->btCollDet == userData1->btCollDet);
+
+      CollisionDetector* cd = userData0->btCollDet;
+
+      CollisionNode* cn1 = userData0->btCollNode;
+      CollisionNode* cn2 = userData1->btCollNode;
+
+      collide = cd->isCollidable(cn1, cn2);
+    }
 
     return collide;
   }