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

@@ -265,6 +265,7 @@ protected:
 
 	};
 
+
 	template <class Spatial_Idexing,class DISTFUNCTOR,class TMARKER> 
 	class ClosestIterator
 	{	
@@ -274,79 +275,45 @@ class ClosestIterator
 		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<ScalarType>(fabs(diff_min.X()),fabs(diff_max.X()));
-		ScalarType diffy=std::min<ScalarType>(fabs(diff_min.Y()),fabs(diff_max.Y()));
-		ScalarType diffz=std::min<ScalarType>(fabs(diff_min.Z()),fabs(diff_max.Z()));
-
-		ScalarType diff=std::min<ScalarType>(diffx,std::min<ScalarType>(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++)
-		{
-			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()
+		bool  _EndGrid()
 		{
-		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 _UpdateRadius()
-	void _NextShell()
 		{
 			if (radius>=max_dist)
 				end=true;
 
-			radius+=voxel_min;
+			radius+=step_size;
 			//control bounds
 			if (radius>max_dist)
 				radius=max_dist;
+		}
 
-			//expand the box
+		///add cell to the curren set of explored cells
-			explored.min-=vcg::Point3i(1,1,1);
+		bool _NextShell()
-			explored.max+=vcg::Point3i(1,1,1);
+		{
 
-			//control right limits of the bound
+			//then expand the box
-			_ControlLimits();			
+			explored=to_explore;
+			_UpdateRadius();
+			Box3<ScalarType> 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())
+			{
+				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;
 		}
 
 
@@ -368,43 +335,28 @@ public:
 		///initialize the Itarator
 		void Init(CoordType _p,const ScalarType &_max_dist)
 		{
-		//CoordType vox=Si.Voxel();
-		CoordType vox=Si.voxel;
-
-		voxel_min=std::min<ScalarType>(vox.V(0),std::min<ScalarType>(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()<Si.siz.X() && c.Y()>=0 && c.Y()<Si.siz.Y() && c.Z()>=0 && c.Z()<Si.siz.Z() );
-			
-		//explored=vcg::Box3i(c,c+vcg::Point3i(1,1,1));
-		explored=vcg::Box3i(c,c);
-		
-		radius=0;
-		//radius_min=0;
-
 			Elems.clear();
 			end=false;
-
 			tm.UnMarkAll();
+			step_size=Si.voxel.Norm();
+			radius=0;
+
+			///inflate the bbox until find a valid bbox
+			while ((!_NextShell())&&(!End()));
+
+			if (!_EndGrid())
+				Refresh();///load elements form currect cell
 
-		_FindSphereRadius();
-		Refresh();
 			///until don't find an element
 			///that is inside the radius
 			while ((!End())&&(Dist()>radius))
 			{
-			if (radius>=max_dist)
+				if ((_NextShell())&&(!_EndGrid()))
-				end=true;
-			_NextShell();
 					Refresh();
-			_ControlEnd();
 			}
+
 			//set to the last element ..the nearest
 			CurrentElem=Elems.end();
 			CurrentElem--;
@@ -419,17 +371,18 @@ public:
 		//and object comes from	previos	that are already in	the	stack
 		void Refresh()
 		{
-		int	x,y,z;
+			int	ix,iy,iz;
-		for( z = explored.min.Z(); z <=	explored.max.Z(); ++z)
+			for( iz = to_explore.min.Z();iz <=	to_explore.max.Z(); ++iz)
-			for(y =	explored.min.Y(); y	<=explored.max.Y();	 ++y)
+				for(iy =	to_explore.min.Y(); iy	<=to_explore.max.Y(); ++iy)
-				for(x =	explored.min.X(); x	<= explored.max.X();)
+					for(ix =	to_explore.min.X(); ix	<= to_explore.max.X();++ix)
+					{
+						if(ix<explored.min[0] || ix>explored.max[0] ||  // this test is to avoid to re-process already analyzed cells.
+							iy<explored.min[1] || iy>explored.max[1] ||
+							iz<explored.min[2] || iz>explored.max[2] )
 						{
-					/*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(ix,iy,iz,first,last);
-					{
-						Si.Grid(x,y,z,first,last);
 							for(l=first;l!=last;++l)
 							{
 								ObjType	*elem=&(**l);
@@ -437,23 +390,17 @@ public:
 								{
 
 									CoordType nearest;
-								ScalarType dist=Si.bbox.Diag();
+									ScalarType dist=max_dist;
-								dist_funct((**l),p,dist,nearest);
+									if (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--;
@@ -466,21 +413,11 @@ public:
 				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 distance<radius
 			while ((!End())&&(Dist()>radius))
 			{
-				if (radius>=max_dist)
+				
-					end=true;
+				if (_NextShell()&&!_EndGrid())
-				_NextShell();
 					Refresh();
-				_ControlEnd();
-			}
 			}	
 		}
 
@@ -527,9 +464,9 @@ protected:
 		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 radius_min;		  //curret radius of explored simplexes
+		ScalarType step_size;		  //radius step
-	ScalarType voxel_min;		  //minimum value of the voxel
 		std::vector<Entry_Type> Elems; //element loaded from the current sphere
 
 		DISTFUNCTOR &dist_funct;