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
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052 #ifndef __VCGLIB_SPATIAL_ITERATORS
00053 #define __VCGLIB_SPATIAL_ITERATORS
00054
00055 #include <vector>
00056 #include <vcg/space/intersection3.h>
00057 #include <vcg/space/point3.h>
00058 #include <vcg/space/box3.h>
00059 #include <vcg/space/ray3.h>
00060 #include <vcg/math/base.h>
00061 #include <algorithm>
00062 #include <float.h>
00063 #include <limits>
00064
00065 namespace vcg{
00066 template <class Spatial_Idexing,class INTFUNCTOR,class TMARKER>
00067 class RayIterator
00068 {
00069 public:
00070 typedef typename Spatial_Idexing::ScalarType ScalarType;
00071 typedef typename vcg::Ray3<ScalarType> RayType;
00072 typedef typename Spatial_Idexing::Box3x IndexingBoxType;
00073 protected:
00074 typedef typename Spatial_Idexing::ObjType ObjType;
00075 typedef typename vcg::Point3<ScalarType> CoordType;
00076 typedef typename Spatial_Idexing::CellIterator CellIterator;
00077 ScalarType max_dist;
00078
00080 void _ControlLimits()
00081 {
00082 for (int i=0;i<3;i++)
00083 {
00084 vcg::Point3i dim=Si.siz;
00085 if (CurrentCell.V(i)<0)
00086 CurrentCell.V(i) = 0;
00087 else
00088 if (CurrentCell.V(i)>=dim.V(i))
00089 CurrentCell.V(i)=dim.V(i)-1;
00090 }
00091 }
00092
00094 void _FindLinePar()
00095 {
00096
00097
00099 vcg::Point3i ip;
00100 Si.PToIP(start,ip);
00101 Si.IPiToPf(ip,goal);
00102 for (int i=0;i<3;i++)
00103 if(r.Direction().V(i)>0.0)
00104 goal.V(i)+=Si.voxel.V(i);
00105
00106 ScalarType gx=goal.X();
00107 ScalarType gy=goal.Y();
00108 ScalarType gz=goal.Z();
00109
00110 dist=(r.Origin()-goal).Norm();
00111
00112 const float LocalMaxScalar = (std::numeric_limits<float>::max)();
00113 const float EPS = std::numeric_limits<float>::min();
00114
00115
00116 ScalarType tx,ty,tz;
00117
00118 if( fabs(r.Direction().X())>EPS )
00119 tx = (gx-r.Origin().X())/r.Direction().X();
00120 else
00121 tx =LocalMaxScalar;
00122
00123 if( fabs(r.Direction().Y())>EPS)
00124 ty = (gy-r.Origin().Y())/r.Direction().Y();
00125 else
00126 ty =LocalMaxScalar;
00127
00128 if( fabs(r.Direction().Z())>EPS )
00129 tz = (gz-r.Origin().Z())/r.Direction().Z();
00130 else
00131 tz =LocalMaxScalar;
00132
00133 t=CoordType(tx,ty,tz);
00134 }
00135
00136 bool _controlEnd()
00137 {
00138 return (((CurrentCell.X()<0)||(CurrentCell.Y()<0)||(CurrentCell.Z()<0))||
00139 ((CurrentCell.X()>=Si.siz.X())||(CurrentCell.Y()>=Si.siz.Y())||(CurrentCell.Z()>=Si.siz.Z())));
00140 }
00141
00142 void _NextCell()
00143 {
00144 assert(!end);
00145 vcg::Box3<ScalarType> bb_current;
00146
00147 Si.IPiToPf(CurrentCell,bb_current.min);
00148 Si.IPiToPf(CurrentCell+vcg::Point3i(1,1,1),bb_current.max);
00149
00150 CoordType inters;
00151 IntersectionRayBox(bb_current,r,inters);
00152 ScalarType testmax_dist=(inters-r.Origin()).Norm();
00153
00154 if (testmax_dist>max_dist)
00155 end=true;
00156 else
00157 {
00158 if( t.X()<t.Y() && t.X()<t.Z() )
00159 {
00160 if(r.Direction().X()<0.0)
00161 {goal.X() -= Si.voxel.X(); --CurrentCell.X();}
00162 else
00163 {goal.X() += Si.voxel.X(); ++CurrentCell.X();}
00164 t.X() = (goal.X()-r.Origin().X())/r.Direction().X();
00165 }
00166 else if( t.Y()<t.Z() ){
00167 if(r.Direction().Y()<0.0)
00168 {goal.Y() -= Si.voxel.Y(); --CurrentCell.Y();}
00169 else
00170 {goal.Y() += Si.voxel.Y(); ++CurrentCell.Y();}
00171 t.Y() = (goal.Y()-r.Origin().Y())/r.Direction().Y();
00172 } else {
00173 if(r.Direction().Z()<0.0)
00174 { goal.Z() -= Si.voxel.Z(); --CurrentCell.Z();}
00175 else
00176 { goal.Z() += Si.voxel.Z(); ++CurrentCell.Z();}
00177 t.Z() = (goal.Z()-r.Origin().Z())/r.Direction().Z();
00178 }
00179
00180 dist=(r.Origin()-goal).Norm();
00181 end=_controlEnd();
00182 }
00183 }
00184
00185 public:
00186
00187
00189 RayIterator(Spatial_Idexing &_Si,
00190 INTFUNCTOR _int_funct
00191 ,const ScalarType &_max_dist)
00192 :Si(_Si),int_funct(_int_funct)
00193 {
00194 max_dist=_max_dist;
00195 };
00196
00197 void SetMarker(TMARKER _tm)
00198 {
00199 tm=_tm;
00200 }
00201
00202 void Init(const RayType _r)
00203 {
00204 r=_r;
00205 end=false;
00206 tm.UnMarkAll();
00207 Elems.clear();
00208
00209
00210 if (Si.bbox.IsIn(r.Origin()))
00211 start=r.Origin();
00212 else
00213 if (!(vcg::IntersectionRayBox<ScalarType>(Si.bbox,r,start))){
00214 end=true;
00215 return;
00216 }
00217 Si.PToIP(start,CurrentCell);
00218 _ControlLimits();
00219 _FindLinePar();
00220
00221 while ((!End())&& Refresh())
00222 _NextCell();
00223
00224 }
00225
00226 bool End()
00227 {return end;}
00228
00229
00232 bool Refresh()
00233 {
00234
00235
00236 typename Spatial_Idexing::CellIterator first,last,l;
00237
00239 Si.Grid(CurrentCell.X(),CurrentCell.Y(),CurrentCell.Z(),first,last);
00240 for(l=first;l!=last;++l)
00241 {
00242 ObjType* elem=&(*(*l));
00243 ScalarType t;
00244 CoordType Int;
00245 if((!elem->IsD())&&(!tm.IsMarked(elem))&&(int_funct((**l),r,t))&&(t<=max_dist))
00246 {
00247 Int=r.Origin()+r.Direction()*t;
00248 Elems.push_back(Entry_Type(elem,t,Int));
00249 tm.Mark(elem);
00250 }
00251 }
00253 std::sort(Elems.begin(),Elems.end());
00254 CurrentElem=Elems.rbegin();
00255
00256 return((Elems.size()==0)||(Dist()>dist));
00257 }
00258
00259 void operator ++()
00260 {
00261 if (!Elems.empty()) Elems.pop_back();
00262
00263 CurrentElem = Elems.rbegin();
00264
00265 if (Dist()>dist)
00266 {
00267 if (!End())
00268 {
00269 _NextCell();
00270 while ((!End())&&Refresh())
00271 _NextCell();
00272 }
00273 }
00274 }
00275
00276 ObjType &operator *(){return *((*CurrentElem).elem);}
00277
00278 CoordType IntPoint()
00279 {return ((*CurrentElem).intersection);}
00280
00281 ScalarType Dist()
00282 {
00283 if (Elems.size()>0)
00284 return ((*CurrentElem).dist);
00285 else
00286 return ((ScalarType)FLT_MAX);
00287 }
00288
00290 void SetIndexStructure(Spatial_Idexing &_Si)
00291 {Si=_Si;}
00292
00293
00294
00295 protected:
00296
00298 struct Entry_Type
00299 {
00300 public:
00301
00302 Entry_Type(ObjType* _elem,ScalarType _dist,CoordType _intersection)
00303 {
00304 elem=_elem;
00305 dist=_dist;
00306 intersection=_intersection;
00307 }
00308 inline bool operator < ( const Entry_Type & l ) const{return (dist > l.dist); }
00309 ObjType* elem;
00310 ScalarType dist;
00311 CoordType intersection;
00312 };
00313
00314 RayType r;
00315 Spatial_Idexing &Si;
00316 bool end;
00317 INTFUNCTOR &int_funct;
00318 TMARKER tm;
00319
00320 std::vector<Entry_Type> Elems;
00321 typedef typename std::vector<Entry_Type>::reverse_iterator ElemIterator;
00322 ElemIterator CurrentElem;
00323
00324 vcg::Point3i CurrentCell;
00325
00326
00327 CoordType start;
00328 CoordType goal;
00329 ScalarType dist;
00330 CoordType t;
00331
00332 };
00333
00334
00335 template <class Spatial_Idexing,class DISTFUNCTOR,class TMARKER>
00336 class ClosestIterator
00337 {
00338 typedef typename Spatial_Idexing::ObjType ObjType;
00339 typedef typename Spatial_Idexing::ScalarType ScalarType;
00340 typedef typename vcg::Point3<ScalarType> CoordType;
00341 typedef typename Spatial_Idexing::CellIterator CellIterator;
00342
00343
00344
00346 bool _EndGrid()
00347 {
00348 if ((explored.min==vcg::Point3i(0,0,0))&&(explored.max==Si.siz))
00349 end =true;
00350 return end;
00351 }
00352
00353 void _UpdateRadius()
00354 {
00355 if (radius>=max_dist)
00356 end=true;
00357
00358 radius+=step_size;
00359
00360 if (radius>max_dist)
00361 radius=max_dist;
00362 }
00363
00365 bool _NextShell()
00366 {
00367
00368
00369 explored=to_explore;
00370 _UpdateRadius();
00371 Box3<ScalarType> b3d(p,radius);
00372 Si.BoxToIBox(b3d,to_explore);
00373 Box3i ibox(Point3i(0,0,0),Si.siz-Point3i(1,1,1));
00374 to_explore.Intersect(ibox);
00375 if (!to_explore.IsNull())
00376 {
00377 assert(!( to_explore.min.X()<0 || to_explore.max.X()>=Si.siz[0] ||
00378 to_explore.min.Y()<0 || to_explore.max.Y()>=Si.siz[1] || to_explore.min.Z()<0
00379 || to_explore.max.Z()>=Si.siz[2] ));
00380 return true;
00381 }
00382 return false;
00383 }
00384
00385
00386
00387 public:
00388
00390 ClosestIterator(Spatial_Idexing &_Si,DISTFUNCTOR _dist_funct):Si(_Si),dist_funct(_dist_funct){}
00391
00393 void SetIndexStructure(Spatial_Idexing &_Si)
00394 {Si=_Si;}
00395
00396 void SetMarker(TMARKER _tm)
00397 {
00398 tm=_tm;
00399 }
00400
00402 void Init(CoordType _p,const ScalarType &_max_dist)
00403 {
00404 explored.SetNull();
00405 to_explore.SetNull();
00406 p=_p;
00407 max_dist=_max_dist;
00408 Elems.clear();
00409 end=false;
00410 tm.UnMarkAll();
00411
00412 step_size=Si.voxel.Norm();
00413 radius=0;
00414
00416 while ((!_NextShell())&&(!End())) {}
00417
00418 while ((!End())&& Refresh()&&(!_EndGrid()))
00419 _NextShell();
00420 }
00421
00422
00423 bool End()
00424 {return end;}
00425
00427
00428
00429 bool Refresh()
00430 {
00431 int ix,iy,iz;
00432 for( iz = to_explore.min.Z();iz <= to_explore.max.Z(); ++iz)
00433 for(iy =to_explore.min.Y(); iy <=to_explore.max.Y(); ++iy)
00434 for(ix =to_explore.min.X(); ix <= to_explore.max.X();++ix)
00435 {
00436
00437 if((explored.IsNull())||
00438 (ix<explored.min[0] || ix>explored.max[0] ||
00439 iy<explored.min[1] || iy>explored.max[1] ||
00440 iz<explored.min[2] || iz>explored.max[2] ))
00441 {
00442 typename Spatial_Idexing::CellIterator first,last,l;
00443
00444 Si.Grid(ix,iy,iz,first,last);
00445 for(l=first;l!=last;++l)
00446 {
00447 ObjType *elem=&(**l);
00448 if (!tm.IsMarked(elem))
00449 {
00450
00451 CoordType nearest;
00452 ScalarType dist=max_dist;
00453 if (dist_funct((**l),p,dist,nearest))
00454 Elems.push_back(Entry_Type(elem,fabs(dist),nearest));
00455 tm.Mark(elem);
00456 }
00457 }
00458 }
00459
00460 }
00461
00463 std::sort(Elems.begin(),Elems.end());
00464 CurrentElem=Elems.rbegin();
00465
00466 return((Elems.size()==0)||(Dist()>radius));
00467 }
00468
00469 bool ToUpdate()
00470 {return ((Elems.size()==0)||(Dist()>radius));}
00471
00472 void operator ++()
00473 {
00474 if (!Elems.empty()) Elems.pop_back();
00475
00476 CurrentElem = Elems.rbegin();
00477
00478 if ((!End())&& ToUpdate())
00479 do{_NextShell();}
00480 while (Refresh()&&(!_EndGrid()));
00481 }
00482
00483 ObjType &operator *(){return *((*CurrentElem).elem);}
00484
00485
00486
00487 ScalarType Dist()
00488 {
00489 if (Elems.size()>0)
00490 return ((*CurrentElem).dist);
00491 else
00492 return ((ScalarType)FLT_MAX);
00493 }
00494
00495 CoordType NearestPoint()
00496 {return ((*CurrentElem).intersection);}
00497
00498 protected:
00499
00501 struct Entry_Type
00502 {
00503 public:
00504
00505 Entry_Type(ObjType* _elem,ScalarType _dist,CoordType _intersection)
00506 {
00507 elem=_elem;
00508 dist=_dist;
00509 intersection=_intersection;
00510 }
00511
00512 inline bool operator < ( const Entry_Type & l ) const{return (dist > l.dist); }
00513
00514 inline bool operator == ( const Entry_Type & l ) const{return (elem == l.elem); }
00515
00516 ObjType* elem;
00517 ScalarType dist;
00518 CoordType intersection;
00519 };
00520
00521 CoordType p;
00522 Spatial_Idexing &Si;
00523 bool end;
00524 ScalarType max_dist;
00525 vcg::Box3i explored;
00526 vcg::Box3i to_explore;
00527 ScalarType radius;
00528 ScalarType step_size;
00529 std::vector<Entry_Type> Elems;
00530
00531 DISTFUNCTOR &dist_funct;
00532 TMARKER tm;
00533
00534 typedef typename std::vector<Entry_Type>::reverse_iterator ElemIterator;
00535 ElemIterator CurrentElem;
00536
00537 };
00538 }
00539
00540 #endif
