diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index a84411af97106608b23ff1e7e55b1a0ac01f287e..385c74f7e208c2f22dca8eb1132b9445557b74bd 100644
--- a/trunk/fcl/CMakeLists.txt
+++ b/trunk/fcl/CMakeLists.txt
@@ -50,4 +50,7 @@ rosbuild_add_gtest(test_core_collision_shape_mesh_consistency test/test_core_col
 target_link_libraries(test_core_collision_shape_mesh_consistency fcl)
 
 rosbuild_add_gtest(test_core_broad_phase test/test_core_broad_phase.cpp)
-target_link_libraries(test_core_broad_phase fcl)
\ No newline at end of file
+target_link_libraries(test_core_broad_phase fcl)
+
+rosbuild_add_gtest(test_core_front_list test/test_core_front_list.cpp)
+target_link_libraries(test_core_front_list fcl)
\ No newline at end of file
diff --git a/trunk/fcl/src/collision.cpp b/trunk/fcl/src/collision.cpp
index 6d942cb7527480448929b1e70c7aefa7d5b71b68..bedb7b300d6cc9582b14790bd3dc41c4ec8b5806 100644
--- a/trunk/fcl/src/collision.cpp
+++ b/trunk/fcl/src/collision.cpp
@@ -74,6 +74,7 @@ int collide(const CollisionObject* o1, const CollisionObject* o2,
   }
   else if(object_type1 == OT_GEOM && object_type2 == OT_BVH)
   {
+    //if(!CollisionFunctionLookTable.collision_matrix[node_type1][node_type2])
     if(!CollisionFunctionLookTable.collision_matrix[node_type2][node_type1])
     {
       std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl;
diff --git a/trunk/fcl/src/collision_func_matrix.cpp b/trunk/fcl/src/collision_func_matrix.cpp
index b69fa79ca70a4f704ce24874cb3a67ad3e5b4bf6..d202c9041313f19c725fc4a20f72d81c7d5762ef 100644
--- a/trunk/fcl/src/collision_func_matrix.cpp
+++ b/trunk/fcl/src/collision_func_matrix.cpp
@@ -950,7 +950,7 @@ int KDOP24PlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int
   MESHSHAPE_COMMON_CODE();
 }
 
-
+// use MESH SHAPE
 /*
 int BoxAABBCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
@@ -1325,6 +1325,7 @@ int PlaneKDOP24Collide(const CollisionObject* o1, const CollisionObject* o2, int
 
 */
 
+
 int AABBAABBCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
 {
   MeshCollisionTraversalNode<AABB> node;
@@ -1493,7 +1494,8 @@ CollisionFunctionMatrix::CollisionFunctionMatrix()
   collision_matrix[BV_KDOP24][GEOM_CYLINDER] = KDOP24CylinderCollide;
   collision_matrix[BV_KDOP24][GEOM_CONVEX] = KDOP24ConvexCollide;
   collision_matrix[BV_KDOP24][GEOM_PLANE] = KDOP24PlaneCollide;
-/*
+
+  /*
   collision_matrix[GEOM_BOX][BV_AABB] = BoxAABBCollide;
   collision_matrix[GEOM_SPHERE][BV_AABB] = SphereAABBCollide;
   collision_matrix[GEOM_CAPSULE][BV_AABB] = CapAABBCollide;
@@ -1541,7 +1543,8 @@ CollisionFunctionMatrix::CollisionFunctionMatrix()
   collision_matrix[GEOM_CYLINDER][BV_KDOP24] = CylinderKDOP24Collide;
   collision_matrix[GEOM_CONVEX][BV_KDOP24] = ConvexKDOP24Collide;
   collision_matrix[GEOM_PLANE][BV_KDOP24] = PlaneKDOP24Collide;
-*/
+  */
+
   collision_matrix[BV_AABB][BV_AABB] = AABBAABBCollide;
   collision_matrix[BV_OBB][BV_OBB] = OBBOBBCollide;
   collision_matrix[BV_RSS][BV_RSS] = RSSRSSCollide;
diff --git a/trunk/fcl/test/test_core_collision.cpp b/trunk/fcl/test/test_core_collision.cpp
index 764e1178f66cb5fcd0b88d0e7cfa7a31a2060c52..2bd4ea8ba475c898ecc1601fb0c4e94a425d8397 100644
--- a/trunk/fcl/test/test_core_collision.cpp
+++ b/trunk/fcl/test/test_core_collision.cpp
@@ -89,8 +89,6 @@ TEST(collision_test, mesh_mesh)
 
   std::vector<Transform> transforms; // t0
   std::vector<Transform> transforms2; // t1
-  std::vector<Transform> transforms_ccd; // t0
-  std::vector<Transform> transforms_ccd2; // t1
   BVH_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
   BVH_REAL delta_trans[] = {1, 1, 1};
   int n = 10;