From 51a9b8a62d04f3edd7f56626f3e0eb9dc74f88ee Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Tue, 3 Jul 2012 08:39:58 +0000
Subject: [PATCH] Fix all known bugs in broadphase.
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@115 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/include/fcl/broad_phase_collision.h | 26 +++++++-
trunk/fcl/include/fcl/hierarchy_tree.h | 13 ++--
trunk/fcl/src/broad_phase_collision.cpp | 61 ++++++++++++-------
trunk/fcl/src/collision.cpp | 2 +
trunk/fcl/src/distance.cpp | 2 +
5 files changed, 71 insertions(+), 33 deletions(-)
diff --git a/trunk/fcl/include/fcl/broad_phase_collision.h b/trunk/fcl/include/fcl/broad_phase_collision.h
index 44954b3f..a7867546 100644
--- a/trunk/fcl/include/fcl/broad_phase_collision.h
+++ b/trunk/fcl/include/fcl/broad_phase_collision.h
@@ -97,6 +97,18 @@ public:
/** \brief update the condition of manager */
virtual void update() = 0;
+ /** \brief update the manager by explicitly given the object updated */
+ virtual void update(CollisionObject* updated_obj)
+ {
+ update();
+ }
+
+ /** \brief update the manager by explicitly given the set of objects update */
+ virtual void update(const std::vector<CollisionObject*>& updated_objs)
+ {
+ update();
+ }
+
/** \brief clear the manager */
virtual void clear() = 0;
@@ -129,7 +141,6 @@ public:
protected:
mutable std::set<std::pair<CollisionObject*, CollisionObject*> > tested_set;
- mutable std::set<CollisionObject*> tested_set2;
mutable bool enable_tested_set_;
};
@@ -424,6 +435,7 @@ bool SpatialHashingCollisionManager<HashTable>::collide_(CollisionObject* obj, v
std::list<CollisionObject*>::const_iterator it;
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;
}
}
@@ -431,6 +443,7 @@ bool SpatialHashingCollisionManager<HashTable>::collide_(CollisionObject* obj, v
std::vector<CollisionObject*> query_result = hash_table->query(overlap_aabb);
for(unsigned int i = 0; i < query_result.size(); ++i)
{
+ if(obj == query_result[i]) continue;
if(callback(obj, query_result[i], cdata)) return true;
}
}
@@ -439,6 +452,7 @@ bool SpatialHashingCollisionManager<HashTable>::collide_(CollisionObject* obj, v
std::list<CollisionObject*>::const_iterator it;
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;
}
}
@@ -475,6 +489,7 @@ bool SpatialHashingCollisionManager<HashTable>::distance_(CollisionObject* obj,
std::list<CollisionObject*>::const_iterator it;
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;
}
@@ -483,6 +498,7 @@ bool SpatialHashingCollisionManager<HashTable>::distance_(CollisionObject* obj,
std::vector<CollisionObject*> query_result = hash_table->query(overlap_aabb);
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;
}
@@ -492,6 +508,7 @@ bool SpatialHashingCollisionManager<HashTable>::distance_(CollisionObject* obj,
std::list<CollisionObject*>::const_iterator it;
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;
}
@@ -1091,8 +1108,11 @@ public:
/** \brief update the condition of manager */
void update();
- /** \brief update a specific object */
- void update(CollisionObject* obj);
+ /** \brief update the manager by explicitly given the object updated */
+ void update(CollisionObject* updated_obj);
+
+ /** \brief update the manager by explicitly given the set of objects update */
+ void update(const std::vector<CollisionObject*>& updated_objs);
/** \brief clear the manager */
void clear()
diff --git a/trunk/fcl/include/fcl/hierarchy_tree.h b/trunk/fcl/include/fcl/hierarchy_tree.h
index 084d4f32..fb52b150 100644
--- a/trunk/fcl/include/fcl/hierarchy_tree.h
+++ b/trunk/fcl/include/fcl/hierarchy_tree.h
@@ -270,29 +270,26 @@ public:
return n_leaves;
}
- NodeType* getRoot() const
+ inline NodeType* getRoot() const
{
return root_node;
}
- NodeType*& getRoot()
+ inline NodeType*& getRoot()
{
return root_node;
}
void print(NodeType* root, int depth)
{
+ for(int i = 0; i < depth; ++i)
+ std::cout << " ";
+ std::cout << " (" << root->bv.min_[0] << ", " << root->bv.min_[1] << ", " << root->bv.min_[2] << "; " << root->bv.max_[0] << ", " << root->bv.max_[1] << ", " << root->bv.max_[2] << ")" << std::endl;
if(root->isLeaf())
{
- for(int i = 0; i < depth; ++i)
- std::cout << " ";
- std::cout << " (" << root->bv.min_[0] << ", " << root->bv.min_[1] << ", " << root->bv.min_[2] << "; " << root->bv.max_[0] << ", " << root->bv.max_[1] << ", " << root->bv.max_[2] << ")" << std::endl;
}
else
{
- for(int i = 0; i < depth; ++i)
- std::cout << " ";
- std::cout << " (" << root->bv.min_[0] << ", " << root->bv.min_[1] << ", " << root->bv.min_[2] << "; " << root->bv.max_[0] << ", " << root->bv.max_[1] << ", " << root->bv.max_[2] << ")" << std::endl;
print(root->childs[0], depth+1);
print(root->childs[1], depth+1);
}
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
index efde3b8c..1cc433b3 100644
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ b/trunk/fcl/src/broad_phase_collision.cpp
@@ -55,7 +55,8 @@ bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cd
if(cdata->done) return true;
std::vector<Contact> contacts;
- int num_contacts = collide(o1, o2, 1, false, false, contacts);
+ // int num_contacts = collide(o1, o2, 1, false, false, contacts);
+ int num_contacts = collide(o1, o2, cdata->num_max_contacts, cdata->exhaustive, cdata->enable_contact, contacts);
cdata->is_collision = (num_contacts > 0);
for(int i = 0; i < num_contacts; ++i)
@@ -1152,11 +1153,14 @@ bool SaPCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionC
while(pos != end_pos)
{
- if((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val))
+ if(pos->aabb->obj != obj)
{
- if(pos->aabb->cached.overlap(obj->getAABB()))
- if(callback(obj, pos->aabb->obj, cdata))
- return true;
+ if((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val))
+ {
+ if(pos->aabb->cached.overlap(obj->getAABB()))
+ if(callback(obj, pos->aabb->obj, cdata))
+ return true;
+ }
}
pos = pos->next[axis];
}
@@ -1220,26 +1224,32 @@ bool SaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceC
if((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val))
{
CollisionObject* curr_obj = pos->aabb->obj;
- if(!enable_tested_set_)
+ if(curr_obj != obj)
{
- if(pos->aabb->cached.distance(obj->getAABB()) < min_dist)
- {
- if(callback(curr_obj, obj, cdata, min_dist))
- return true;
- }
- }
- else
- {
- bool not_find = (tested_set2.find(curr_obj) == tested_set2.end());
- if(not_find)
+ if(!enable_tested_set_)
{
if(pos->aabb->cached.distance(obj->getAABB()) < min_dist)
{
if(callback(curr_obj, obj, cdata, min_dist))
return true;
}
-
- tested_set2.insert(curr_obj);
+ }
+ 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(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));
+ }
}
}
}
@@ -1907,6 +1917,7 @@ void DynamicAABBTreeCollisionManager::registerObjects(const std::vector<Collisio
else
{
std::vector<DynamicAABBNode*> leaves(other_objs.size());
+ table.rehash(other_objs.size());
for(size_t i = 0; i < other_objs.size(); ++i)
{
DynamicAABBNode* node = new DynamicAABBNode;
@@ -1963,18 +1974,24 @@ void DynamicAABBTreeCollisionManager::update()
setup_ = false;
}
-void DynamicAABBTreeCollisionManager::update(CollisionObject* obj)
+void DynamicAABBTreeCollisionManager::update(CollisionObject* updated_obj)
{
- DynamicAABBTable::const_iterator it = table.find(obj);
+ DynamicAABBTable::const_iterator it = table.find(updated_obj);
if(it != table.end())
{
DynamicAABBNode* node = it->second;
- if(!node->bv.equal(obj->getAABB()))
- dtree.update(node, obj->getAABB());
+ if(!node->bv.equal(updated_obj->getAABB()))
+ dtree.update(node, updated_obj->getAABB());
}
setup_ = false;
}
+void DynamicAABBTreeCollisionManager::update(const std::vector<CollisionObject*>& updated_objs)
+{
+ for(size_t i = 0; i < updated_objs.size(); ++i)
+ update(updated_objs[i]);
+}
+
bool DynamicAABBTreeCollisionManager::collisionRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, CollisionCallBack callback) const
{
if(root1->isLeaf() && root2->isLeaf())
diff --git a/trunk/fcl/src/collision.cpp b/trunk/fcl/src/collision.cpp
index 46be2c23..64a2b194 100644
--- a/trunk/fcl/src/collision.cpp
+++ b/trunk/fcl/src/collision.cpp
@@ -137,6 +137,8 @@ int collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
{
GJKSolver_libccd solver;
return collide<GJKSolver_libccd>(o1, tf1, o2, tf2, &solver, num_max_contacts, exhaustive, enable_contact, contacts);
+ // GJKSolver_indep solver;
+ // return collide<GJKSolver_indep>(o1, tf1, o2, tf2, &solver, num_max_contacts, exhaustive, enable_contact, contacts);
}
}
diff --git a/trunk/fcl/src/distance.cpp b/trunk/fcl/src/distance.cpp
index b3883a89..e9cfdda4 100644
--- a/trunk/fcl/src/distance.cpp
+++ b/trunk/fcl/src/distance.cpp
@@ -121,6 +121,8 @@ BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
{
GJKSolver_libccd solver;
return distance<GJKSolver_libccd>(o1, tf1, o2, tf2, &solver);
+ // GJKSolver_indep solver;
+ // return distance<GJKSolver_indep>(o1, tf1, o2, tf2, &solver);
}
--
GitLab