Merge pull request #64 from dartsim/fix_broadphase_register_empty_set

src/broadphase/broadphase_SaP.cpp

src/broadphase/broadphase_dynamic_AABB_tree.cpp

src/broadphase/broadphase_dynamic_AABB_tree_array.cpp

test/test_fcl_broadphase.cpp