git-svn-id: https://kforge.ros.org/fcl/fcl_ros@21 253336fb-580f-4252-a368-f3cef5a2a82b

trunk/fcl/src/collision.cpp

@@ -43,7 +43,6 @@
 namespace fcl
 {
 
-
 static CollisionFunctionMatrix CollisionFunctionLookTable;
 
 int collide(const CollisionObject* o1, const CollisionObject* o2,
@@ -62,6 +61,12 @@ int collide(const CollisionObject* o1, const CollisionObject* o2,
   const OBJECT_TYPE object_type2 = o2->getObjectType();
   const NODE_TYPE node_type2 = o2->getNodeType();
 
+  if(!CollisionFunctionLookTable.collision_matrix[node_type1][node_type2])
+  {
+    std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl;
+    return 0;
+  }
+
 
   if(object_type1 == OT_GEOM && object_type2 == OT_GEOM)
   {

trunk/fcl/test/test_core_broad_phase.cpp

@@ -126,6 +126,7 @@ TEST(test_core, broad_phase_collision)
     ASSERT_TRUE(query_res1 == query_res4);
   }
 
+
   for(unsigned int i = 0; i < env.size(); ++i)
     delete env[i];
   for(unsigned int i = 0; i < query.size(); ++i)