diff --git a/vcg/space/index/grid_closest.h b/vcg/space/index/grid_closest.h index 8b6f29cc..65f7dae8 100644 --- a/vcg/space/index/grid_closest.h +++ b/vcg/space/index/grid_closest.h @@ -24,66 +24,68 @@ History $Log: not supported by cvs2svn $ +Revision 1.2 2005/09/28 08:27:11 cignoni +Added a control to avoid multiple check of the same cells during radial expansion +Still miss some code to properly initialize when point is out of the BBox of the grid. + Revision 1.1 2005/09/27 15:09:38 cignoni First Version ****************************************************************************/ -///** Returns the closest posistion of a point p and its distance -//@param p a 3d point -//@param max_dist maximum distance not to search beyond. -//@param dist_funct (templated type) a functor object used to calculate distance from a grid object to the point p. -//@param dist the returned closest distance -//@param res the returned closest point +///** Returns the closest posistion of a point _p and its distance +//@param _p a 3d point +//@param _maxDist maximum distance not to search beyond. +//@param _getPointDistance (templated type) a functor object used to calculate distance from a grid object to the point _p. +//@param _minDist the returned closest distance +//@param _closestPt the returned closest point //@return The closest element //*/ ///* // A DISTFUNCT object must implement an operator () with signature: -// bool operator () (const ObjType& obj, const CoordType & p, ScalarType & min_dist, CoordType & res); +// bool operator () (const ObjType& obj, const CoordType & _p, ScalarType & min_dist, CoordType & _closestPt); //*/ #ifndef __VCGLIB_GRID_CLOSEST #define __VCGLIB_GRID_CLOSEST +#include namespace vcg{ - - template - typename SPATIAL_INDEX::ObjPtr GetClosest( const typename SPATIAL_INDEX::CoordType & p, - const typename SPATIAL_INDEX::ScalarType & max_dist, - DISTFUNCTOR & dist_funct, - typename SPATIAL_INDEX::ScalarType & dist, - typename SPATIAL_INDEX:: CoordType & res, - TMARKER tm, - SPATIAL_INDEX &Si) + + template + typename SPATIAL_INDEX::ObjPtr GridClosest(SPATIAL_INDEX &Si,OBJPOINTDISTFUNCTOR _getPointDistance, + OBJMARKER & _marker,const typename SPATIAL_INDEX::CoordType & _p, + const typename SPATIAL_INDEX::ScalarType & _maxDist,typename SPATIAL_INDEX::ScalarType & _minDist, + typename SPATIAL_INDEX:: CoordType &_closestPt) { typedef SPATIAL_INDEX::ObjPtr ObjPtr; typedef SPATIAL_INDEX SpatialIndex; typedef SPATIAL_INDEX::CoordType CoordType; typedef SPATIAL_INDEX::ScalarType ScalarType; - // Initialize min_dist with max_dist to exploit early rejection test. - dist = max_dist; + // Initialize min_dist with _maxDist to exploit early rejection test. + _minDist = _maxDist; - ScalarType dx = ( (p[0]-Si.bbox.min[0])/Si.voxel[0] ); - ScalarType dy = ( (p[1]-Si.bbox.min[1])/Si.voxel[1] ); - ScalarType dz = ( (p[2]-Si.bbox.min[2])/Si.voxel[2] ); + ScalarType dx = ( (_p[0]-Si.bbox.min[0])/Si.voxel[0] ); + ScalarType dy = ( (_p[1]-Si.bbox.min[1])/Si.voxel[1] ); + ScalarType dz = ( (_p[2]-Si.bbox.min[2])/Si.voxel[2] ); int ix = int( dx ); int iy = int( dy ); int iz = int( dz ); - if (ix<0) ix=0; - if (iy<0) iy=0; - if (iz<0) iz=0; - if (ix>=Si.siz[0]-1) ix=Si.siz[0]-1; - if (iy>=Si.siz[1]-1) iy=Si.siz[1]-1; - if (iz>=Si.siz[2]-1) iz=Si.siz[2]-1; + if (ix<0) ix=0; + if (iy<0) iy=0; + if (iz<0) iz=0; + if (ix>=Si.siz[0]-1) ix=Si.siz[0]-1; + if (iy>=Si.siz[1]-1) iy=Si.siz[1]-1; + if (iz>=Si.siz[2]-1) iz=Si.siz[2]-1; - if (!Si.bbox.IsIn(p)){ + if (!Si.bbox.IsIn(_p)){ assert (0);///the grid has to be extended until the point - - } + + } double voxel_min=Si.voxel[0]; if (voxel_minElem()->IsD() && l->Elem()->Dist(p,min_dist,t_res)) { - //if (!l->Elem()->IsD() && dist_funct(*(l->Elem()), p, min_dist, t_res)) { // <-- NEW: use of distance functor - if (dist_funct((**l), p,dist, t_res)) // <-- NEW: use of distance functor + //if (!l->Elem()->IsD() && l->Elem()->Dist(_p,min_dist,t_res)) { + //if (!l->Elem()->IsD() && _getPointDistance(*(l->Elem()), _p, min_dist, t_res)) { // <-- NEW: use of distance functor + if (_getPointDistance((**l), _p,_minDist, t_res)) // <-- NEW: use of distance functor { winner=elem; - res=t_res; + _closestPt=t_res; } - tm.Mark(elem); + _marker.Mark(elem); } } }; @@ -134,13 +136,13 @@ namespace vcg{ //return winner; // the portion of the grid that have already been checked. - Point3i done_min, done_max; + Point3i done_min, done_max; // the new box that we want to traverse: todo is a superset of done. - Point3i todo_min=Point3i(ix,iy,iz), todo_max=Point3i(ix,iy,iz); + Point3i todo_min=Point3i(ix,iy,iz), todo_max=Point3i(ix,iy,iz); // we should traverse only (todo - done). - while (dist>radius) { + while (_minDist>radius) { done_min=todo_min; done_max=todo_max; todo_min[0]--; if (todo_min[0]<0) todo_min[0]=0; todo_min[1]--; if (todo_min[1]<0) todo_min[1]=0; @@ -152,33 +154,76 @@ namespace vcg{ for (ix=todo_min[0]; ix<=todo_max[0]; ix++) for (iy=todo_min[1]; iy<=todo_max[1]; iy++) for (iz=todo_min[2]; iz<=todo_max[2]; iz++) - if(ixdone_max[0] || // this test is to avoid to re-process already analyzed cells. - iydone_max[1] || - izdone_max[2] ) - { - Si.Grid( ix, iy, iz, first, last ); - for(l=first;l!=last;++l) + if(ixdone_max[0] || // this test is to avoid to re-process already analyzed cells. + iydone_max[1] || + izdone_max[2] ) { - if (!(**l).IsD()) + Si.Grid( ix, iy, iz, first, last ); + for(l=first;l!=last;++l) { - ObjPtr elem=&(**l); - if( ! tm.IsMarked(elem)) + if (!(**l).IsD()) { - //if (!l->Elem()->IsD() && l->Elem()->Dist(p,min_dist,t_res)) { - if (dist_funct((**l), p, dist, t_res)) + ObjPtr elem=&(**l); + if( ! _marker.IsMarked(elem)) { - winner=elem; - res=t_res; - }; - tm.Mark(elem); + //if (!l->Elem()->IsD() && l->Elem()->Dist(_p,min_dist,t_res)) { + if (_getPointDistance((**l), _p, _minDist, t_res)) + { + winner=elem; + _closestPt=t_res; + }; + _marker.Mark(elem); + } } - } - }; - } + }; + } }; return winner; }; + template + unsigned int GridGetKClosest(SPATIALINDEXING &_Si,OBJPOINTDISTFUNCTOR & _getPointDistance, + OBJMARKER & _marker, const unsigned int _k, const typename SPATIALINDEXING::CoordType & _p, + const typename SPATIALINDEXING::ScalarType & _maxDist,OBJPTRCONTAINER & _objectPtrs, + DISTCONTAINER & _distances, POINTCONTAINER & _points) + { + typedef vcg::ClosestIterator ClosestIteratorType; + ClosestIteratorType Cli=ClosestIteratorType(_Si,_getPointDistance); + Cli.SetMarker(_marker); + Cli.Init(_p,_maxDist); + unsigned int i=0; + _objectPtrs.clear(); + _distances.clear(); + _points.clear(); + while ((!Cli.End())&&(i<_k)) + { + _objectPtrs.push_back(&(*Cli)); + _distances.push_back(Cli.Dist()); + _points.push_back(Cli.NearestPoint()); + ++Cli; + i++; + } + return (i); + }; + + template + typename SPATIALINDEXING::ObjPtr GridDoRay(SPATIALINDEXING &_Si,OBJRAYISECTFUNCTOR &_rayIntersector, + OBJMARKER &_marker, const Ray3 & _ray, + const typename SPATIALINDEXING::ScalarType & _maxDist,typename SPATIALINDEXING::ScalarType & _t) + { + typedef vcg::RayIterator RayIteratorType; + RayIteratorType RayIte=RayIteratorType(_Si,_rayIntersector); + RayIte.SetMarker(_marker); + RayIte.Init(_ray); + + if (!RayIte.End()) + { + _t=RayIte.Dist(); + return(&(*RayIte)); + } + return 0; + } }//end namespace vcg #endif