From bccc1ad129d12ee1240bd028ddd967c3fe48bc67 Mon Sep 17 00:00:00 2001 From: nicopietroni Date: Mon, 3 Oct 2005 10:06:53 +0000 Subject: [PATCH] optimized Closest iterator added possibility to pass a point p that is outside the bbox of the indexing structure --- vcg/space/index/space_iterators.h | 883 ++++++++++++++---------------- 1 file changed, 410 insertions(+), 473 deletions(-) diff --git a/vcg/space/index/space_iterators.h b/vcg/space/index/space_iterators.h index c91cbcb0..12f8a51b 100644 --- a/vcg/space/index/space_iterators.h +++ b/vcg/space/index/space_iterators.h @@ -13,530 +13,467 @@ namespace vcg{ -template -class RayIterator -{ -protected: - typedef typename Spatial_Idexing::ObjType ObjType; - typedef typename Spatial_Idexing::ScalarType ScalarType; - typedef typename vcg::Point3 CoordType; - typedef typename vcg::Ray3 RayType; - typedef typename Spatial_Idexing::CellIterator CellIterator; + template + class RayIterator + { + protected: + typedef typename Spatial_Idexing::ObjType ObjType; + typedef typename Spatial_Idexing::ScalarType ScalarType; + typedef typename vcg::Point3 CoordType; + typedef typename vcg::Ray3 RayType; + typedef typename Spatial_Idexing::CellIterator CellIterator; - ///control right bonding current cell index (only on initialization) - void _ControlLimits() - { - for (int i=0;i<3;i++) + ///control right bonding current cell index (only on initialization) + void _ControlLimits() { - vcg::Point3i dim=Si.siz; - if (CurrentCell.V(i)<0) - CurrentCell.V(i) = 0; - else - if (CurrentCell.V(i)>=dim.V(i)) - CurrentCell.V(i)=dim.V(i)-1; - } - } - - ///find initial line parameters - void _FindLinePar() - { - /* Punti goal */ - - ///da verificare se vanno oltre ai limiti - vcg::Point3i ip; - Si.PToIP(start,ip); - Si.IPToP(ip,goal); - for (int i=0;i<3;i++) - if(r.Direction().V(i)>0.0) - goal.V(i)+=Si.voxel.V(i); - - ScalarType gx=goal.X(); - ScalarType gy=goal.Y(); - ScalarType gz=goal.Z(); - - dist=(r.Origin()-goal).Norm(); - - const float MAXFLOAT = FLT_MAX; - const float EPSILON = 1e-50f; - - /* Parametri della linea */ - ScalarType tx,ty,tz; - - if( fabs(r.Direction().X())>EPSILON ) - tx = (gx-r.Origin().X())/r.Direction().X(); - else - tx =MAXFLOAT; - - if( fabs(r.Direction().Y())>EPSILON ) - ty = (gy-r.Origin().Y())/r.Direction().Y(); - else - ty =MAXFLOAT; - - if( fabs(r.Direction().Z())>EPSILON ) - tz = (gz-r.Origin().Z())/r.Direction().Z(); - else - tz =MAXFLOAT; - - t=CoordType(tx,ty,tz); - } - - bool _controlEnd() - { - return (((CurrentCell.X()<0)||(CurrentCell.Y()<0)||(CurrentCell.Z()<0))|| - ((CurrentCell.X()>=Si.siz.X())||(CurrentCell.Y()>=Si.siz.Y())||(CurrentCell.Z()>=Si.siz.Z()))); - } - - void _NextCell() - { - assert(!end); - if( t.X()(Si.bbox,r,start)) - { - Si.PToIP(start,CurrentCell); - _ControlLimits(); - _FindLinePar(); - //go to first intersection - while ((!End())&& Refresh()) - _NextCell(); - } - else - end=true; - } - - bool End() - {return end;} - - - ///refresh current cell intersection , return true if there are - ///at lest 1 intersection - bool Refresh() - { - //Elems.clear(); - - Spatial_Idexing::CellIterator first,last,l; - - ///take first, last iterators to elements in the cell - Si.Grid(CurrentCell.X(),CurrentCell.Y(),CurrentCell.Z(),first,last); - for(l=first;l!=last;++l) - { - ObjType* elem=&(*(*l)); - ScalarType t; - CoordType Int; - if((!tm.IsMarked(elem))&&(int_funct((**l),r,t))) + for (int i=0;i<3;i++) { - Int=r.Origin()+r.Direction()*t; - Elems.push_back(Entry_Type(elem,t,Int)); - tm.Mark(elem); + vcg::Point3i dim=Si.siz; + if (CurrentCell.V(i)<0) + CurrentCell.V(i) = 0; + else + if (CurrentCell.V(i)>=dim.V(i)) + CurrentCell.V(i)=dim.V(i)-1; } } - ////then control if there are more than 1 element - if (Elems.size()>1) - std::sort(Elems.begin(),Elems.end()); - - CurrentElem=Elems.end(); - CurrentElem--; - return((Elems.size()==0)||(Dist()>dist)); - } - - void operator ++() - { - //if (CurrentElem!=Elems.end()) - if (Elems.size()>0) + ///find initial line parameters + void _FindLinePar() { - CurrentElem--; - //std::pop_heap(Elems.begin(),Elems.end()); - Elems.pop_back(); + /* Punti goal */ + + ///da verificare se vanno oltre ai limiti + vcg::Point3i ip; + Si.PToIP(start,ip); + Si.IPToP(ip,goal); + for (int i=0;i<3;i++) + if(r.Direction().V(i)>0.0) + goal.V(i)+=Si.voxel.V(i); + + ScalarType gx=goal.X(); + ScalarType gy=goal.Y(); + ScalarType gz=goal.Z(); + + dist=(r.Origin()-goal).Norm(); + + const float MAXFLOAT = FLT_MAX; + const float EPSILON = 1e-50f; + + /* Parametri della linea */ + ScalarType tx,ty,tz; + + if( fabs(r.Direction().X())>EPSILON ) + tx = (gx-r.Origin().X())/r.Direction().X(); + else + tx =MAXFLOAT; + + if( fabs(r.Direction().Y())>EPSILON ) + ty = (gy-r.Origin().Y())/r.Direction().Y(); + else + ty =MAXFLOAT; + + if( fabs(r.Direction().Z())>EPSILON ) + tz = (gz-r.Origin().Z())/r.Direction().Z(); + else + tz =MAXFLOAT; + + t=CoordType(tx,ty,tz); } - /*if (CurrentElem==Elems.end()) - {*/ - if (Dist()>dist) + + bool _controlEnd() { - if (!End()) + return (((CurrentCell.X()<0)||(CurrentCell.Y()<0)||(CurrentCell.Z()<0))|| + ((CurrentCell.X()>=Si.siz.X())||(CurrentCell.Y()>=Si.siz.Y())||(CurrentCell.Z()>=Si.siz.Z()))); + } + + void _NextCell() + { + assert(!end); + if( t.X()0) - return ((*CurrentElem).dist); - else - return ((ScalarType)FLT_MAX); - } - //{return ((*CurrentElem).dist);} - - ///set the current spatial indexing structure used - void SetIndexStructure(Spatial_Idexing &_Si) - {Si=_Si;} - - - -protected: - - ///structure that mantain for the current cell pre-calculated data - typedef struct Entry_Type - { public: - Entry_Type(ObjType* _elem,ScalarType _dist,CoordType _intersection) + + ///contructor + RayIterator(Spatial_Idexing &_Si,INTFUNCTOR _int_funct):Si(_Si),int_funct(_int_funct){ + }; + + void SetMarker(TMARKER _tm) { - elem=_elem; - dist=_dist; - intersection=_intersection; + tm=_tm; } - inline bool operator < ( const Entry_Type & l ) const{return (dist > l.dist); } - ObjType* elem; + + void Init(const RayType _r) + { + r=_r; + end=false; + tm.UnMarkAll(); + Elems.clear(); + //CoordType ip; + //control if intersect the bounding box of the mesh + if(vcg::Intersection_Ray_Box(Si.bbox,r,start)) + { + Si.PToIP(start,CurrentCell); + _ControlLimits(); + _FindLinePar(); + //go to first intersection + while ((!End())&& Refresh()) + _NextCell(); + } + else + end=true; + } + + bool End() + {return end;} + + + ///refresh current cell intersection , return true if there are + ///at lest 1 intersection + bool Refresh() + { + //Elems.clear(); + + Spatial_Idexing::CellIterator first,last,l; + + ///take first, last iterators to elements in the cell + Si.Grid(CurrentCell.X(),CurrentCell.Y(),CurrentCell.Z(),first,last); + for(l=first;l!=last;++l) + { + ObjType* elem=&(*(*l)); + ScalarType t; + CoordType Int; + if((!tm.IsMarked(elem))&&(int_funct((**l),r,t))) + { + Int=r.Origin()+r.Direction()*t; + Elems.push_back(Entry_Type(elem,t,Int)); + tm.Mark(elem); + } + } + ////then control if there are more than 1 element + if (Elems.size()>1) + std::sort(Elems.begin(),Elems.end()); + + CurrentElem=Elems.end(); + CurrentElem--; + + return((Elems.size()==0)||(Dist()>dist)); + } + + void operator ++() + { + //if (CurrentElem!=Elems.end()) + if (Elems.size()>0) + { + CurrentElem--; + //std::pop_heap(Elems.begin(),Elems.end()); + Elems.pop_back(); + } + /*if (CurrentElem==Elems.end()) + {*/ + if (Dist()>dist) + { + if (!End()) + { + _NextCell(); + while ((!End())&&Refresh()) + _NextCell(); + } + } + } + + ObjType &operator *(){return *((*CurrentElem).elem);} + + CoordType IntPoint() + {return ((*CurrentElem).intersection);} + + ScalarType Dist() + { + if (Elems.size()>0) + return ((*CurrentElem).dist); + else + return ((ScalarType)FLT_MAX); + } + //{return ((*CurrentElem).dist);} + + ///set the current spatial indexing structure used + void SetIndexStructure(Spatial_Idexing &_Si) + {Si=_Si;} + + + + protected: + + ///structure that mantain for the current cell pre-calculated data + typedef struct Entry_Type + { + public: + + Entry_Type(ObjType* _elem,ScalarType _dist,CoordType _intersection) + { + elem=_elem; + dist=_dist; + intersection=_intersection; + } + inline bool operator < ( const Entry_Type & l ) const{return (dist > l.dist); } + ObjType* elem; + ScalarType dist; + CoordType intersection; + }; + + RayType r; //ray to find intersections + Spatial_Idexing &Si; //reference to spatial index algorithm + bool end; //true if the scan is terminated + INTFUNCTOR &int_funct; + TMARKER tm; + + std::vector Elems; //element loaded from curren cell + typedef typename std::vector::iterator ElemIterator; + ElemIterator CurrentElem; //iterator to current element + + vcg::Point3i CurrentCell; //current cell + + //used for raterization + CoordType start; + CoordType goal; ScalarType dist; - CoordType intersection; + CoordType t; + }; - RayType r; //ray to find intersections - Spatial_Idexing &Si; //reference to spatial index algorithm - bool end; //true if the scan is terminated - INTFUNCTOR &int_funct; - TMARKER tm; - std::vector Elems; //element loaded from curren cell - typedef typename std::vector::iterator ElemIterator; - ElemIterator CurrentElem; //iterator to current element - - vcg::Point3i CurrentCell; //current cell - - //used for raterization - CoordType start; - CoordType goal; - ScalarType dist; - CoordType t; - -}; - -template -class ClosestIterator -{ - typedef typename Spatial_Idexing::ObjType ObjType; - typedef typename Spatial_Idexing::ScalarType ScalarType; - typedef typename vcg::Point3 CoordType; - typedef typename Spatial_Idexing::CellIterator CellIterator; + template + class ClosestIterator + { + typedef typename Spatial_Idexing::ObjType ObjType; + typedef typename Spatial_Idexing::ScalarType ScalarType; + typedef typename vcg::Point3 CoordType; + typedef typename Spatial_Idexing::CellIterator CellIterator; - ///find the radius of curren sphere - ///considering more nearest cell to current radius - void _FindSphereRadius() - { - //find diffence between the initial point - //and the cell narest to the sphere - CoordType min; - CoordType max; - Si.IPToP(explored.min,min); - Si.IPToP(explored.max,max); - CoordType diff_min=p-min; - CoordType diff_max=p-max; - ScalarType diffx=std::min(fabs(diff_min.X()),fabs(diff_max.X())); - ScalarType diffy=std::min(fabs(diff_min.Y()),fabs(diff_max.Y())); - ScalarType diffz=std::min(fabs(diff_min.Z()),fabs(diff_max.Z())); - - ScalarType diff=std::min(diffx,std::min(diffy,diffz)); - - //radius_min=0; - radius=diff; - //radius+=diff; - if (radius>max_dist) - radius=max_dist; - } - - ///control right cell value of current bounding box - /// of explored cells - void _ControlLimits() - { - vcg::Point3i dim=Si.siz; - for (int i=0;i<3;i++) + ///control the end of scanning + bool _EndGrid() { - if (explored.min.V(i)<-1) - explored.min.V(i) = -1; - if (explored.max.V(i)>Si.siz.V(i)) - explored.max.V(i) =Si.siz.V(i); - } - } - - bool _OutOfLimits(vcg::Point3i p) - { - for (int i=0;i<3;i++) - if ((p.V(i)==-1)||(p.V(i)>=Si.siz.V(i))) - return true ; - - return false; - } - - ///control the end of scanning - void _ControlEnd() - { - if ((explored.min==vcg::Point3i(-1,-1,-1))&&(explored.max==Si.siz)&&(Elems.size()==0)) + if ((explored.min<=vcg::Point3i(0,0,0))&&(explored.max>=Si.siz-vcg::Point3i(1,1,1))) end =true; - } + return end; + } - ///add cell to the curren set of explored cells - void _NextShell() - { + void _UpdateRadius() + { if (radius>=max_dist) end=true; - radius+=voxel_min; + radius+=step_size; //control bounds if (radius>max_dist) radius=max_dist; - - //expand the box - explored.min-=vcg::Point3i(1,1,1); - explored.max+=vcg::Point3i(1,1,1); - - //control right limits of the bound - _ControlLimits(); - } - - - -public: - - ///contructor - ClosestIterator(Spatial_Idexing &_Si,DISTFUNCTOR _dist_funct):Si(_Si),dist_funct(_dist_funct){} - - ///set the current spatial indexing structure used - void SetIndexStructure(Spatial_Idexing &_Si) - {Si=_Si;} - - void SetMarker(TMARKER _tm) - { - tm=_tm; - } - - ///initialize the Itarator - void Init(CoordType _p,const ScalarType &_max_dist) - { - //CoordType vox=Si.Voxel(); - CoordType vox=Si.voxel; - - voxel_min=std::min(vox.V(0),std::min(vox.V(1),vox.V(2))); - p=_p; - max_dist=_max_dist; - - //initialize the explored region - //finding cell coordinate of initial point - vcg::Point3i c; - Si.PToIP(p,c); - - assert( c.X()>=0 && c.X()=0 && c.Y()=0 && c.Z()radius)) - { - if (radius>=max_dist) - end=true; - _NextShell(); - Refresh(); - _ControlEnd(); } - //set to the last element ..the nearest - CurrentElem=Elems.end(); - CurrentElem--; - - } - - //return true if the scan is complete - bool End() - {return end;} - ///refresh Object found also considering current shere radius, - //and object comes from previos that are already in the stack - void Refresh() - { - int x,y,z; - for( z = explored.min.Z(); z <= explored.max.Z(); ++z) - for(y = explored.min.Y(); y <=explored.max.Y(); ++y) - for(x = explored.min.X(); x <= explored.max.X();) - { - /*vcg::Point3i CurrentCell=vcg::Point3i(x,y,z);*/ - Spatial_Idexing::CellIterator first,last,l; - ///take first, last iterators to elements in the cell - if (!_OutOfLimits(vcg::Point3i(x,y,z))) - { - Si.Grid(x,y,z,first,last); - for(l=first;l!=last;++l) - { - ObjType *elem=&(**l); - if (!tm.IsMarked(elem)) - { - - CoordType nearest; - ScalarType dist=Si.bbox.Diag(); - dist_funct((**l),p,dist,nearest); - Elems.push_back(Entry_Type(elem,fabs(dist),nearest)); - tm.Mark(elem); - } - } - } - if( ( ( y == explored.min.Y()) || ( y == explored.max.Y())) || - ( ( z == explored.min.Z()) || ( z == explored.max.Z())) || - ( x == explored.max.X())) - ++x; - else - x=explored.max.X(); - } - - std::sort(Elems.begin(),Elems.end()); - //std::unique(Elems.begin(),Elems.end()); - - CurrentElem=Elems.end(); - CurrentElem--; - } - - void operator ++() - { - if (Elems.size()>0) + ///add cell to the curren set of explored cells + bool _NextShell() { - CurrentElem--; - Elems.pop_back(); - } - if (Dist()>radius) - { - _NextShell(); - Refresh(); - //continue to scan until finish the scanning or the - //first element (the nearest for ordering of the structure) - //is at distanceradius)) + + //then expand the box + explored=to_explore; + _UpdateRadius(); + Box3 b3d(p,radius); + Si.BoxToIBox(b3d,to_explore); + Box3i ibox(Point3i(0,0,0),Si.siz-Point3i(1,1,1)); + to_explore.Intersect(ibox); + if (!to_explore.IsNull()) { - if (radius>=max_dist) - end=true; - _NextShell(); - Refresh(); - _ControlEnd(); + assert(!( to_explore.min.X()<0 || to_explore.max.X()>=Si.siz[0] || + to_explore.min.Y()<0 || to_explore.max.Y()>=Si.siz[1] || to_explore.min.Z()<0 + || to_explore.max.Z()>=Si.siz[2] )); + return true; } + return false; } - } - ObjType &operator *(){return *((*CurrentElem).elem);} - - //return distance of the element form the point if no element - //are in the vector then return max dinstance - ScalarType Dist() - { - if (Elems.size()>0) - return ((*CurrentElem).dist); - else - return ((ScalarType)FLT_MAX); - } - - CoordType NearestPoint() - {return ((*CurrentElem).intersection);} -protected: - ///structure that mantain for the current cell pre-calculated data - typedef struct Entry_Type - { public: - Entry_Type(ObjType* _elem,ScalarType _dist,CoordType _intersection) + ///contructor + ClosestIterator(Spatial_Idexing &_Si,DISTFUNCTOR _dist_funct):Si(_Si),dist_funct(_dist_funct){} + + ///set the current spatial indexing structure used + void SetIndexStructure(Spatial_Idexing &_Si) + {Si=_Si;} + + void SetMarker(TMARKER _tm) { - elem=_elem; - dist=_dist; - intersection=_intersection; + tm=_tm; } - inline bool operator < ( const Entry_Type & l ) const{return (dist > l.dist); } + ///initialize the Itarator + void Init(CoordType _p,const ScalarType &_max_dist) + { + p=_p; + max_dist=_max_dist; + Elems.clear(); + end=false; + tm.UnMarkAll(); + step_size=Si.voxel.Norm(); + radius=0; - inline bool operator == ( const Entry_Type & l ) const{return (elem == l.elem); } + ///inflate the bbox until find a valid bbox + while ((!_NextShell())&&(!End())); - ObjType* elem; - ScalarType dist; - CoordType intersection; - }; + if (!_EndGrid()) + Refresh();///load elements form currect cell - CoordType p; //initial point - Spatial_Idexing &Si; //reference to spatial index algorithm - bool end; //true if the scan is terminated - ScalarType max_dist; //max distance when the scan terminate - vcg::Box3i explored; //current bounding box explored - ScalarType radius; //curret radius for sphere expansion - //ScalarType radius_min; //curret radius of explored simplexes - ScalarType voxel_min; //minimum value of the voxel - std::vector Elems; //element loaded from the current sphere - - DISTFUNCTOR &dist_funct; - TMARKER tm; + ///until don't find an element + ///that is inside the radius + while ((!End())&&(Dist()>radius)) + { + if ((_NextShell())&&(!_EndGrid())) + Refresh(); + } - typedef typename std::vector::iterator ElemIterator; - ElemIterator CurrentElem; //iterator to current element + //set to the last element ..the nearest + CurrentElem=Elems.end(); + CurrentElem--; + + } + + //return true if the scan is complete + bool End() + {return end;} + + ///refresh Object found also considering current shere radius, + //and object comes from previos that are already in the stack + void Refresh() + { + int ix,iy,iz; + for( iz = to_explore.min.Z();iz <= to_explore.max.Z(); ++iz) + for(iy = to_explore.min.Y(); iy <=to_explore.max.Y(); ++iy) + for(ix = to_explore.min.X(); ix <= to_explore.max.X();++ix) + { + if(ixexplored.max[0] || // this test is to avoid to re-process already analyzed cells. + iyexplored.max[1] || + izexplored.max[2] ) + { + Spatial_Idexing::CellIterator first,last,l; + + Si.Grid(ix,iy,iz,first,last); + for(l=first;l!=last;++l) + { + ObjType *elem=&(**l); + if (!tm.IsMarked(elem)) + { + + CoordType nearest; + ScalarType dist=max_dist; + if (dist_funct((**l),p,dist,nearest)) + Elems.push_back(Entry_Type(elem,fabs(dist),nearest)); + tm.Mark(elem); + } + } + } + + } + + std::sort(Elems.begin(),Elems.end()); + + CurrentElem=Elems.end(); + CurrentElem--; + } + + void operator ++() + { + if (Elems.size()>0) + { + CurrentElem--; + Elems.pop_back(); + } + while ((!End())&&(Dist()>radius)) + { + + if (_NextShell()&&!_EndGrid()) + Refresh(); + } + } + + ObjType &operator *(){return *((*CurrentElem).elem);} + + //return distance of the element form the point if no element + //are in the vector then return max dinstance + ScalarType Dist() + { + if (Elems.size()>0) + return ((*CurrentElem).dist); + else + return ((ScalarType)FLT_MAX); + } + + CoordType NearestPoint() + {return ((*CurrentElem).intersection);} + + protected: + + ///structure that mantain for the current cell pre-calculated data + typedef struct Entry_Type + { + public: + + Entry_Type(ObjType* _elem,ScalarType _dist,CoordType _intersection) + { + elem=_elem; + dist=_dist; + intersection=_intersection; + } + + inline bool operator < ( const Entry_Type & l ) const{return (dist > l.dist); } + + inline bool operator == ( const Entry_Type & l ) const{return (elem == l.elem); } + + ObjType* elem; + ScalarType dist; + CoordType intersection; + }; + + CoordType p; //initial point + Spatial_Idexing &Si; //reference to spatial index algorithm + bool end; //true if the scan is terminated + ScalarType max_dist; //max distance when the scan terminate + vcg::Box3i explored; //current bounding box explored + vcg::Box3i to_explore; //current bounding box explored + ScalarType radius; //curret radius for sphere expansion + ScalarType step_size; //radius step + std::vector Elems; //element loaded from the current sphere + + DISTFUNCTOR &dist_funct; + TMARKER tm; + + typedef typename std::vector::iterator ElemIterator; + ElemIterator CurrentElem; //iterator to current element }; }