grid_static_ptr.h

Go to the documentation of this file.

/****************************************************************************
* VCGLib                                                            o o     *
* Visual and Computer Graphics Library                            o     o   *
*                                                                _   O  _   *
* Copyright(C) 2004                                                \/)\/    *
* Visual Computing Lab                                            /\/|      *
* ISTI - Italian National Research Council                           |      *
*                                                                    \      *
* All rights reserved.                                                      *
*                                                                           *
* This program is free software; you can redistribute it and/or modify      *   
* it under the terms of the GNU General Public License as published by      *
* the Free Software Foundation; either version 2 of the License, or         *
* (at your option) any later version.                                       *
*                                                                           *
* This program is distributed in the hope that it will be useful,           *
* but WITHOUT ANY WARRANTY; without even the implied warranty of            *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
* GNU General Public License (http://www.gnu.org/licenses/gpl.txt)          *
* for more details.                                                         *
*                                                                           *
****************************************************************************/

#ifndef __VCGLIB_UGRID
#define __VCGLIB_UGRID

#include <vector>
#include <algorithm>
#include <stdio.h>

#include <vcg/space/box3.h>
#include <vcg/space/line3.h>
#include <vcg/space/index/grid_util.h>
#include <vcg/space/index/grid_closest.h>
#include <vcg/simplex/face/distance.h>

namespace vcg {

        template < class OBJTYPE, class FLT=float >
        class GridStaticPtr: public BasicGrid<FLT>, SpatialIndex<OBJTYPE,FLT>
        {
        public:
                typedef OBJTYPE ObjType;
                typedef ObjType* ObjPtr;
                typedef typename ObjType::ScalarType ScalarType;
                typedef Point3<ScalarType> CoordType;
                typedef Box3<ScalarType> Box3x;
                typedef Line3<ScalarType> Line3x;
                typedef GridStaticPtr<OBJTYPE,FLT> GridPtrType;
    typedef BasicGrid<FLT> BT;

                class Link
                {
                public:
                        inline Link(){};
                        inline Link(ObjPtr nt, const int ni ){
                                assert(ni>=0);
                                t = nt;
                                i = ni;
                        };


                        inline bool operator <  ( const Link & l ) const{ return i <   l.i; } 
                        inline bool operator <= ( const Link & l ) const{ return i <=  l.i; }
                        inline bool operator >  ( const Link & l ) const{ return i >   l.i; }
                        inline bool operator >= ( const Link & l ) const{ return i >=  l.i; }
                        inline bool operator == ( const Link & l ) const{ return i ==  l.i; }
                        inline bool operator != ( const Link & l ) const{ return i !=  l.i; }

                        inline ObjPtr & Elem() {
                                return t;
                        }

                        ObjType &operator *(){return *(t);}

                        inline int & Index() {
                                return i;
                        }

                private:
                        ObjPtr t;
                        int i;


                };//end class Link

                typedef Link* Cell;
                typedef Cell CellIterator;

                std::vector<Link>   links;   

                std::vector<Cell> grid;   



    bool Empty() const {return links.empty();}

                inline void Grid( Point3i p, const int axis,
                        std::vector<Cell*> & cl) 
                {
#ifndef NDEBUG
      if ( p[0]<0 || p[0] > BT::siz[0] || 
                                p[1]<0 || p[1]> BT::siz[1] || 
                                p[2]<0 || p[2]> BT::siz[2] )
                                assert(0);
                        //return NULL;
                        else
#endif
        assert(((unsigned int) p[0]+BT::siz[0]*p[1]+BT::siz[1]*p[2])<grid.size());

                        int axis0 = (axis+1)%3;
                        int axis1 = (axis+2)%3;
                        int i,j,x,y;
                        x = p[axis0];
                        y = p[axis1];
                        for(i = std::max(x-1,0); i <= std::min( x,BT::siz[axis0]-1);++i)        
                                for(j = std::max(y-1,0); j <= std::min( y,this->siz[axis1]-1);++j){
                                        p[axis0]=i;
                                        p[axis1]=j;
                                        cl.push_back(Grid(p[0]+BT::siz[0]*(p[1]+BT::siz[1]*p[2])));
                                }
                }


                // Official access functions
                Cell* Grid(const  int i) {
                        return &grid[i];
                }

                void Grid( const Cell* g, Cell & first, Cell & last )
                {
                        first = *g;
                        last  = *(g+1);
                }

                inline Cell* Grid( const int x, const int y, const int z )
                {
                        assert(!( x<0 || x>=BT::siz[0] || y<0 || y>=BT::siz[1] || z<0 || z>=BT::siz[2] ));
                        assert(grid.size()>0);
                        return &*grid.begin() + ( x+BT::siz[0]*(y+BT::siz[1]*z) );
                }

                inline Cell* Grid( const Point3i &pi)
                {
                        return Grid(pi[0],pi[1],pi[2]);
                }

                void Grid( const int x, const int y, const int z, Cell & first, Cell & last )
                {
                        Cell* g = Grid(x,y,z);
                        first = *g;
                        last  = *(g+1);
                }

                void Grid( const Point3<ScalarType> & p, Cell & first, Cell & last )
                {
                        Cell* g = Grid(GridP(p));

                        first = *g;
                        last  = *(g+1);
                }


                template <class Box3Type>
                        void SetBBox( const Box3Type & b )
                {
      this->bbox.Import( b );
                        ScalarType t = this->bbox.Diag()/100.0;
                        if(t == 0) t = ScalarType(1e-20);  // <--- Some doubts on this (Cigno 5/1/04)
                        this->bbox.Offset(t);
                        this->dim  = this->bbox.max - this->bbox.min;
                }



//              void ShowStats(FILE *fp)
//              {
//                      // Conto le entry
//                      //int nentry = 0;
//                      //Hist H;
//                      //H.SetRange(0,1000,1000);
//                      //int pg;
//                      //for(pg=0;pg<grid.size()-1;++pg)
//                      //      if( grid[pg]!=grid[pg+1] )
//                      //      {
//                      //              ++nentry;
//                      //              H.Add(grid[pg+1]-grid[pg]);
//                      //      }

//                      //      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",
//                      //      siz[0],siz[1],siz[2],grid.size()-1,
//                      //      double(nentry)*100.0/(grid.size()-1),links.size(),H.Avg(),H.Percentile(.25),H.Percentile(.5),H.Percentile(.75)
//                      //
//                      //);
//              }

                template <class OBJITER>
                inline void Set(const OBJITER & _oBegin, const OBJITER & _oEnd, int _size=0)
                {
                        Box3<FLT> _bbox;
                        Box3<FLT> b;
                        for(OBJITER i = _oBegin; i!= _oEnd; ++i)
                        {
                                (*i).GetBBox(b);
                                _bbox.Add(b);                           
                        }
                        if(_size ==0) 
                                        _size=(int)std::distance<OBJITER>(_oBegin,_oEnd);

                        ScalarType infl=_bbox.Diag()/_size;
                        _bbox.min-=vcg::Point3<FLT>(infl,infl,infl);
                        _bbox.max+=vcg::Point3<FLT>(infl,infl,infl);
                        
                        Set(_oBegin,_oEnd,_bbox,_size);
                }                       
    
                        
                        
                        // This function automatically compute a reasonable size for the uniform grid providing the side (radius) of the cell
                        //
                        // Note that the bbox must be already 'inflated' so to be sure that no object will fall on the border of the grid.
                        
                        template <class OBJITER>
                                inline void SetWithRadius(const OBJITER & _oBegin, const OBJITER & _oEnd, FLT _cellRadius)
                        {
                                  Box3<FLT> _bbox;
                                  Box3<FLT> b;
                                  for(OBJITER i = _oBegin; i!= _oEnd; ++i)
                                  {
                                          (*i).GetBBox(b);
                                          _bbox.Add(b);
                                  }

                                  _bbox.min-=vcg::Point3<FLT>(_cellRadius,_cellRadius,_cellRadius);
                                  _bbox.max+=vcg::Point3<FLT>(_cellRadius,_cellRadius,_cellRadius);

                                  Point3i _siz;
                                  Point3<FLT> _dim = _bbox.max - _bbox.min;
                                  _dim/=_cellRadius;
                                  assert(_dim[0]>0 && _dim[1]>0 && _dim[2]>0 );
                                  _siz[0] = (int)ceil(_dim[0]);
                                  _siz[1] = (int)ceil(_dim[1]);
                                  _siz[2] = (int)ceil(_dim[2]);

                                  Set(_oBegin,_oEnd, _bbox,_siz);
                        }                       
                        
                        
                // This function automatically compute a reasonable size for the uniform grid such that the number of cells is
                // the same of the nubmer of elements to be inserted in the grid.
                //
                // Note that the bbox must be already 'inflated' so to be sure that no object will fall on the border of the grid.
                
                        template <class OBJITER>
                        inline void Set(const OBJITER & _oBegin, const OBJITER & _oEnd, const Box3x &_bbox, int _size=0)
                        {
                          if(_size==0)
                                _size=(int)std::distance<OBJITER>(_oBegin,_oEnd);
                          Point3<FLT> _dim = _bbox.max - _bbox.min;
                          Point3i _siz;
                          BestDim( _size, _dim, _siz );

                          Set(_oBegin,_oEnd,_bbox,_siz);
                        }


                // This is the REAL LOW LEVEL function
                        
                template <class OBJITER>
    inline void Set(const OBJITER & _oBegin, const OBJITER & _oEnd, const Box3x &_bbox, Point3i _siz)
                {
                        OBJITER i;
                        
                        this->bbox=_bbox;
                        this->siz=_siz;
                        
                        // find voxel size starting from the provided bbox and grid size. 
                        
                        this->dim  = this->bbox.max - this->bbox.min;
                        this->voxel[0] = this->dim[0]/this->siz[0];
                        this->voxel[1] = this->dim[1]/this->siz[1];
                        this->voxel[2] = this->dim[2]/this->siz[2];                     
                        
        // Allocate the grid (add one more for the final sentinel)
                                grid.resize( this->siz[0]*this->siz[1]*this->siz[2]+1 );

        // Insert all the objects into the grid
                                links.clear();
                                for(i=_oBegin; i!=_oEnd; ++i)
                                {
                                        Box3x bb;                       // Boundig box del tetraedro corrente
                                        (*i).GetBBox(bb);
                                        bb.Intersect(this->bbox);
                                        if(! bb.IsNull() )
                                        {

                                                Box3i ib;               // Boundig box in voxels
                                                this->BoxToIBox( bb,ib );
                                                int x,y,z;
                                                for(z=ib.min[2];z<=ib.max[2];++z)
                                                {
                                                        int bz = z*this->siz[1];
                                                        for(y=ib.min[1];y<=ib.max[1];++y)
                                                        {
                                                                int by = (y+bz)*this->siz[0];
                                                                for(x=ib.min[0];x<=ib.max[0];++x)
                                                                        // Inserire calcolo cella corrente
                                                                        // if( pt->Intersect( ... )
                                                                        links.push_back( Link(&(*i),by+x) );
                                                        }
                                                }
                                        }
                                }
                                // Push della sentinella
                                /*links.push_back( Link((typename ContainerType::iterator)NULL,
                                (grid.size()-1)));*/

                                links.push_back( Link( NULL,    int(grid.size())-1) );

                                // Ordinamento dei links
                                sort( links.begin(), links.end() );

                                // Creazione puntatori ai links
                                typename std::vector<Link>::iterator pl;
                                unsigned int pg;
                                pl = links.begin();
                                for(pg=0;pg<grid.size();++pg)
                                {
                                        assert(pl!=links.end());
                                        grid[pg] = &*pl;
                                        while( (int)pg == pl->Index() ) // Trovato inizio
                                        {
                                                ++pl;           // Ricerca prossimo blocco
                                                if(pl==links.end())
                                                        break;
                                        }
                                }

                }               


                int MemUsed()
                {
                        return sizeof(GridStaticPtr)+ sizeof(Link)*links.size() + 
                                sizeof(Cell) * grid.size();
                }

                template <class OBJPOINTDISTFUNCTOR, class OBJMARKER>
                        ObjPtr  GetClosest(OBJPOINTDISTFUNCTOR & _getPointDistance, OBJMARKER & _marker, 
                                const typename OBJPOINTDISTFUNCTOR::QueryType & _p, const ScalarType & _maxDist,ScalarType & _minDist, CoordType & _closestPt)
                {
                        return (vcg::GridClosest<GridPtrType,OBJPOINTDISTFUNCTOR,OBJMARKER>(*this,_getPointDistance,_marker, _p,_maxDist,_minDist,_closestPt));
                }


                template <class OBJPOINTDISTFUNCTOR, class OBJMARKER, class OBJPTRCONTAINER,class DISTCONTAINER, class POINTCONTAINER>
                        unsigned int GetKClosest(OBJPOINTDISTFUNCTOR & _getPointDistance,OBJMARKER & _marker, 
                        const unsigned int _k, const CoordType & _p, const ScalarType & _maxDist,OBJPTRCONTAINER & _objectPtrs,
                        DISTCONTAINER & _distances, POINTCONTAINER & _points)
                {
                        return (vcg::GridGetKClosest<GridPtrType,
                                OBJPOINTDISTFUNCTOR,OBJMARKER,OBJPTRCONTAINER,DISTCONTAINER,POINTCONTAINER>(*this,_getPointDistance,_marker,_k,_p,_maxDist,_objectPtrs,_distances,_points));
                }

                template <class OBJPOINTDISTFUNCTOR, class OBJMARKER, class OBJPTRCONTAINER, class DISTCONTAINER, class POINTCONTAINER>
                        unsigned int GetInSphere(OBJPOINTDISTFUNCTOR & _getPointDistance, 
                        OBJMARKER & _marker,
                        const CoordType & _p,
                        const ScalarType & _r,
                        OBJPTRCONTAINER & _objectPtrs,
                        DISTCONTAINER & _distances, 
                        POINTCONTAINER & _points)
                {
                        return(vcg::GridGetInSphere<GridPtrType,
                                OBJPOINTDISTFUNCTOR,OBJMARKER,OBJPTRCONTAINER,DISTCONTAINER,POINTCONTAINER>
                                (*this,_getPointDistance,_marker,_p,_r,_objectPtrs,_distances,_points));
                }

                template <class OBJMARKER, class OBJPTRCONTAINER>
                        unsigned int GetInBox(OBJMARKER & _marker, 
                        const vcg::Box3<ScalarType> _bbox,
                        OBJPTRCONTAINER & _objectPtrs) 
                {
                        return(vcg::GridGetInBox<GridPtrType,OBJMARKER,OBJPTRCONTAINER>
                                (*this,_marker,_bbox,_objectPtrs));
                }

                template <class OBJRAYISECTFUNCTOR, class OBJMARKER>
                        ObjPtr DoRay(OBJRAYISECTFUNCTOR & _rayIntersector, OBJMARKER & _marker, const Ray3<ScalarType> & _ray, const ScalarType & _maxDist, ScalarType & _t) 
                {
                        return(vcg::GridDoRay<GridPtrType,OBJRAYISECTFUNCTOR,OBJMARKER>(*this,_rayIntersector,_marker,_ray,_maxDist,_t));
                }
                
    /* If the grid has a cubic voxel of side <radius> this function
     process all couple of elementes in neighbouring cells.
                 GATHERFUNCTOR needs to expose this method:
       bool operator()(OBJTYPE *v1, OBJTYPE *v2);
       which is then called ONCE per unordered pair v1,v2.
     example:
   
     struct GFunctor {           
       double radius2, iradius2;
       GFunctor(double radius) { radius2 = radius*radius; iradius2 = 1/radius2; }
    
       bool operator()(CVertex *v1, CVertex *v2) {
         Point3d &p = v1->P();    
         Point3d &q = v2->P();
         double dist2 = (p-q).SquaredNorm();
         if(dist2 < radius2) {
           double w = exp(dist2*iradius2);
           //do something
         }              
       }
    }; */
                  
                template <class GATHERFUNCTOR> 
    void Gather(GATHERFUNCTOR gfunctor) {
      static int corner[8*3] = { 0, 0, 0,  1, 0, 0,  0, 1, 0,  0, 0, 1,
                                 0, 1, 1,  1, 0, 1,  1, 1, 0,  1, 1, 1 };

      static int diagonals[14*2] = { 0, 0, 
                                     0, 1, 0, 2, 0, 3, 0, 4, 0, 5, 0, 6, 0, 7,
                                     2, 3, 1, 3, 1, 2,                       
                                     1, 4, 2, 5, 3, 6 };
  
      Cell ostart, oend, dstart, dend;
      for(int z = 0; z < this->siz[2]; z++) {
        for(int y = 0; y < this->siz[1]; y++) {
          for(int x = 0; x < this->siz[0]; x++) {            

            Grid(x, y, z, ostart, oend);

            for(Cell c = ostart; c != oend; c++) 
              for(Cell s = c+1; s != oend; s++) 
                gfunctor(c->Elem(), s->Elem());
                    
            for(int d = 2; d < 28; d += 2) { //skipping self
              int *cs = corner + 3*diagonals[d];
              int *ce = corner + 3*diagonals[d+1];
              if((x + cs[0] < this->siz[0]) && (y + cs[1] < this->siz[1]) && (z + cs[2] < this->siz[2]) &&
                 (x + ce[0] < this->siz[0]) && (y + ce[1] < this->siz[1]) && (z + ce[2] < this->siz[2])) {

                 Grid(x+cs[0], y+cs[1], z+cs[2], ostart, oend);
                 Grid(x+ce[0], y+ce[1], z+ce[2], dstart, dend);

                 for(Cell c = ostart; c != oend; c++) 
                   for(Cell s = dstart; s != dend; s++) 
                     gfunctor(c->Elem(), s->Elem());                               
              }
            }
          }
        }
      }
    }


        }; //end class GridStaticPtr

} // end namespace

#endif