00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #ifndef OST_SPATIAL_ORGANIZER_HI
00020 #define OST_SPATIAL_ORGANIZER_HI
00021
00022 #include <vector>
00023 #include <map>
00024 #include <cmath>
00025 #include <limits>
00026
00027 #include <ost/geom/geom.hh>
00028
00029 namespace ost { namespace mol {
00030
00032
00033
00034
00035
00036
00037 template <class ITEM, class VEC=geom::Vec3>
00038 class DLLEXPORT_OST_MOL SpatialOrganizer {
00039 public:
00040 typedef std::vector<ITEM> ItemList;
00041
00042 private:
00043 struct Index {
00044 Index():
00045 u(0),v(0),w(0) {}
00046 Index(int uu, int vv, int ww):
00047 u(uu),v(vv),w(ww) {}
00048
00049 bool operator<(const Index& other) const {
00050 return w!=other.w ? w<other.w : (v!=other.v ? v<other.v : u<other.u);
00051 }
00052
00053 int u,v,w;
00054
00055 static Index Max(const Index& a, const Index& b) {
00056 Index m;
00057 m.u = a.u > b.u ? a.u : b.u;
00058 m.v = a.v > b.v ? a.v : b.v;
00059 m.w = a.w > b.w ? a.w : b.w;
00060 return m;
00061 }
00062
00063 static Index Min(const Index& a, const Index& b) {
00064 Index m;
00065 m.u = a.u < b.u ? a.u : b.u;
00066 m.v = a.v < b.v ? a.v : b.v;
00067 m.w = a.w < b.w ? a.w : b.w;
00068 return m;
00069 }
00070
00071 };
00072
00073 struct Entry {
00074 Entry(const ITEM& i, const VEC& p): item(i), pos(p) {}
00075
00076 ITEM item;
00077 VEC pos;
00078 };
00079
00080 typedef std::vector<Entry> EntryList;
00081
00082 typedef std::map<Index,EntryList> ItemMap;
00083
00084 public:
00085
00086 SpatialOrganizer(Real delta): delta_(delta) {
00087 if(delta==0.0) {
00088 throw "delta cannot be zero";
00089 }
00090 }
00091
00092 void Add(const ITEM& item, const VEC& pos) {
00093 bool first = map_.empty();
00094 Index indx=gen_index(pos);
00095 map_[indx].push_back(Entry(item,pos));
00096 if (!first) {
00097 min_ = Index::Min(min_, indx);
00098 max_ = Index::Max(max_, indx);
00099 } else {
00100 min_ = indx;
00101 max_ = indx;
00102 }
00103 }
00104 void Remove(const ITEM& item) {
00105 typename ItemMap::iterator i=map_.begin();
00106 for (; i!=map_.end(); ++i) {
00107 for (size_t j=0; j<i->second.size(); ++j) {
00108 if (i->second[j].item==item) {
00109 i->second.erase(i->second.begin()+j);
00110 return;
00111 }
00112 }
00113 }
00114 }
00115
00116 bool HasWithin(const VEC& pos, Real dist) const {
00117 Real dist2=dist*dist;
00118 Index imin = Index::Max(min_, gen_index(pos-VEC(dist,dist,dist)));
00119 Index imax = Index::Min(max_, gen_index(pos+VEC(dist,dist,dist)));
00120 const size_t tmp = (imax.u-imin.u+1)*(imax.v-imin.v+1)*(imax.w-imin.w+1);
00121 if (tmp > map_.size()) {
00122 return this->has_within_all_buckets(pos, dist2);
00123 }
00124 for(int wc=imin.w;wc<=imax.w;++wc) {
00125 for(int vc=imin.v;vc<=imax.v;++vc) {
00126 for(int uc=imin.u;uc<=imax.u;++uc) {
00127 typename ItemMap::const_iterator map_it = map_.find(Index(uc,vc,wc));
00128 if(map_it!=map_.end()) {
00129 for(typename EntryList::const_iterator entry_it = map_it->second.begin();
00130 entry_it != map_it->second.end(); ++entry_it) {
00131
00132
00133
00134
00135
00136
00137 Real delta_x = entry_it->pos[0]-pos[0];
00138 Real delta_y = entry_it->pos[1]-pos[1];
00139 Real delta_z = entry_it->pos[2]-pos[2];
00140 if(delta_x*delta_x+delta_y*delta_y+delta_z*delta_z<=dist2) {
00141 return true;
00142 }
00143 }
00144 }
00145 }
00146 }
00147 }
00148 return false;
00149 }
00150
00151 ItemList FindWithin(const VEC& pos, Real dist) const {
00152 Real dist2=dist*dist;
00153 Index imin = gen_index(pos-VEC(dist,dist,dist));
00154 Index imax = gen_index(pos+VEC(dist,dist,dist));
00155
00156 ItemList item_list;
00157
00158 for(int wc=imin.w;wc<=imax.w;++wc) {
00159 for(int vc=imin.v;vc<=imax.v;++vc) {
00160 for(int uc=imin.u;uc<=imax.u;++uc) {
00161 typename ItemMap::const_iterator map_it = map_.find(Index(uc,vc,wc));
00162
00163 if(map_it!=map_.end()) {
00164 for(typename EntryList::const_iterator entry_it = map_it->second.begin();
00165 entry_it != map_it->second.end(); ++entry_it) {
00166
00167
00168
00169
00170
00171
00172 Real delta_x = entry_it->pos[0]-pos[0];
00173 Real delta_y = entry_it->pos[1]-pos[1];
00174 Real delta_z = entry_it->pos[2]-pos[2];
00175 if(delta_x*delta_x+delta_y*delta_y+delta_z*delta_z<=dist2) {
00176 item_list.push_back(entry_it->item);
00177 }
00178 }
00179 }
00180 }
00181 }
00182 }
00183 return item_list;
00184 }
00185 void Clear()
00186 {
00187 map_.clear();
00188 }
00189 void Swap(SpatialOrganizer& o) {
00190 map_.swap(o.map_);
00191 std::swap(delta_,o.delta_);
00192 std::swap(min_, o.min_);
00193 std::swap(max_, o.max_);
00194 }
00195
00196 private:
00197 bool has_within_all_buckets(const VEC& pos, Real dist2) const {
00198 for (typename ItemMap::const_iterator
00199 i = map_.begin(), e = map_.end(); i!=e; ++i) {
00200 for(typename EntryList::const_iterator j = i->second.begin();
00201 j != i->second.end(); ++j) {
00202 Real delta_x = j->pos[0]-pos[0];
00203 Real delta_y = j->pos[1]-pos[1];
00204 Real delta_z = j->pos[2]-pos[2];
00205 if(delta_x*delta_x+delta_y*delta_y+delta_z*delta_z<=dist2) {
00206 return true;
00207 }
00208 }
00209 }
00210 return false;
00211 }
00212 ItemMap map_;
00213 Real delta_;
00214 Index min_;
00215 Index max_;
00216
00217 Index gen_index(const VEC& pos) const {
00218 Index nrvo(static_cast<int>(round(pos[0]/delta_)),
00219 static_cast<int>(round(pos[1]/delta_)),
00220 static_cast<int>(round(pos[2]/delta_)));
00221 return nrvo;
00222 }
00223
00224 geom::Vec3 gen_middle(const Index& i) const {
00225 geom::Vec3 nrvo((static_cast<Real>(i.u)+0.5)*delta_,
00226 (static_cast<Real>(i.v)+0.5)*delta_,
00227 (static_cast<Real>(i.w)+0.5)*delta_);
00228 return nrvo;
00229 }
00230
00231 };
00232
00233 }}
00234
00235 #endif