From 44e916dd2dcc09ebb6ccdbbf3671e383963eac84 Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Mon, 2 Jul 2012 22:59:52 +0000
Subject: [PATCH] fix more bugs. the new dynamicAABBTree broadphase works quite
well in both distance and collision. besides the old self-collision,
collision with an object, distance to an object interface, added
self-distance, collision with a set of objects, distance to a set of objects.
This simplifies the function in collision_detection_fcl and also makes some
optimization possible.
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@114 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/include/fcl/broad_phase_collision.h | 284 ++----------
trunk/fcl/include/fcl/hierarchy_tree.h | 83 ++--
trunk/fcl/src/broad_phase_collision.cpp | 432 ++++++++++++++++--
3 files changed, 474 insertions(+), 325 deletions(-)
diff --git a/trunk/fcl/include/fcl/broad_phase_collision.h b/trunk/fcl/include/fcl/broad_phase_collision.h
index 4228b5bc..44954b3f 100644
--- a/trunk/fcl/include/fcl/broad_phase_collision.h
+++ b/trunk/fcl/include/fcl/broad_phase_collision.h
@@ -71,6 +71,11 @@ typedef bool (*DistanceCallBack)(CollisionObject* o1, CollisionObject* o2, void*
class BroadPhaseCollisionManager
{
public:
+ BroadPhaseCollisionManager()
+ {
+ enable_tested_set_ = false;
+ }
+
virtual ~BroadPhaseCollisionManager() {}
/** \brief add objects to the manager */
@@ -121,6 +126,12 @@ public:
/** \brief the number of objects managed by the manager */
virtual size_t size() const = 0;
+
+protected:
+ mutable std::set<std::pair<CollisionObject*, CollisionObject*> > tested_set;
+ mutable std::set<CollisionObject*> tested_set2;
+ mutable bool enable_tested_set_;
+
};
@@ -305,7 +316,7 @@ protected:
// all objects in the scene
- std::list<CollisionObject*> objs;
+ std::list<CollisionObject*> objs;
// objects in the scene limit (given by scene_min and scene_max) are in the spatial hash table
HashTable* hash_table;
@@ -635,7 +646,6 @@ public:
elist[0] = NULL;
elist[1] = NULL;
elist[2] = NULL;
- enable_tested_set = true;
}
~SaPCollisionManager()
@@ -830,8 +840,7 @@ protected:
bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const;
-public:
- bool enable_tested_set;
+ bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
};
@@ -1017,17 +1026,6 @@ protected:
char minmax;
};
- /** \brief Functor for sorting end points */
- struct SortByValue
- {
- bool operator()(const EndPoint& a, const EndPoint& b) const
- {
- if(a.value < b.value)
- return true;
- return false;
- }
- };
-
/** \brief Extention interval tree's interval to SAP interval, adding more information */
struct SAPInterval : public SimpleInterval
{
@@ -1057,7 +1055,6 @@ protected:
/** \brief tag for whether the interval tree is maintained suitably */
bool setup_;
-
};
@@ -1076,57 +1073,26 @@ public:
max_tree_nonbalanced_level = 4;
tree_incremental_balance_pass = 10;
tree_topdown_balance_threshold = 128;
+ setup_ = false;
}
+
+ /** \brief add objects to the manager */
+ void registerObjects(const std::vector<CollisionObject*>& other_objs);
/** \brief add one object to the manager */
- void registerObject(CollisionObject* obj)
- {
- DynamicAABBNode* node = dtree.insert(obj->getAABB(), obj);
- table[obj] = node;
- }
+ void registerObject(CollisionObject* obj);
/** \brief remove one object from the manager */
- void unregisterObject(CollisionObject* obj)
- {
- DynamicAABBNode* node = table[obj];
- table.erase(obj);
- dtree.remove(node);
- }
+ void unregisterObject(CollisionObject* obj);
/** \brief initialize the manager, related with the specific type of manager */
- void setup()
- {
- size_t height = dtree.getMaxHeight(dtree.getRoot());
- size_t 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);
- }
+ void setup();
/** \brief update the condition of manager */
- void update()
- {
- for(DynamicAABBTable::const_iterator it = table.begin(); it != table.end(); ++it)
- {
- CollisionObject* obj = it->first;
- DynamicAABBNode* node = it->second;
- if(!node->bv.equal(obj->getAABB()))
- dtree.update(node, obj->getAABB());
- }
- }
+ void update();
/** \brief update a specific object */
- void update(CollisionObject* obj)
- {
- DynamicAABBTable::const_iterator it = table.find(obj);
- if(it != table.end())
- {
- DynamicAABBNode* node = it->second;
- if(!node->bv.equal(obj->getAABB()))
- dtree.update(node, obj->getAABB());
- }
- }
+ void update(CollisionObject* obj);
/** \brief clear the manager */
void clear()
@@ -1200,211 +1166,19 @@ private:
HierarchyTree<AABB> dtree;
boost::unordered_map<CollisionObject*, DynamicAABBNode*> table;
- bool collisionRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, CollisionCallBack callback) const
- {
- if(root1->isLeaf() && root2->isLeaf())
- {
- if(!root1->bv.overlap(root2->bv)) return false;
- return callback(static_cast<CollisionObject*>(root1->data), static_cast<CollisionObject*>(root2->data), cdata);
- }
-
- if(!root1->bv.overlap(root2->bv)) return false;
-
- if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size())))
- {
- if(collisionRecurse(root1->childs[0], root2, cdata, callback))
- return true;
- if(collisionRecurse(root1->childs[1], root2, cdata, callback))
- return true;
- }
- else
- {
- if(collisionRecurse(root1, root2->childs[0], cdata, callback))
- return true;
- if(collisionRecurse(root1, root2->childs[1], cdata, callback))
- return true;
- }
- return false;
- }
-
- bool collisionRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, CollisionCallBack callback) const
- {
- if(root->isLeaf())
- {
- if(!root->bv.overlap(query->getAABB())) return false;
- return callback(static_cast<CollisionObject*>(root->data), query, cdata);
- }
-
- if(!root->bv.overlap(query->getAABB())) return false;
-
- int select_res = select(query->getAABB(), *(root->childs[0]), *(root->childs[1]));
-
- if(collisionRecurse(root->childs[select_res], query, cdata, callback))
- return true;
-
- if(collisionRecurse(root->childs[1-select_res], query, cdata, callback))
- return true;
-
- return false;
- }
-
- bool selfCollisionRecurse(DynamicAABBNode* root, void* cdata, CollisionCallBack callback) const
- {
- if(root->isLeaf()) return false;
-
- if(selfCollisionRecurse(root->childs[0], cdata, callback))
- return true;
-
- if(selfCollisionRecurse(root->childs[1], cdata, callback))
- return true;
-
- if(collisionRecurse(root->childs[0], root->childs[1], cdata, callback))
- return true;
-
- return false;
- }
-
- bool distanceRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const
- {
- if(root1->isLeaf() && root2->isLeaf())
- {
- CollisionObject* root1_obj = static_cast<CollisionObject*>(root1->data);
- CollisionObject* root2_obj = static_cast<CollisionObject*>(root2->data);
- return callback(root1_obj, root2_obj, cdata, min_dist);
- }
-
- if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size())))
- {
- BVH_REAL d1 = root2->bv.distance(root1->childs[0]->bv);
- BVH_REAL d2 = root2->bv.distance(root1->childs[1]->bv);
-
- if(d2 < d1)
- {
- if(d2 < min_dist)
- {
- if(distanceRecurse(root1->childs[1], root2, cdata, callback, min_dist))
- return true;
- }
-
- if(d1 < min_dist)
- {
- if(distanceRecurse(root1->childs[0], root2, cdata, callback, min_dist))
- return true;
- }
- }
- else
- {
- if(d1 < min_dist)
- {
- if(distanceRecurse(root1->childs[0], root2, cdata, callback, min_dist))
- return true;
- }
-
- if(d2 < min_dist)
- {
- if(distanceRecurse(root1->childs[1], root2, cdata, callback, min_dist))
- return true;
- }
- }
- }
- else
- {
- BVH_REAL d1 = root1->bv.distance(root2->childs[0]->bv);
- BVH_REAL d2 = root1->bv.distance(root2->childs[1]->bv);
-
- if(d2 < d1)
- {
- if(d2 < min_dist)
- {
- if(distanceRecurse(root1, root2->childs[1], cdata, callback, min_dist))
- return true;
- }
-
- if(d1 < min_dist)
- {
- if(distanceRecurse(root1, root2->childs[0], cdata, callback, min_dist))
- return true;
- }
- }
- else
- {
- if(d1 < min_dist)
- {
- if(distanceRecurse(root1, root2->childs[0], cdata, callback, min_dist))
- return true;
- }
-
- if(d2 < min_dist)
- {
- if(distanceRecurse(root1, root2->childs[1], cdata, callback, min_dist))
- return true;
- }
- }
- }
-
- return false;
- }
-
- bool distanceRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const
- {
- if(root->isLeaf())
- {
- CollisionObject* root_obj = static_cast<CollisionObject*>(root->data);
- return callback(root_obj, query, cdata, min_dist);
- }
-
- BVH_REAL d1 = query->getAABB().distance(root->childs[0]->bv);
- BVH_REAL d2 = query->getAABB().distance(root->childs[1]->bv);
-
- if(d2 < d1)
- {
- if(d2 < min_dist)
- {
- if(distanceRecurse(root->childs[1], query, cdata, callback, min_dist))
- return true;
- }
-
- if(d1 < min_dist)
- {
- if(distanceRecurse(root->childs[0], query, cdata, callback, min_dist))
- return true;
- }
- }
- else
- {
- if(d1 < min_dist)
- {
- if(distanceRecurse(root->childs[0], query, cdata, callback, min_dist))
- return true;
- }
-
- if(d2 < min_dist)
- {
- if(distanceRecurse(root->childs[1], query, cdata, callback, min_dist))
- return true;
- }
- }
-
- return false;
- }
+ bool setup_;
- bool selfDistanceRecurse(DynamicAABBNode* root, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const
- {
- if(root->isLeaf()) return false;
+ bool collisionRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, CollisionCallBack callback) const;
- if(selfDistanceRecurse(root->childs[0], cdata, callback, min_dist))
- return true;
+ bool collisionRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, CollisionCallBack callback) const;
- if(selfDistanceRecurse(root->childs[1], cdata, callback, min_dist))
- return true;
+ bool selfCollisionRecurse(DynamicAABBNode* root, void* cdata, CollisionCallBack callback) const;
- if(distanceRecurse(root->childs[0], root->childs[1], cdata, callback, min_dist))
- return true;
+ bool distanceRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const;
- return false;
- }
+ bool distanceRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const;
-
+ bool selfDistanceRecurse(DynamicAABBNode* root, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const;
};
diff --git a/trunk/fcl/include/fcl/hierarchy_tree.h b/trunk/fcl/include/fcl/hierarchy_tree.h
index 026a5ba8..084d4f32 100644
--- a/trunk/fcl/include/fcl/hierarchy_tree.h
+++ b/trunk/fcl/include/fcl/hierarchy_tree.h
@@ -189,7 +189,8 @@ public:
{
if(root_node)
{
- std::vector<NodeType*> leaves(n_leaves);
+ std::vector<NodeType*> leaves;
+ leaves.reserve(n_leaves);
fetchLeaves(root_node, leaves);
bottomup(leaves);
root_node = leaves[0];
@@ -201,10 +202,10 @@ public:
{
if(root_node)
{
- std::vector<NodeType*> leaves(n_leaves);
+ std::vector<NodeType*> leaves;
+ leaves.reserve(n_leaves);
fetchLeaves(root_node, leaves);
- topdown(leaves, bu_threshold);
- root_node = leaves[0];
+ root_node = topdown(leaves, bu_threshold);
}
}
@@ -274,35 +275,28 @@ public:
return root_node;
}
-private:
+ NodeType*& getRoot()
+ {
+ return root_node;
+ }
- /** \brief sort node n and its parent according to their memory position */
- NodeType* sort(NodeType* n, NodeType*& r)
+ void print(NodeType* root, int depth)
{
- NodeType* p = n->parent;
- if(p > n)
+ if(root->isLeaf())
{
- int i = indexOf(n);
- int j = 1 - i;
- NodeType* s = p->childs[j];
- NodeType* q = p->parent;
- if(q) q->childs[indexOf(p)] = n; else r = n;
- s->parent = n;
- p->parent = n;
- n->parent = q;
- p->childs[0] = n->childs[0];
- p->childs[1] = n->childs[1];
- n->childs[0]->parent = p;
- n->childs[1]->parent = p;
- n->childs[i] = p;
- n->childs[j] = s;
- std::swap(p->bv, n->bv);
- return p;
+ 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);
}
- return n;
}
-
-
/** \brief construct a tree for a set of leaves from top */
NodeType* topdown(std::vector<NodeType*>& leaves, int bu_threshold)
@@ -326,7 +320,6 @@ private:
++splitcount[j][x.dot(axis[j]) > 0 ? 1 : 0];
}
}
-
for(size_t i = 0; i < 3; ++i)
{
@@ -373,6 +366,34 @@ private:
return leaves[0];
}
+private:
+
+ /** \brief sort node n and its parent according to their memory position */
+ NodeType* sort(NodeType* n, NodeType*& r)
+ {
+ NodeType* p = n->parent;
+ if(p > n)
+ {
+ int i = indexOf(n);
+ int j = 1 - i;
+ NodeType* s = p->childs[j];
+ NodeType* q = p->parent;
+ if(q) q->childs[indexOf(p)] = n; else r = n;
+ s->parent = n;
+ p->parent = n;
+ n->parent = q;
+ p->childs[0] = n->childs[0];
+ p->childs[1] = n->childs[1];
+ n->childs[0]->parent = p;
+ n->childs[1]->parent = p;
+ n->childs[i] = p;
+ n->childs[j] = s;
+ std::swap(p->bv, n->bv);
+ return p;
+ }
+ return n;
+ }
+
/** \brief construct a tree for a set of leaves from bottom */
void bottomup(std::vector<NodeType*>& leaves)
@@ -499,14 +520,16 @@ private:
/** \brief Delete all internal nodes and return all leaves nodes with given depth from root */
void fetchLeaves(NodeType* root, std::vector<NodeType*>& leaves, int depth = -1)
{
- if(!root->isLeaf() && depth)
+ if((!root->isLeaf()) && depth)
{
fetchLeaves(root->childs[0], leaves, depth-1);
fetchLeaves(root->childs[1], leaves, depth-1);
deleteNode(root);
}
else
+ {
leaves.push_back(root);
+ }
}
static size_t indexOf(NodeType* node)
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
index b72cb8e9..efde3b8c 100644
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ b/trunk/fcl/src/broad_phase_collision.cpp
@@ -560,9 +560,11 @@ bool SSaPCollisionManager::distance_(CollisionObject* obj, void* cdata, Distance
dummy_vector = dummy_vector * 2 - obj->getAABB().max_;
}
}
- if(pos_end1 != objs_x.end()) pos_start1 = pos_end1;
- if(pos_end2 != objs_y.end()) pos_start2 = pos_end2;
- if(pos_end3 != objs_z.end()) pos_start3 = pos_end3;
+
+ // yes, following is wrong, will result in too large distance.
+ // if(pos_end1 != objs_x.end()) pos_start1 = pos_end1;
+ // if(pos_end2 != objs_y.end()) pos_start2 = pos_end2;
+ // if(pos_end3 != objs_z.end()) pos_start3 = pos_end3;
}
else if(status == 0)
break;
@@ -1122,34 +1124,51 @@ void SaPCollisionManager::getObjects(std::vector<CollisionObject*>& objs) const
}
}
-void SaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
+bool SaPCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
{
- if(size() == 0) return;
-
size_t axis = optimal_axis;
const AABB& obj_aabb = obj->getAABB();
BVH_REAL min_val = obj_aabb.min_[axis];
BVH_REAL max_val = obj_aabb.max_[axis];
+
+ EndPoint dummy;
+ SaPAABB dummy_aabb;
+ dummy_aabb.cached = obj_aabb;
+ dummy.minmax = 1;
+ dummy.aabb = &dummy_aabb;
- std::vector<EndPoint*>::const_iterator res_it = std::lower_bound(velist[axis].begin(), velist[axis].end(), min_val,
+ // compute stop_pos by binary search, this is cheaper than check it in while iteration linearly
+ std::vector<EndPoint*>::const_iterator res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy,
boost::bind(std::less<BVH_REAL>(),
boost::bind(static_cast<BVH_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _1, axis),
- min_val));
-
- if(res_it == velist[axis].end()) return;
+ boost::bind(static_cast<BVH_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _2, axis)));
- EndPoint* pos = *res_it;
+ EndPoint* end_pos = NULL;
+ if(res_it != velist[axis].end())
+ end_pos = *res_it;
- while((pos != NULL) && (pos->getVal(axis) <= max_val))
+ EndPoint* pos = elist[axis];
+
+ while(pos != end_pos)
{
- if(pos->aabb->cached.overlap(obj->getAABB()))
+ if((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val))
{
- if(callback(obj, pos->aabb->obj, cdata))
- return;
+ if(pos->aabb->cached.overlap(obj->getAABB()))
+ if(callback(obj, pos->aabb->obj, cdata))
+ return true;
}
pos = pos->next[axis];
}
+
+ return false;
+}
+
+void SaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
+{
+ if(size() == 0) return;
+
+ collide_(obj, cdata, callback);
}
bool SaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const
@@ -1170,8 +1189,6 @@ bool SaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceC
EndPoint* start_pos = elist[axis];
- std::set<CollisionObject*> tested;
-
while(1)
{
old_min_distance = min_dist;
@@ -1203,14 +1220,27 @@ 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 || (tested.find(curr_obj) == tested.end()))
+ if(!enable_tested_set_)
{
if(pos->aabb->cached.distance(obj->getAABB()) < min_dist)
{
if(callback(curr_obj, obj, cdata, min_dist))
return true;
}
- if(enable_tested_set) tested.insert(curr_obj);
+ }
+ else
+ {
+ bool not_find = (tested_set2.find(curr_obj) == tested_set2.end());
+ if(not_find)
+ {
+ if(pos->aabb->cached.distance(obj->getAABB()) < min_dist)
+ {
+ if(callback(curr_obj, obj, cdata, min_dist))
+ return true;
+ }
+
+ tested_set2.insert(curr_obj);
+ }
}
}
@@ -1294,11 +1324,20 @@ void SaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, vo
return;
}
- for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(); it != AABB_arr.end(); ++it)
+ if(this->size() < other_manager->size())
{
- for(std::list<SaPAABB*>::const_iterator it2 = other_manager->AABB_arr.begin(); it2 != other_manager->AABB_arr.end(); ++it2)
+ for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(); it != AABB_arr.end(); ++it)
{
- if(callback((*it)->obj, (*it2)->obj, cdata)) return;
+ if(other_manager->collide_((*it)->obj, cdata, callback))
+ return;
+ }
+ }
+ else
+ {
+ for(std::list<SaPAABB*>::const_iterator it = other_manager->AABB_arr.begin(); it != other_manager->AABB_arr.end(); ++it)
+ {
+ if(collide_((*it)->obj, cdata, callback))
+ return;
}
}
}
@@ -1313,14 +1352,21 @@ void SaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, v
}
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(this->size() < other_manager->size())
{
- for(std::list<SaPAABB*>::const_iterator it2 = other_manager->AABB_arr.begin(); it2 != other_manager->AABB_arr.end(); ++it2)
+ for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(); it != AABB_arr.end(); ++it)
{
- if((*it)->obj->getAABB().distance((*it2)->obj->getAABB()) < min_dist)
- {
- if(callback((*it)->obj, (*it2)->obj, cdata, min_dist)) return;
- }
+ if(other_manager->distance_((*it)->obj, cdata, callback, min_dist))
+ return;
+ }
+ }
+ else
+ {
+ for(std::list<SaPAABB*>::const_iterator it = other_manager->AABB_arr.begin(); it != other_manager->AABB_arr.end(); ++it)
+ {
+ if(distance_((*it)->obj, cdata, callback, min_dist))
+ return;
}
}
}
@@ -1339,9 +1385,9 @@ void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj)
EndPoint p;
p.value = obj->getAABB().min_[0];
- std::vector<EndPoint>::iterator start1 = std::lower_bound(endpoints[0].begin(), endpoints[0].end(), p, SortByValue());
+ std::vector<EndPoint>::iterator start1 = std::lower_bound(endpoints[0].begin(), endpoints[0].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
p.value = obj->getAABB().max_[0];
- std::vector<EndPoint>::iterator end1 = std::upper_bound(start1, endpoints[0].end(), p, SortByValue());
+ std::vector<EndPoint>::iterator end1 = std::upper_bound(start1, endpoints[0].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
if(start1 < end1)
{
@@ -1365,9 +1411,9 @@ void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj)
}
p.value = obj->getAABB().min_[1];
- std::vector<EndPoint>::iterator start2 = std::lower_bound(endpoints[1].begin(), endpoints[1].end(), p, SortByValue());
+ std::vector<EndPoint>::iterator start2 = std::lower_bound(endpoints[1].begin(), endpoints[1].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
p.value = obj->getAABB().max_[1];
- std::vector<EndPoint>::iterator end2 = std::upper_bound(start2, endpoints[1].end(), p, SortByValue());
+ std::vector<EndPoint>::iterator end2 = std::upper_bound(start2, endpoints[1].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
if(start2 < end2)
{
@@ -1392,9 +1438,9 @@ void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj)
p.value = obj->getAABB().min_[2];
- std::vector<EndPoint>::iterator start3 = std::lower_bound(endpoints[2].begin(), endpoints[2].end(), p, SortByValue());
+ std::vector<EndPoint>::iterator start3 = std::lower_bound(endpoints[2].begin(), endpoints[2].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
p.value = obj->getAABB().max_[2];
- std::vector<EndPoint>::iterator end3 = std::upper_bound(start3, endpoints[2].end(), p, SortByValue());
+ std::vector<EndPoint>::iterator end3 = std::upper_bound(start3, endpoints[2].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
if(start3 < end3)
{
@@ -1447,9 +1493,9 @@ void IntervalTreeCollisionManager::setup()
{
if(!setup_)
{
- std::sort(endpoints[0].begin(), endpoints[0].end(), SortByValue());
- std::sort(endpoints[1].begin(), endpoints[1].end(), SortByValue());
- std::sort(endpoints[2].begin(), endpoints[2].end(), SortByValue());
+ std::sort(endpoints[0].begin(), endpoints[0].end(), boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
+ std::sort(endpoints[1].begin(), endpoints[1].end(), boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
+ std::sort(endpoints[2].begin(), endpoints[2].end(), boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2));
for(int i = 0; i < 3; ++i)
delete interval_trees[i];
@@ -1729,10 +1775,15 @@ void IntervalTreeCollisionManager::collide(void* cdata, CollisionCallBack callba
void IntervalTreeCollisionManager::distance(void* cdata, DistanceCallBack callback) const
{
+ enable_tested_set_ = true;
+ tested_set.clear();
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;
+
+ enable_tested_set_ = false;
+ tested_set.clear();
}
void IntervalTreeCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
@@ -1811,10 +1862,31 @@ bool IntervalTreeCollisionManager::checkDist(std::deque<SimpleInterval*>::const_
SAPInterval* ivl = static_cast<SAPInterval*>(*pos_start);
if(ivl->obj != obj)
{
- if(ivl->obj->getAABB().distance(obj->getAABB()) < min_dist)
+ if(!enable_tested_set_)
{
- if(callback(ivl->obj, obj, cdata, min_dist))
- return true;
+ if(ivl->obj->getAABB().distance(obj->getAABB()) < min_dist)
+ {
+ if(callback(ivl->obj, obj, cdata, min_dist))
+ return true;
+ }
+ }
+ 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(ivl->obj->getAABB().distance(obj->getAABB()) < min_dist)
+ {
+ if(callback(ivl->obj, obj, cdata, min_dist))
+ 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));
+ }
}
}
@@ -1826,5 +1898,285 @@ bool IntervalTreeCollisionManager::checkDist(std::deque<SimpleInterval*>::const_
+void DynamicAABBTreeCollisionManager::registerObjects(const std::vector<CollisionObject*>& other_objs)
+{
+ if(size() > 0)
+ {
+ BroadPhaseCollisionManager::registerObjects(other_objs);
+ }
+ else
+ {
+ std::vector<DynamicAABBNode*> leaves(other_objs.size());
+ for(size_t i = 0; i < other_objs.size(); ++i)
+ {
+ DynamicAABBNode* node = new DynamicAABBNode;
+ node->bv = other_objs[i]->getAABB();
+ node->parent = NULL;
+ node->childs[1] = NULL;
+ node->data = other_objs[i];
+ table[other_objs[i]] = node;
+ leaves[i] = node;
+ }
+
+ dtree.getRoot() = dtree.topdown(leaves, tree_topdown_balance_threshold);
+ setup_ = true;
+ }
+}
+
+void DynamicAABBTreeCollisionManager::registerObject(CollisionObject* obj)
+{
+ DynamicAABBNode* node = dtree.insert(obj->getAABB(), obj);
+ table[obj] = node;
+}
+
+void DynamicAABBTreeCollisionManager::unregisterObject(CollisionObject* obj)
+{
+ DynamicAABBNode* node = table[obj];
+ table.erase(obj);
+ dtree.remove(node);
+}
+
+void DynamicAABBTreeCollisionManager::setup()
+{
+ if(!setup_)
+ {
+ size_t height = dtree.getMaxHeight(dtree.getRoot());
+ size_t 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;
+ }
+}
+
+
+void DynamicAABBTreeCollisionManager::update()
+{
+ for(DynamicAABBTable::const_iterator it = table.begin(); it != table.end(); ++it)
+ {
+ CollisionObject* obj = it->first;
+ DynamicAABBNode* node = it->second;
+ if(!node->bv.equal(obj->getAABB()))
+ dtree.update(node, obj->getAABB());
+ }
+ setup_ = false;
+}
+
+void DynamicAABBTreeCollisionManager::update(CollisionObject* obj)
+{
+ DynamicAABBTable::const_iterator it = table.find(obj);
+ if(it != table.end())
+ {
+ DynamicAABBNode* node = it->second;
+ if(!node->bv.equal(obj->getAABB()))
+ dtree.update(node, obj->getAABB());
+ }
+ setup_ = false;
+}
+
+bool DynamicAABBTreeCollisionManager::collisionRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, CollisionCallBack callback) const
+{
+ if(root1->isLeaf() && root2->isLeaf())
+ {
+ if(!root1->bv.overlap(root2->bv)) return false;
+ return callback(static_cast<CollisionObject*>(root1->data), static_cast<CollisionObject*>(root2->data), cdata);
+ }
+
+ if(!root1->bv.overlap(root2->bv)) return false;
+
+ if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size())))
+ {
+ if(collisionRecurse(root1->childs[0], root2, cdata, callback))
+ return true;
+ if(collisionRecurse(root1->childs[1], root2, cdata, callback))
+ return true;
+ }
+ else
+ {
+ if(collisionRecurse(root1, root2->childs[0], cdata, callback))
+ return true;
+ if(collisionRecurse(root1, root2->childs[1], cdata, callback))
+ return true;
+ }
+ return false;
+}
+
+bool DynamicAABBTreeCollisionManager::collisionRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, CollisionCallBack callback) const
+{
+ if(root->isLeaf())
+ {
+ if(!root->bv.overlap(query->getAABB())) return false;
+ return callback(static_cast<CollisionObject*>(root->data), query, cdata);
+ }
+
+ if(!root->bv.overlap(query->getAABB())) return false;
+
+ int select_res = select(query->getAABB(), *(root->childs[0]), *(root->childs[1]));
+
+ if(collisionRecurse(root->childs[select_res], query, cdata, callback))
+ return true;
+
+ if(collisionRecurse(root->childs[1-select_res], query, cdata, callback))
+ return true;
+
+ return false;
+}
+
+bool DynamicAABBTreeCollisionManager::selfCollisionRecurse(DynamicAABBNode* root, void* cdata, CollisionCallBack callback) const
+{
+ if(root->isLeaf()) return false;
+
+ if(selfCollisionRecurse(root->childs[0], cdata, callback))
+ return true;
+
+ if(selfCollisionRecurse(root->childs[1], cdata, callback))
+ return true;
+
+ if(collisionRecurse(root->childs[0], root->childs[1], cdata, callback))
+ return true;
+
+ return false;
+}
+
+bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const
+{
+ if(root1->isLeaf() && root2->isLeaf())
+ {
+ CollisionObject* root1_obj = static_cast<CollisionObject*>(root1->data);
+ CollisionObject* root2_obj = static_cast<CollisionObject*>(root2->data);
+ return callback(root1_obj, root2_obj, cdata, min_dist);
+ }
+
+ if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size())))
+ {
+ BVH_REAL d1 = root2->bv.distance(root1->childs[0]->bv);
+ BVH_REAL d2 = root2->bv.distance(root1->childs[1]->bv);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(root1->childs[1], root2, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(root1->childs[0], root2, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(root1->childs[0], root2, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(root1->childs[1], root2, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ }
+ else
+ {
+ BVH_REAL d1 = root1->bv.distance(root2->childs[0]->bv);
+ BVH_REAL d2 = root1->bv.distance(root2->childs[1]->bv);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(root1, root2->childs[1], cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(root1, root2->childs[0], cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(root1, root2->childs[0], cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(root1, root2->childs[1], cdata, callback, min_dist))
+ return true;
+ }
+ }
+ }
+
+ return false;
+}
+
+bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const
+{
+ if(root->isLeaf())
+ {
+ CollisionObject* root_obj = static_cast<CollisionObject*>(root->data);
+ return callback(root_obj, query, cdata, min_dist);
+ }
+
+ BVH_REAL d1 = query->getAABB().distance(root->childs[0]->bv);
+ BVH_REAL d2 = query->getAABB().distance(root->childs[1]->bv);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(root->childs[1], query, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(root->childs[0], query, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(root->childs[0], query, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(root->childs[1], query, cdata, callback, min_dist))
+ return true;
+ }
+ }
+
+ return false;
+}
+
+bool DynamicAABBTreeCollisionManager::selfDistanceRecurse(DynamicAABBNode* root, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const
+{
+ if(root->isLeaf()) return false;
+
+ if(selfDistanceRecurse(root->childs[0], cdata, callback, min_dist))
+ return true;
+
+ if(selfDistanceRecurse(root->childs[1], cdata, callback, min_dist))
+ return true;
+
+ if(distanceRecurse(root->childs[0], root->childs[1], cdata, callback, min_dist))
+ return true;
+
+ return false;
+}
}
--
GitLab