diff --git a/trunk/fcl/include/fcl/collision_data.h b/trunk/fcl/include/fcl/collision_data.h
index a3b84443a813e6ec71c1257882bb991d293a29d0..f98d4fb6004f303d75832f6b589ca9d7e0661fab 100644
--- a/trunk/fcl/include/fcl/collision_data.h
+++ b/trunk/fcl/include/fcl/collision_data.h
@@ -125,23 +125,41 @@ struct CostSource
   /// @brief cost density in the AABB region
   FCL_REAL cost_density;
 
+  FCL_REAL total_cost;
+
   CostSource(const Vec3f& aabb_min_, const Vec3f& aabb_max_, FCL_REAL cost_density_) : aabb_min(aabb_min_),
                                                                                        aabb_max(aabb_max_),
                                                                                        cost_density(cost_density_)
   {
+    total_cost = cost_density * (aabb_max[0] - aabb_min[0]) * (aabb_max[1] - aabb_min[1]) * (aabb_max[2] - aabb_min[2]);
   }
 
   CostSource(const AABB& aabb, FCL_REAL cost_density_) : aabb_min(aabb.min_),
                                                          aabb_max(aabb.max_),
                                                          cost_density(cost_density_)
   {
+    total_cost = cost_density * (aabb_max[0] - aabb_min[0]) * (aabb_max[1] - aabb_min[1]) * (aabb_max[2] - aabb_min[2]);
   }
 
   CostSource() {}
 
   bool operator < (const CostSource& other) const
   {
-    return cost_density < other.cost_density;
+    if(total_cost < other.total_cost) 
+      return false;
+    if(total_cost > other.total_cost)
+      return true;
+    
+    if(cost_density < other.cost_density)
+      return false;
+    if(cost_density > other.cost_density)
+      return true;
+  
+    for(size_t i = 0; i < 3; ++i)
+      if(aabb_min[i] != other.aabb_min[i])
+	return aabb_min[i] < other.aabb_min[i];
+ 
+    return false;
   }
 };
 
@@ -201,8 +219,8 @@ public:
   inline void addCostSource(const CostSource& c, std::size_t num_max_cost_sources)
   {
     cost_sources.insert(c);
-    if(cost_sources.size() > num_max_cost_sources)
-      cost_sources.erase(cost_sources.begin());            
+    while (cost_sources.size() > num_max_cost_sources)
+      cost_sources.erase(--cost_sources.end());
   }
 
   /// @brief return binary collision result
diff --git a/trunk/fcl/include/fcl/octree.h b/trunk/fcl/include/fcl/octree.h
index 6a4db448457a19ec2816e69f3ac27a2e7847375c..d4ea5420239373bcf0b02bebd3aaef5415b304e3 100644
--- a/trunk/fcl/include/fcl/octree.h
+++ b/trunk/fcl/include/fcl/octree.h
@@ -80,6 +80,8 @@ public:
   inline AABB getRootBV() const
   {
     FCL_REAL delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2;
+
+    // std::cout << "octree size " << delta << std::endl;
     return AABB(Vec3f(-delta, -delta, -delta), Vec3f(delta, delta, delta));
   }
 
diff --git a/trunk/fcl/include/fcl/traversal/traversal_node_octree.h b/trunk/fcl/include/fcl/traversal/traversal_node_octree.h
index f9a54216056379974d9a318d55e0715af7d13127..b4fa121e54812e725b93ef16bbaf003a93de59b3 100644
--- a/trunk/fcl/include/fcl/traversal/traversal_node_octree.h
+++ b/trunk/fcl/include/fcl/traversal/traversal_node_octree.h
@@ -329,6 +329,7 @@ private:
             computeBV<AABB, Box>(box, box_tf, aabb1);
             computeBV<AABB, S>(s, tf2, aabb2);
             aabb1.overlap(aabb2, overlap_part);
+	    // std::cout << "octree cost " << root1->getOccupancy() << std::endl;
             cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * s.cost_density), crequest->num_max_cost_sources);
           }
 
@@ -353,6 +354,7 @@ private:
             computeBV<AABB, Box>(box, box_tf, aabb1);
             computeBV<AABB, S>(s, tf2, aabb2);
             aabb1.overlap(aabb2, overlap_part);
+	    // std::cout << "octree cost " << root1->getOccupancy() << std::endl;
             cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * s.cost_density), crequest->num_max_cost_sources);            
           }
         }