00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00038 #ifndef FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_H
00039 #define FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_H
00040
00041 #include "fcl/broadphase/broadphase.h"
00042 #include "fcl/broadphase/hierarchy_tree.h"
00043 #include "fcl/octree.h"
00044 #include "fcl/BV/BV.h"
00045 #include "fcl/shape/geometric_shapes_utility.h"
00046 #include <boost/unordered_map.hpp>
00047 #include <boost/bind.hpp>
00048 #include <limits>
00049
00050
00051 namespace fcl
00052 {
00053
00054 class DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager
00055 {
00056 public:
00057 typedef NodeBase<AABB> DynamicAABBNode;
00058 typedef boost::unordered_map<CollisionObject*, DynamicAABBNode*> DynamicAABBTable;
00059
00060 int max_tree_nonbalanced_level;
00061 int tree_incremental_balance_pass;
00062 int& tree_topdown_balance_threshold;
00063 int& tree_topdown_level;
00064 int tree_init_level;
00065
00066 bool octree_as_geometry_collide;
00067 bool octree_as_geometry_distance;
00068
00069
00070 DynamicAABBTreeCollisionManager() : tree_topdown_balance_threshold(dtree.bu_threshold),
00071 tree_topdown_level(dtree.topdown_level)
00072 {
00073 max_tree_nonbalanced_level = 10;
00074 tree_incremental_balance_pass = 10;
00075 tree_topdown_balance_threshold = 2;
00076 tree_topdown_level = 0;
00077 tree_init_level = 0;
00078 setup_ = false;
00079
00080
00081 octree_as_geometry_collide = true;
00082 octree_as_geometry_distance = false;
00083 }
00084
00086 void registerObjects(const std::vector<CollisionObject*>& other_objs);
00087
00089 void registerObject(CollisionObject* obj);
00090
00092 void unregisterObject(CollisionObject* obj);
00093
00095 void setup();
00096
00098 void update();
00099
00101 void update(CollisionObject* updated_obj);
00102
00104 void update(const std::vector<CollisionObject*>& updated_objs);
00105
00107 void clear()
00108 {
00109 dtree.clear();
00110 table.clear();
00111 }
00112
00114 void getObjects(std::vector<CollisionObject*>& objs) const
00115 {
00116 objs.resize(this->size());
00117 std::transform(table.begin(), table.end(), objs.begin(), boost::bind(&DynamicAABBTable::value_type::first, _1));
00118 }
00119
00121 void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
00122 {
00123 if(size() == 0) return;
00124 switch(obj->getCollisionGeometry()->getNodeType())
00125 {
00126 case GEOM_OCTREE:
00127 {
00128 if(!octree_as_geometry_collide)
00129 {
00130 const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
00131 collisionRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback);
00132 }
00133 else
00134 collisionRecurse(dtree.getRoot(), obj, cdata, callback);
00135 }
00136 break;
00137 default:
00138 collisionRecurse(dtree.getRoot(), obj, cdata, callback);
00139 }
00140 }
00141
00143 void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
00144 {
00145 if(size() == 0) return;
00146 FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
00147 switch(obj->getCollisionGeometry()->getNodeType())
00148 {
00149 case GEOM_OCTREE:
00150 {
00151 if(!octree_as_geometry_distance)
00152 {
00153 const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
00154 distanceRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist);
00155 }
00156 else
00157 distanceRecurse(dtree.getRoot(), obj, cdata, callback, min_dist);
00158 }
00159 break;
00160 default:
00161 distanceRecurse(dtree.getRoot(), obj, cdata, callback, min_dist);
00162 }
00163 }
00164
00166 void collide(void* cdata, CollisionCallBack callback) const
00167 {
00168 if(size() == 0) return;
00169 selfCollisionRecurse(dtree.getRoot(), cdata, callback);
00170 }
00171
00173 void distance(void* cdata, DistanceCallBack callback) const
00174 {
00175 if(size() == 0) return;
00176 FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
00177 selfDistanceRecurse(dtree.getRoot(), cdata, callback, min_dist);
00178 }
00179
00181 void collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
00182 {
00183 DynamicAABBTreeCollisionManager* other_manager = static_cast<DynamicAABBTreeCollisionManager*>(other_manager_);
00184 if((size() == 0) || (other_manager->size() == 0)) return;
00185 collisionRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), cdata, callback);
00186 }
00187
00189 void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
00190 {
00191 DynamicAABBTreeCollisionManager* other_manager = static_cast<DynamicAABBTreeCollisionManager*>(other_manager_);
00192 if((size() == 0) || (other_manager->size() == 0)) return;
00193 FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
00194 distanceRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), cdata, callback, min_dist);
00195 }
00196
00198 bool empty() const
00199 {
00200 return dtree.empty();
00201 }
00202
00204 size_t size() const
00205 {
00206 return dtree.size();
00207 }
00208
00209 const HierarchyTree<AABB>& getTree() const { return dtree; }
00210
00211
00212 private:
00213 HierarchyTree<AABB> dtree;
00214 boost::unordered_map<CollisionObject*, DynamicAABBNode*> table;
00215
00216 bool setup_;
00217
00218 bool collisionRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, CollisionCallBack callback) const;
00219
00220 bool collisionRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, CollisionCallBack callback) const;
00221
00222 bool selfCollisionRecurse(DynamicAABBNode* root, void* cdata, CollisionCallBack callback) const;
00223
00224 bool distanceRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
00225
00226 bool distanceRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
00227
00228 bool selfDistanceRecurse(DynamicAABBNode* root, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
00229
00230 void update_(CollisionObject* updated_obj);
00231
00233 bool collisionRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback) const;
00234
00235 bool distanceRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
00236
00237 };
00238
00239
00240
00241 }
00242
00243 #endif