//----------------------------------------------------------------------------
//  EgalTech 2015-2018
//----------------------------------------------------------------------------
// File : HashGrids2d.cpp        Data : 07.12.18         Versione : 1.9l1
// Contenuto : Funzioni della classe HashGrids2d.
//
//
//
// Modifiche : 04.07.15 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "DllMain.h"
#include "/EgtDev/Include/EGkHashGrids2d.h"
#include "/EgtDev/Include/EGnStringUtils.h"
#include <algorithm>

using namespace std ;

//----------------------------------------------------------------------------
const size_t xCellCount = 16 ; 
const size_t yCellCount = 16 ; 
const size_t cellVectorSize = 16 ; 
const size_t occupiedCellsVectorSize = 256 ; 
const size_t minimalGridDensity = 1 ; 
const size_t gridActivationThreshold = 64 ; 
const double hierarchyFactor = 2 ; 
const double MIN_CELL_SIZE = 5.0 ;

//----------------------------------------------------------------------------
//   HashGrid2d
//----------------------------------------------------------------------------
class HashGrid2d
{
   private :
      struct Cell
      {
         HashGrids2d::PtrObjVector* m_Objs ; // Vettore dei puntatori agli oggetti nella cella, come puntatore
         int*       m_neighborOffset ;       // Puntatore ad array con offsets per accedere direttamente ai vicini
         size_t     m_occupiedCellsId ;      // Indice della cella nel vettore delle celle occupate
         Cell( void)
            : m_Objs( nullptr), m_neighborOffset( nullptr), m_occupiedCellsId( 0) {}
      } ;

      typedef vector<Cell*>  CellVector ;

   public :
      explicit HashGrid2d( double dCellSpan) ;
      ~HashGrid2d( void) ;
      double GetCellSpan( void) const
         { return m_dCellSpan ; }
      void Add( HashGrids2d::ObjData& obj) ;
      void Remove( HashGrids2d::ObjData& obj) ;
      void Update( HashGrids2d::ObjData& obj) ;
      void Find( const BBox3d& b3Test, INTVECTOR& vnIds) ;
      void Clear( void) ;

   private :
      void InitNeighborOffsets( void) ;
      size_t Hash( const Point3d& ptP) const ;
      void Add( HashGrids2d::ObjData& obj, Cell* cell) ;
      void Remove( HashGrids2d::ObjData& obj, Cell* cell) ;
      void Enlarge( void) ;
      static inline bool PowerOfTwo( size_t number) ;

   private :
      Cell* m_cell ;           // Vettore di celle della griglia

      size_t m_xCellCount ;    // Numero di celle allocate sulla direzione X
      size_t m_yCellCount ;    // Numero di celle allocate sulla direzione Y

      size_t m_xHashMask ;     // Maschera di bit per calcolo hash di X
      size_t m_yHashMask ;     // Maschera di bit per calcolo hash di Y

      size_t m_xyCellCount ;   // Numero di celle nel piano XY ( == numero totale)

      size_t m_enlargementThreshold ;  // Soglia corrente per incremetare le dimensioni della griglia

      double m_dCellSpan ;              // Dimensione di una cella (cubica) della griglia
      double m_dInvCellSpan ;           // Inverso della dimensione di una cella

      CellVector m_occupiedCells ;     // Vettore delle celle occupate in questa griglia

      size_t m_objCount ;              // Numero di oggetti presenti in questa griglia

      int m_stdNeighborOffset[9] ;     // Array degli offset standard per le adiacenze

      BBox3d m_b3Grid ;                // Box totale degli oggetti inseriti in questa griglia
} ;

//----------------------------------------------------------------------------
HashGrid2d::HashGrid2d( double dCellSpan)
{
  // Initialization of all member variables and ...
   m_xCellCount = PowerOfTwo( xCellCount) ? xCellCount : 16 ;
   m_yCellCount = PowerOfTwo( yCellCount) ? yCellCount : 16 ;

   m_xHashMask = m_xCellCount - 1 ;
   m_yHashMask = m_yCellCount - 1 ;

   m_xyCellCount  = m_xCellCount * m_yCellCount ;

   m_enlargementThreshold = m_xyCellCount / minimalGridDensity ;

   // allocazione dell'array lineare che rappresenta lo hash grid.
   m_cell = new Cell[ m_xyCellCount] ;

   // ogni cella è già inizializzata come vuota
   // imposto gli offset ai vicini
   InitNeighborOffsets() ;

   m_dCellSpan = max( dCellSpan, 10 * EPS_SMALL) ;
   m_dInvCellSpan = 1. / dCellSpan ;

   m_occupiedCells.reserve( occupiedCellsVectorSize) ;

   m_objCount = 0 ;
}

//----------------------------------------------------------------------------
HashGrid2d::~HashGrid2d( void)
{
   Clear() ;

   for ( Cell* pCell = m_cell ; pCell != nullptr && pCell < m_cell + m_xyCellCount ; ++ pCell) {
      if ( pCell->m_neighborOffset != m_stdNeighborOffset)
         delete[] pCell->m_neighborOffset ;
   }
   delete[] m_cell ;
}

//----------------------------------------------------------------------------
void
HashGrid2d::Add( HashGrids2d::ObjData& obj)
{
   // If adding the body will cause the total number of bodies assigned to this grid to exceed the
   // enlargement threshold, the size of this hash grid must be increased.
   if ( m_objCount == m_enlargementThreshold)
      Enlarge() ;

   // Calculate (and store) the hash value (= the body's cell association) and ...
   size_t h = Hash( obj.box.GetMin()) ;
   obj.nHash = h ;

   // ... insert the body into the corresponding cell.
   Cell* pCell = m_cell + h ;
   Add( obj, pCell) ;
   ++ m_objCount ;

  // Aggiorno box di griglia
   m_b3Grid.Add( obj.box) ;
}

//----------------------------------------------------------------------------
void
HashGrid2d::Remove( HashGrids2d::ObjData& obj)
{
   // The stored hash value (= the body's cell association) is used in order to directly access the
   // cell from which this body will be removed.
   Cell* pCell = m_cell + obj.nHash ;
   Remove( obj, pCell) ;
   -- m_objCount ;
}

//----------------------------------------------------------------------------
void
HashGrid2d::Update( HashGrids2d::ObjData& obj)
{
   // The hash value is recomputed based on the body's current spatial location.
   size_t newHash = Hash( obj.box.GetMin()) ;
   size_t oldHash = obj.nHash ;

   // If this new hash value is identical to the hash value of the previous time step, the body
   // remains assigned to its current grid cell.
   if ( newHash == oldHash)
      return ;

   // Only if the hash value changes, the cell association has to be changed, too - meaning, the
   // body has to be removed from its currently assigned cell and ...
   Cell* pCell = m_cell + oldHash ;
   Remove( obj, pCell) ;

   obj.nHash = newHash ;

   // ... stored in the cell that corresponds to the new hash value.
   pCell = m_cell + newHash ;
   Add( obj, pCell) ;

  // Aggiorno box di griglia
   m_b3Grid.Add( obj.box) ;
}

//----------------------------------------------------------------------------
void
HashGrid2d::Find( const BBox3d& b3Test, INTVECTOR& vnIds)
{
  // Limito il box a quello di griglia
   BBox3d b3Int ;
   if ( ! b3Test.FindIntersectionXY( m_b3Grid, b3Int))
      return ;

  // Recupero gli estremi del box
   Point3d ptMin ;
   double dXDim, dYDim, dZDim ;
   if ( ! b3Int.GetMinDim( ptMin, dXDim, dYDim, dZDim))
      return ;
  // Sposto p.to minimo in meno di una cella (oggetti possono occupare 2 celle) e allargo tutto di EPS_SMALL
   ptMin -= Vector3d( 1, 1, 0) * ( m_dCellSpan + EPS_SMALL) ;
   dXDim += m_dCellSpan + 2 * EPS_SMALL ;
   dYDim += m_dCellSpan + 2 * EPS_SMALL ;

  // Numero di celle da esplorare sui 3 assi
   int nXSpan = min( static_cast<int>( ceil( dXDim * m_dInvCellSpan)), int( m_xCellCount)) ;
   int nYSpan = min( static_cast<int>( ceil( dYDim * m_dInvCellSpan)), int( m_yCellCount)) ;

   //string sOut = "Celle=" + ToString( int( m_xyCellCount)) + " Occupate=" + ToString( int(m_occupiedCells.size())) + " Span=" + ToString( nXSpan * nYSpan) ;
   //LOG_INFO( GetEGkLogger(), sOut.c_str()) ;

  // Se conviene verificare queste celle
   if ( nXSpan * nYSpan < 5 * int( m_occupiedCells.size())) {
     // cella di base
      int nX = static_cast<int>( Hash( ptMin)) ;
      for ( int i = 0 ; i <= nXSpan ; ++ i) {
         int nY = nX ;
         for ( int j = 0 ; j <= nYSpan ; ++ j) {
           // inserisco in lista gli oggetti della cella
            if ( m_cell[nY].m_Objs != nullptr) {
               for ( auto pObj : *( m_cell[nY].m_Objs)) {
                  if ( b3Int.OverlapsXY( pObj->box))
                     vnIds.push_back( pObj->nId) ;
               }
            }
           // passo alla successiva in Y+
            nY += m_cell[nY].m_neighborOffset[7] ;
         }
        // passo alla successiva in X+
         nX += m_cell[nX].m_neighborOffset[5] ;
      }
   }

  // altrimenti verifico direttamente tutte e sole quelle occupate
   else {
     // ciclo sulle celle occupate
      for ( auto cell : m_occupiedCells) {
        // inserisco in lista gli oggetti della cella
         if ( cell->m_Objs != nullptr) {
            for ( auto pObj : *(cell->m_Objs)) {
               if ( b3Int.OverlapsXY( pObj->box))
                  vnIds.push_back( pObj->nId) ;
            }
         }
      }
   }
}

//----------------------------------------------------------------------------
void
HashGrid2d::Clear( void)
{
   for ( auto cell : m_occupiedCells) {
      delete cell->m_Objs ;
      cell->m_Objs = nullptr ;
   }
   m_occupiedCells.clear() ;
   m_objCount = 0 ;
   m_b3Grid.Reset() ;
}

//----------------------------------------------------------------------------
void
HashGrid2d::InitNeighborOffsets( void)
{
   int xc   = static_cast<int>( m_xCellCount) ;
   int yc   = static_cast<int>( m_yCellCount) ;
   int xyc  = static_cast<int>( m_xyCellCount) ;

   // Initialization of the grid-global offset array that is valid for all inner cells in the hash grid.
   unsigned int i = 0 ;
   for ( int yy = -xc ; yy <= xc ; yy += xc) {
      for ( int xx = -1 ; xx <= 1 ; ++xx, ++i) {
         m_stdNeighborOffset[i] = xx + yy ;
      }
   }

   // Allocation and initialization of the offset arrays of all the border cells. All inner cells
   // are set to point to the grid-global offset array.
   Cell* c = m_cell ;
   for ( int y = 0 ; y < yc ; ++ y) {
      for ( int x = 0 ; x < xc ; ++ x, ++ c) {
        // cella di bordo
         if ( x == 0  ||  x == (xc - 1)  ||
              y == 0  ||  y == (yc - 1)) {

            c->m_neighborOffset = new int[9] ;

            i = 0 ;
            for ( int yy = -xc ; yy <= xc ; yy += xc) {
               int yo = yy ;
               if ( y == 0  &&  yy == -xc) {
                  yo = xyc - xc ;
               }
               else if ( y == (yc - 1)  &&  yy == xc) {
                  yo = xc - xyc ;
               }

               for ( int xx = -1 ; xx <= 1 ; ++xx, ++i) {
                  int xo = xx ;
                  if ( x == 0  &&  xx == -1) {
                     xo = xc - 1 ;
                  }
                  else if ( x == (xc - 1)  &&  xx == 1) {
                     xo = 1 - xc ;
                  }

                  c->m_neighborOffset[i] = xo + yo ;
               }
            }
         }
        // cella interna
         else {
            c->m_neighborOffset = m_stdNeighborOffset ;
         }
      }
   }
}

//----------------------------------------------------------------------------
size_t
HashGrid2d::Hash( const Point3d& ptP) const
{
   size_t xHash ;
   if ( ptP.x < 0) {
      double i = ( - ptP.x ) * m_dInvCellSpan ;
      xHash  = m_xCellCount - 1 - ( static_cast<size_t>( i ) & m_xHashMask) ;
   }
   else {
      double i = ptP.x * m_dInvCellSpan ;
      xHash  = static_cast<size_t>( i ) & m_xHashMask ;
   }

   size_t yHash ;
   if ( ptP.y < 0) {
      double i = ( - ptP.y ) * m_dInvCellSpan ;
      yHash  = m_yCellCount - 1 - ( static_cast<size_t>( i ) & m_yHashMask) ;
   }
   else {
      double i = ptP.y * m_dInvCellSpan ;
      yHash  = static_cast<size_t>( i ) & m_yHashMask ;
   }

   return ( xHash + yHash * m_xCellCount) ;
}

//----------------------------------------------------------------------------
void
HashGrid2d::Add( HashGrids2d::ObjData& obj, Cell* cell)
{
   // If this cell is already occupied by other bodies, which means the pointer to the body
   // container holds a valid address and thus the container itself is properly initialized, then
   // the body is simply added to this already existing body container. Note that the index position
   // is memorized (=> "body->setCellId()") in order to ensure constant time removal.
   if ( cell->m_Objs != nullptr) {
      obj.nCellId = cell->m_Objs->size() ;
      cell->m_Objs->push_back( &obj) ;
   }

   // If, however, the cell is still empty, then the object container, first of all, must be created
   // (i.e., allocated) and properly initialized (i.e., sufficient initial storage capacity must be
   // reserved). Furthermore, the cell must be inserted into the grid-global vector 'm_occupiedCells'
   // in which all cells that are currently occupied by bodies are recorded.
   else {
      cell->m_Objs = new HashGrids2d::PtrObjVector ;
      cell->m_Objs->reserve( cellVectorSize) ;

      obj.nCellId = 0 ;
      cell->m_Objs->push_back( &obj) ;

      cell->m_occupiedCellsId = m_occupiedCells.size() ;
      m_occupiedCells.push_back( cell) ;
   }
}

//----------------------------------------------------------------------------
void
HashGrid2d::Remove( HashGrids2d::ObjData& obj, Cell* cell )
{
   // If the body is the last body that is stored in this cell ...
   if ( cell->m_Objs->size() == 1) {
                                       // ... the cell's body container is destroyed and ...
      delete cell->m_Objs ;
      cell->m_Objs = nullptr ;

      // ... the cell is removed from the grid-global vector 'm_occupiedCells' that records all
      // body-occupied cells. Since the cell memorized its index (=> 'm_occupiedCellsId') in this
      // vector, it can be removed in constant time, O(1).
      if ( cell->m_occupiedCellsId == m_occupiedCells.size() - 1) {
         m_occupiedCells.pop_back() ;
      }
      else {
         Cell* lastCell = m_occupiedCells.back() ;
         m_occupiedCells.pop_back() ;
         lastCell->m_occupiedCellsId = cell->m_occupiedCellsId ;
         m_occupiedCells[ cell->m_occupiedCellsId ] = lastCell ;
      }
   }
   // If the body is *not* the last body that is stored in this cell ...
   else {
      size_t cellId = obj.nCellId ;

      // ... the body is removed from the cell's body container. Since the body memorized its
      // index (=> 'cellId') in this container, it can be removed in constant time, O(1).
      if ( cellId == cell->m_Objs->size() - 1) {
         cell->m_Objs->pop_back() ;
      }
      else {
         HashGrids2d::ObjData* lastElement = cell->m_Objs->back() ;
         cell->m_Objs->pop_back() ;
         lastElement->nCellId = cellId ;
         (*cell->m_Objs)[ cellId] = lastElement ;
      }
   }
}

//----------------------------------------------------------------------------
void
HashGrid2d::Enlarge( void)
{
   HashGrids2d::PtrObjVector PObjVecTemp ;
   PObjVecTemp.reserve( m_objCount) ;

   // All objs that are assigned to this grid are temporarily removed, ...
   for ( auto cell = m_occupiedCells.begin() ; cell < m_occupiedCells.end() ; ++ cell) {
      HashGrids2d::PtrObjVector* cellBodies = (*cell)->m_Objs ;
      for ( auto e = cellBodies->begin() ; e < cellBodies->end() ; ++ e) {
         PObjVecTemp.push_back( *e) ;
      }
   }

   // ... the grid's current data structures are deleted, ...
   Clear() ;

   for ( auto pCell = m_cell ; pCell < m_cell + m_xyCellCount ; ++ pCell) {
      if ( pCell->m_neighborOffset != m_stdNeighborOffset)
         delete[] pCell->m_neighborOffset ;
      pCell->m_neighborOffset = nullptr ;
   }
   delete[] m_cell ;
   m_cell = nullptr ;

   // ... the number of cells is doubled in each coordinate direction, ...
   m_xCellCount *= 2 ;
   m_yCellCount *= 2 ;

   m_xHashMask = m_xCellCount - 1 ;
   m_yHashMask = m_yCellCount - 1 ;

   m_xyCellCount = m_xCellCount * m_yCellCount ;

   // ... a new threshold for enlarging this hash grid is set, ...
   m_enlargementThreshold = m_xyCellCount / minimalGridDensity ;

   // ... a new linear array of cells representing this enlarged hash grid is allocated and ...
   m_cell = new Cell[ m_xyCellCount] ;

   // ... initialized, and finally ...
   InitNeighborOffsets() ;

   // ... all previously removed objs are reinserted.
   for ( auto p = PObjVecTemp.begin() ; p < PObjVecTemp.end() ; ++ p) {
      Add( **p) ;
   }
}

//----------------------------------------------------------------------------
bool
HashGrid2d::PowerOfTwo( size_t number)
{
   return ( ( number > 0) && ( ( number & ( number - 1)) == 0)) ;
}


//----------------------------------------------------------------------------
//   HashGrids2d
//----------------------------------------------------------------------------
HashGrids2d::HashGrids2d( void)
{
   try {
     // Finchè il numero di oggetti non supera la soglia non si usano le griglie
      m_nonGridObjs.reserve( gridActivationThreshold) ;
      m_bActivate = true ;
      m_bGridActive = false ;
   }
   catch(...) {
      LOG_ERROR( GetEGkLogger(), "Error in HashGrids2d constructor") ;
   }
}

//----------------------------------------------------------------------------
HashGrids2d::~HashGrids2d( void)
{
  // Delete all grids that are stored in the grid hierarchy (=> m_GridList).
   for ( auto pGrid : m_GridList) {
      delete pGrid ;
   }
}

//----------------------------------------------------------------------------
void
HashGrids2d::SetActivationGrid( bool bActivate)
{
   m_bActivate = bActivate ;
}

//----------------------------------------------------------------------------
bool
HashGrids2d::Add( int nObjId, const BBox3d& box)
{
   try {
     // The body is marked as being added to 'm_objsToAdd' by setting the grid pointer to nullptr and
     // setting the cell-ID to '0'. Additionally, the hash value is used to memorize the body's
     // index position in the 'm_objsToAdd' vector.
      m_ObjsList.emplace_back( nObjId, box, nullptr, m_objsToAdd.size(), 0) ;

     // inserisco nel Map
      m_ObjsMap.emplace( nObjId, &(m_ObjsList.back())) ;

     // Temporarily add the body to 'm_objsToAdd'. As soon as "findContacts()" is called, all
     // bodies stored in 'm_objsToAdd' are finally inserted into the data structure.
      m_objsToAdd.push_back( &(m_ObjsList.back())) ;

     // Aggiorno il box complessivo
      m_b3Objs.Add( box) ;

      return true ;
   }
   catch(...) {
      LOG_ERROR( GetEGkLogger(), "Error in HashGrids2d::Add") ;
      return false ;
   }
}

//----------------------------------------------------------------------------
bool
HashGrids2d::Modify( int nObjId, const BBox3d& box)
{
  // Cerco l'oggetto con l'Id voluto
   auto iIter = m_ObjsMap.find( nObjId) ;
   if ( iIter == m_ObjsMap.end())
      return false ;
   ObjData* pObj = iIter->second ;
   if ( pObj == nullptr)
      return false ;

  // Modifico il suo box
   pObj->box = box ;

  // Aggiorno il box complessivo
   m_b3Objs.Add( box) ;

   return true ;
}

//----------------------------------------------------------------------------
bool
HashGrids2d::Remove( int nObjId)
{
  // Cerco l'oggetto con l'Id voluto
   auto iIter = m_ObjsMap.find( nObjId) ;
   if ( iIter == m_ObjsMap.end())
      return false ;
   ObjData* pObj = iIter->second ;
   if ( pObj == nullptr)
      return false ;

  // Recupero la griglia di appartenenza
   HashGrid2d* pGrid = pObj->pHGrid ;

  // The body is stored in a hash grid from which it must be removed.
   if ( pGrid != nullptr) {
      pGrid->Remove( *pObj) ;
   }
  // The body's grid pointer is equal to nullptr.
  // => The body is either stored in 'm_objsToAdd' (-> cell-ID = 0) or 'm_nonGridObjs' (-> cell-ID = 1).
   else {
      if ( pObj->nCellId == 0) {
        // the body's hash value => index of this body in 'm_objsToAdd'
         if ( pObj->nHash == m_objsToAdd.size() - 1) {
            m_objsToAdd.pop_back() ;
         }
         else if ( pObj->nHash < m_objsToAdd.size()) {
            ObjData* pLastObj = m_objsToAdd.back() ;
            m_objsToAdd.pop_back() ;
            pLastObj->nHash = pObj->nHash ;
            m_objsToAdd[ pObj->nHash] = pLastObj ;
         }
         else
            return false ;
      }
      else {
         // the body's hash value => index of this body in 'm_nonGridObjs'
         if ( pObj->nHash == m_nonGridObjs.size() - 1) {
            m_nonGridObjs.pop_back();
         }
         else if ( pObj->nHash < m_nonGridObjs.size()) {
            ObjData* pLastObj = m_nonGridObjs.back() ;
            m_nonGridObjs.pop_back() ;
            pLastObj->nHash = pObj->nHash ;
            m_nonGridObjs[ pObj->nHash] = pLastObj ;
         }
         else
            return false ;
      }
   }
   return true ;
}

//----------------------------------------------------------------------------
bool
HashGrids2d::Update( void)
{
   try {
     // Salvo stato di precedente attivazione delle griglie
      bool bGridActivePrev = m_bGridActive ;
     // Inseriamo gli oggetti presenti nel vettore m_objsToAdd
      if ( ! m_objsToAdd.empty()) {
         for ( auto pObj : m_objsToAdd) {
            if ( m_bGridActive)
               addGrid( *pObj) ;
            else
               addList( *pObj) ;
         }
         m_objsToAdd.clear() ;
      }
     // Aggiorniamo per eventuali modifiche agli oggetti già precedentemente presenti nelle griglie
      if ( bGridActivePrev) {
         for ( auto& Obj : m_ObjsList) {
            HashGrid2d* pGrid = Obj.pHGrid ;
            if ( pGrid != nullptr) {
               double dSize = 0 ;
               Obj.box.GetDiameter( dSize) ;
               double dCellSpan = pGrid->GetCellSpan() ;

               if ( dSize >= dCellSpan  ||  dSize < ( dCellSpan / hierarchyFactor)) {
                  pGrid->Remove( Obj) ;
                  addGrid( Obj) ;
               }
               else {
                  pGrid->Update( Obj) ;
               }
            }
         }
      }
      return true ;
   }
   catch(...) {
      LOG_ERROR( GetEGkLogger(), "Error in HashGrids2d::Update") ;
      return false ;
   }
}

//----------------------------------------------------------------------------
bool
HashGrids2d::Find( const BBox3d& b3Test, INTVECTOR& vnIds) const
{
  // pulisco il risultato
   vnIds.clear() ;
   vnIds.reserve( 128) ;

  // limito con box globale
   BBox3d b3Int ;
   if ( ! b3Test.FindIntersectionXY( m_b3Objs, b3Int))
      return false ;

  // ricerca nelle griglie
   if ( m_bGridActive) {
      for ( auto pGrid : m_GridList)
         pGrid->Find( b3Int, vnIds) ;
   }

  // ricerca negli oggetti fuori griglia
   for ( auto pObj : m_nonGridObjs) {
      if ( b3Int.OverlapsXY( pObj->box))
         vnIds.push_back( pObj->nId) ;
   }

  // ordino il risultato ed elimino gli indici ripetuti
   sort( vnIds.begin(), vnIds.end()) ;
   vnIds.erase( unique( vnIds.begin(), vnIds.end()), vnIds.end()) ;

   return ( ! vnIds.empty()) ;
}

//----------------------------------------------------------------------------
void
HashGrids2d::Clear( void)
{
   m_ObjsList.clear() ;
   m_ObjsMap.clear() ;
   m_objsToAdd.clear() ;
   m_nonGridObjs.clear() ;
   for ( auto pGrid : m_GridList)
      delete pGrid ;
   m_GridList.clear() ;
   m_bActivate = true ;
   m_bGridActive = false ;
   m_b3Objs.Reset() ;
}

//----------------------------------------------------------------------------
void
HashGrids2d::addGrid( ObjData& obj)
{
   double size = - 1 ;
   obj.box.GetDiameter( size) ;

   // If the body is finite in size, it must be assigned to a grid with suitably sized cells.
   if ( size > - EPS_ZERO) {

      size = max( size, MIN_CELL_SIZE) ;

      HashGrid2d* pGrid = nullptr ;

      if ( m_GridList.empty()) {
         // If no hash grid yet exists in the hierarchy, an initial hash grid is created
         // based on the body's size.

         pGrid = new HashGrid2d( size * sqrt( hierarchyFactor)) ;
      }
      else {
         // Check the hierarchy for a hash grid with suitably sized cells - if such a grid does not
         // yet exist, it will be created.

         double cellSpan = 0;
         for ( auto g = m_GridList.begin(); g != m_GridList.end(); ++ g) {
            pGrid     = *g;
            cellSpan = pGrid->GetCellSpan();

            if ( size < cellSpan) {
               cellSpan /= hierarchyFactor ;
               if ( size < cellSpan ) {
                  while ( size < cellSpan)
                     cellSpan /= hierarchyFactor ;
                  pGrid = new HashGrid2d( cellSpan * hierarchyFactor) ;
                  m_GridList.insert( g, pGrid) ;
               }

               pGrid->Add( obj) ;
               obj.pHGrid = pGrid ;

               return ;
            }
         }

         while ( size >= cellSpan)
            cellSpan *= hierarchyFactor ;
         pGrid = new HashGrid2d( cellSpan) ;
      }

      pGrid->Add( obj) ;
      obj.pHGrid = pGrid ;

      m_GridList.push_back( pGrid) ;

      return ;
   }

   // The body - which is infinite in size - is marked as being added to 'm_nonGridObjs' by setting
   // the grid pointer to nullptr and setting the cell-ID to '1'. Additionally, the hash value is used
   // to memorize the body's index position in the 'm_nonGridObjs' vector.

   obj.pHGrid = nullptr ;
   obj.nHash = m_nonGridObjs.size() ;
   obj.nCellId = 1 ;

   m_nonGridObjs.push_back( &obj) ;
}

//----------------------------------------------------------------------------
void
HashGrids2d::addList( ObjData& obj)
{
  // Se abilitato e superata la soglia ...
   if ( m_bActivate  &&  m_nonGridObjs.size() == gridActivationThreshold) {
      if ( gridActivationThreshold > 0) {

        // all objs stored in 'm_nonGridObjs' are inserted in grids
         for ( size_t i = 0; i < gridActivationThreshold; ++i ) {
            addGrid( *m_nonGridObjs[i] );
         }

        // ... the 'm_nonGridObjs' vector is cleared ...
         m_nonGridObjs.clear() ;
      }

      addGrid( obj) ;

     // ... and the usage of the hierarchical hash grids is activated irrevocably.
      m_bGridActive = true ;

      return ;
   }

   // The body is marked as being added to 'm_nonGridObjs' by setting the grid pointer to nullptr and
   // setting the cell-ID to '1'. Additionally, the hash value is used to memorize the body's index
   // position in the 'm_nonGridObjs' vector.
   obj.pHGrid = nullptr ;
   obj.nHash = m_nonGridObjs.size() ;
   obj.nCellId = 1 ;
   m_nonGridObjs.push_back( &obj) ;
}