From 4201f05a90bcc08fc401f97faa87244b8a4b7826 Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Thu, 5 Jul 2012 23:01:28 +0000
Subject: [PATCH] Fix memory leak in interval tree
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@117 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/include/fcl/BV/AABB.h | 4 +-
trunk/fcl/include/fcl/broad_phase_collision.h | 80 ++++++++++---
trunk/fcl/include/fcl/hierarchy_tree.h | 109 +++++++++++-------
trunk/fcl/src/BV/AABB.cpp | 33 +++++-
trunk/fcl/src/broad_phase_collision.cpp | 82 ++++++-------
5 files changed, 208 insertions(+), 100 deletions(-)
diff --git a/trunk/fcl/include/fcl/BV/AABB.h b/trunk/fcl/include/fcl/BV/AABB.h
index 02066cac..cdd6a303 100644
--- a/trunk/fcl/include/fcl/BV/AABB.h
+++ b/trunk/fcl/include/fcl/BV/AABB.h
@@ -186,7 +186,9 @@ public:
return (min_ + max_) * 0.5;
}
- BVH_REAL distance(const AABB& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
+ BVH_REAL distance(const AABB& other, Vec3f* P, Vec3f* Q) const;
+
+ BVH_REAL distance(const AABB& other) const;
inline bool equal(const AABB& other) const
{
diff --git a/trunk/fcl/include/fcl/broad_phase_collision.h b/trunk/fcl/include/fcl/broad_phase_collision.h
index 9eacf4f7..d900bbaa 100644
--- a/trunk/fcl/include/fcl/broad_phase_collision.h
+++ b/trunk/fcl/include/fcl/broad_phase_collision.h
@@ -143,6 +143,18 @@ protected:
mutable std::set<std::pair<CollisionObject*, CollisionObject*> > tested_set;
mutable bool enable_tested_set_;
+ bool inTestedSet(CollisionObject* a, CollisionObject* b) const
+ {
+ if(a < b) return tested_set.find(std::make_pair(a, b)) != tested_set.end();
+ else return tested_set.find(std::make_pair(b, a)) != tested_set.end();
+ }
+
+ void insertTestedSet(CollisionObject* a, CollisionObject* b) const
+ {
+ if(a < b) tested_set.insert(std::make_pair(a, b));
+ else tested_set.insert(std::make_pair(b, a));
+ }
+
};
@@ -504,7 +516,7 @@ bool SpatialHashingCollisionManager<HashTable>::collide_(CollisionObject* obj, v
for(it = objs_outside_scene_limit.begin(); it != objs_outside_scene_limit.end(); ++it)
{
if(obj == *it) continue;
- if(callback(obj, *it, cdata)) return true;
+ if(callback(obj, *it, cdata)) return true;
}
}
@@ -558,8 +570,20 @@ bool SpatialHashingCollisionManager<HashTable>::distance_(CollisionObject* obj,
for(it = objs_outside_scene_limit.begin(); it != objs_outside_scene_limit.end(); ++it)
{
if(obj == *it) continue;
- if(obj->getAABB().distance((*it)->getAABB()) < min_dist)
- if(callback(obj, *it, cdata, min_dist)) return true;
+ if(!enable_tested_set_)
+ {
+ if(obj->getAABB().distance((*it)->getAABB()) < min_dist)
+ if(callback(obj, *it, cdata, min_dist)) return true;
+ }
+ else
+ {
+ if(!inTestedSet(obj, *it))
+ {
+ if(obj->getAABB().distance((*it)->getAABB()) < min_dist)
+ if(callback(obj, *it, cdata, min_dist)) return true;
+ insertTestedSet(obj, *it);
+ }
+ }
}
}
@@ -567,8 +591,20 @@ bool SpatialHashingCollisionManager<HashTable>::distance_(CollisionObject* obj,
for(unsigned int i = 0; i < query_result.size(); ++i)
{
if(obj == query_result[i]) continue;
- if(obj->getAABB().distance(query_result[i]->getAABB()) < min_dist)
- if(callback(obj, query_result[i], cdata, min_dist)) return true;
+ if(!enable_tested_set_)
+ {
+ if(obj->getAABB().distance(query_result[i]->getAABB()) < min_dist)
+ if(callback(obj, query_result[i], cdata, min_dist)) return true;
+ }
+ else
+ {
+ if(!inTestedSet(obj, query_result[i]))
+ {
+ if(obj->getAABB().distance(query_result[i]->getAABB()) < min_dist)
+ if(callback(obj, query_result[i], cdata, min_dist)) return true;
+ insertTestedSet(obj, query_result[i]);
+ }
+ }
}
}
else
@@ -577,19 +613,27 @@ bool SpatialHashingCollisionManager<HashTable>::distance_(CollisionObject* obj,
for(it = objs_outside_scene_limit.begin(); it != objs_outside_scene_limit.end(); ++it)
{
if(obj == *it) continue;
- if(obj->getAABB().distance((*it)->getAABB()) < min_dist)
- if(callback(obj, *it, cdata, min_dist)) return true;
+ if(!enable_tested_set_)
+ {
+ if(obj->getAABB().distance((*it)->getAABB()) < min_dist)
+ if(callback(obj, *it, cdata, min_dist)) return true;
+ }
+ else
+ {
+ if(!inTestedSet(obj, *it))
+ {
+ if(obj->getAABB().distance((*it)->getAABB()) < min_dist)
+ if(callback(obj, *it, cdata, min_dist)) return true;
+ insertTestedSet(obj, *it);
+ }
+ }
}
}
if(status == 1)
{
if(old_min_distance < std::numeric_limits<BVH_REAL>::max())
- {
- if(min_dist < old_min_distance) break;
- else
- min_dist = std::numeric_limits<BVH_REAL>::max();
- }
+ break;
else
{
if(min_dist < old_min_distance)
@@ -656,10 +700,16 @@ void SpatialHashingCollisionManager<HashTable>::collide(void* cdata, CollisionCa
template<typename HashTable>
void SpatialHashingCollisionManager<HashTable>::distance(void* cdata, DistanceCallBack callback) const
{
+ enable_tested_set_ = true;
+ tested_set.clear();
+
BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
for(std::list<CollisionObject*>::const_iterator it = objs.begin(); it != objs.end(); ++it)
- if(distance_(*it, cdata, callback, min_dist)) return;
+ if(distance_(*it, cdata, callback, min_dist)) break;
+
+ enable_tested_set_ = false;
+ tested_set.clear();
}
template<typename HashTable>
@@ -1228,9 +1278,9 @@ public:
DynamicAABBTreeCollisionManager()
{
- max_tree_nonbalanced_level = 4;
+ max_tree_nonbalanced_level = 10;
tree_incremental_balance_pass = 10;
- tree_topdown_balance_threshold = 128;
+ tree_topdown_balance_threshold = 16;
setup_ = false;
}
diff --git a/trunk/fcl/include/fcl/hierarchy_tree.h b/trunk/fcl/include/fcl/hierarchy_tree.h
index fb52b150..cbd20735 100644
--- a/trunk/fcl/include/fcl/hierarchy_tree.h
+++ b/trunk/fcl/include/fcl/hierarchy_tree.h
@@ -133,7 +133,7 @@ public:
}
/** \brief update one leaf node's bounding volume */
- void update(NodeType* leaf, const BV& bv)
+ void update_(NodeType* leaf, const BV& bv)
{
NodeType* root = removeLeaf(leaf);
if(root)
@@ -168,11 +168,18 @@ public:
insertLeaf(root, leaf);
}
+ bool update(NodeType* leaf, const BV& bv)
+ {
+ if(leaf->bv.contain(bv)) return false;
+ update_(leaf, bv);
+ return true;
+ }
+
/** \brief update one leaf's bounding volume, with prediction */
bool update(NodeType* leaf, const BV& bv, const Vec3f& vel, BVH_REAL margin)
{
if(leaf->bv.contain(bv)) return false;
- update(leaf, bv);
+ update_(leaf, bv);
return true;
}
@@ -180,7 +187,7 @@ public:
bool update(NodeType* leaf, const BV& bv, const Vec3f& vel)
{
if(leaf->bv.contain(bv)) return false;
- update(leaf, bv);
+ update_(leaf, bv);
return true;
}
@@ -209,6 +216,27 @@ public:
}
}
+ /** \brief refit */
+ void refit()
+ {
+ if(root_node)
+ {
+ recurseRefit(root_node);
+ }
+ }
+
+ void recurseRefit(NodeType* node)
+ {
+ if(!node->isLeaf())
+ {
+ recurseRefit(node->childs[0]);
+ recurseRefit(node->childs[1]);
+ node->bv = node->childs[0]->bv + node->childs[1]->bv;
+ }
+ else
+ return;
+ }
+
size_t getMaxHeight(NodeType* node) const
{
if(!node->isLeaf())
@@ -295,9 +323,47 @@ public:
}
}
+
+ /** \brief construct a tree for a set of leaves from bottom -- very heavy way */
+ void bottomup(std::vector<NodeType*>& leaves)
+ {
+ while(leaves.size() > 1)
+ {
+ BVH_REAL min_size = std::numeric_limits<BVH_REAL>::max();
+ int min_idx[2] = {-1, -1};
+ for(size_t i = 0; i < leaves.size(); ++i)
+ {
+ for(size_t j = i+1; j < leaves.size(); ++j)
+ {
+ BVH_REAL cur_size = (leaves[i]->bv + leaves[j]->bv).size();
+ if(cur_size < min_size)
+ {
+ min_size = cur_size;
+ min_idx[0] = i;
+ min_idx[1] = j;
+ }
+ }
+ }
+
+ NodeType* n[2] = {leaves[min_idx[0]], leaves[min_idx[1]]};
+ NodeType* p = createNode(NULL, n[0]->bv, n[1]->bv, NULL);
+ p->childs[0] = n[0];
+ p->childs[1] = n[1];
+ n[0]->parent = p;
+ n[1]->parent = p;
+ leaves[min_idx[0]] = p;
+ NodeType* tmp = leaves[min_idx[1]];
+ leaves[min_idx[1]] = leaves.back();
+ leaves.back() = tmp;
+ leaves.pop_back();
+ }
+ }
+
+
/** \brief construct a tree for a set of leaves from top */
NodeType* topdown(std::vector<NodeType*>& leaves, int bu_threshold)
{
+ n_leaves = leaves.size();
static const Vec3f axis[3] = {Vec3f(1, 0, 0), Vec3f(0, 1, 0), Vec3f(0, 0, 1)};
if(leaves.size() > 1)
{
@@ -391,43 +457,6 @@ private:
return n;
}
-
- /** \brief construct a tree for a set of leaves from bottom */
- void bottomup(std::vector<NodeType*>& leaves)
- {
- while(leaves.size() > 1)
- {
- BVH_REAL min_size = std::numeric_limits<BVH_REAL>::max();
- int min_idx[2] = {-1, -1};
- for(size_t i = 0; i < leaves.size(); ++i)
- {
- for(size_t j = i+1; j < leaves.size(); ++j)
- {
- BVH_REAL cur_size = (leaves[i]->bv + leaves[j]->bv).size();
- if(cur_size < min_size)
- {
- min_size = cur_size;
- min_idx[0] = i;
- min_idx[1] = j;
- }
- }
- }
-
- NodeType* n[2] = {leaves[min_idx[0]], leaves[min_idx[1]]};
- NodeType* p = createNode(NULL, n[0]->bv, n[1]->bv, NULL);
- p->childs[0] = n[0];
- p->childs[1] = n[1];
- n[0]->parent = p;
- n[1]->parent = p;
- leaves[min_idx[0]] = p;
- NodeType* tmp = leaves[min_idx[1]];
- leaves[min_idx[1]] = leaves.back();
- leaves.back() = tmp;
- leaves.pop_back();
- }
- }
-
-
/** \brief Insert a leaf node and also update its ancestors */
void insertLeaf(NodeType* root, NodeType* leaf)
{
diff --git a/trunk/fcl/src/BV/AABB.cpp b/trunk/fcl/src/BV/AABB.cpp
index 63099f7b..b645d52f 100644
--- a/trunk/fcl/src/BV/AABB.cpp
+++ b/trunk/fcl/src/BV/AABB.cpp
@@ -54,10 +54,10 @@ BVH_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const
BVH_REAL result = 0;
for(size_t i = 0; i < 3; ++i)
{
- BVH_REAL amin = min_[i];
- BVH_REAL amax = max_[i];
- BVH_REAL bmin = other.min_[i];
- BVH_REAL bmax = other.max_[i];
+ const BVH_REAL& amin = min_[i];
+ const BVH_REAL& amax = max_[i];
+ const BVH_REAL& bmin = other.min_[i];
+ const BVH_REAL& bmax = other.max_[i];
if(amin > bmax)
{
@@ -102,4 +102,29 @@ BVH_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const
return sqrt(result);
}
+BVH_REAL AABB::distance(const AABB& other) const
+{
+ BVH_REAL result = 0;
+ for(size_t i = 0; i < 3; ++i)
+ {
+ const BVH_REAL& amin = min_[i];
+ const BVH_REAL& amax = max_[i];
+ const BVH_REAL& bmin = other.min_[i];
+ const BVH_REAL& bmax = other.max_[i];
+
+ if(amin > bmax)
+ {
+ BVH_REAL delta = bmax - amin;
+ result += delta * delta;
+ }
+ else if(bmin > amax)
+ {
+ BVH_REAL delta = amax - bmin;
+ result += delta * delta;
+ }
+ }
+
+ return sqrt(result);
+}
+
}
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
index ca6560cd..a5c03031 100644
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ b/trunk/fcl/src/broad_phase_collision.cpp
@@ -159,8 +159,9 @@ void NaiveCollisionManager::collide(void* cdata, CollisionCallBack callback) con
it2 = it1; it2++;
for(; it2 != objs.end(); ++it2)
{
- if(callback(*it1, *it2, cdata))
- return;
+ if((*it1)->getAABB().overlap((*it2)->getAABB()))
+ if(callback(*it1, *it2, cdata))
+ return;
}
}
}
@@ -202,8 +203,9 @@ void NaiveCollisionManager::collide(BroadPhaseCollisionManager* other_manager_,
{
for(it2 = other_manager->objs.begin(); it2 != other_manager->objs.end(); ++it2)
{
- if(callback((*it1), (*it2), cdata))
- return;
+ if((*it1)->getAABB().overlap((*it2)->getAABB()))
+ if(callback((*it1), (*it2), cdata))
+ return;
}
}
}
@@ -539,11 +541,7 @@ bool SSaPCollisionManager::distance_(CollisionObject* obj, void* cdata, Distance
if(status == 1)
{
if(old_min_distance < std::numeric_limits<BVH_REAL>::max())
- {
- if(min_dist < old_min_distance) break;
- else // the initial min distance estimate is not correct, so turn to use conservative estimate, dummy_vector not changed this time.
- min_dist = std::numeric_limits<BVH_REAL>::max();
- }
+ break;
else
{
// from infinity to a finite one, only need one additional loop
@@ -1254,19 +1252,15 @@ bool SaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceC
}
else
{
- bool not_find;
- if(curr_obj < obj) not_find = (tested_set.find(std::make_pair(curr_obj, obj)) == tested_set.end());
- else not_find = (tested_set.find(std::make_pair(obj, curr_obj)) == tested_set.end());
- if(not_find)
+ if(!inTestedSet(curr_obj, obj))
{
if(pos->aabb->cached.distance(obj->getAABB()) < min_dist)
{
if(callback(curr_obj, obj, cdata, min_dist))
return true;
}
-
- if(curr_obj < obj) tested_set.insert(std::make_pair(curr_obj, obj));
- else tested_set.insert(std::make_pair(obj, curr_obj));
+
+ insertTestedSet(curr_obj, obj);
}
}
}
@@ -1278,11 +1272,7 @@ bool SaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceC
if(status == 1)
{
if(old_min_distance < std::numeric_limits<BVH_REAL>::max())
- {
- if(min_dist < old_min_distance) break;
- else
- min_dist = std::numeric_limits<BVH_REAL>::max();
- }
+ break;
else
{
if(min_dist < old_min_distance)
@@ -1334,13 +1324,19 @@ void SaPCollisionManager::distance(void* cdata, DistanceCallBack callback) const
{
if(size() == 0) return;
+ enable_tested_set_ = true;
+ tested_set.clear();
+
BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(); it != AABB_arr.end(); ++it)
{
if(distance_((*it)->obj, cdata, callback, min_dist))
- return;
+ break;
}
+
+ enable_tested_set_ = false;
+ tested_set.clear();
}
void SaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
@@ -1665,9 +1661,6 @@ void IntervalTreeCollisionManager::clear()
endpoints[1].clear();
endpoints[2].clear();
- for(int i = 0; i < 3; ++i)
- obj_interval_maps[i].clear();
-
delete interval_trees[0]; interval_trees[0] = NULL;
delete interval_trees[1]; interval_trees[1] = NULL;
delete interval_trees[2]; interval_trees[2] = NULL;
@@ -1681,6 +1674,9 @@ void IntervalTreeCollisionManager::clear()
}
}
+ for(int i = 0; i < 3; ++i)
+ obj_interval_maps[i].clear();
+
setup_ = false;
}
@@ -1809,11 +1805,7 @@ bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, void* cdata,
if(status == 1)
{
if(old_min_distance < std::numeric_limits<BVH_REAL>::max())
- {
- if(min_dist < old_min_distance) break;
- else
- min_dist = std::numeric_limits<BVH_REAL>::max();
- }
+ break;
else
{
if(min_dist < old_min_distance)
@@ -1902,7 +1894,7 @@ void IntervalTreeCollisionManager::distance(void* cdata, DistanceCallBack callba
BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
for(size_t i = 0; i < endpoints[0].size(); ++i)
- if(distance_(endpoints[0][i].obj, cdata, callback, min_dist)) return;
+ if(distance_(endpoints[0][i].obj, cdata, callback, min_dist)) break;
enable_tested_set_ = false;
tested_set.clear();
@@ -1994,11 +1986,7 @@ bool IntervalTreeCollisionManager::checkDist(std::deque<SimpleInterval*>::const_
}
else
{
- bool not_find;
- if(ivl->obj < obj) not_find = (tested_set.find(std::make_pair(ivl->obj, obj)) == tested_set.end());
- else not_find = (tested_set.find(std::make_pair(obj, ivl->obj)) == tested_set.end());
-
- if(not_find)
+ if(!inTestedSet(ivl->obj, obj))
{
if(ivl->obj->getAABB().distance(obj->getAABB()) < min_dist)
{
@@ -2006,8 +1994,7 @@ bool IntervalTreeCollisionManager::checkDist(std::deque<SimpleInterval*>::const_
return true;
}
- if(ivl->obj < obj) tested_set.insert(std::make_pair(ivl->obj, obj));
- else tested_set.insert(std::make_pair(obj, ivl->obj));
+ insertTestedSet(ivl->obj, obj);
}
}
}
@@ -2063,12 +2050,14 @@ void DynamicAABBTreeCollisionManager::setup()
{
if(!setup_)
{
- size_t height = dtree.getMaxHeight(dtree.getRoot());
- size_t num = dtree.size();
+ int height = dtree.getMaxHeight(dtree.getRoot());
+ int num = dtree.size();
+
if(height - std::log((BVH_REAL)num) / std::log(2.0) < max_tree_nonbalanced_level)
dtree.balanceIncremental(10);
else
dtree.balanceTopdown(tree_topdown_balance_threshold);
+
setup_ = true;
}
}
@@ -2076,6 +2065,7 @@ void DynamicAABBTreeCollisionManager::setup()
void DynamicAABBTreeCollisionManager::update()
{
+ /*
for(DynamicAABBTable::const_iterator it = table.begin(); it != table.end(); ++it)
{
CollisionObject* obj = it->first;
@@ -2083,6 +2073,18 @@ void DynamicAABBTreeCollisionManager::update()
if(!node->bv.equal(obj->getAABB()))
dtree.update(node, obj->getAABB());
}
+ */
+
+
+ for(DynamicAABBTable::const_iterator it = table.begin(); it != table.end(); ++it)
+ {
+ CollisionObject* obj = it->first;
+ DynamicAABBNode* node = it->second;
+ node->bv = obj->getAABB();
+ }
+
+ dtree.refit();
+
setup_ = false;
}
--
GitLab