diff --git a/src/broadphase/broadphase_SaP.cpp b/src/broadphase/broadphase_SaP.cpp
index 81cb7f86ebdf432cca5022c5916c5ab19443606d..ca2ad8161ca2beab214de6e7ad74dd4d67008cb2 100644
--- a/src/broadphase/broadphase_SaP.cpp
+++ b/src/broadphase/broadphase_SaP.cpp
@@ -89,6 +89,8 @@ void SaPCollisionManager::unregisterObject(CollisionObject* obj)
 
 void SaPCollisionManager::registerObjects(const std::vector<CollisionObject*>& other_objs)
 {
+  if(other_objs.empty()) return;
+
   if(size() > 0)
     BroadPhaseCollisionManager::registerObjects(other_objs);
   else
@@ -272,6 +274,8 @@ void SaPCollisionManager::registerObject(CollisionObject* obj)
 
 void SaPCollisionManager::setup()
 {
+  if(size() == 0) return;
+
   FCL_REAL scale[3];
   scale[0] = (velist[0].back())->getVal(0) - velist[0][0]->getVal(0);
   scale[1] = (velist[1].back())->getVal(1) - velist[1][0]->getVal(1);;
diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp
index 69dc7d798e459f3814b49d3395c24997d650163a..cba7ed6ca81cbc5f7797beb0ec5fd1cc26c1d89d 100644
--- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp
+++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp
@@ -634,6 +634,8 @@ bool selfDistanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root,
 
 void DynamicAABBTreeCollisionManager::registerObjects(const std::vector<CollisionObject*>& other_objs)
 {
+  if(other_objs.empty()) return;
+
   if(size() > 0)
   {
     BroadPhaseCollisionManager::registerObjects(other_objs);
diff --git a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
index c8364517058f98e750fda35fabb9c8f696067c04..936a67436794c4408566f02d55462edc44ff48c5 100644
--- a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
+++ b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
@@ -657,6 +657,8 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nod
 
 void DynamicAABBTreeCollisionManager_Array::registerObjects(const std::vector<CollisionObject*>& other_objs)
 {
+  if(other_objs.empty()) return;
+
   if(size() > 0)
   {
     BroadPhaseCollisionManager::registerObjects(other_objs);
diff --git a/test/test_fcl_broadphase.cpp b/test/test_fcl_broadphase.cpp
index 55382ade8b5a159883e0e1f63e8acbcd0d93a4f1..9e69d0a69d7606873292aefec5b2545327c94d17 100644
--- a/test/test_fcl_broadphase.cpp
+++ b/test/test_fcl_broadphase.cpp
@@ -149,6 +149,26 @@ BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_self_distance)
   broad_phase_self_distance_test(200, 5000);
 }
 
+/// check broad phase collision for empty collision object set and queries
+BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_empty)
+{
+  broad_phase_collision_test(2000, 0, 0, 10, false, false);
+  broad_phase_collision_test(2000, 0, 1000, 10, false, false);
+  broad_phase_collision_test(2000, 100, 0, 10, false, false);
+
+  broad_phase_collision_test(2000, 0, 0, 10, false, true);
+  broad_phase_collision_test(2000, 0, 1000, 10, false, true);
+  broad_phase_collision_test(2000, 100, 0, 10, false, true);
+
+  broad_phase_collision_test(2000, 0, 0, 10, true, false);
+  broad_phase_collision_test(2000, 0, 1000, 10, true, false);
+  broad_phase_collision_test(2000, 100, 0, 10, true, false);
+
+  broad_phase_collision_test(2000, 0, 0, 10, true, true);
+  broad_phase_collision_test(2000, 0, 1000, 10, true, true);
+  broad_phase_collision_test(2000, 100, 0, 10, true, true);
+}
+
 /// check broad phase collision and self collision, only return collision or not
 BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_binary)
 {