vcglib: grid_static_ptr.h Source File

00001 /****************************************************************************
00002 * VCGLib                                                            o o     *
00003 * Visual and Computer Graphics Library                            o     o   *
00004 *                                                                _   O  _   *
00005 * Copyright(C) 2004                                                \/)\/    *
00006 * Visual Computing Lab                                            /\/|      *
00007 * ISTI - Italian National Research Council                           |      *
00008 *                                                                    \      *
00009 * All rights reserved.                                                      *
00010 *                                                                           *
00011 * This program is free software; you can redistribute it and/or modify      *   
00012 * it under the terms of the GNU General Public License as published by      *
00013 * the Free Software Foundation; either version 2 of the License, or         *
00014 * (at your option) any later version.                                       *
00015 *                                                                           *
00016 * This program is distributed in the hope that it will be useful,           *
00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of            *
00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
00019 * GNU General Public License (http://www.gnu.org/licenses/gpl.txt)          *
00020 * for more details.                                                         *
00021 *                                                                           *
00022 ****************************************************************************/
00023 
00024 #ifndef __VCGLIB_UGRID
00025 #define __VCGLIB_UGRID
00026 
00027 #include <vector>
00028 #include <algorithm>
00029 #include <stdio.h>
00030 
00031 #include <vcg/space/box3.h>
00032 #include <vcg/space/line3.h>
00033 #include <vcg/space/index/grid_util.h>
00034 #include <vcg/space/index/grid_closest.h>
00035 #include <vcg/simplex/face/distance.h>
00036 
00037 namespace vcg {
00038 
00071         template < class OBJTYPE, class FLT=float >
00072         class GridStaticPtr: public BasicGrid<FLT>, SpatialIndex<OBJTYPE,FLT>
00073         {
00074         public:
00075                 typedef OBJTYPE ObjType;
00076                 typedef ObjType* ObjPtr;
00077                 typedef typename ObjType::ScalarType ScalarType;
00078                 typedef Point3<ScalarType> CoordType;
00079                 typedef Box3<ScalarType> Box3x;
00080                 typedef Line3<ScalarType> Line3x;
00081                 typedef GridStaticPtr<OBJTYPE,FLT> GridPtrType;
00082     typedef BasicGrid<FLT> BT;
00083 
00087                 class Link
00088                 {
00089                 public:
00091                         inline Link(){};
00093                         inline Link(ObjPtr nt, const int ni ){
00094                                 assert(ni>=0);
00095                                 t = nt;
00096                                 i = ni;
00097                         };
00098 
00099 
00100                         inline bool operator <  ( const Link & l ) const{ return i <   l.i; } 
00101                         inline bool operator <= ( const Link & l ) const{ return i <=  l.i; }
00102                         inline bool operator >  ( const Link & l ) const{ return i >   l.i; }
00103                         inline bool operator >= ( const Link & l ) const{ return i >=  l.i; }
00104                         inline bool operator == ( const Link & l ) const{ return i ==  l.i; }
00105                         inline bool operator != ( const Link & l ) const{ return i !=  l.i; }
00106 
00107                         inline ObjPtr & Elem() {
00108                                 return t;
00109                         }
00110 
00111                         ObjType &operator *(){return *(t);}
00112 
00113                         inline int & Index() {
00114                                 return i;
00115                         }
00116 
00117                 private:
00119                         ObjPtr t;
00121                         int i;
00122 
00123 
00124                 };//end class Link
00125 
00126                 typedef Link* Cell;
00127                 typedef Cell CellIterator;
00128 
00129                 std::vector<Link>   links;   
00130 
00131                 std::vector<Cell> grid;   
00132 
00133 
00134 
00135 
00138                 inline void Grid( Point3i p, const int axis,
00139                         std::vector<Cell*> & cl) 
00140                 {
00141 #ifndef NDEBUG
00142       if ( p[0]<0 || p[0] > BT::siz[0] || 
00143                                 p[1]<0 || p[1]> BT::siz[1] || 
00144                                 p[2]<0 || p[2]> BT::siz[2] )
00145                                 assert(0);
00146                         //return NULL;
00147                         else
00148 #endif
00149         assert(((unsigned int) p[0]+BT::siz[0]*p[1]+BT::siz[1]*p[2])<grid.size());
00150 
00151                         int axis0 = (axis+1)%3;
00152                         int axis1 = (axis+2)%3;
00153                         int i,j,x,y;
00154                         x = p[axis0];
00155                         y = p[axis1];
00156                         for(i = std::max(x-1,0); i <= std::min( x,BT::siz[axis0]-1);++i)        
00157                                 for(j = std::max(y-1,0); j <= std::min( y,this->siz[axis1]-1);++j){
00158                                         p[axis0]=i;
00159                                         p[axis1]=j;
00160                                         cl.push_back(Grid(p[0]+BT::siz[0]*(p[1]+BT::siz[1]*p[2])));
00161                                 }
00162                 }
00163 
00164 
00166                 // Official access functions
00169                 Cell* Grid(const  int i) {
00170                         return &grid[i];
00171                 }
00172 
00173                 void Grid( const Cell* g, Cell & first, Cell & last )
00174                 {
00175                         first = *g;
00176                         last  = *(g+1);
00177                 }
00178 
00180                 inline Cell* Grid( const int x, const int y, const int z )
00181                 {
00182                         assert(!( x<0 || x>=BT::siz[0] || y<0 || y>=BT::siz[1] || z<0 || z>=BT::siz[2] ));
00183                         assert(grid.size()>0);
00184                         return &*grid.begin() + ( x+BT::siz[0]*(y+BT::siz[1]*z) );
00185                 }
00186 
00187                 inline Cell* Grid( const Point3i &pi)
00188                 {
00189                         return Grid(pi[0],pi[1],pi[2]);
00190                 }
00191 
00192                 void Grid( const int x, const int y, const int z, Cell & first, Cell & last )
00193                 {
00194                         Cell* g = Grid(x,y,z);
00195                         first = *g;
00196                         last  = *(g+1);
00197                 }
00198 
00199                 void Grid( const Point3<ScalarType> & p, Cell & first, Cell & last )
00200                 {
00201                         Cell* g = Grid(GridP(p));
00202 
00203                         first = *g;
00204                         last  = *(g+1);
00205                 }
00206 
00207 
00210                 template <class Box3Type>
00211                         void SetBBox( const Box3Type & b )
00212                 {
00213       this->bbox.Import( b );
00214                         ScalarType t = this->bbox.Diag()/100.0;
00215                         if(t == 0) t = ScalarType(1e-20);  // <--- Some doubts on this (Cigno 5/1/04)
00216                         this->bbox.Offset(t);
00217                         this->dim  = this->bbox.max - this->bbox.min;
00218                 }
00219 
00220 
00221 
00222                 void ShowStats(FILE *fp)
00223                 {
00224                         // Conto le entry
00225                         //int nentry = 0;
00226                         //Hist H;
00227                         //H.SetRange(0,1000,1000);
00228                         //int pg;
00229                         //for(pg=0;pg<grid.size()-1;++pg)
00230                         //      if( grid[pg]!=grid[pg+1] )
00231                         //      {
00232                         //              ++nentry;
00233                         //              H.Add(grid[pg+1]-grid[pg]);
00234                         //      }
00235 
00236                         //      fprintf(fp,"Uniform Grid: %d x %d x %d (%d voxels), %.1f%% full, %d links \nNon empty Cell Occupancy Distribution Avg: %f (%4.0f %4.0f %4.0f) \n",
00237                         //      siz[0],siz[1],siz[2],grid.size()-1,
00238                         //      double(nentry)*100.0/(grid.size()-1),links.size(),H.Avg(),H.Percentile(.25),H.Percentile(.5),H.Percentile(.75)
00239                         //    
00240                         //);
00241                 }
00242 
00243                 template <class OBJITER>
00244                 inline void Set(const OBJITER & _oBegin, const OBJITER & _oEnd, int _size=0)
00245                 {
00246                         Box3<FLT> _bbox;
00247                         Box3<FLT> b;
00248                         OBJITER i;
00249                         for(i = _oBegin; i!= _oEnd; ++i)
00250                         {
00251                                 (*i).GetBBox(b);
00252                                 _bbox.Add(b);                           
00253                         }
00255                         if(_size ==0) 
00256                                         _size=(int)std::distance<OBJITER>(_oBegin,_oEnd);
00257                                         
00258                         ScalarType infl=_bbox.Diag()/_size;
00259                         _bbox.min-=vcg::Point3<FLT>(infl,infl,infl);
00260                         _bbox.max+=vcg::Point3<FLT>(infl,infl,infl);
00261                         
00262                         Set(_oBegin,_oEnd,_bbox);
00263                 }                       
00264     
00265                         
00266                         
00267                         // This function automatically compute a reasonable size for the uniform grid providing the side (radius) of the cell
00268                         //
00269                         // Note that the bbox must be already 'inflated' so to be sure that no object will fall on the border of the grid.
00270                         
00271                         template <class OBJITER>
00272                                 inline void Set(const OBJITER & _oBegin, const OBJITER & _oEnd, const Box3x &_bbox, FLT radius)
00273                         {
00274                                         Point3i _siz;
00275                                         Point3<FLT> _dim = _bbox.max - _bbox.min;
00276                                         _dim/=radius;
00277                                         assert(_dim[0]>0 && _dim[1]>0 && _dim[2]>0 );
00278                                         _siz[0] = (int)ceil(_dim[0]);
00279                                         _siz[1] = (int)ceil(_dim[1]);
00280                                         _siz[2] = (int)ceil(_dim[2]);
00281 
00282                                         Point3<FLT> offset=Point3<FLT>::Construct(_siz);
00283                                         offset*=radius;
00284                                         offset -= (_bbox.max - _bbox.min);
00285                                         offset /=2;
00286                                         
00287                                         assert( offset[0]>=0 && offset[1]>=0 && offset[2]>=0 );
00288                                         
00289                                         Box3x bb = _bbox;
00290                                         bb.min -= offset;
00291                                         bb.max += offset;
00292                                         
00293                                         Set(_oBegin,_oEnd, bb,_siz);
00294                         }                       
00295                         
00296                         
00297                 // This function automatically compute a reasonable size for the uniform grid such that the number of cells is
00298                 // the same of the nubmer of elements to be inserted in the grid.
00299                 //
00300                 // Note that the bbox must be already 'inflated' so to be sure that no object will fall on the border of the grid.
00301                 
00302                         template <class OBJITER>
00303                 inline void Set(const OBJITER & _oBegin, const OBJITER & _oEnd, const Box3x &_bbox)
00304                 {
00305       int _size=(int)std::distance<OBJITER>(_oBegin,_oEnd);
00306                         Point3<FLT> _dim = _bbox.max - _bbox.min;
00307                         Point3i _siz;
00308                         BestDim( _size, _dim, _siz );
00309                         
00310             Set(_oBegin,_oEnd,_bbox,_siz);
00311                 }                       
00312                         
00313                         
00314                 template <class OBJITER>
00315     inline void Set(const OBJITER & _oBegin, const OBJITER & _oEnd, const Box3x &_bbox, Point3i _siz)
00316                 {
00317                         OBJITER i;
00318                         
00319                         this->bbox=_bbox;
00320                         this->siz=_siz;
00321                         
00322                         // find voxel size starting from the provided bbox and grid size. 
00323                         
00324                         this->dim  = this->bbox.max - this->bbox.min;
00325                         this->voxel[0] = this->dim[0]/this->siz[0];
00326                         this->voxel[1] = this->dim[1]/this->siz[1];
00327                         this->voxel[2] = this->dim[2]/this->siz[2];                     
00328                         
00329         // Allocate the grid (add one more for the final sentinel)
00330                                 grid.resize( this->siz[0]*this->siz[1]*this->siz[2]+1 );
00331 
00332         // Insert all the objects into the grid
00333                                 links.clear();
00334                                 for(i=_oBegin; i!=_oEnd; ++i)
00335                                 {
00336                                         Box3x bb;                       // Boundig box del tetraedro corrente
00337                                         (*i).GetBBox(bb);
00338                                         bb.Intersect(this->bbox);
00339                                         if(! bb.IsNull() )
00340                                         {
00341 
00342                                                 Box3i ib;               // Boundig box in voxels
00343                                                 this->BoxToIBox( bb,ib );
00344                                                 int x,y,z;
00345                                                 for(z=ib.min[2];z<=ib.max[2];++z)
00346                                                 {
00347                                                         int bz = z*this->siz[1];
00348                                                         for(y=ib.min[1];y<=ib.max[1];++y)
00349                                                         {
00350                                                                 int by = (y+bz)*this->siz[0];
00351                                                                 for(x=ib.min[0];x<=ib.max[0];++x)
00352                                                                         // Inserire calcolo cella corrente
00353                                                                         // if( pt->Intersect( ... )
00354                                                                         links.push_back( Link(&(*i),by+x) );
00355                                                         }
00356                                                 }
00357                                         }
00358                                 }
00359                                 // Push della sentinella
00360                                 /*links.push_back( Link((typename ContainerType::iterator)NULL,
00361                                 (grid.size()-1)));*/
00362 
00363                                 links.push_back( Link( NULL,    int(grid.size())-1) );
00364 
00365                                 // Ordinamento dei links
00366                                 sort( links.begin(), links.end() );
00367 
00368                                 // Creazione puntatori ai links
00369                                 typename std::vector<Link>::iterator pl;
00370                                 unsigned int pg;
00371                                 pl = links.begin();
00372                                 for(pg=0;pg<grid.size();++pg)
00373                                 {
00374                                         assert(pl!=links.end());
00375                                         grid[pg] = &*pl;
00376                                         while( (int)pg == pl->Index() ) // Trovato inizio
00377                                         {
00378                                                 ++pl;           // Ricerca prossimo blocco
00379                                                 if(pl==links.end())
00380                                                         break;
00381                                         }
00382                                 }
00383 
00384                 }               
00385 
00386 
00387                 int MemUsed()
00388                 {
00389                         return sizeof(GridStaticPtr)+ sizeof(Link)*links.size() + 
00390                                 sizeof(Cell) * grid.size();
00391                 }
00392 
00393                 template <class OBJPOINTDISTFUNCTOR, class OBJMARKER>
00394                         ObjPtr  GetClosest(OBJPOINTDISTFUNCTOR & _getPointDistance, OBJMARKER & _marker, 
00395                                 const typename OBJPOINTDISTFUNCTOR::QueryType & _p, const ScalarType & _maxDist,ScalarType & _minDist, CoordType & _closestPt)
00396                 {
00397                         return (vcg::GridClosest<GridPtrType,OBJPOINTDISTFUNCTOR,OBJMARKER>(*this,_getPointDistance,_marker, _p,_maxDist,_minDist,_closestPt));
00398                 }
00399 
00400 
00401                 template <class OBJPOINTDISTFUNCTOR, class OBJMARKER, class OBJPTRCONTAINER,class DISTCONTAINER, class POINTCONTAINER>
00402                         unsigned int GetKClosest(OBJPOINTDISTFUNCTOR & _getPointDistance,OBJMARKER & _marker, 
00403                         const unsigned int _k, const CoordType & _p, const ScalarType & _maxDist,OBJPTRCONTAINER & _objectPtrs,
00404                         DISTCONTAINER & _distances, POINTCONTAINER & _points)
00405                 {
00406                         return (vcg::GridGetKClosest<GridPtrType,
00407                                 OBJPOINTDISTFUNCTOR,OBJMARKER,OBJPTRCONTAINER,DISTCONTAINER,POINTCONTAINER>(*this,_getPointDistance,_marker,_k,_p,_maxDist,_objectPtrs,_distances,_points));
00408                 }
00409 
00410                 template <class OBJPOINTDISTFUNCTOR, class OBJMARKER, class OBJPTRCONTAINER, class DISTCONTAINER, class POINTCONTAINER>
00411                         unsigned int GetInSphere(OBJPOINTDISTFUNCTOR & _getPointDistance, 
00412                         OBJMARKER & _marker,
00413                         const CoordType & _p,
00414                         const ScalarType & _r,
00415                         OBJPTRCONTAINER & _objectPtrs,
00416                         DISTCONTAINER & _distances, 
00417                         POINTCONTAINER & _points)
00418                 {
00419                         return(vcg::GridGetInSphere<GridPtrType,
00420                                 OBJPOINTDISTFUNCTOR,OBJMARKER,OBJPTRCONTAINER,DISTCONTAINER,POINTCONTAINER>
00421                                 (*this,_getPointDistance,_marker,_p,_r,_objectPtrs,_distances,_points));
00422                 }
00423 
00424                 template <class OBJMARKER, class OBJPTRCONTAINER>
00425                         unsigned int GetInBox(OBJMARKER & _marker, 
00426                         const vcg::Box3<ScalarType> _bbox,
00427                         OBJPTRCONTAINER & _objectPtrs) 
00428                 {
00429                         return(vcg::GridGetInBox<GridPtrType,OBJMARKER,OBJPTRCONTAINER>
00430                                 (*this,_marker,_bbox,_objectPtrs));
00431                 }
00432 
00433                 template <class OBJRAYISECTFUNCTOR, class OBJMARKER>
00434                         ObjPtr DoRay(OBJRAYISECTFUNCTOR & _rayIntersector, OBJMARKER & _marker, const Ray3<ScalarType> & _ray, const ScalarType & _maxDist, ScalarType & _t) 
00435                 {
00436                         return(vcg::GridDoRay<GridPtrType,OBJRAYISECTFUNCTOR,OBJMARKER>(*this,_rayIntersector,_marker,_ray,_maxDist,_t));
00437                 }
00438                 
00439     /* If the grid has a cubic voxel of side <radius> this function
00440      process all couple of elementes in neighbouring cells.
00441                  GATHERFUNCTOR needs to expose this method:
00442        bool operator()(OBJTYPE *v1, OBJTYPE *v2);
00443        which is then called ONCE per unordered pair v1,v2.
00444      example:
00445    
00446      struct GFunctor {           
00447        double radius2, iradius2;
00448        GFunctor(double radius) { radius2 = radius*radius; iradius2 = 1/radius2; }
00449     
00450        bool operator()(CVertex *v1, CVertex *v2) {
00451          Point3d &p = v1->P();    
00452          Point3d &q = v2->P();
00453          double dist2 = (p-q).SquaredNorm();
00454          if(dist2 < radius2) {
00455            double w = exp(dist2*iradius2);
00456            //do something
00457          }              
00458        }
00459     }; */
00460                   
00461                 template <class GATHERFUNCTOR> 
00462     void Gather(GATHERFUNCTOR gfunctor) {
00463       static int corner[8*3] = { 0, 0, 0,  1, 0, 0,  0, 1, 0,  0, 0, 1,
00464                                  0, 1, 1,  1, 0, 1,  1, 1, 0,  1, 1, 1 };
00465 
00466       static int diagonals[14*2] = { 0, 0, 
00467                                      0, 1, 0, 2, 0, 3, 0, 4, 0, 5, 0, 6, 0, 7,
00468                                      2, 3, 1, 3, 1, 2,                       
00469                                      1, 4, 2, 5, 3, 6 };
00470   
00471       Cell ostart, oend, dstart, dend;
00472       for(int z = 0; z < this->siz[2]; z++) {
00473         for(int y = 0; y < this->siz[1]; y++) {
00474           for(int x = 0; x < this->siz[0]; x++) {            
00475 
00476             Grid(x, y, z, ostart, oend);
00477 
00478             for(Cell c = ostart; c != oend; c++) 
00479               for(Cell s = c+1; s != oend; s++) 
00480                 gfunctor(c->Elem(), s->Elem());
00481                     
00482             for(int d = 2; d < 28; d += 2) { //skipping self
00483               int *cs = corner + 3*diagonals[d];
00484               int *ce = corner + 3*diagonals[d+1];
00485               if((x + cs[0] < this->siz[0]) && (y + cs[1] < this->siz[1]) && (z + cs[2] < this->siz[2]) &&
00486                  (x + ce[0] < this->siz[0]) && (y + ce[1] < this->siz[1]) && (z + ce[2] < this->siz[2])) {
00487 
00488                  Grid(x+cs[0], y+cs[1], z+cs[2], ostart, oend);
00489                  Grid(x+ce[0], y+ce[1], z+ce[2], dstart, dend);
00490 
00491                  for(Cell c = ostart; c != oend; c++) 
00492                    for(Cell s = dstart; s != dend; s++) 
00493                      gfunctor(c->Elem(), s->Elem());                               
00494               }
00495             }
00496           }
00497         }
00498       }
00499     }
00500 
00501 
00502         }; //end class GridStaticPtr
00503 
00504 } // end namespace
00505 
00506 #endif