diff --git a/BBox3d.cpp b/BBox3d.cpp index 1b5c869..1869c49 100644 --- a/BBox3d.cpp +++ b/BBox3d.cpp @@ -187,6 +187,16 @@ BBox3d::GetRadius( double& dRad) const return true ; } +//---------------------------------------------------------------------------- +bool +BBox3d::GetDiameter( double& dDiam) const +{ + if ( ! IsValid()) + return false ; + dDiam = ( m_ptMax - m_ptMin).Len() ; + return true ; +} + //---------------------------------------------------------------------------- void BBox3d::Translate( const Vector3d& vtMove) diff --git a/EgtGeomKernel.rc b/EgtGeomKernel.rc index 96b1d77..0683286 100644 Binary files a/EgtGeomKernel.rc and b/EgtGeomKernel.rc differ diff --git a/EgtGeomKernel.vcxproj b/EgtGeomKernel.vcxproj index 47bbca7..d42f500 100644 --- a/EgtGeomKernel.vcxproj +++ b/EgtGeomKernel.vcxproj @@ -292,6 +292,8 @@ copy $(TargetPath) \EgtProg\Dll64 + + @@ -445,6 +447,8 @@ copy $(TargetPath) \EgtProg\Dll64 + + diff --git a/EgtGeomKernel.vcxproj.filters b/EgtGeomKernel.vcxproj.filters index 5897fbe..b8d7b28 100644 --- a/EgtGeomKernel.vcxproj.filters +++ b/EgtGeomKernel.vcxproj.filters @@ -285,6 +285,12 @@ File di origine\Gdb + + File di origine\Base + + + File di origine\Base + @@ -668,6 +674,12 @@ File di intestazione\Include + + File di intestazione + + + File di intestazione + diff --git a/HashGrids2d.cpp b/HashGrids2d.cpp new file mode 100644 index 0000000..3dd992b --- /dev/null +++ b/HashGrids2d.cpp @@ -0,0 +1,763 @@ +//---------------------------------------------------------------------------- +// 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 + +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 ; + +//---------------------------------------------------------------------------- +// 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 std::vector 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 = static_cast( ceil( dXDim * m_dInvCellSpan)) ; + int nYSpan = static_cast( ceil( dYDim * m_dInvCellSpan)) ; + // cella di base + int nX = static_cast( 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] ; + } +} + +//---------------------------------------------------------------------------- +void +HashGrid2d::Clear( void) +{ + for ( CellVector::iterator cell = m_occupiedCells.begin(); cell < m_occupiedCells.end(); ++cell) { + delete (*cell)->m_Objs ; + (*cell)->m_Objs = nullptr ; + } + m_occupiedCells.clear() ; + m_objCount = 0 ; +} + +//---------------------------------------------------------------------------- +void +HashGrid2d::InitNeighborOffsets( void) +{ + int xc = static_cast( m_xCellCount) ; + int yc = static_cast( m_yCellCount) ; + int xyc = static_cast( 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( i ) & m_xHashMask) ; + } + else { + double i = ptP.x * m_dInvCellSpan ; + xHash = static_cast( i ) & m_xHashMask ; + } + + size_t yHash ; + if ( ptP.y < 0) { + double i = ( - ptP.y ) * m_dInvCellSpan ; + yHash = m_yCellCount - 1 - ( static_cast( i ) & m_yHashMask) ; + } + else { + double i = ptP.y * m_dInvCellSpan ; + yHash = static_cast( 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 > 0) { + 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 * std::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) ; +} + diff --git a/HashGrids2d.h b/HashGrids2d.h new file mode 100644 index 0000000..4e7df0d --- /dev/null +++ b/HashGrids2d.h @@ -0,0 +1,67 @@ +//---------------------------------------------------------------------------- +// EgalTech 2015-2015 +//---------------------------------------------------------------------------- +// File : HashGrids2d.h Data : 04.07.15 Versione : 1.6g1 +// Contenuto : Dichiarazione della classe HashGrids2d. +// +// +// +// Modifiche : 04.07.15 DS Creazione modulo. +// +// +//---------------------------------------------------------------------------- + +#pragma once + +#include "EgtDev/Include/EGkBBox3d.h" +#include "EgtDev/Include/EgtNumCollection.h" +#include + +//---------------------------------------------------------------------------- +class HashGrids2d +{ + public : + HashGrids2d( void) ; + ~HashGrids2d( void) ; + void SetActivationGrid( bool bActivate) ; + bool Add( int nObjId, const BBox3d& box) ; + bool Modify( int nObjId, const BBox3d& box) ; + bool Remove( int nObjId) ; + bool Update( void) ; + bool Find( const BBox3d& b3Test, INTVECTOR& vnIds) ; + void Clear( void) ; + + friend class HashGrid2d ; + + private : + struct ObjData { + int nId ; + BBox3d box ; + HashGrid2d* pHGrid ; + size_t nHash ; + size_t nCellId ; + ObjData( void) + : nId( -1), box(), pHGrid( nullptr), nHash( 0), nCellId( 0) {} + ObjData( int nI, const BBox3d& bb, HashGrid2d* pHG, size_t nH, size_t nCI) + : nId( nI), box( bb), pHGrid( pHG), nHash( nH), nCellId( nCI) {} + } ; + typedef std::list ObjList ; + typedef std::vector PtrObjVector ; + typedef std::unordered_map IntPObjUmap ; + + private : + typedef std::list GridList ; // Tipo per lista di hash grid + + private : + void addGrid( ObjData& obj) ; + void addList( ObjData& obj) ; + + private : + ObjList m_ObjsList ; // Lista degli oggetti + IntPObjUmap m_ObjsMap ; // Map da Id a PtrObj + PtrObjVector m_objsToAdd ; // Vettore di puntatori agli oggetti da inserire + PtrObjVector m_nonGridObjs ; // Vettore di puntatori agli oggetti non assegnati alle griglie (per dimensioni o perchè pochi) + GridList m_GridList ; // Lista delle griglie di dimensione fissa ( in ordine crescente di dimensione di cella) + bool m_bActivate ; // Flag che abilita l'attivazione delle griglie + bool m_bGridActive ; // Flag di attivazione delle griglie +} ; diff --git a/HashGrids3d.cpp b/HashGrids3d.cpp new file mode 100644 index 0000000..b805d84 --- /dev/null +++ b/HashGrids3d.cpp @@ -0,0 +1,805 @@ +//---------------------------------------------------------------------------- +// EgalTech 2015-2015 +//---------------------------------------------------------------------------- +// File : HashGrids3d.cpp Data : 02.07.15 Versione : 1.6g1 +// Contenuto : Funzioni della classe HashGrids3d. +// +// +// +// Modifiche : 02.07.15 DS Creazione modulo. +// +// +//---------------------------------------------------------------------------- + +//--------------------------- Include ---------------------------------------- +#include "stdafx.h" +#include "HashGrids3d.h" +#include "DllMain.h" +#include + +using namespace std ; + +//---------------------------------------------------------------------------- +const size_t xCellCount = 16 ; +const size_t yCellCount = 16 ; +const size_t zCellCount = 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 ; + +//---------------------------------------------------------------------------- +// HashGrid3d +//---------------------------------------------------------------------------- +class HashGrid3d +{ + private : + struct Cell + { + HashGrids3d::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 std::vector CellVector ; + + public : + explicit HashGrid3d( double dCellSpan) ; + ~HashGrid3d( void) ; + double GetCellSpan( void) const + { return m_dCellSpan ; } + void Add( HashGrids3d::ObjData& obj) ; + void Remove( HashGrids3d::ObjData& obj) ; + void Update( HashGrids3d::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( HashGrids3d::ObjData& obj, Cell* cell) ; + void Remove( HashGrids3d::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_zCellCount ; // Numero di celle allocate sulla direzione Z + + 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_zHashMask ; // Maschera di bit per calcolo hash di Z + + size_t m_xyCellCount ; // Numero di celle nel piano XY + size_t m_xyzCellCount ; // Numero totale di celle + + 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[27] ; // Array degli offset standard per le adiacenze +} ; + +//---------------------------------------------------------------------------- +HashGrid3d::HashGrid3d( double dCellSpan) +{ + // Initialization of all member variables and ... + m_xCellCount = PowerOfTwo( xCellCount) ? xCellCount : 16 ; + m_yCellCount = PowerOfTwo( yCellCount) ? yCellCount : 16 ; + m_zCellCount = PowerOfTwo( zCellCount) ? zCellCount : 16 ; + + m_xHashMask = m_xCellCount - 1 ; + m_yHashMask = m_yCellCount - 1 ; + m_zHashMask = m_zCellCount - 1 ; + + m_xyCellCount = m_xCellCount * m_yCellCount ; + m_xyzCellCount = m_xyCellCount * m_zCellCount ; + + m_enlargementThreshold = m_xyzCellCount / minimalGridDensity ; + + // allocazione dell'array lineare che rappresenta lo hash grid. + m_cell = new Cell[ m_xyzCellCount] ; + + // 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 ; +} + +//---------------------------------------------------------------------------- +HashGrid3d::~HashGrid3d( void) +{ + Clear() ; + + for ( Cell* pCell = m_cell ; pCell < m_cell + m_xyzCellCount ; ++ pCell) { + if ( pCell->m_neighborOffset != m_stdNeighborOffset) + delete[] pCell->m_neighborOffset ; + } + delete[] m_cell ; +} + +//---------------------------------------------------------------------------- +void +HashGrid3d::Add( HashGrids3d::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 +HashGrid3d::Remove( HashGrids3d::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 +HashGrid3d::Update( HashGrids3d::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 +HashGrid3d::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, 1) * ( m_dCellSpan + EPS_SMALL) ; + dXDim += m_dCellSpan + 2 * EPS_SMALL ; + dYDim += m_dCellSpan + 2 * EPS_SMALL ; + dZDim += m_dCellSpan + 2 * EPS_SMALL ; + // numero di celle da esplorare sui 3 assi + int nXSpan = static_cast( ceil( dXDim * m_dInvCellSpan)) ; + int nYSpan = static_cast( ceil( dYDim * m_dInvCellSpan)) ; + int nZSpan = static_cast( ceil( dZDim * m_dInvCellSpan)) ; + // cella di base + int nX = static_cast( Hash( ptMin)) ; + for ( int i = 0 ; i <= nXSpan ; ++ i) { + int nY = nX ; + for ( int j = 0 ; j <= nYSpan ; ++ j) { + int nZ = nY ; + for ( int k = 0 ; k <= nZSpan ; ++ k) { + // inserisco in lista gli oggetti della cella + if ( m_cell[nZ].m_Objs != nullptr) { + for ( auto pObj : *( m_cell[nZ].m_Objs)) { + if ( b3Test.Overlaps( pObj->box)) + vnIds.push_back( pObj->nId) ; + } + } + // passo alla successiva in Z+ + nZ += m_cell[nZ].m_neighborOffset[22] ; + } + // passo alla successiva in Y+ + nY += m_cell[nY].m_neighborOffset[16] ; + } + // passo alla successiva in X+ + nX += m_cell[nX].m_neighborOffset[14] ; + } +} + +//---------------------------------------------------------------------------- +void +HashGrid3d::Clear( void) +{ + for ( CellVector::iterator cell = m_occupiedCells.begin(); cell < m_occupiedCells.end(); ++cell) { + delete (*cell)->m_Objs ; + (*cell)->m_Objs = nullptr ; + } + m_occupiedCells.clear() ; + m_objCount = 0 ; +} + +//---------------------------------------------------------------------------- +void +HashGrid3d::InitNeighborOffsets( void) +{ + int xc = static_cast( m_xCellCount) ; + int yc = static_cast( m_yCellCount) ; + int zc = static_cast( m_zCellCount) ; + int xyc = static_cast( m_xyCellCount) ; + int xyzc = static_cast( m_xyzCellCount) ; + + // Initialization of the grid-global offset array that is valid for all inner cells in the hash grid. + unsigned int i = 0 ; + for ( int zz = -xyc ; zz <= xyc ; zz += xyc) { + for ( int yy = -xc ; yy <= xc ; yy += xc) { + for ( int xx = -1 ; xx <= 1 ; ++xx, ++i) { + m_stdNeighborOffset[i] = xx + yy + zz ; + } + } + } + + // 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 z = 0 ; z < zc ; ++ z) { + 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) || + z == 0 || z == (zc - 1)) { + + c->m_neighborOffset = new int[27] ; + + i = 0 ; + for ( int zz = -xyc; zz <= xyc; zz += xyc ) { + int zo = zz ; + if ( z == 0 && zz == -xyc) { + zo = xyzc - xyc ; + } + else if ( z == (zc - 1) && zz == xyc) { + zo = xyc - xyzc ; + } + + 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 + zo ; + } + } + } + } + // cella interna + else { + c->m_neighborOffset = m_stdNeighborOffset ; + } + } + } + } +} + +//---------------------------------------------------------------------------- +size_t +HashGrid3d::Hash( const Point3d& ptP) const +{ + size_t xHash ; + if ( ptP.x < 0) { + double i = ( - ptP.x ) * m_dInvCellSpan ; + xHash = m_xCellCount - 1 - ( static_cast( i ) & m_xHashMask) ; + } + else { + double i = ptP.x * m_dInvCellSpan ; + xHash = static_cast( i ) & m_xHashMask ; + } + + size_t yHash ; + if ( ptP.y < 0) { + double i = ( - ptP.y ) * m_dInvCellSpan ; + yHash = m_yCellCount - 1 - ( static_cast( i ) & m_yHashMask) ; + } + else { + double i = ptP.y * m_dInvCellSpan ; + yHash = static_cast( i ) & m_yHashMask ; + } + + size_t zHash ; + if ( ptP.z < 0) { + double i = ( - ptP.z ) * m_dInvCellSpan ; + zHash = m_zCellCount - 1 - ( static_cast( i ) & m_zHashMask) ; + } + else { + double i = ptP.z * m_dInvCellSpan ; + zHash = static_cast( i ) & m_zHashMask ; + } + + return ( xHash + yHash * m_xCellCount + zHash * m_xyCellCount) ; +} + +//---------------------------------------------------------------------------- +void +HashGrid3d::Add( HashGrids3d::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 HashGrids3d::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 +HashGrid3d::Remove( HashGrids3d::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 { + HashGrids3d::ObjData* lastElement = cell->m_Objs->back() ; + cell->m_Objs->pop_back() ; + lastElement->nCellId = cellId ; + (*cell->m_Objs)[ cellId] = lastElement ; + } + } +} + +//---------------------------------------------------------------------------- +void +HashGrid3d::Enlarge( void) +{ + HashGrids3d::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) { + HashGrids3d::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_xyzCellCount ; ++ 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_zCellCount *= 2 ; + + m_xHashMask = m_xCellCount - 1 ; + m_yHashMask = m_yCellCount - 1 ; + m_zHashMask = m_zCellCount - 1 ; + + m_xyCellCount = m_xCellCount * m_yCellCount ; + m_xyzCellCount = m_xyCellCount * m_zCellCount ; + + // ... a new threshold for enlarging this hash grid is set, ... + m_enlargementThreshold = m_xyzCellCount / minimalGridDensity ; + + // ... a new linear array of cells representing this enlarged hash grid is allocated and ... + m_cell = new Cell[ m_xyzCellCount] ; + + // ... initialized, and finally ... + InitNeighborOffsets() ; + + // ... all previously removed objs are reinserted. + for ( auto p = PObjVecTemp.begin() ; p < PObjVecTemp.end() ; ++ p) { + Add( **p) ; + } +} + +//---------------------------------------------------------------------------- +bool +HashGrid3d::PowerOfTwo( size_t number) +{ + return ( ( number > 0) && ( ( number & ( number - 1)) == 0)) ; +} + + +//---------------------------------------------------------------------------- +// HashGrids3d +//---------------------------------------------------------------------------- +HashGrids3d::HashGrids3d( 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 HashGrids3d constructor") ; + } +} + +//---------------------------------------------------------------------------- +HashGrids3d::~HashGrids3d( void) +{ + // Delete all grids that are stored in the grid hierarchy (=> m_GridList). + for ( auto pGrid : m_GridList) { + delete pGrid ; + } +} + +//---------------------------------------------------------------------------- +void +HashGrids3d::SetActivationGrid( bool bActivate) +{ + m_bActivate = bActivate ; +} + +//---------------------------------------------------------------------------- +bool +HashGrids3d::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 HashGrids3d::Add") ; + return false ; + } +} + +//---------------------------------------------------------------------------- +bool +HashGrids3d::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 +HashGrids3d::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 ; + + HashGrid3d* 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 +HashGrids3d::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) { + HashGrid3d* 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 HashGrids3d::Update") ; + return false ; + } +} + +//---------------------------------------------------------------------------- +bool +HashGrids3d::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.Overlaps( 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 +HashGrids3d::Clear( void) +{ + for ( auto pGrid : m_GridList) { + delete pGrid ; + } + m_GridList.clear() ; + + m_bGridActive = false ; + + m_nonGridObjs.clear() ; + + m_objsToAdd.clear() ; +} + +//---------------------------------------------------------------------------- +void +HashGrids3d::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 > 0) { + HashGrid3d* 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 HashGrid3d( size * std::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 HashGrid3d( cellSpan * hierarchyFactor) ; + m_GridList.insert( g, pGrid) ; + } + + pGrid->Add( obj) ; + obj.pHGrid = pGrid ; + + return ; + } + } + + while ( size >= cellSpan) + cellSpan *= hierarchyFactor ; + pGrid = new HashGrid3d( 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 +HashGrids3d::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) ; +} + diff --git a/HashGrids3d.h b/HashGrids3d.h new file mode 100644 index 0000000..9be10f8 --- /dev/null +++ b/HashGrids3d.h @@ -0,0 +1,67 @@ +//---------------------------------------------------------------------------- +// EgalTech 2015-2015 +//---------------------------------------------------------------------------- +// File : HashGrids3d.h Data : 04.07.15 Versione : 1.6g1 +// Contenuto : Dichiarazione della classe HashGrids3d. +// +// +// +// Modifiche : 02.07.15 DS Creazione modulo. +// +// +//---------------------------------------------------------------------------- + +#pragma once + +#include "EgtDev/Include/EGkBBox3d.h" +#include "EgtDev/Include/EgtNumCollection.h" +#include + +//---------------------------------------------------------------------------- +class HashGrids3d +{ + public : + HashGrids3d( void) ; + ~HashGrids3d( void) ; + void SetActivationGrid( bool bActivate) ; + bool Add( int nObjId, const BBox3d& box) ; + bool Modify( int nObjId, const BBox3d& box) ; + bool Remove( int nObjId) ; + bool Update( void) ; + bool Find( const BBox3d& b3Test, INTVECTOR& vnIds) ; + void Clear( void) ; + + friend class HashGrid3d ; + + private : + struct ObjData { + int nId ; + BBox3d box ; + HashGrid3d* pHGrid ; + size_t nHash ; + size_t nCellId ; + ObjData( void) + : nId( -1), box(), pHGrid( nullptr), nHash( 0), nCellId( 0) {} + ObjData( int nI, const BBox3d& bb, HashGrid3d* pHG, size_t nH, size_t nCI) + : nId( nI), box( bb), pHGrid( pHG), nHash( nH), nCellId( nCI) {} + } ; + typedef std::list ObjList ; + typedef std::vector PtrObjVector ; + typedef std::unordered_map IntPObjUmap ; + + private : + typedef std::list GridList ; // Tipo per lista di hash grid + + private : + void addGrid( ObjData& obj) ; + void addList( ObjData& obj) ; + + private : + ObjList m_ObjsList ; // Lista degli oggetti + IntPObjUmap m_ObjsMap ; // Map da Id a PtrObj + PtrObjVector m_objsToAdd ; // Vettore di puntatori agli oggetti da inserire + PtrObjVector m_nonGridObjs ; // Vettore di puntatori agli oggetti non assegnati alle griglie (per dimensioni o perchè pochi) + GridList m_GridList ; // Lista delle griglie di dimensione fissa ( in ordine crescente di dimensione di cella) + bool m_bActivate ; // Flag che abilita l'attivazione delle griglie + bool m_bGridActive ; // Flag di attivazione delle griglie +} ; diff --git a/IntersCrvCompoCrvCompo.cpp b/IntersCrvCompoCrvCompo.cpp index db8d5cc..e7b4089 100644 --- a/IntersCrvCompoCrvCompo.cpp +++ b/IntersCrvCompoCrvCompo.cpp @@ -15,6 +15,7 @@ #include "stdafx.h" #include "IntersCrvCompoCrvCompo.h" #include "CurveAux.h" +#include "HashGrids2d.h" #include using namespace std ; @@ -54,33 +55,60 @@ IntersCrvCompoCrvCompo::IntersCrvCompoCrvCompo( const ICurveComposite& CCompoA, return ; m_dCrvBSpan = dEnd - dStart ; - // doppio ciclo sulle entità delle curve composite - // !!! questa parte è O(N^2), vanno usate Grid Gerarchiche o BVH per renderla O(N*logN) !!! - int nCountA = 0 ; - for ( const ICurve* pCrvA = CCompoA.GetFirstCurve() ; - pCrvA != nullptr ; - pCrvA = CCompoA.GetNextCurve(), ++ nCountA) { - int nCountB = 0 ; - for ( const ICurve* pCrvB = CCompoB.GetFirstCurve() ; - pCrvB != nullptr ; - pCrvB = CCompoB.GetNextCurve(), ++ nCountB) { - // eseguo l'intersezione di queste curve semplici - IntersCurveCurve intCC( *pCrvA, *pCrvB) ; - // ne recupero i risultati - int nCurrInters = intCC.GetNumInters() ; - if ( nCurrInters > 0) { - m_nNumInters += nCurrInters ; - m_bOverlaps = ( intCC.GetOverlaps() ? true : m_bOverlaps) ; - for ( int i = 0 ; i < nCurrInters ; ++ i) { - IntCrvCrvInfo aInfo ; - intCC.GetIntCrvCrvInfo( i, aInfo) ; - aInfo.IciA[0].dU += nCountA ; - aInfo.IciB[0].dU += nCountB ; - if ( aInfo.bOverlap) { - aInfo.IciA[1].dU += nCountA ; - aInfo.IciB[1].dU += nCountB ; - } - m_Info.push_back( aInfo) ; + // creo HashGrids2d per curva con maggior numero di elementi + const int LIM_CRVNBRSQUARED = 4095 ; + int nCrvNbrA = CCompoA.GetCurveNumber() ; + int nCrvNbrB = CCompoB.GetCurveNumber() ; + if ( nCrvNbrA >= nCrvNbrB) { + HashGrids2d HHGrids ; + HHGrids.SetActivationGrid( nCrvNbrA * nCrvNbrB > LIM_CRVNBRSQUARED) ; + for ( int nA = 0 ; nA < nCrvNbrA ; ++ nA) { + const ICurve* pCrvA = CCompoA.GetCurve( nA) ; + BBox3d boxA ; + pCrvA->GetLocalBBox( boxA) ; + if ( ! HHGrids.Add( nA, boxA)) + return ; + } + if ( ! HHGrids.Update()) + return ; + for ( int nB = 0 ; nB < nCrvNbrB ; ++ nB) { + const ICurve* pCrvB = CCompoB.GetCurve( nB) ; + BBox3d boxB ; + pCrvB->GetLocalBBox( boxB) ; + INTVECTOR vnIds ; + if ( HHGrids.Find( boxB, vnIds)) { + for ( int i = 0 ; i < int( vnIds.size()) ; ++ i) { + int nA = vnIds[i] ; + const ICurve* pCrvA = CCompoA.GetCurve( nA) ; + // eseguo l'intersezione di queste curve semplici + IntersSimpleCurves( *pCrvA, nA, *pCrvB, nB) ; + } + } + } + } + else { + HashGrids2d HHGrids ; + HHGrids.SetActivationGrid( nCrvNbrA * nCrvNbrB > LIM_CRVNBRSQUARED) ; + for ( int nB = 0 ; nB < nCrvNbrB ; ++ nB) { + const ICurve* pCrvB = CCompoB.GetCurve( nB) ; + BBox3d boxB ; + pCrvB->GetLocalBBox( boxB) ; + if ( ! HHGrids.Add( nB, boxB)) + return ; + } + if ( ! HHGrids.Update()) + return ; + for ( int nA = 0 ; nA < nCrvNbrA ; ++ nA) { + const ICurve* pCrvA = CCompoA.GetCurve( nA) ; + BBox3d boxA ; + pCrvA->GetLocalBBox( boxA) ; + INTVECTOR vnIds ; + if ( HHGrids.Find( boxA, vnIds)) { + for ( int i = 0 ; i < int( vnIds.size()) ; ++ i) { + int nB = vnIds[i] ; + const ICurve* pCrvB = CCompoB.GetCurve( nB) ; + // eseguo l'intersezione di queste curve semplici + IntersSimpleCurves( *pCrvA, nA, *pCrvB, nB) ; } } } @@ -92,207 +120,207 @@ IntersCrvCompoCrvCompo::IntersCrvCompoCrvCompo( const ICurveComposite& CCompoA, // sistemazione di intersezioni coincidenti for ( int i = 0 ; i < m_nNumInters ; ++ i) { for ( int j = 0 ; j < m_nNumInters ; ++ j) { - // se i due indici coincidono, passo oltre - if ( i == j) - continue ; - // calcolo sottoindici - int ki = 0 ; // del successivo si prende sempre il primo - int kj = ( m_Info[j].bOverlap ? 1 : 0) ; // del precedente si prende il secondo se overlap - // verifico se precedente e corrente si riferiscono alla stessa intersezione (10 * EPS_SMALL) - if ( SqDistXY( m_Info[j].IciA[kj].ptI, m_Info[i].IciA[ki].ptI) < ( 100 * EPS_SMALL * EPS_SMALL) && - SqDistXY( m_Info[j].IciB[kj].ptI, m_Info[i].IciB[ki].ptI) < ( 100 * EPS_SMALL * EPS_SMALL) && - CompatibleParamA( m_Info[j], m_Info[i], m_bCrvAClosed, m_dCrvASpan) && - CompatibleParamB( m_Info[j], m_Info[i], m_bCrvBClosed, m_dCrvBSpan)) { - // caso DET-NULL -> NULL-DET per prima curva - if ( m_Info[j].IciA[kj].nPrevTy != ICCT_NULL && m_Info[j].IciA[kj].nNextTy == ICCT_NULL && - m_Info[i].IciA[ki].nPrevTy == ICCT_NULL && m_Info[i].IciA[ki].nNextTy != ICCT_NULL) { - // per la prima curva tengo i determinati - m_Info[i].IciA[ki].nPrevTy = m_Info[j].IciA[kj].nPrevTy ; - m_Info[j].IciA[kj].nNextTy = m_Info[i].IciA[ki].nNextTy ; - // se overlap equiverso - if ( m_Info[i].bOverlap && m_Info[i].bCBOverEq) { - // per la seconda curva ogni sottotipo è il duale di quello della prima - m_Info[i].IciB[ki].nPrevTy = GetDualIcct( m_Info[i].IciA[ki].nPrevTy) ; - m_Info[i].IciB[ki].nNextTy = GetDualIcct( m_Info[i].IciA[ki].nNextTy) ; + // se i due indici coincidono, passo oltre + if ( i == j) + continue ; + // calcolo sottoindici + int ki = 0 ; // del successivo si prende sempre il primo + int kj = ( m_Info[j].bOverlap ? 1 : 0) ; // del precedente si prende il secondo se overlap + // verifico se precedente e corrente si riferiscono alla stessa intersezione (10 * EPS_SMALL) + if ( SqDistXY( m_Info[j].IciA[kj].ptI, m_Info[i].IciA[ki].ptI) < ( 100 * EPS_SMALL * EPS_SMALL) && + SqDistXY( m_Info[j].IciB[kj].ptI, m_Info[i].IciB[ki].ptI) < ( 100 * EPS_SMALL * EPS_SMALL) && + CompatibleParamA( m_Info[j], m_Info[i], m_bCrvAClosed, m_dCrvASpan) && + CompatibleParamB( m_Info[j], m_Info[i], m_bCrvBClosed, m_dCrvBSpan)) { + // caso DET-NULL -> NULL-DET per prima curva + if ( m_Info[j].IciA[kj].nPrevTy != ICCT_NULL && m_Info[j].IciA[kj].nNextTy == ICCT_NULL && + m_Info[i].IciA[ki].nPrevTy == ICCT_NULL && m_Info[i].IciA[ki].nNextTy != ICCT_NULL) { + // per la prima curva tengo i determinati + m_Info[i].IciA[ki].nPrevTy = m_Info[j].IciA[kj].nPrevTy ; + m_Info[j].IciA[kj].nNextTy = m_Info[i].IciA[ki].nNextTy ; + // se overlap equiverso + if ( m_Info[i].bOverlap && m_Info[i].bCBOverEq) { + // per la seconda curva ogni sottotipo è il duale di quello della prima + m_Info[i].IciB[ki].nPrevTy = GetDualIcct( m_Info[i].IciA[ki].nPrevTy) ; + m_Info[i].IciB[ki].nNextTy = GetDualIcct( m_Info[i].IciA[ki].nNextTy) ; + } + // se altrimenti overlap controverso + else if ( m_Info[i].bOverlap && ! m_Info[i].bCBOverEq) { + // per la seconda curva ogni sottotipo è come quello della prima ma in posizione invertita + m_Info[i].IciB[ki].nPrevTy = m_Info[i].IciA[ki].nNextTy ; + m_Info[i].IciB[ki].nNextTy = m_Info[i].IciA[ki].nPrevTy ; + } + // se overlap equiverso + if ( m_Info[j].bOverlap && m_Info[j].bCBOverEq) { + m_Info[j].IciB[kj].nPrevTy = GetDualIcct( m_Info[i].IciA[ki].nPrevTy) ; + m_Info[j].IciB[kj].nNextTy = GetDualIcct( m_Info[i].IciA[ki].nNextTy) ; + } + // se altrimenti overlap controverso + else if ( m_Info[j].bOverlap && ! m_Info[j].bCBOverEq) { + m_Info[j].IciB[kj].nPrevTy = m_Info[i].IciA[ki].nNextTy ; + m_Info[j].IciB[kj].nNextTy = m_Info[i].IciA[ki].nPrevTy ; + } + // medio parametri e punti separatamente per le due curve + MediaParamPoints( m_Info[i].IciA[ki], m_Info[j].IciA[kj]) ; + MediaParamPoints( m_Info[i].IciB[ki], m_Info[j].IciB[kj]) ; + // se entrambi overlap non cancello + if ( m_Info[j].bOverlap && m_Info[i].bOverlap) + continue ; + // cancello un singolo + if ( m_Info[i].bOverlap) { + EraseOtherInfo( i, j) ; + } + else { + EraseCurrentInfo( i, j) ; + break ; + } } - // se altrimenti overlap controverso - else if ( m_Info[i].bOverlap && ! m_Info[i].bCBOverEq) { - // per la seconda curva ogni sottotipo è come quello della prima ma in posizione invertita - m_Info[i].IciB[ki].nPrevTy = m_Info[i].IciA[ki].nNextTy ; - m_Info[i].IciB[ki].nNextTy = m_Info[i].IciA[ki].nPrevTy ; + // caso NULL-DET -> DET-NULL per prima curva (possibile su inizio/fine di curva chiusa) + else if ( m_Info[j].IciA[kj].nPrevTy == ICCT_NULL && m_Info[j].IciA[kj].nNextTy != ICCT_NULL && + m_Info[i].IciA[ki].nPrevTy != ICCT_NULL && m_Info[i].IciA[ki].nNextTy == ICCT_NULL) { + // per la prima curva tengo i determinati + m_Info[i].IciA[ki].nNextTy = m_Info[j].IciA[kj].nNextTy ; + m_Info[j].IciA[kj].nPrevTy = m_Info[i].IciA[ki].nPrevTy ; + // se overlap equiverso + if ( m_Info[i].bOverlap && m_Info[i].bCBOverEq) { + // per la seconda curva ogni sottotipo è il duale di quello della prima + m_Info[i].IciB[ki].nPrevTy = GetDualIcct( m_Info[i].IciA[ki].nPrevTy) ; + m_Info[i].IciB[ki].nNextTy = GetDualIcct( m_Info[i].IciA[ki].nNextTy) ; + } + // se altrimenti overlap controverso + else if ( m_Info[i].bOverlap && ! m_Info[i].bCBOverEq) { + // per la seconda curva ogni sottotipo è come quello della prima ma in posizione scambiata + m_Info[i].IciB[ki].nPrevTy = m_Info[i].IciA[ki].nNextTy ; + m_Info[i].IciB[ki].nNextTy = m_Info[i].IciA[ki].nPrevTy ; + } + // se overlap equiverso + if ( m_Info[j].bOverlap && m_Info[j].bCBOverEq) { + // per la seconda curva ogni sottotipo è il duale di quello della prima + m_Info[j].IciB[kj].nPrevTy = GetDualIcct( m_Info[i].IciA[ki].nPrevTy) ; + m_Info[j].IciB[kj].nNextTy = GetDualIcct( m_Info[i].IciA[ki].nNextTy) ; + } + // se altrimenti overlap controverso + else if ( m_Info[i].bOverlap && ! m_Info[i].bCBOverEq) { + // per la seconda curva ogni sottotipo è come quello della prima ma in posizione scambiata + m_Info[j].IciB[kj].nPrevTy = m_Info[i].IciA[ki].nNextTy ; + m_Info[j].IciB[kj].nNextTy = m_Info[i].IciA[ki].nPrevTy ; + } + // medio parametri e punti separatamente per le due curve + MediaParamPoints( m_Info[i].IciA[ki], m_Info[j].IciA[kj]) ; + MediaParamPoints( m_Info[i].IciB[ki], m_Info[j].IciB[kj]) ; + // se entrambi overlap non cancello + if ( m_Info[j].bOverlap && m_Info[i].bOverlap) + continue ; + // cancello un singolo + if ( m_Info[i].bOverlap) { + EraseOtherInfo( i, j) ; + } + else { + EraseCurrentInfo( i, j) ; + break ; + } } - // se overlap equiverso - if ( m_Info[j].bOverlap && m_Info[j].bCBOverEq) { - m_Info[j].IciB[kj].nPrevTy = GetDualIcct( m_Info[i].IciA[ki].nPrevTy) ; - m_Info[j].IciB[kj].nNextTy = GetDualIcct( m_Info[i].IciA[ki].nNextTy) ; + // caso DET-NULL -> NULL-DET per seconda curva + else if ( m_Info[j].IciB[kj].nPrevTy != ICCT_NULL && m_Info[j].IciB[kj].nNextTy == ICCT_NULL && + m_Info[i].IciB[ki].nPrevTy == ICCT_NULL && m_Info[i].IciB[ki].nNextTy != ICCT_NULL) { + // per la seconda curva tengo i determinati + m_Info[i].IciB[ki].nPrevTy = m_Info[j].IciB[kj].nPrevTy ; + m_Info[j].IciB[kj].nNextTy = m_Info[i].IciB[ki].nNextTy ; + // se overlap equiverso + if ( m_Info[i].bOverlap && m_Info[i].bCBOverEq) { + // per la prima curva ogni sottotipo è il duale di quello della seconda + m_Info[i].IciA[ki].nPrevTy = GetDualIcct( m_Info[i].IciB[ki].nPrevTy) ; + m_Info[i].IciA[ki].nNextTy = GetDualIcct( m_Info[i].IciB[ki].nNextTy) ; + } + // se altrimenti overlap controverso + else if ( m_Info[i].bOverlap && ! m_Info[i].bCBOverEq) { + // per la prima curva ogni sottotipo è come quello della seconda ma in posizione scambiata + m_Info[i].IciA[ki].nPrevTy = m_Info[i].IciB[ki].nNextTy ; + m_Info[i].IciA[ki].nNextTy = m_Info[i].IciB[ki].nPrevTy ; + } + // se overlap equiverso + if ( m_Info[j].bOverlap && m_Info[j].bCBOverEq) { + // per la prima curva ogni sottotipo è il duale di quello della seconda + m_Info[j].IciA[kj].nPrevTy = GetDualIcct( m_Info[i].IciB[ki].nPrevTy) ; + m_Info[j].IciA[kj].nNextTy = GetDualIcct( m_Info[i].IciB[ki].nNextTy) ; + } + // se altrimenti overlap controverso + else if ( m_Info[j].bOverlap && ! m_Info[j].bCBOverEq) { + // per la prima curva ogni sottotipo è come quello della seconda ma in posizione scambiata + m_Info[j].IciA[kj].nPrevTy = m_Info[i].IciB[ki].nNextTy ; + m_Info[j].IciA[kj].nNextTy = m_Info[i].IciB[ki].nPrevTy ; + } + // medio parametri e punti separatamente per le due curve + MediaParamPoints( m_Info[i].IciA[ki], m_Info[j].IciA[kj]) ; + MediaParamPoints( m_Info[i].IciB[ki], m_Info[j].IciB[kj]) ; + // se entrambi overlap non cancello + if ( m_Info[j].bOverlap && m_Info[i].bOverlap) + continue ; + // cancello un singolo + if ( m_Info[i].bOverlap) { + EraseOtherInfo( i, j) ; + } + else { + EraseCurrentInfo( i, j) ; + break ; + } } - // se altrimenti overlap controverso - else if ( m_Info[j].bOverlap && ! m_Info[j].bCBOverEq) { - m_Info[j].IciB[kj].nPrevTy = m_Info[i].IciA[ki].nNextTy ; - m_Info[j].IciB[kj].nNextTy = m_Info[i].IciA[ki].nPrevTy ; + // caso NULL-DET -> DET-NULL per seconda curva (possibile su inizio/fine di curva chiusa) + else if ( m_Info[j].IciB[kj].nPrevTy == ICCT_NULL && m_Info[j].IciB[kj].nNextTy != ICCT_NULL && + m_Info[i].IciB[ki].nPrevTy != ICCT_NULL && m_Info[i].IciB[ki].nNextTy == ICCT_NULL) { + // per la seconda curva tengo i determinati + m_Info[i].IciB[ki].nNextTy = m_Info[j].IciB[kj].nNextTy ; + m_Info[j].IciB[kj].nPrevTy = m_Info[i].IciB[ki].nPrevTy ; + // se overlap equiverso + if ( m_Info[i].bOverlap && m_Info[i].bCBOverEq) { + // per la prima curva ogni sottotipo è il duale di quello della seconda + m_Info[i].IciA[ki].nPrevTy = GetDualIcct( m_Info[i].IciB[ki].nPrevTy) ; + m_Info[i].IciA[ki].nNextTy = GetDualIcct( m_Info[i].IciB[ki].nNextTy) ; + } + // se altrimenti overlap controverso + else if ( m_Info[i].bOverlap && ! m_Info[i].bCBOverEq) { + // per la prima curva ogni sottotipo è come quello della seconda ma in posizione scambiata + m_Info[i].IciA[ki].nPrevTy = m_Info[i].IciB[ki].nNextTy ; + m_Info[i].IciA[ki].nNextTy = m_Info[i].IciB[ki].nPrevTy ; + } + // se overlap equiverso + if ( m_Info[j].bOverlap && m_Info[j].bCBOverEq) { + // per la prima curva ogni sottotipo è il duale di quello della seconda + m_Info[j].IciA[kj].nPrevTy = GetDualIcct( m_Info[i].IciB[ki].nPrevTy) ; + m_Info[j].IciA[kj].nNextTy = GetDualIcct( m_Info[i].IciB[ki].nNextTy) ; + } + // se altrimenti overlap controverso + else if ( m_Info[j].bOverlap && ! m_Info[j].bCBOverEq) { + // per la prima curva ogni sottotipo è come quello della seconda ma in posizione scambiata + m_Info[j].IciA[kj].nPrevTy = m_Info[i].IciB[ki].nNextTy ; + m_Info[j].IciA[kj].nNextTy = m_Info[i].IciB[ki].nPrevTy ; + } + // medio parametri e punti separatamente per le due curve + MediaParamPoints( m_Info[i].IciA[ki], m_Info[j].IciA[kj]) ; + MediaParamPoints( m_Info[i].IciB[ki], m_Info[j].IciB[kj]) ; + // se entrambi overlap non cancello + if ( m_Info[j].bOverlap && m_Info[i].bOverlap) + continue ; + // cancello un singolo + if ( m_Info[i].bOverlap) { + EraseOtherInfo( i, j) ; + } + else { + EraseCurrentInfo( i, j) ; + break ; + } } - // medio parametri e punti separatamente per le due curve - MediaParamPoints( m_Info[i].IciA[ki], m_Info[j].IciA[kj]) ; - MediaParamPoints( m_Info[i].IciB[ki], m_Info[j].IciB[kj]) ; - // se entrambi overlap non cancello - if ( m_Info[j].bOverlap && m_Info[i].bOverlap) - continue ; - // cancello un singolo - if ( m_Info[i].bOverlap) { - EraseOtherInfo( i, j) ; - } - else { + // caso NULL-NULL per corrente di prima curva + else if ( m_Info[i].IciA[ki].nPrevTy == ICCT_NULL && m_Info[i].IciA[ki].nNextTy == ICCT_NULL) { + // cancello l'intersezione corrente (non aggiunge nulla rispetto alla precedente) EraseCurrentInfo( i, j) ; break ; } - } - // caso NULL-DET -> DET-NULL per prima curva (possibile su inizio/fine di curva chiusa) - else if ( m_Info[j].IciA[kj].nPrevTy == ICCT_NULL && m_Info[j].IciA[kj].nNextTy != ICCT_NULL && - m_Info[i].IciA[ki].nPrevTy != ICCT_NULL && m_Info[i].IciA[ki].nNextTy == ICCT_NULL) { - // per la prima curva tengo i determinati - m_Info[i].IciA[ki].nNextTy = m_Info[j].IciA[kj].nNextTy ; - m_Info[j].IciA[kj].nPrevTy = m_Info[i].IciA[ki].nPrevTy ; - // se overlap equiverso - if ( m_Info[i].bOverlap && m_Info[i].bCBOverEq) { - // per la seconda curva ogni sottotipo è il duale di quello della prima - m_Info[i].IciB[ki].nPrevTy = GetDualIcct( m_Info[i].IciA[ki].nPrevTy) ; - m_Info[i].IciB[ki].nNextTy = GetDualIcct( m_Info[i].IciA[ki].nNextTy) ; - } - // se altrimenti overlap controverso - else if ( m_Info[i].bOverlap && ! m_Info[i].bCBOverEq) { - // per la seconda curva ogni sottotipo è come quello della prima ma in posizione scambiata - m_Info[i].IciB[ki].nPrevTy = m_Info[i].IciA[ki].nNextTy ; - m_Info[i].IciB[ki].nNextTy = m_Info[i].IciA[ki].nPrevTy ; - } - // se overlap equiverso - if ( m_Info[j].bOverlap && m_Info[j].bCBOverEq) { - // per la seconda curva ogni sottotipo è il duale di quello della prima - m_Info[j].IciB[kj].nPrevTy = GetDualIcct( m_Info[i].IciA[ki].nPrevTy) ; - m_Info[j].IciB[kj].nNextTy = GetDualIcct( m_Info[i].IciA[ki].nNextTy) ; - } - // se altrimenti overlap controverso - else if ( m_Info[i].bOverlap && ! m_Info[i].bCBOverEq) { - // per la seconda curva ogni sottotipo è come quello della prima ma in posizione scambiata - m_Info[j].IciB[kj].nPrevTy = m_Info[i].IciA[ki].nNextTy ; - m_Info[j].IciB[kj].nNextTy = m_Info[i].IciA[ki].nPrevTy ; - } - // medio parametri e punti separatamente per le due curve - MediaParamPoints( m_Info[i].IciA[ki], m_Info[j].IciA[kj]) ; - MediaParamPoints( m_Info[i].IciB[ki], m_Info[j].IciB[kj]) ; - // se entrambi overlap non cancello - if ( m_Info[j].bOverlap && m_Info[i].bOverlap) - continue ; - // cancello un singolo - if ( m_Info[i].bOverlap) { + // caso NULL-NULL per precedente di prima curva + else if ( m_Info[j].IciA[kj].nPrevTy == ICCT_NULL && m_Info[j].IciA[kj].nNextTy == ICCT_NULL) { + // cancello l'intersezione precedente (non aggiunge nulla rispetto alla corrente) EraseOtherInfo( i, j) ; } - else { - EraseCurrentInfo( i, j) ; - break ; - } } - // caso DET-NULL -> NULL-DET per seconda curva - else if ( m_Info[j].IciB[kj].nPrevTy != ICCT_NULL && m_Info[j].IciB[kj].nNextTy == ICCT_NULL && - m_Info[i].IciB[ki].nPrevTy == ICCT_NULL && m_Info[i].IciB[ki].nNextTy != ICCT_NULL) { - // per la seconda curva tengo i determinati - m_Info[i].IciB[ki].nPrevTy = m_Info[j].IciB[kj].nPrevTy ; - m_Info[j].IciB[kj].nNextTy = m_Info[i].IciB[ki].nNextTy ; - // se overlap equiverso - if ( m_Info[i].bOverlap && m_Info[i].bCBOverEq) { - // per la prima curva ogni sottotipo è il duale di quello della seconda - m_Info[i].IciA[ki].nPrevTy = GetDualIcct( m_Info[i].IciB[ki].nPrevTy) ; - m_Info[i].IciA[ki].nNextTy = GetDualIcct( m_Info[i].IciB[ki].nNextTy) ; - } - // se altrimenti overlap controverso - else if ( m_Info[i].bOverlap && ! m_Info[i].bCBOverEq) { - // per la prima curva ogni sottotipo è come quello della seconda ma in posizione scambiata - m_Info[i].IciA[ki].nPrevTy = m_Info[i].IciB[ki].nNextTy ; - m_Info[i].IciA[ki].nNextTy = m_Info[i].IciB[ki].nPrevTy ; - } - // se overlap equiverso - if ( m_Info[j].bOverlap && m_Info[j].bCBOverEq) { - // per la prima curva ogni sottotipo è il duale di quello della seconda - m_Info[j].IciA[kj].nPrevTy = GetDualIcct( m_Info[i].IciB[ki].nPrevTy) ; - m_Info[j].IciA[kj].nNextTy = GetDualIcct( m_Info[i].IciB[ki].nNextTy) ; - } - // se altrimenti overlap controverso - else if ( m_Info[j].bOverlap && ! m_Info[j].bCBOverEq) { - // per la prima curva ogni sottotipo è come quello della seconda ma in posizione scambiata - m_Info[j].IciA[kj].nPrevTy = m_Info[i].IciB[ki].nNextTy ; - m_Info[j].IciA[kj].nNextTy = m_Info[i].IciB[ki].nPrevTy ; - } - // medio parametri e punti separatamente per le due curve - MediaParamPoints( m_Info[i].IciA[ki], m_Info[j].IciA[kj]) ; - MediaParamPoints( m_Info[i].IciB[ki], m_Info[j].IciB[kj]) ; - // se entrambi overlap non cancello - if ( m_Info[j].bOverlap && m_Info[i].bOverlap) - continue ; - // cancello un singolo - if ( m_Info[i].bOverlap) { - EraseOtherInfo( i, j) ; - } - else { - EraseCurrentInfo( i, j) ; - break ; - } - } - // caso NULL-DET -> DET-NULL per seconda curva (possibile su inizio/fine di curva chiusa) - else if ( m_Info[j].IciB[kj].nPrevTy == ICCT_NULL && m_Info[j].IciB[kj].nNextTy != ICCT_NULL && - m_Info[i].IciB[ki].nPrevTy != ICCT_NULL && m_Info[i].IciB[ki].nNextTy == ICCT_NULL) { - // per la seconda curva tengo i determinati - m_Info[i].IciB[ki].nNextTy = m_Info[j].IciB[kj].nNextTy ; - m_Info[j].IciB[kj].nPrevTy = m_Info[i].IciB[ki].nPrevTy ; - // se overlap equiverso - if ( m_Info[i].bOverlap && m_Info[i].bCBOverEq) { - // per la prima curva ogni sottotipo è il duale di quello della seconda - m_Info[i].IciA[ki].nPrevTy = GetDualIcct( m_Info[i].IciB[ki].nPrevTy) ; - m_Info[i].IciA[ki].nNextTy = GetDualIcct( m_Info[i].IciB[ki].nNextTy) ; - } - // se altrimenti overlap controverso - else if ( m_Info[i].bOverlap && ! m_Info[i].bCBOverEq) { - // per la prima curva ogni sottotipo è come quello della seconda ma in posizione scambiata - m_Info[i].IciA[ki].nPrevTy = m_Info[i].IciB[ki].nNextTy ; - m_Info[i].IciA[ki].nNextTy = m_Info[i].IciB[ki].nPrevTy ; - } - // se overlap equiverso - if ( m_Info[j].bOverlap && m_Info[j].bCBOverEq) { - // per la prima curva ogni sottotipo è il duale di quello della seconda - m_Info[j].IciA[kj].nPrevTy = GetDualIcct( m_Info[i].IciB[ki].nPrevTy) ; - m_Info[j].IciA[kj].nNextTy = GetDualIcct( m_Info[i].IciB[ki].nNextTy) ; - } - // se altrimenti overlap controverso - else if ( m_Info[j].bOverlap && ! m_Info[j].bCBOverEq) { - // per la prima curva ogni sottotipo è come quello della seconda ma in posizione scambiata - m_Info[j].IciA[kj].nPrevTy = m_Info[i].IciB[ki].nNextTy ; - m_Info[j].IciA[kj].nNextTy = m_Info[i].IciB[ki].nPrevTy ; - } - // medio parametri e punti separatamente per le due curve - MediaParamPoints( m_Info[i].IciA[ki], m_Info[j].IciA[kj]) ; - MediaParamPoints( m_Info[i].IciB[ki], m_Info[j].IciB[kj]) ; - // se entrambi overlap non cancello - if ( m_Info[j].bOverlap && m_Info[i].bOverlap) - continue ; - // cancello un singolo - if ( m_Info[i].bOverlap) { - EraseOtherInfo( i, j) ; - } - else { - EraseCurrentInfo( i, j) ; - break ; - } - } - // caso NULL-NULL per corrente di prima curva - else if ( m_Info[i].IciA[ki].nPrevTy == ICCT_NULL && m_Info[i].IciA[ki].nNextTy == ICCT_NULL) { - // cancello l'intersezione corrente (non aggiunge nulla rispetto alla precedente) - EraseCurrentInfo( i, j) ; - break ; - } - // caso NULL-NULL per precedente di prima curva - else if ( m_Info[j].IciA[kj].nPrevTy == ICCT_NULL && m_Info[j].IciA[kj].nNextTy == ICCT_NULL) { - // cancello l'intersezione precedente (non aggiunge nulla rispetto alla corrente) - EraseOtherInfo( i, j) ; - } - } } } @@ -433,6 +461,32 @@ IntersCrvCompoCrvCompo::IntersCrvCompoCrvCompo( const ICurveComposite& CCompoA, OrderNonManifoldInters( m_Info, CCompoA, CCompoB) ; } +//---------------------------------------------------------------------------- +bool +IntersCrvCompoCrvCompo::IntersSimpleCurves( const ICurve& CurveA, int nA, const ICurve& CurveB, int nB) +{ + // eseguo l'intersezione di queste curve semplici + IntersCurveCurve intCC( CurveA, CurveB) ; + // ne recupero i risultati + int nCurrInters = intCC.GetNumInters() ; + if ( nCurrInters > 0) { + m_nNumInters += nCurrInters ; + m_bOverlaps = ( intCC.GetOverlaps() ? true : m_bOverlaps) ; + for ( int j = 0 ; j < nCurrInters ; ++ j) { + IntCrvCrvInfo aInfo ; + intCC.GetIntCrvCrvInfo( j, aInfo) ; + aInfo.IciA[0].dU += nA ; + aInfo.IciB[0].dU += nB ; + if ( aInfo.bOverlap) { + aInfo.IciA[1].dU += nA ; + aInfo.IciB[1].dU += nB ; + } + m_Info.push_back( aInfo) ; + } + } + return true ; +} + //---------------------------------------------------------------------------- bool IntersCrvCompoCrvCompo::EraseCurrentInfo( int& nIndCurr, int& nIndOther) diff --git a/IntersCrvCompoCrvCompo.h b/IntersCrvCompoCrvCompo.h index f886df1..6f38627 100644 --- a/IntersCrvCompoCrvCompo.h +++ b/IntersCrvCompoCrvCompo.h @@ -38,8 +38,7 @@ class IntersCrvCompoCrvCompo private : IntersCrvCompoCrvCompo( void) ; - //bool CompatibleParamA( int nInd1, int nInd2) ; - //bool CompatibleParamB( int nInd1, int nInd2) ; + bool IntersSimpleCurves( const ICurve& CurveA, int nA, const ICurve& CurveB, int nB) ; bool EraseCurrentInfo( int& nIndCurr, int& nIndOther) ; bool EraseOtherInfo( int& nIndCurr, int& nIndOther) ; diff --git a/IntersCurveCurve.cpp b/IntersCurveCurve.cpp index a2881dc..cf4e933 100644 --- a/IntersCurveCurve.cpp +++ b/IntersCurveCurve.cpp @@ -37,61 +37,86 @@ IntersCurveCurve::IntersCurveCurve( const ICurve& CurveA, const ICurve& CurveB, // inizializzazioni m_bOverlaps = false ; m_nNumInters = 0 ; - m_pCurve[0] = &CurveA ; - m_pCurve[1] = &CurveB ; + m_pOriCrv[0] = &CurveA ; + m_pOriCrv[1] = &CurveB ; + + // ciclo sulle curve per verificare se da approssimare + for ( int i = 0 ; i < 2 ; ++ i) { + // se curva è arco da approssimare oppure è curva di Bezier + if ( ( m_pOriCrv[i]->GetType() == CRV_ARC && IsArcToApprox( *m_pOriCrv[i])) || + m_pOriCrv[i]->GetType() == CRV_BEZ) { + // approssimo con rette + PolyLine PL ; + if ( ! m_pOriCrv[i]->ApproxWithLines( EPS_SMALL, ANG_TOL_STD_DEG, ICurve::APL_STD, PL)) + return ; + m_pTmpCrv[i].Set( CreateBasicCurveComposite()) ; + if ( IsNull( m_pTmpCrv[i])) + return ; + if ( ! GetBasicCurveComposite( Get( m_pTmpCrv[i]))->FromPolyLine( PL)) + return ; + m_pCurve[i] = Get( m_pTmpCrv[i]) ; + } + else + m_pCurve[i] = m_pOriCrv[i] ; + } // chiamo calcolatore opportuno - switch ( CurveA.GetType()) { + switch ( m_pCurve[0]->GetType()) { case CRV_LINE : - switch ( CurveB.GetType()) { + switch ( m_pCurve[1]->GetType()) { case CRV_LINE : - LineLineCalculate( CurveA, CurveB, bAreSegments) ; + LineLineCalculate( *m_pCurve[0], *m_pCurve[1], bAreSegments) ; break ; case CRV_ARC : - LineArcCalculate( CurveA, CurveB) ; - break ; - case CRV_BEZ : + LineArcCalculate( *m_pCurve[0], *m_pCurve[1]) ; break ; case CRV_COMPO : - LineCrvCompoCalculate( CurveA, CurveB) ; + LineCrvCompoCalculate( *m_pCurve[0], *m_pCurve[1]) ; break ; } break ; case CRV_ARC : - switch ( CurveB.GetType()) { + switch ( m_pCurve[1]->GetType()) { case CRV_LINE : - ArcLineCalculate( CurveA, CurveB) ; + ArcLineCalculate( *m_pCurve[0], *m_pCurve[1]) ; break ; case CRV_ARC : - ArcArcCalculate( CurveA, CurveB) ; - break ; - case CRV_BEZ : + ArcArcCalculate( *m_pCurve[0], *m_pCurve[1]) ; break ; case CRV_COMPO : - ArcCrvCompoCalculate( CurveA, CurveB) ; + ArcCrvCompoCalculate( *m_pCurve[0], *m_pCurve[1]) ; break ; } break ; - case CRV_BEZ : - break ; case CRV_COMPO : - switch ( CurveB.GetType()) { + switch ( m_pCurve[1]->GetType()) { case CRV_LINE : - CrvCompoLineCalculate( CurveA, CurveB) ; + CrvCompoLineCalculate( *m_pCurve[0], *m_pCurve[1]) ; break ; case CRV_ARC : - CrvCompoArcCalculate( CurveA, CurveB) ; - break ; - case CRV_BEZ : + CrvCompoArcCalculate( *m_pCurve[0], *m_pCurve[1]) ; break ; case CRV_COMPO : - CrvCompoCrvCompoCalculate( CurveA, CurveB) ; + CrvCompoCrvCompoCalculate( *m_pCurve[0], *m_pCurve[1]) ; break ; } break ; } } +//---------------------------------------------------------------------------- +bool +IntersCurveCurve::IsArcToApprox( const ICurve& Curve) +{ + // recupero l'arco + const CurveArc* pArc = GetBasicCurveArc( &Curve) ; + if ( pArc == nullptr) + return false ; + // verifico se non è nel piano XY e ha più di un giro al centro + return ( ( ! pArc->GetNormVersor().IsZplus() && ! pArc->GetNormVersor().IsZminus()) || + fabs( pArc->GetAngCenter()) > ANG_FULL + EPS_ANG_ZERO) ; +} + //---------------------------------------------------------------------------- void IntersCurveCurve::LineLineCalculate( const ICurve& CurveA, const ICurve& CurveB, bool bAreSegments) @@ -229,6 +254,19 @@ IntersCurveCurve::GetIntCrvCrvInfo( int nInd, IntCrvCrvInfo& aInfo) if ( nInd < 0 || nInd >= m_nNumInters) return false ; aInfo = m_Info[nInd] ; + // se curve originali approssimate, devo ricalcolare i parametri dei punti di intersezione + if ( m_pCurve[0] != m_pOriCrv[0]) { + if ( ! m_pOriCrv[0]->GetParamAtPoint( aInfo.IciA[0].ptI, aInfo.IciA[0].dU, 10 * EPS_SMALL)) + return false ; + if ( aInfo.bOverlap && ! m_pOriCrv[0]->GetParamAtPoint( aInfo.IciA[1].ptI, aInfo.IciA[1].dU, 10 * EPS_SMALL)) + return false ; + } + if ( m_pCurve[1] != m_pOriCrv[1]) { + if ( ! m_pOriCrv[1]->GetParamAtPoint( aInfo.IciB[0].ptI, aInfo.IciB[0].dU, 10 * EPS_SMALL)) + return false ; + if ( aInfo.bOverlap && ! m_pOriCrv[1]->GetParamAtPoint( aInfo.IciB[1].ptI, aInfo.IciB[1].dU, 10 * EPS_SMALL)) + return false ; + } return true ; }