Commit 727d0089 authored by isucan's avatar isucan
Browse files

bugfixes after robot testing

git-svn-id: https://kforge.ros.org/fcl/fcl_ros@155 253336fb-580f-4252-a368-f3cef5a2a82b
parent bb558b51
......@@ -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
......
......@@ -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));
}
......
......@@ -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);
}
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment