Dario Sassi
782 lines
26 KiB
C++
782 lines
26 KiB
C++
//----------------------------------------------------------------------------
|
|
// EgalTech 2015-2015
|
|
//----------------------------------------------------------------------------
|
|
// File : HashGrids2d.cpp Data : 04.07.15 Versione : 1.6g1
|
|
// Contenuto : Funzioni della classe HashGrids2d.
|
|
//
|
|
//
|
|
//
|
|
// Modifiche : 04.07.15 DS Creazione modulo.
|
|
//
|
|
//
|
|
//----------------------------------------------------------------------------
|
|
|
|
//--------------------------- Include ----------------------------------------
|
|
#include "stdafx.h"
|
|
#include "HashGrids2d.h"
|
|
#include "DllMain.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 = 8 ;
|
|
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
|
|
} ;
|
|
|
|
//----------------------------------------------------------------------------
|
|
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 < 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 ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
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) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
void
|
|
HashGrid2d::Find( const BBox3d& b3Test, INTVECTOR& vnIds)
|
|
{
|
|
// recupero gli estremi del box
|
|
Point3d ptMin ;
|
|
double dXDim, dYDim, dZDim ;
|
|
if ( ! b3Test.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)) ;
|
|
// verifico se conviene verificare queste celle o direttamente tutte e sole quelle occupate
|
|
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 ( b3Test.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] ;
|
|
}
|
|
}
|
|
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 ( b3Test.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 ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
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 ;
|
|
}
|
|
delete[] m_cell ;
|
|
|
|
// ... 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())) ;
|
|
|
|
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 ;
|
|
|
|
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.size() > 0 ) {
|
|
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)
|
|
{
|
|
// pulisco il risultato
|
|
vnIds.clear() ;
|
|
vnIds.reserve( 128) ;
|
|
|
|
// ricerca nelle griglie
|
|
if ( m_bGridActive) {
|
|
for ( auto pGrid : m_GridList)
|
|
pGrid->Find( b3Test, vnIds) ;
|
|
}
|
|
|
|
// ricerca negli oggetti fuori griglia
|
|
for ( auto pObj : m_nonGridObjs) {
|
|
if ( b3Test.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.size() > 0) ;
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
void
|
|
HashGrids2d::Clear( void)
|
|
{
|
|
for ( auto pGrid : m_GridList) {
|
|
delete pGrid ;
|
|
}
|
|
m_GridList.clear() ;
|
|
|
|
m_bGridActive = false ;
|
|
|
|
m_nonGridObjs.clear() ;
|
|
|
|
m_objsToAdd.clear() ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
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) ;
|
|
}
|