From 727d00890fba18ec6e6c6afce84d3e82cceb8460 Mon Sep 17 00:00:00 2001
From: isucan <isucan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Tue, 7 Aug 2012 02:24:22 +0000
Subject: [PATCH] bugfixes after robot testing
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@155 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/include/fcl/collision_data.h | 24 ++++++++++++++++---
trunk/fcl/include/fcl/octree.h | 2 ++
.../fcl/traversal/traversal_node_octree.h | 2 ++
3 files changed, 25 insertions(+), 3 deletions(-)
diff --git a/trunk/fcl/include/fcl/collision_data.h b/trunk/fcl/include/fcl/collision_data.h
index a3b84443..f98d4fb6 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 6a4db448..d4ea5420 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 f9a54216..b4fa121e 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);
}
}
--
GitLab