Dario Sassi
64c954ad4b
- fabs sostituito da abs - in Zmap razionalizzazione operazioni taglio spilloni - in SurfTriMesh UpdateFaceting senza più chiamate recursive.
448 lines
16 KiB
C++
448 lines
16 KiB
C++
//----------------------------------------------------------------------------
|
|
// EgalTech 2014-2014
|
|
//----------------------------------------------------------------------------
|
|
// File : PointGrid3d.cpp Data : 15.05.14 Versione : 1.5e5
|
|
// Contenuto : Implementazione della classe PointGrid3d.
|
|
// Indicizzazione spaziale di Point3d mediante hash grid.
|
|
//
|
|
//
|
|
// Modifiche : 15.05.14 DS Creazione modulo.
|
|
//
|
|
//
|
|
//----------------------------------------------------------------------------
|
|
|
|
//--------------------------- Include ----------------------------------------
|
|
#include "stdafx.h"
|
|
#include "\EgtDev\Include\EGkPointGrid3d.h"
|
|
#include <algorithm>
|
|
#include <utility>
|
|
|
|
using namespace std ;
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Struct IBox
|
|
//----------------------------------------------------------------------------
|
|
struct IBox {
|
|
int nXmin, nXmax ;
|
|
int nYmin, nYmax ;
|
|
int nZmin, nZmax ;
|
|
IBox( void) : nXmin( INT_MAX), nXmax( INT_MIN),
|
|
nYmin( INT_MAX), nYmax( INT_MIN),
|
|
nZmin( INT_MAX), nZmax( INT_MIN) {}
|
|
void Reset( void)
|
|
{ nXmin = nYmin = nZmin = INT_MAX ;
|
|
nXmax = nYmax = nZmax = INT_MIN ; }
|
|
bool Encloses( int nX, int nY, int nZ)
|
|
{ return ( nX >= nXmin && nX <= nXmax &&
|
|
nY >= nYmin && nY <= nYmax &&
|
|
nZ >= nZmin && nZ <= nZmax) ; }
|
|
} ;
|
|
|
|
//----------------------------------------------------------------------------
|
|
static const int N_SUBIBOX = 6 ;
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
SubtractIBox( const IBox& ibA, const IBox& ibB, IBox ibR[N_SUBIBOX])
|
|
{
|
|
// ibR = ibA - ibB (il risultato può essere formato da 6 box)
|
|
// se ibA non include alcunchè
|
|
if ( ibA.nXmin > ibA.nXmax || ibA.nYmin > ibA.nYmax || ibA.nZmin > ibA.nZmax) {
|
|
for ( int i = 0 ; i < N_SUBIBOX ; ++ i)
|
|
ibR[i].Reset() ;
|
|
return true ;
|
|
}
|
|
// se ibB non include alcunchè
|
|
if ( ibB.nXmin > ibB.nXmax || ibB.nYmin > ibB.nYmax || ibB.nZmin > ibB.nZmax) {
|
|
ibR[0] = ibA ;
|
|
for ( int i = 1 ; i < N_SUBIBOX ; ++ i)
|
|
ibR[i].Reset() ;
|
|
return true ;
|
|
}
|
|
// lungo asse X
|
|
if ( ibA.nXmin < ibB.nXmin) {
|
|
ibR[0].nXmin = ibA.nXmin ;
|
|
ibR[0].nXmax = ibB.nXmin - 1 ;
|
|
ibR[0].nYmin = ibA.nYmin ;
|
|
ibR[0].nYmax = ibA.nYmax ;
|
|
ibR[0].nZmin = ibA.nZmin ;
|
|
ibR[0].nZmax = ibA.nZmax ;
|
|
}
|
|
else
|
|
ibR[0].Reset() ;
|
|
if ( ibA.nXmax > ibB.nXmax) {
|
|
ibR[1].nXmin = ibB.nXmax + 1 ;
|
|
ibR[1].nXmax = ibA.nXmax ;
|
|
ibR[1].nYmin = ibA.nYmin ;
|
|
ibR[1].nYmax = ibA.nYmax ;
|
|
ibR[1].nZmin = ibA.nZmin ;
|
|
ibR[1].nZmax = ibA.nZmax ;
|
|
}
|
|
else
|
|
ibR[1].Reset() ;
|
|
// lungo asse Y
|
|
if ( ibA.nYmin < ibB.nYmin) {
|
|
ibR[2].nXmin = ibB.nXmin ;
|
|
ibR[2].nXmax = ibB.nXmax ;
|
|
ibR[2].nYmin = ibA.nYmin ;
|
|
ibR[2].nYmax = ibB.nYmin - 1 ;
|
|
ibR[2].nZmin = ibA.nZmin ;
|
|
ibR[2].nZmax = ibA.nZmax ;
|
|
}
|
|
else
|
|
ibR[2].Reset() ;
|
|
if ( ibA.nYmax > ibB.nYmax) {
|
|
ibR[3].nXmin = ibB.nXmin ;
|
|
ibR[3].nXmax = ibB.nXmax ;
|
|
ibR[3].nYmin = ibB.nYmax + 1 ;
|
|
ibR[3].nYmax = ibA.nYmax ;
|
|
ibR[3].nZmin = ibA.nZmin ;
|
|
ibR[3].nZmax = ibA.nZmax ;
|
|
}
|
|
else
|
|
ibR[3].Reset() ;
|
|
// lungo asse Z
|
|
if ( ibA.nZmin < ibB.nZmin) {
|
|
ibR[4].nXmin = ibB.nXmin ;
|
|
ibR[4].nXmax = ibB.nXmax ;
|
|
ibR[4].nYmin = ibB.nYmin ;
|
|
ibR[4].nYmax = ibB.nYmax ;
|
|
ibR[4].nZmin = ibA.nZmin ;
|
|
ibR[4].nZmax = ibB.nZmin - 1 ;
|
|
}
|
|
else
|
|
ibR[4].Reset() ;
|
|
if ( ibA.nZmax > ibB.nZmax) {
|
|
ibR[5].nXmin = ibB.nXmin ;
|
|
ibR[5].nXmax = ibB.nXmax ;
|
|
ibR[5].nYmin = ibB.nYmin ;
|
|
ibR[5].nYmax = ibB.nYmax ;
|
|
ibR[5].nZmin = ibB.nZmax + 1 ;
|
|
ibR[5].nZmax = ibA.nZmax ;
|
|
}
|
|
else
|
|
ibR[5].Reset() ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Class PointGrid3d
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
PointGrid3d::Init( int nBuckets, double dCellDim)
|
|
{
|
|
m_dCellDim = max( dCellDim, EPS_SMALL) ;
|
|
m_dInvCellDim = 1 / m_dCellDim ;
|
|
m_BBox.Reset() ;
|
|
m_MMap.clear() ;
|
|
try { m_MMap.rehash( nBuckets) ;}
|
|
catch(...) { return false ;}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
PointGrid3d::InsertPoint( const Point3d& ptP, int nId)
|
|
{
|
|
// inserimento nel map
|
|
int nKey = PointHash( Get1dCellNbr( ptP.x), Get1dCellNbr( ptP.y), Get1dCellNbr( ptP.z)) ;
|
|
m_MMap.emplace( nKey, make_pair( ptP, nId)) ;
|
|
// aggiornamento del bbox complessivo
|
|
m_BBox.Add( ptP) ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
PointGrid3d::RemovePoint( const Point3d& ptP, int nId)
|
|
{
|
|
// recupero gli elementi con la chiave
|
|
int nKey = PointHash( Get1dCellNbr( ptP.x), Get1dCellNbr( ptP.y), Get1dCellNbr( ptP.z)) ;
|
|
IPNTI_UMMAP_CRANGE MMrange = m_MMap.equal_range( nKey) ;
|
|
// cerco quello con l'Id voluto
|
|
for ( ; MMrange.first != MMrange.second ; ++ MMrange.first) {
|
|
// se coincidono punto e Id lo cancello
|
|
if ( SqDist( (*MMrange.first).second.first, ptP) < SQ_EPS_SMALL &&
|
|
abs( (*MMrange.first).second.second) == abs( nId)) {
|
|
m_MMap.erase( MMrange.first) ;
|
|
return true ;
|
|
}
|
|
}
|
|
|
|
return false ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
PointGrid3d::Find( const Point3d& ptTest, double dTol, INTVECTOR& vnIds) const
|
|
{
|
|
// pulisco il risultato
|
|
vnIds.clear() ;
|
|
// determino il range di celle sui tre assi
|
|
IBox iBox ;
|
|
if ( ! Get3dRangeNbr( ptTest, dTol, iBox))
|
|
return false ;
|
|
// ciclo su tutte le celle del range
|
|
for ( int i = iBox.nXmin ; i <= iBox.nXmax ; ++ i) {
|
|
for ( int j = iBox.nYmin ; j <= iBox.nYmax ; ++ j) {
|
|
for ( int k = iBox.nZmin ; k <= iBox.nZmax ; ++ k) {
|
|
IPNTI_UMMAP_CRANGE MMrange = m_MMap.equal_range( PointHash( i, j, k)) ;
|
|
for ( ; MMrange.first != MMrange.second ; ++ MMrange.first) {
|
|
if ( SqDist( (*MMrange.first).second.first, ptTest) < ( dTol * dTol))
|
|
vnIds.push_back( (*MMrange.first).second.second) ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return ( vnIds.size() > 0) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
PointGrid3d::Find( const BBox3d& b3Test, INTVECTOR& vnIds) const
|
|
{
|
|
// pulisco il risultato
|
|
vnIds.clear() ;
|
|
// determino il range di celle sui tre assi
|
|
IBox iBox ;
|
|
if ( ! Get3dRangeNbr( b3Test, iBox))
|
|
return false ;
|
|
// ciclo su tutte le celle del range
|
|
for ( int i = iBox.nXmin ; i <= iBox.nXmax ; ++ i) {
|
|
for ( int j = iBox.nYmin ; j <= iBox.nYmax ; ++ j) {
|
|
for ( int k = iBox.nZmin ; k <= iBox.nZmax ; ++ k) {
|
|
IPNTI_UMMAP_CRANGE MMrange = m_MMap.equal_range( PointHash( i, j, k)) ;
|
|
for ( ; MMrange.first != MMrange.second ; ++ MMrange.first) {
|
|
if ( b3Test.Encloses( (*MMrange.first).second.first))
|
|
vnIds.push_back( (*MMrange.first).second.second) ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return ( vnIds.size() > 0) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
PointGrid3d::Find( const Point3d& ptTest, double dTol, int& nId) const
|
|
{
|
|
// determino il range di celle sui tre assi
|
|
IBox iBox ;
|
|
if ( ! Get3dRangeNbr( ptTest, dTol, iBox))
|
|
return false ;
|
|
// ciclo su tutte le celle del range
|
|
for ( int i = iBox.nXmin ; i <= iBox.nXmax ; ++ i) {
|
|
for ( int j = iBox.nYmin ; j <= iBox.nYmax ; ++ j) {
|
|
for ( int k = iBox.nZmin ; k <= iBox.nZmax ; ++ k) {
|
|
IPNTI_UMMAP_CRANGE MMrange = m_MMap.equal_range( PointHash( i, j, k)) ;
|
|
for ( ; MMrange.first != MMrange.second ; ++ MMrange.first) {
|
|
if ( SqDist( (*MMrange.first).second.first, ptTest) < ( dTol * dTol)) {
|
|
nId = (*MMrange.first).second.second ;
|
|
return true ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return false ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
PointGrid3d::FindNearest( const Point3d& ptTest, INTVECTOR& vnIds) const
|
|
{
|
|
const double NEAR_TOL = 100 * EPS_SMALL ;
|
|
const int SIZE_LIMIT = 1000 ;
|
|
// pulisco il risultato
|
|
vnIds.clear() ;
|
|
// se ci sono pochi elementi, eseguo direttamente la ricerca sugli stessi
|
|
if ( m_MMap.size() < SIZE_LIMIT) {
|
|
// ciclo di ricerca sugli elementi
|
|
bool bFound = false ;
|
|
double dMinDist ;
|
|
double dSqMinDist ;
|
|
for ( const auto& PntI : m_MMap) {
|
|
// quadrato della distanza
|
|
double dSqDist = SqDist( ptTest, PntI.second.first) ;
|
|
// altro punto con la stessa minima distanza già trovata
|
|
if ( bFound && abs( dSqDist - dSqMinDist) < 2 * dMinDist * NEAR_TOL + NEAR_TOL * NEAR_TOL) {
|
|
// inserisco il punto nel vettore dei risultati
|
|
vnIds.push_back( PntI.second.second) ;
|
|
}
|
|
// primo punto o punto con minima distanza più bassa
|
|
else if ( ! bFound || dSqDist < dSqMinDist) {
|
|
bFound = true ;
|
|
// aggiorno i minimi
|
|
dSqMinDist = dSqDist ;
|
|
dMinDist = sqrt( dSqMinDist) ;
|
|
// pulisco il vettore dei risultati ed inserisco questo
|
|
vnIds.clear() ;
|
|
vnIds.push_back( PntI.second.second) ;
|
|
}
|
|
}
|
|
return bFound ;
|
|
}
|
|
// altrimenti, verifico cerchi concentrici di celle
|
|
else {
|
|
// raggio di ricerca
|
|
double dRad = max( m_BBox.DistFromPoint( ptTest), NEAR_TOL) ;
|
|
// delta di incremento per raggio di ricerca
|
|
const int N_CELL = 10 ;
|
|
double dDelta = N_CELL * m_dCellDim ;
|
|
// massimo numero di step di ricerca basato su raggio box di ingombro
|
|
double dBoxRad ;
|
|
if ( ! m_BBox.GetRadius( dBoxRad))
|
|
return false ;
|
|
int nMaxSteps = int( 2 * ceil( dBoxRad / dDelta)) ;
|
|
nMaxSteps = max( nMaxSteps, 1) ;
|
|
// ciclo di ricerca con raggio crescente
|
|
IBox iBoxPrev ;
|
|
IBox ibR[N_SUBIBOX] ;
|
|
bool bFound = false ;
|
|
double dMinDist ;
|
|
double dSqMinDist ;
|
|
for ( int m = 0 ; m <= nMaxSteps ; ++ m) {
|
|
// determino il range di celle sui tre assi
|
|
IBox iBox ;
|
|
if ( ! Get3dRangeNbr( ptTest, dRad, iBox))
|
|
continue ;
|
|
// tolgo il range precedente
|
|
SubtractIBox( iBox, iBoxPrev, ibR) ;
|
|
// ciclo su tutte le celle dei range rimanenti
|
|
for ( int l = 0 ; l < N_SUBIBOX ; ++ l) {
|
|
for ( int i = ibR[l].nXmin ; i <= ibR[l].nXmax ; ++ i) {
|
|
for ( int j = ibR[l].nYmin ; j <= ibR[l].nYmax ; ++ j) {
|
|
for ( int k = ibR[l].nZmin ; k <= ibR[l].nZmax ; ++ k) {
|
|
// ciclo sui punti della cella
|
|
IPNTI_UMMAP_CRANGE MMrange = m_MMap.equal_range( PointHash( i, j, k)) ;
|
|
for ( ; MMrange.first != MMrange.second ; ++ MMrange.first) {
|
|
// se distanza inferiore al minimo, aggiorno...
|
|
double dSqDist = SqDist( (*MMrange.first).second.first, ptTest) ;
|
|
// altro punto con la stessa minima distanza già trovata
|
|
if ( bFound && abs( dSqDist - dSqMinDist) < 2 * dMinDist * NEAR_TOL + NEAR_TOL * NEAR_TOL) {
|
|
// inserisco il punto nel vettore dei risultati
|
|
vnIds.push_back( (*MMrange.first).second.second) ;
|
|
}
|
|
// primo punto o punto con minima distanza più bassa
|
|
else if ( ! bFound || dSqDist < dSqMinDist) {
|
|
bFound = true ;
|
|
// aggiorno i minimi
|
|
dSqMinDist = dSqDist ;
|
|
dMinDist = sqrt( dSqMinDist) ;
|
|
// pulisco il vettore dei risultati ed inserisco questo
|
|
vnIds.clear() ;
|
|
vnIds.push_back( (*MMrange.first).second.second) ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
// se trovato, inutile continuare perchè ci si allontanerà
|
|
if ( bFound)
|
|
return true ;
|
|
// incremento il raggio di ricerca
|
|
dRad += dDelta ;
|
|
// salvo box di precedente ricerca
|
|
iBoxPrev = iBox ;
|
|
}
|
|
|
|
return bFound ;
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
PointGrid3d::FindNearest( const Point3d& ptTest, double dTol, int& nId) const
|
|
{
|
|
// determino il range di celle sui tre assi
|
|
IBox iBox ;
|
|
if ( ! Get3dRangeNbr( ptTest, dTol, iBox))
|
|
return false ;
|
|
// ciclo su tutte le celle del range
|
|
bool bFound = false ;
|
|
double dMinSqDist = dTol * dTol ;
|
|
for ( int i = iBox.nXmin ; i <= iBox.nXmax ; ++ i) {
|
|
for ( int j = iBox.nYmin ; j <= iBox.nYmax ; ++ j) {
|
|
for ( int k = iBox.nZmin ; k <= iBox.nZmax ; ++ k) {
|
|
IPNTI_UMMAP_CRANGE MMrange = m_MMap.equal_range( PointHash( i, j, k)) ;
|
|
for ( ; MMrange.first != MMrange.second ; ++ MMrange.first) {
|
|
double dSqDist = SqDist( (*MMrange.first).second.first, ptTest) ;
|
|
if ( dSqDist < dMinSqDist) {
|
|
bFound = true ;
|
|
dMinSqDist = dSqDist ;
|
|
nId = (*MMrange.first).second.second ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return bFound ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
PointGrid3d::First( int& nId) const
|
|
{
|
|
// se vuoto
|
|
if ( m_MMap.empty())
|
|
return false ;
|
|
// recupero il primo
|
|
nId = (*m_MMap.begin()).second.second ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
int
|
|
PointGrid3d::Get1dCellNbr( double dCoord) const
|
|
{
|
|
return static_cast<int>( floor( dCoord * m_dInvCellDim)) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
PointGrid3d::Get3dRangeNbr( const BBox3d& b3Test, IBox& iBox) const
|
|
{
|
|
// calcolo intersezione tra box
|
|
BBox3d b3Int ;
|
|
if ( ! m_BBox.FindIntersection( b3Test, b3Int))
|
|
return false ;
|
|
// ricavo gli indici sui tre assi degli estremi del box
|
|
iBox.nXmin = Get1dCellNbr( b3Int.GetMin().x) ;
|
|
iBox.nYmin = Get1dCellNbr( b3Int.GetMin().y) ;
|
|
iBox.nZmin = Get1dCellNbr( b3Int.GetMin().z) ;
|
|
iBox.nXmax = Get1dCellNbr( b3Int.GetMax().x) ;
|
|
iBox.nYmax = Get1dCellNbr( b3Int.GetMax().y) ;
|
|
iBox.nZmax = Get1dCellNbr( b3Int.GetMax().z) ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
PointGrid3d::Get3dRangeNbr( const Point3d& ptTest, double dTol, IBox& iBox) const
|
|
{
|
|
// calcolo intersezione tra box
|
|
BBox3d b3Int ;
|
|
if ( ! m_BBox.FindIntersection( BBox3d( ptTest, dTol), b3Int))
|
|
return false ;
|
|
// ricavo gli indici sui tre assi degli estremi del box
|
|
iBox.nXmin = Get1dCellNbr( b3Int.GetMin().x) ;
|
|
iBox.nYmin = Get1dCellNbr( b3Int.GetMin().y) ;
|
|
iBox.nZmin = Get1dCellNbr( b3Int.GetMin().z) ;
|
|
iBox.nXmax = Get1dCellNbr( b3Int.GetMax().x) ;
|
|
iBox.nYmax = Get1dCellNbr( b3Int.GetMax().y) ;
|
|
iBox.nZmax = Get1dCellNbr( b3Int.GetMax().z) ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
int
|
|
PointGrid3d::PointHash( int nX, int nY, int nZ) const
|
|
{
|
|
return ( nX * 73856093 ^ nY * 19349653 ^ nZ * 83492791) ;
|
|
}
|