//----------------------------------------------------------------------------
//  EgalTech 2014-2014
//----------------------------------------------------------------------------
// File : PointGrid3d.cpp        Data : 15.05.14          Versione : 1.5e5
// Contenuto : Implementazione della classe PointGrid3d.
//             Indicizzazione spaziale di Point3d mediante hash grid.
//
//
// Modifiche : 15.05.14 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "\EgtDev\Include\EGkPointGrid3d.h"
#include <utility>

using namespace std ;

//----------------------------------------------------------------------------
bool
PointGrid3d::Init( int nBuckets, double dCellDim)
{
   m_dCellDim = max( dCellDim, EPS_SMALL) ;
   m_dInvCellDim = 1 / m_dCellDim ;
   try { m_MMap.rehash( nBuckets) ;}
   catch(...) { return false ;}
   return true ;
}

//----------------------------------------------------------------------------
bool
PointGrid3d::InsertPoint( const Point3d& ptP, int nId)
{
   int nKey = PointHash( Get1dCellNbr( ptP.x), Get1dCellNbr( ptP.y), Get1dCellNbr( ptP.z)) ;
   m_MMap.insert( make_pair( nKey, make_pair( ptP, nId))) ;
   return true ;
}

//----------------------------------------------------------------------------
bool
PointGrid3d::Find( const Point3d& ptTest, double dTol, INTVECTOR& vnIds)
{
  // ricavo gli indici sui tre assi degli estremi del box
   int nXmin = Get1dCellNbr( ptTest.x - dTol) ;
   int nXmax = Get1dCellNbr( ptTest.x + dTol) ;
   int nYmin = Get1dCellNbr( ptTest.y - dTol) ;
   int nYmax = Get1dCellNbr( ptTest.y + dTol) ;
   int nZmin = Get1dCellNbr( ptTest.z - dTol) ;
   int nZmax = Get1dCellNbr( ptTest.z + dTol) ;
  // pulisco il risultato
   vnIds.clear() ;
  // ciclo su tutte le celle comprese tra gli estremi
   for ( int i = nXmin ; i <= nXmax ; ++ i) {
      for ( int j = nYmin ; j <= nYmax ; ++ j) {
         for ( int k = nZmin ; k <= nZmax ; ++ k) {
            pair<IPNTI_UMMAP::iterator, IPNTI_UMMAP::iterator> MMrange =
                                  m_MMap.equal_range( PointHash( i, j, k)) ;
            for ( ; MMrange.first != MMrange.second ; ++ MMrange.first) {
               if ( SqDist( (*MMrange.first).second.first, ptTest) < ( dTol * dTol))
                  vnIds.push_back( (*MMrange.first).second.second) ;
            }
         }
      }
   }

   return ( vnIds.size() > 0) ;
}

//----------------------------------------------------------------------------
bool
PointGrid3d::Find( const Point3d& ptTest, double dTol, int& nId)
{
  // ricavo gli indici sui tre assi degli estremi del box
   int nXmin = Get1dCellNbr( ptTest.x - dTol) ;
   int nXmax = Get1dCellNbr( ptTest.x + dTol) ;
   int nYmin = Get1dCellNbr( ptTest.y - dTol) ;
   int nYmax = Get1dCellNbr( ptTest.y + dTol) ;
   int nZmin = Get1dCellNbr( ptTest.z - dTol) ;
   int nZmax = Get1dCellNbr( ptTest.z + dTol) ;
  // ciclo su tutte le celle comprese tra gli estremi
   for ( int i = nXmin ; i <= nXmax ; ++ i) {
      for ( int j = nYmin ; j <= nYmax ; ++ j) {
         for ( int k = nZmin ; k <= nZmax ; ++ k) {
            pair<IPNTI_UMMAP::iterator, IPNTI_UMMAP::iterator> MMrange =
                                  m_MMap.equal_range( PointHash( i, j, k)) ;
            for ( ; MMrange.first != MMrange.second ; ++ MMrange.first) {
               if ( SqDist( (*MMrange.first).second.first, ptTest) < ( dTol * dTol)) {
                  nId = (*MMrange.first).second.second ;
                  return true ;
               }
            }
         }
      }
   }

   return false ;
}

//----------------------------------------------------------------------------
int
PointGrid3d::Get1dCellNbr( double dCoord)
{
   return static_cast<int>( floor( dCoord * m_dInvCellDim)) ;
}

//----------------------------------------------------------------------------
int
PointGrid3d::PointHash( int nX, int nY, int nZ)
{
   return ( nX * 73856093 ^ nY * 19349663 ^ nZ * 83492791) ;
}