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
00037 #ifndef FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H
00038 #define FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H
00039
00040 #include "fcl/broadphase/broadphase.h"
00041 #include "fcl/broadphase/hierarchy_tree.h"
00042 #include "fcl/octree.h"
00043 #include "fcl/BV/BV.h"
00044 #include "fcl/shape/geometric_shapes_utility.h"
00045 #include <boost/unordered_map.hpp>
00046 #include <boost/bind.hpp>
00047 #include <limits>
00048
00049
00050 namespace fcl
00051 {
00052
00053 class DynamicAABBTreeCollisionManager_Array : public BroadPhaseCollisionManager
00054 {
00055 public:
00056 typedef implementation_array::NodeBase<AABB> DynamicAABBNode;
00057 typedef boost::unordered_map<CollisionObject*, size_t> DynamicAABBTable;
00058
00059 int max_tree_nonbalanced_level;
00060 int tree_incremental_balance_pass;
00061 int& tree_topdown_balance_threshold;
00062 int& tree_topdown_level;
00063 int tree_init_level;
00064
00065 bool octree_as_geometry_collide;
00066 bool octree_as_geometry_distance;
00067
00068 DynamicAABBTreeCollisionManager_Array() : tree_topdown_balance_threshold(dtree.bu_threshold),
00069 tree_topdown_level(dtree.topdown_level)
00070 {
00071 max_tree_nonbalanced_level = 10;
00072 tree_incremental_balance_pass = 10;
00073 tree_topdown_balance_threshold = 2;
00074 tree_topdown_level = 0;
00075 tree_init_level = 0;
00076 setup_ = false;
00077
00078
00079 octree_as_geometry_collide = true;
00080 octree_as_geometry_distance = false;
00081 }
00082
00084 void registerObjects(const std::vector<CollisionObject*>& other_objs);
00085
00087 void registerObject(CollisionObject* obj);
00088
00090 void unregisterObject(CollisionObject* obj);
00091
00093 void setup();
00094
00096 void update();
00097
00099 void update(CollisionObject* updated_obj);
00100
00102 void update(const std::vector<CollisionObject*>& updated_objs);
00103
00105 void clear()
00106 {
00107 dtree.clear();
00108 table.clear();
00109 }
00110
00112 void getObjects(std::vector<CollisionObject*>& objs) const
00113 {
00114 objs.resize(this->size());
00115 std::transform(table.begin(), table.end(), objs.begin(), boost::bind(&DynamicAABBTable::value_type::first, _1));
00116 }
00117
00119 void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
00120 {
00121 if(size() == 0) return;
00122 switch(obj->getCollisionGeometry()->getNodeType())
00123 {
00124 case GEOM_OCTREE:
00125 {
00126 if(!octree_as_geometry_collide)
00127 {
00128 const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
00129 collisionRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback);
00130 }
00131 else
00132 collisionRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback);
00133 }
00134 break;
00135 default:
00136 collisionRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback);
00137 }
00138 }
00139
00141 void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
00142 {
00143 if(size() == 0) return;
00144 FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
00145 switch(obj->getCollisionGeometry()->getNodeType())
00146 {
00147 case GEOM_OCTREE:
00148 {
00149 if(!octree_as_geometry_distance)
00150 {
00151 const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
00152 distanceRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist);
00153 }
00154 else
00155 distanceRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback, min_dist);
00156 }
00157 break;
00158 default:
00159 distanceRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback, min_dist);
00160 }
00161 }
00162
00164 void collide(void* cdata, CollisionCallBack callback) const
00165 {
00166 if(size() == 0) return;
00167 selfCollisionRecurse(dtree.getNodes(), dtree.getRoot(), cdata, callback);
00168 }
00169
00171 void distance(void* cdata, DistanceCallBack callback) const
00172 {
00173 if(size() == 0) return;
00174 FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
00175 selfDistanceRecurse(dtree.getNodes(), dtree.getRoot(), cdata, callback, min_dist);
00176 }
00177
00179 void collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
00180 {
00181 DynamicAABBTreeCollisionManager_Array* other_manager = static_cast<DynamicAABBTreeCollisionManager_Array*>(other_manager_);
00182 if((size() == 0) || (other_manager->size() == 0)) return;
00183 collisionRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), cdata, callback);
00184 }
00185
00187 void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
00188 {
00189 DynamicAABBTreeCollisionManager_Array* other_manager = static_cast<DynamicAABBTreeCollisionManager_Array*>(other_manager_);
00190 if((size() == 0) || (other_manager->size() == 0)) return;
00191 FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
00192 distanceRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), cdata, callback, min_dist);
00193 }
00194
00196 bool empty() const
00197 {
00198 return dtree.empty();
00199 }
00200
00202 size_t size() const
00203 {
00204 return dtree.size();
00205 }
00206
00207
00208 const implementation_array::HierarchyTree<AABB>& getTree() const { return dtree; }
00209
00210 private:
00211 implementation_array::HierarchyTree<AABB> dtree;
00212 boost::unordered_map<CollisionObject*, size_t> table;
00213
00214 bool setup_;
00215
00216 bool collisionRecurse(DynamicAABBNode* nodes1, size_t root1, DynamicAABBNode* nodes2, size_t root2, void* cdata, CollisionCallBack callback) const;
00217
00218 bool collisionRecurse(DynamicAABBNode* nodes, size_t root, CollisionObject* query, void* cdata, CollisionCallBack callback) const;
00219
00220 bool selfCollisionRecurse(DynamicAABBNode* nodes, size_t root, void* cdata, CollisionCallBack callback) const;
00221
00222 bool distanceRecurse(DynamicAABBNode* nodes1, size_t root1, DynamicAABBNode* nodes2, size_t root2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
00223
00224 bool distanceRecurse(DynamicAABBNode* nodes, size_t root, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
00225
00226 bool selfDistanceRecurse(DynamicAABBNode* nodes, size_t root, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
00227
00228 void update_(CollisionObject* updated_obj);
00229
00231 bool collisionRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback) const;
00232
00233 bool distanceRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
00234
00235
00236 };
00237
00238
00239 }
00240
00241 #endif