Dario Sassi
49340d2629
- in CollisionAvoid aumentato MAX_MOVE per calcoli con travi (elevazione su attacchi/uscite).
869 lines
31 KiB
C++
869 lines
31 KiB
C++
//----------------------------------------------------------------------------
|
|
// EgalTech 2018-2024
|
|
//----------------------------------------------------------------------------
|
|
// File : CAvToolSurfTm.cpp Data : 07.06.24 Versione : 2.6f2
|
|
// Contenuto : Implementazione della classe CAvToolSurfTm.
|
|
//
|
|
//
|
|
//
|
|
// Modifiche : 27.04.18 DS Creazione modulo.
|
|
// 07.06.24 DS Con tolleranza lineare negativa non si controlla il punto medio.
|
|
//
|
|
//----------------------------------------------------------------------------
|
|
|
|
#include "stdafx.h"
|
|
#include "CAvToolTriangle.h"
|
|
#include "CAvToolSurfTm.h"
|
|
#include "DllMain.h"
|
|
#include "/EgtDev/Include/EGkDistPointLine.h"
|
|
#include "/EgtDev/Include/EGnStringUtils.h"
|
|
#include <thread>
|
|
#include <future>
|
|
|
|
using namespace std ;
|
|
|
|
//----------------------------------------------------------------------------
|
|
const int STEP_PE = 50 ;
|
|
const double MAX_MOVE = 20000 ;
|
|
|
|
//----------------------------------------------------------------------------
|
|
ICAvToolSurfTm*
|
|
CreateCAvToolSurfTm( void)
|
|
{
|
|
return static_cast<ICAvToolSurfTm*> ( new(nothrow) CAvToolSurfTm) ;
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
// CAvToolSurfTm
|
|
//----------------------------------------------------------------------------
|
|
CAvToolSurfTm::CAvToolSurfTm( void)
|
|
: m_frMove( false), m_Tool( false)
|
|
{
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CAvToolSurfTm::Clear( void)
|
|
{
|
|
// pulisco la lista dei puntatori a Stm
|
|
m_vSTM.clear() ;
|
|
// pulisco e inizializzo la prima posizione della lista delle basi per gli indici dei triangoli
|
|
m_vBaseInd.clear() ;
|
|
m_vBaseInd.emplace_back( 0) ;
|
|
// pulisco HashGrid 2d
|
|
m_HGrids.Clear() ;
|
|
// reset utensile
|
|
m_Tool.Clear() ;
|
|
// reset riferimento
|
|
m_frMove.Reset( false) ;
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CAvToolSurfTm::SetSurfTm( const ISurfTriMesh& Stm)
|
|
{
|
|
// pulisco la lista dei puntatori a Stm
|
|
m_vSTM.clear() ;
|
|
// pulisco e inizializzo la prima posizione della lista delle basi per gli indici dei triangoli
|
|
m_vBaseInd.clear() ;
|
|
m_vBaseInd.emplace_back( 0) ;
|
|
// pulisco HashGrid 2d
|
|
m_HGrids.Clear() ;
|
|
// non tocco l'utensile
|
|
// aggiungo la superficie
|
|
return AddSurfTm( Stm) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CAvToolSurfTm::AddSurfTm( const ISurfTriMesh& Stm)
|
|
{
|
|
// verifico validità superficie
|
|
const SurfTriMesh* pStm = GetBasicSurfTriMesh( &Stm) ;
|
|
if ( pStm == nullptr || ! pStm->IsValid())
|
|
return false ;
|
|
// inserisco superficie in lista
|
|
m_vSTM.emplace_back( pStm) ;
|
|
// inserisco base per indice triangoli prossima superficie
|
|
m_vBaseInd.emplace_back( m_vBaseInd.back() + pStm->GetTriangleSize()) ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CAvToolSurfTm::SetStdTool( double dH, double dR, double dCornR)
|
|
{
|
|
return m_Tool.SetStdTool( "", dH, dR, dCornR, 0, 0) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CAvToolSurfTm::SetAdvTool( double dH, double dR, double dTipH, double dTipR, double dCornR)
|
|
{
|
|
return m_Tool.SetAdvTool( "", dH, dR, dTipH, dTipR, dCornR, 0, 0) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CAvToolSurfTm::SetSawTool( double dH, double dR, double dThick, double dStemR, double dCornR)
|
|
{
|
|
return m_Tool.SetSawTool( "", dH, dR, dThick, dStemR, dCornR, 0) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CAvToolSurfTm::SetGenTool( const ICurveComposite* pToolOutline)
|
|
{
|
|
return m_Tool.SetGenTool( "", pToolOutline, 0) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CAvToolSurfTm::TestPosition( const Point3d& ptT, const Vector3d& vtDir, const Vector3d& vtMove,
|
|
double& dTotDist, Vector3d* pvtTriaN) const
|
|
{
|
|
// Se utensile non definito, errore
|
|
if ( m_Tool.GetType() == Tool::UNDEF)
|
|
return false ;
|
|
// Se direzioni non definite, errore
|
|
if ( vtDir.IsSmall() || vtMove.IsSmall())
|
|
return false ;
|
|
// Se riferimento di movimento già presente
|
|
if ( m_frMove.IsValid()) {
|
|
// Calcolo nuovo riferimento di movimento
|
|
Frame3d frMove ;
|
|
if ( ! AreSameOrOppositeVectorApprox( vtDir, vtMove))
|
|
frMove.Set( ORIG, vtMove, vtDir) ;
|
|
else
|
|
frMove.Set( ORIG, vtMove) ;
|
|
// Se riferimenti di movimento uguali, sfrutto HashGrid 2d
|
|
if ( AreSameFrame( frMove, m_frMove)) {
|
|
// Eseguo controllo
|
|
Point3d ptCurr = ptT ;
|
|
Vector3d vtTriaN ;
|
|
dTotDist = MyTestPositionHG( ptCurr, vtDir, vtTriaN) ;
|
|
if ( pvtTriaN != nullptr)
|
|
*pvtTriaN = vtTriaN ;
|
|
return ( dTotDist > - EPS_SMALL) ;
|
|
}
|
|
}
|
|
// Altrimenti eseguo controllo diretto
|
|
Point3d ptCurr = ptT ;
|
|
Vector3d vtTriaN ;
|
|
dTotDist = MyTestPosition( ptCurr, vtDir, vtMove, vtTriaN) ;
|
|
if ( pvtTriaN != nullptr)
|
|
*pvtTriaN = vtTriaN ;
|
|
return ( dTotDist > - EPS_SMALL) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CAvToolSurfTm::TestPositionAdv( const Point3d& ptT, const Vector3d& vtDir, const Vector3d& vtMove,
|
|
double& dTotDist, VCT3DVECTOR& vVtN) const
|
|
{
|
|
// Funzione per calcolo collisione tra utensile e superfici ;
|
|
// dToTDist è la distanza di traslazione del punto ptT lungo vtDir per evitare la collisione,
|
|
// vVtN è la normale del triangolo che genera collsione ( NB. Nel caso di più triangoli concorrenti,
|
|
// vengono restituite tutte le normali trovate)
|
|
|
|
// Inizializzazione parametri
|
|
dTotDist = 0 ;
|
|
vVtN.clear() ;
|
|
// Se utensile non definito, errore
|
|
if ( m_Tool.GetType() == Tool::UNDEF)
|
|
return false ;
|
|
// Se direzioni non definite, errore
|
|
if ( vtDir.IsSmall() || vtMove.IsSmall())
|
|
return false ;
|
|
// Se riferimento di movimento già presente
|
|
if ( m_frMove.IsValid()) {
|
|
// Calcolo nuovo riferimento di movimento
|
|
Frame3d frMove ;
|
|
if ( ! AreSameOrOppositeVectorApprox( vtDir, vtMove))
|
|
frMove.Set( ORIG, vtMove, vtDir) ;
|
|
else
|
|
frMove.Set( ORIG, vtMove) ;
|
|
// Se riferimenti di movimento uguali, sfrutto HashGrid 2d
|
|
if ( AreSameFrame( frMove, m_frMove)) {
|
|
// Eseguo controllo
|
|
Point3d ptCurr = ptT ;
|
|
dTotDist = MyTestPositionHGAdv( ptCurr, vtDir, vVtN) ;
|
|
return ( dTotDist > - EPS_SMALL) ;
|
|
}
|
|
}
|
|
// Altrimenti eseguo controllo diretto
|
|
Point3d ptCurr = ptT ;
|
|
dTotDist = MyTestPositionAdv( ptCurr, vtDir, vtMove, vVtN) ;
|
|
return ( dTotDist > - EPS_SMALL) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CAvToolSurfTm::TestSeries( PNTUVECTOR& vPntM, const Vector3d& vtDir, const Vector3d& vtMove, double dProgCoeff)
|
|
{
|
|
// Se utensile non definito, errore
|
|
if ( m_Tool.GetType() == Tool::UNDEF)
|
|
return false ;
|
|
// Se direzioni non definite, errore
|
|
if ( vtDir.IsSmall() || vtMove.IsSmall())
|
|
return false ;
|
|
// Se vettore vuoto, non devo fare alcunché
|
|
if ( vPntM.empty())
|
|
return true ;
|
|
// Calcolo nuovo riferimento di movimento
|
|
Frame3d frMove ;
|
|
if ( ! AreSameOrOppositeVectorApprox( vtDir, vtMove))
|
|
frMove.Set( ORIG, vtMove, vtDir) ;
|
|
else
|
|
frMove.Set( ORIG, vtMove) ;
|
|
// Se riferimento di movimento non presente o diverso dal calcolato
|
|
if ( ! m_frMove.IsValid() || ! AreSameFrame( frMove, m_frMove)) {
|
|
// Salvo nuovo riferimento
|
|
m_frMove = frMove ;
|
|
// Ricalcolo HashGrid
|
|
if ( ! PrepareHashGrid())
|
|
return false ;
|
|
}
|
|
// Determino il numero di punti dell'insieme
|
|
m_nTotPnt = int( vPntM.size()) ;
|
|
// Recupero il numero massimo di thread concorrenti
|
|
int nThreadMax = thread::hardware_concurrency() ;
|
|
bool bOk = true ;
|
|
// Se un solo thread o pochi punti
|
|
if ( nThreadMax <= 1 || m_nTotPnt < 500) {
|
|
m_nCurrPnt = 0 ;
|
|
bOk = TestSubSeries( -1, vPntM, vtDir, 0, m_nTotPnt - 1, dProgCoeff) ;
|
|
ProcessEvents( int( 100 * dProgCoeff), 0) ;
|
|
}
|
|
// altrimenti
|
|
else {
|
|
const int MAX_PARTS = 32 ;
|
|
INTINTVECTOR vFstLst( MAX_PARTS) ;
|
|
// calcolo le parti del vettore
|
|
int nPartCnt = min( nThreadMax, MAX_PARTS) ;
|
|
int nPartDim = m_nTotPnt / nPartCnt + 1 ;
|
|
for ( int i = 0 ; i < nPartCnt ; ++ i) {
|
|
vFstLst[i].first = i * nPartDim ;
|
|
vFstLst[i].second = min( ( i + 1) * nPartDim, m_nTotPnt) - 1 ;
|
|
}
|
|
// processo le parti
|
|
m_nCurrPnt = 0 ;
|
|
m_bBreak = false ;
|
|
future<bool> vRes[MAX_PARTS] ;
|
|
for ( int i = 0 ; i < nPartCnt ; ++ i)
|
|
vRes[i] = async( launch::async, &CAvToolSurfTm::TestSubSeries, this, i, ref( vPntM), cref( vtDir), vFstLst[i].first, vFstLst[i].second, dProgCoeff) ;
|
|
// attendo i risultati
|
|
int nFin = 0 ;
|
|
int nNextPE = 0 ;
|
|
while ( nFin < nPartCnt) {
|
|
for ( int i = 0 ; i < nPartCnt ; ++ i) {
|
|
if ( vRes[i].valid() && vRes[i].wait_for( chrono::nanoseconds{ 1}) == future_status::ready) {
|
|
bOk = vRes[i].get() && bOk ;
|
|
++ nFin ;
|
|
}
|
|
}
|
|
if ( m_nCurrPnt > nNextPE) {
|
|
int nRes = ProcessEvents( int( m_nCurrPnt * 100. / m_nTotPnt * dProgCoeff), 10) ;
|
|
nNextPE += STEP_PE ;
|
|
if ( nRes == 1)
|
|
m_bBreak = true ;
|
|
}
|
|
}
|
|
ProcessEvents( int( 100 * dProgCoeff), 0) ;
|
|
}
|
|
return bOk ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CAvToolSurfTm::TestSubSeries( int nId, PNTUVECTOR& vPntM, const Vector3d& vtDir, int nFirst, int nLast, double dProgCoeff)
|
|
{
|
|
// Se vettore vuoto, non devo fare alcunché
|
|
if ( vPntM.empty())
|
|
return true ;
|
|
// Ciclo sui punti da verificare
|
|
for ( int i = nFirst ; i <= nLast ; ++ i) {
|
|
// verifico il punto
|
|
Vector3d vtTriaN ;
|
|
double dMove = MyTestPositionHG( vPntM[i].first, vtDir, vtTriaN) ;
|
|
vPntM[i].second = dMove ;
|
|
if ( dMove < - EPS_SMALL)
|
|
return false ;
|
|
++ m_nCurrPnt ;
|
|
// se singolo thread
|
|
if ( nId == -1) {
|
|
// gestione eventi (ogni STEP_PE punti)
|
|
if (( m_nCurrPnt % STEP_PE) == 0) {
|
|
int nRes = ProcessEvents( int( m_nCurrPnt * 100. / m_nTotPnt * dProgCoeff), 0) ;
|
|
if ( nRes == 1)
|
|
return false ;
|
|
}
|
|
}
|
|
// altrimenti multithread
|
|
else {
|
|
if ( m_bBreak)
|
|
return false ;
|
|
}
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CAvToolSurfTm::TestPath( PNTULIST& lPntM, const Vector3d& vtDir, const Vector3d& vtMove, double dLinTol, double dProgCoeff)
|
|
{
|
|
// Se utensile non definito, errore
|
|
if ( m_Tool.GetType() == Tool::UNDEF)
|
|
return false ;
|
|
// Se direzioni non definite, errore
|
|
if ( vtDir.IsSmall() || vtMove.IsSmall())
|
|
return false ;
|
|
// Se lista vuota, non devo fare alcunché
|
|
if ( lPntM.empty())
|
|
return true ;
|
|
// Controllo la tolleranza lineare (se negativa non vanno fatti controlli sui punti medi)
|
|
if ( dLinTol > -EPS_ZERO)
|
|
dLinTol = max( dLinTol, EPS_SMALL) ;
|
|
else
|
|
dLinTol = -1 ;
|
|
// Calcolo nuovo riferimento di movimento
|
|
Frame3d frMove ;
|
|
if ( ! AreSameOrOppositeVectorApprox( vtDir, vtMove))
|
|
frMove.Set( ORIG, vtMove, vtDir) ;
|
|
else
|
|
frMove.Set( ORIG, vtMove) ;
|
|
// Se riferimento di movimento non presente o diverso dal calcolato
|
|
if ( ! m_frMove.IsValid() || ! AreSameFrame( frMove, m_frMove)) {
|
|
// Salvo nuovo riferimento
|
|
m_frMove = frMove ;
|
|
// Ricalcolo HashGrid
|
|
if ( ! PrepareHashGrid())
|
|
return false ;
|
|
}
|
|
// Determino il numero di punti del path
|
|
m_nTotPnt = int( lPntM.size()) ;
|
|
// Recupero il numero massimo di thread concorrenti
|
|
int nThreadMax = thread::hardware_concurrency() ;
|
|
bool bOk = true ;
|
|
// Se un solo thread o pochi punti
|
|
if ( nThreadMax <= 1 || m_nTotPnt < 500) {
|
|
m_nCurrPnt = 0 ;
|
|
bOk = TestSubPath( -1, lPntM, vtDir, dLinTol, dProgCoeff) ;
|
|
ProcessEvents( int( 100 * dProgCoeff), 0) ;
|
|
}
|
|
// altrimenti
|
|
else {
|
|
const int MAX_PARTS = 32 ;
|
|
PNTULIST vlPntM[MAX_PARTS] ;
|
|
// divido la lista in parti
|
|
int nPartCnt = min( nThreadMax, MAX_PARTS) ;
|
|
int nPartDim = m_nTotPnt / nPartCnt ;
|
|
for ( int i = nPartCnt - 1 ; i > 0 ; -- i) {
|
|
auto itSplit = prev( lPntM.end(), nPartDim) ;
|
|
vlPntM[i].splice( vlPntM[i].end(), lPntM, itSplit, lPntM.end()) ;
|
|
vlPntM[i].push_front( lPntM.back()) ;
|
|
}
|
|
vlPntM[0].splice( vlPntM[0].end(), lPntM) ;
|
|
// processo le parti
|
|
m_nCurrPnt = 0 ;
|
|
m_bBreak = false ;
|
|
future<bool> vRes[MAX_PARTS] ;
|
|
for ( int i = 0 ; i < nPartCnt ; ++ i)
|
|
vRes[i] = async( launch::async, &CAvToolSurfTm::TestSubPath, this, i, ref( vlPntM[i]), cref( vtDir), dLinTol, dProgCoeff) ;
|
|
// attendo i risultati
|
|
int nFin = 0 ;
|
|
int nNextPE = 0 ;
|
|
while ( nFin < nPartCnt) {
|
|
for ( int i = 0 ; i < nPartCnt ; ++ i) {
|
|
if ( vRes[i].valid() && vRes[i].wait_for( chrono::nanoseconds{ 1}) == future_status::ready) {
|
|
bOk = vRes[i].get() && bOk ;
|
|
++ nFin ;
|
|
}
|
|
}
|
|
if ( m_nCurrPnt > nNextPE) {
|
|
int nRes = ProcessEvents( int( m_nCurrPnt * 100. / m_nTotPnt * dProgCoeff), 10) ;
|
|
nNextPE += STEP_PE ;
|
|
if ( nRes == 1)
|
|
m_bBreak = true ;
|
|
}
|
|
}
|
|
// unisco le liste risultati
|
|
for ( int i = 0 ; i < nPartCnt ; ++ i) {
|
|
if ( i > 0)
|
|
lPntM.pop_back() ;
|
|
lPntM.splice( lPntM.end(), vlPntM[i]) ;
|
|
}
|
|
ProcessEvents( int( 100 * dProgCoeff), 0) ;
|
|
}
|
|
return bOk ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CAvToolSurfTm::TestSeriesAdv( PNTUVVECTVECTOR& vPntM, const Vector3d& vtDir, const Vector3d& vtMove, double dProgCoeff)
|
|
{
|
|
// NB. la posizione del punto non viene modificata :
|
|
// get<0> vPntM[i] è il punto su cui viene posizionata la testa dell'utensile ( const)
|
|
// get<1> vPntM[i] è il parametro di traslazione del punto lungo vtDir per evitare collisioni con i triangoli
|
|
// get<2> vPntM[i] è un vettore di Vector3d contenente tutte le normali di tangenza ( a meno di 10 * EPS_SMALL)
|
|
|
|
// Se utensile non definito, errore
|
|
if ( m_Tool.GetType() == Tool::UNDEF)
|
|
return false ;
|
|
// Se direzioni non definite, errore
|
|
if ( vtDir.IsSmall() || vtMove.IsSmall())
|
|
return false ;
|
|
// Se vettore vuoto, non devo fare alcunché
|
|
if ( vPntM.empty())
|
|
return true ;
|
|
// Calcolo nuovo riferimento di movimento
|
|
Frame3d frMove ;
|
|
if ( ! AreSameOrOppositeVectorApprox( vtDir, vtMove))
|
|
frMove.Set( ORIG, vtMove, vtDir) ;
|
|
else
|
|
frMove.Set( ORIG, vtMove) ;
|
|
// Se riferimento di movimento non presente o diverso dal calcolato
|
|
if ( ! m_frMove.IsValid() || ! AreSameFrame( frMove, m_frMove)) {
|
|
// Salvo nuovo riferimento
|
|
m_frMove = frMove ;
|
|
// Ricalcolo HashGrid
|
|
if ( ! PrepareHashGrid())
|
|
return false ;
|
|
}
|
|
// Determino il numero di punti del path
|
|
m_nTotPnt = int( vPntM.size()) ;
|
|
// Recupero il numero massimo di thread concorrenti
|
|
int nThreadMax = thread::hardware_concurrency() ;
|
|
bool bOk = true ;
|
|
// Se un solo thread o pochi punti
|
|
if ( nThreadMax <= 1 || m_nTotPnt < 500) {
|
|
m_nCurrPnt = 0 ;
|
|
bOk = TestSubSeriesAdv( -1, vPntM, vtDir, 0, m_nTotPnt - 1, dProgCoeff) ;
|
|
ProcessEvents( int( 100 * dProgCoeff), 0) ;
|
|
}
|
|
// altrimenti
|
|
else {
|
|
const int MAX_PARTS = 32 ;
|
|
INTINTVECTOR vFstLst( MAX_PARTS) ;
|
|
// calcolo le parti del vettore
|
|
int nPartCnt = min( nThreadMax, MAX_PARTS) ;
|
|
int nPartDim = m_nTotPnt / nPartCnt + 1 ;
|
|
for ( int i = 0 ; i < nPartCnt ; ++ i) {
|
|
vFstLst[i].first = i * nPartDim ;
|
|
vFstLst[i].second = min( ( i + 1) * nPartDim, m_nTotPnt) - 1 ;
|
|
}
|
|
// processo le parti
|
|
m_nCurrPnt = 0 ;
|
|
m_bBreak = false ;
|
|
future<bool> vRes[MAX_PARTS] ;
|
|
for ( int i = 0 ; i < nPartCnt ; ++ i)
|
|
vRes[i] = async( launch::async, &CAvToolSurfTm::TestSubSeriesAdv, this, i, ref( vPntM), cref( vtDir), vFstLst[i].first, vFstLst[i].second, dProgCoeff) ;
|
|
// attendo i risultati
|
|
int nFin = 0 ;
|
|
int nNextPE = 0 ;
|
|
while ( nFin < nPartCnt) {
|
|
for ( int i = 0 ; i < nPartCnt ; ++ i) {
|
|
if ( vRes[i].valid() && vRes[i].wait_for( chrono::nanoseconds{ 1}) == future_status::ready) {
|
|
bOk = vRes[i].get() && bOk ;
|
|
++ nFin ;
|
|
}
|
|
}
|
|
if ( m_nCurrPnt > nNextPE) {
|
|
int nRes = ProcessEvents( int( m_nCurrPnt * 100. / m_nTotPnt * dProgCoeff), 10) ;
|
|
nNextPE += STEP_PE ;
|
|
if ( nRes == 1)
|
|
m_bBreak = true ;
|
|
}
|
|
}
|
|
ProcessEvents( int( 100 * dProgCoeff), 0) ;
|
|
}
|
|
return bOk ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CAvToolSurfTm::TestSubPath( int nId, PNTULIST& lPntM, const Vector3d& vtDir, double dLinTol, double dProgCoeff)
|
|
{
|
|
// Se lista vuota, non devo fare alcunché
|
|
if ( lPntM.empty())
|
|
return true ;
|
|
// Ciclo sui punti
|
|
Point3d ptPrev, ptCurr ;
|
|
auto itPntMPrev = lPntM.end() ;
|
|
auto itPntMCurr = lPntM.begin() ;
|
|
while ( itPntMCurr != lPntM.end()) {
|
|
// verifico il punto
|
|
ptCurr = itPntMCurr->first ;
|
|
Vector3d vtTriaN ;
|
|
double dMove = MyTestPositionHG( itPntMCurr->first, vtDir, vtTriaN) ;
|
|
itPntMCurr->second = dMove ;
|
|
if ( dMove < - EPS_SMALL)
|
|
return false ;
|
|
// se esiste il punto precedente e richiesto devo verificare il medio
|
|
if ( itPntMPrev != lPntM.end() && dLinTol > 0) {
|
|
MyTestMidPointHG( lPntM, itPntMPrev, itPntMCurr, ptPrev, ptCurr, vtDir, dLinTol, 1) ;
|
|
}
|
|
// passo al successivo
|
|
ptPrev = ptCurr ;
|
|
itPntMPrev = itPntMCurr ;
|
|
++ itPntMCurr ;
|
|
++ m_nCurrPnt ;
|
|
// se singolo thread
|
|
if ( nId == -1) {
|
|
// gestione eventi (ogni STEP_PE punti)
|
|
if (( m_nCurrPnt % STEP_PE) == 0) {
|
|
int nRes = ProcessEvents( int( m_nCurrPnt * 100. / m_nTotPnt * dProgCoeff), 0) ;
|
|
if ( nRes == 1)
|
|
return false ;
|
|
}
|
|
}
|
|
// altrimenti multithread
|
|
else {
|
|
if ( m_bBreak)
|
|
return false ;
|
|
}
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CAvToolSurfTm::TestSubSeriesAdv( int nId, PNTUVVECTVECTOR& vPntM, const Vector3d& vtDir, int nFirst, int nLast, double dProgCoeff)
|
|
{
|
|
// Se vettore vuoto, non devo fare alcunché
|
|
if ( vPntM.empty())
|
|
return true ;
|
|
// Ciclo sui punti da verificare
|
|
for ( int i = nFirst ; i <= nLast ; ++ i) {
|
|
// verifico il punto
|
|
Point3d ptCurr = get<0>( vPntM[i]) ;
|
|
get<1>( vPntM[i]) = MyTestPositionHGAdv( ptCurr, vtDir, get<2>( vPntM[i])) ;
|
|
if ( get<1>( vPntM[i]) < - EPS_SMALL)
|
|
return false ;
|
|
++ m_nCurrPnt ;
|
|
// se singolo thread
|
|
if ( nId == -1) {
|
|
// gestione eventi (ogni STEP_PE punti)
|
|
if (( m_nCurrPnt % STEP_PE) == 0) {
|
|
int nRes = ProcessEvents( int( m_nCurrPnt * 100. / m_nTotPnt * dProgCoeff), 0) ;
|
|
if ( nRes == 1)
|
|
return false ;
|
|
}
|
|
}
|
|
// altrimenti multithread
|
|
else {
|
|
if ( m_bBreak)
|
|
return false ;
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CAvToolSurfTm::MyTestMidPointHG( PNTULIST& lPntM, const PNTULIST::iterator& itPntMPrev, const PNTULIST::iterator& itPntMCurr,
|
|
const Point3d& ptPrev, const Point3d& ptCurr, const Vector3d& vtDir, double dLinTol, int nLev) const
|
|
{
|
|
// se superato limite di ricursione, esco
|
|
const int MAX_LEV = 10 ;
|
|
if ( nLev > MAX_LEV)
|
|
return true ;
|
|
// determino il punto medio tra gli originali
|
|
Point3d ptMid = Media( ptPrev, ptCurr, 0.5) ;
|
|
// ne effettuo la correzione per evitare la collisione
|
|
Point3d ptNewMid = ptMid ;
|
|
Vector3d vtTriaN ;
|
|
double dMidMove = MyTestPositionHG( ptNewMid, vtDir, vtTriaN) ;
|
|
if ( dMidMove < - EPS_SMALL)
|
|
return false ;
|
|
// massima distanza ammissibile
|
|
double dMaxSqDist = max( m_Tool.GetRefRadius() * m_Tool.GetRefRadius(), 0.5 * 0.5) ;
|
|
// se punto medio oltre tolleranza dalla linea tra i punti includenti o linea troppo lunga, devo aggiungerlo
|
|
if ( abs(( Media( itPntMPrev->first, itPntMCurr->first, 0.5) - ptNewMid) * m_frMove.VersZ()) > 0.5 * dLinTol ||
|
|
SqDist( itPntMPrev->first, itPntMCurr->first) > dMaxSqDist) {
|
|
// aggiungo
|
|
lPntM.emplace( itPntMCurr, ptNewMid, dMidMove) ;
|
|
auto itPntMMid = itPntMCurr ;
|
|
-- itPntMMid ;
|
|
// verifico intervallo precedente
|
|
MyTestMidPointHG( lPntM, itPntMPrev, itPntMMid, ptPrev, ptMid, vtDir, dLinTol, nLev + 1) ;
|
|
// verifico intervallo successivo
|
|
MyTestMidPointHG( lPntM, itPntMMid, itPntMCurr, ptMid, ptCurr, vtDir, dLinTol, nLev + 1) ;
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
double
|
|
CAvToolSurfTm::MyTestPosition( Point3d& ptT, const Vector3d& vtDir, const Vector3d& vtMove, Vector3d& vtTriaN) const
|
|
{
|
|
// box dell'utensile con suo movimento
|
|
BBox3d b3Tool ;
|
|
// utensile
|
|
b3Tool.Add( ptT) ;
|
|
b3Tool.Add( ptT - vtDir * m_Tool.GetHeigth()) ;
|
|
if ( vtDir.IsX())
|
|
b3Tool.Expand( 0, m_Tool.GetRadius(), m_Tool.GetRadius()) ;
|
|
else if ( vtDir.IsY())
|
|
b3Tool.Expand( m_Tool.GetRadius(), 0, m_Tool.GetRadius()) ;
|
|
else if ( vtDir.IsZ())
|
|
b3Tool.Expand( m_Tool.GetRadius(), m_Tool.GetRadius(), 0) ;
|
|
else {
|
|
double dExpandX = m_Tool.GetRadius() * sqrt( 1 - vtDir.x * vtDir.x) ;
|
|
double dExpandY = m_Tool.GetRadius() * sqrt( 1 - vtDir.y * vtDir.y) ;
|
|
double dExpandZ = m_Tool.GetRadius() * sqrt( 1 - vtDir.z * vtDir.z) ;
|
|
b3Tool.Expand( dExpandX, dExpandY, dExpandZ) ;
|
|
}
|
|
// aggiungo movimento
|
|
BBox3d b3Moved = b3Tool ;
|
|
b3Moved.Translate( MAX_MOVE * vtMove) ;
|
|
b3Tool.Add( b3Moved) ;
|
|
|
|
// determino movimento minimo per evitare collisione con superfici
|
|
double dTotDist = 0 ;
|
|
vtTriaN = V_NULL ;
|
|
for ( auto pStm : m_vSTM) {
|
|
INTVECTOR vTria ;
|
|
if ( pStm->GetAllTriaOverlapBox( b3Tool, vTria)) {
|
|
for ( int nTria : vTria) {
|
|
Triangle3d Tria ;
|
|
if ( pStm->GetTriangle( nTria, Tria)) {
|
|
double dDist = CAvToolTriangle( m_Tool, ptT, vtDir, Tria, vtMove) ;
|
|
if ( dDist < - EPS_SMALL)
|
|
return -1 ;
|
|
if ( dDist > EPS_SMALL) {
|
|
dTotDist += dDist ;
|
|
ptT += dDist * vtMove ;
|
|
vtTriaN = Tria.GetN() ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return dTotDist ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
double
|
|
CAvToolSurfTm::MyTestPositionAdv( Point3d& ptT, const Vector3d& vtDir, const Vector3d& vtMove, VCT3DVECTOR& vVtTriaN) const
|
|
{
|
|
// box dell'utensile con suo movimento
|
|
BBox3d b3Tool ;
|
|
// utensile
|
|
b3Tool.Add( ptT) ;
|
|
b3Tool.Add( ptT - vtDir * m_Tool.GetHeigth()) ;
|
|
if ( vtDir.IsX())
|
|
b3Tool.Expand( 0, m_Tool.GetRadius(), m_Tool.GetRadius()) ;
|
|
else if ( vtDir.IsY())
|
|
b3Tool.Expand( m_Tool.GetRadius(), 0, m_Tool.GetRadius()) ;
|
|
else if ( vtDir.IsZ())
|
|
b3Tool.Expand( m_Tool.GetRadius(), m_Tool.GetRadius(), 0) ;
|
|
else {
|
|
double dExpandX = m_Tool.GetRadius() * sqrt( 1 - vtDir.x * vtDir.x) ;
|
|
double dExpandY = m_Tool.GetRadius() * sqrt( 1 - vtDir.y * vtDir.y) ;
|
|
double dExpandZ = m_Tool.GetRadius() * sqrt( 1 - vtDir.z * vtDir.z) ;
|
|
b3Tool.Expand( dExpandX, dExpandY, dExpandZ) ;
|
|
}
|
|
// aggiungo movimento
|
|
BBox3d b3Moved = b3Tool ;
|
|
b3Moved.Translate( MAX_MOVE * vtMove) ;
|
|
b3Tool.Add( b3Moved) ;
|
|
|
|
// determino movimento minimo per evitare collisione con superfici
|
|
double dTotDist = 0 ;
|
|
vVtTriaN.clear() ;
|
|
for ( auto pStm : m_vSTM) {
|
|
INTVECTOR vTria ;
|
|
if ( pStm->GetAllTriaOverlapBox( b3Tool, vTria)) {
|
|
for ( int nTria : vTria) {
|
|
Triangle3d Tria ;
|
|
if ( pStm->GetTriangle( nTria, Tria)) {
|
|
double dDist = CAvToolTriangle( m_Tool, ptT, vtDir, Tria, vtMove) ;
|
|
if ( dDist < - EPS_SMALL)
|
|
return -1 ;
|
|
// se devo traslare il punto, c'è collisione
|
|
if ( dDist > EPS_SMALL) {
|
|
if ( dDist > 10 * EPS_SMALL) {
|
|
vVtTriaN.clear() ;
|
|
dTotDist += dDist ;
|
|
ptT += ( dDist - 5 * EPS_SMALL) * m_frMove.VersZ() ;
|
|
}
|
|
vVtTriaN.push_back( Tria.GetN()) ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return dTotDist ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
double
|
|
CAvToolSurfTm::MyTestPositionHG( Point3d& ptT, const Vector3d& vtDir, Vector3d& vtTriaN) const
|
|
{
|
|
// calcolo box utensile nel riferimento di movimento
|
|
BBox3d b3Tool ;
|
|
Point3d ptTL = ptT ; ptTL.ToLoc( m_frMove) ;
|
|
Vector3d vtDirL = vtDir ; vtDirL.ToLoc( m_frMove) ;
|
|
b3Tool.Add( ptTL) ;
|
|
b3Tool.Add( ptTL - vtDirL * m_Tool.GetHeigth()) ;
|
|
if ( vtDirL.IsX())
|
|
b3Tool.Expand( 0, m_Tool.GetRadius(), m_Tool.GetRadius()) ;
|
|
else if ( vtDirL.IsY())
|
|
b3Tool.Expand( m_Tool.GetRadius(), 0, m_Tool.GetRadius()) ;
|
|
else if ( vtDirL.IsZ())
|
|
b3Tool.Expand( m_Tool.GetRadius(), m_Tool.GetRadius(), 0) ;
|
|
else {
|
|
double dExpandX = m_Tool.GetRadius() * sqrt( 1 - vtDirL.x * vtDirL.x) ;
|
|
double dExpandY = m_Tool.GetRadius() * sqrt( 1 - vtDirL.y * vtDirL.y) ;
|
|
double dExpandZ = m_Tool.GetRadius() * sqrt( 1 - vtDirL.z * vtDirL.z) ;
|
|
b3Tool.Expand( dExpandX, dExpandY, dExpandZ) ;
|
|
}
|
|
|
|
// ciclo sui triangoli che intersecano box in 2d
|
|
double dTotDist = 0 ;
|
|
vtTriaN = V_NULL ;
|
|
INTVECTOR vnIds ;
|
|
if ( m_HGrids.Find( b3Tool, vnIds)) {
|
|
for ( int i = 0 ; i < int( vnIds.size()) ; ++ i) {
|
|
int nInd = vnIds[i] ;
|
|
int nSurf = GetSurfInd( nInd) ;
|
|
if ( nSurf == -1)
|
|
return -1 ;
|
|
int nT = nInd - m_vBaseInd[nSurf] ;
|
|
Triangle3d Tria ;
|
|
if ( ! m_vSTM[nSurf]->GetTriangle( nT, Tria))
|
|
return -1 ;
|
|
double dDist = CAvToolTriangle( m_Tool, ptT, vtDir, Tria, m_frMove.VersZ()) ;
|
|
if ( dDist > EPS_SMALL) {
|
|
dTotDist += dDist ;
|
|
ptT += dDist * m_frMove.VersZ() ;
|
|
vtTriaN = Tria.GetN() ;
|
|
}
|
|
else if ( dDist < -EPS_SMALL)
|
|
return -1 ;
|
|
}
|
|
}
|
|
|
|
return dTotDist ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
double
|
|
CAvToolSurfTm::MyTestPositionHGAdv( Point3d& ptT, const Vector3d& vtDir, VCT3DVECTOR& vVtTriaN) const
|
|
{
|
|
// calcolo box utensile nel riferimento di movimento
|
|
BBox3d b3Tool ;
|
|
Point3d ptTL = ptT ; ptTL.ToLoc( m_frMove) ;
|
|
Vector3d vtDirL = vtDir ; vtDirL.ToLoc( m_frMove) ;
|
|
b3Tool.Add( ptTL) ;
|
|
b3Tool.Add( ptTL - vtDirL * m_Tool.GetHeigth()) ;
|
|
if ( vtDirL.IsX())
|
|
b3Tool.Expand( 0, m_Tool.GetRadius(), m_Tool.GetRadius()) ;
|
|
else if ( vtDirL.IsY())
|
|
b3Tool.Expand( m_Tool.GetRadius(), 0, m_Tool.GetRadius()) ;
|
|
else if ( vtDirL.IsZ())
|
|
b3Tool.Expand( m_Tool.GetRadius(), m_Tool.GetRadius(), 0) ;
|
|
else {
|
|
double dExpandX = m_Tool.GetRadius() * sqrt( 1 - vtDirL.x * vtDirL.x) ;
|
|
double dExpandY = m_Tool.GetRadius() * sqrt( 1 - vtDirL.y * vtDirL.y) ;
|
|
double dExpandZ = m_Tool.GetRadius() * sqrt( 1 - vtDirL.z * vtDirL.z) ;
|
|
b3Tool.Expand( dExpandX, dExpandY, dExpandZ) ;
|
|
}
|
|
|
|
// ciclo sui triangoli che intersecano box in 2d
|
|
double dTotDist = 0. ;
|
|
INTVECTOR vnIds ;
|
|
if ( m_HGrids.Find( b3Tool, vnIds)) {
|
|
for ( int i = 0 ; i < int( vnIds.size()) ; ++ i) {
|
|
// recupero la superficie
|
|
int nInd = vnIds[i] ;
|
|
int nSurf = GetSurfInd( nInd) ;
|
|
if ( nSurf == -1)
|
|
return -1 ;
|
|
// recupero il triangolo
|
|
int nT = nInd - m_vBaseInd[nSurf] ;
|
|
Triangle3d Tria ;
|
|
if ( ! m_vSTM[nSurf]->GetTriangle( nT, Tria))
|
|
return -1 ;
|
|
// calcolo della collisione
|
|
double dDist = CAvToolTriangle( m_Tool, ptT, vtDir, Tria, m_frMove.VersZ()) ;
|
|
if ( dDist < - EPS_SMALL)
|
|
return -1 ;
|
|
// se devo traslare il punto, c'è collisione
|
|
if ( dDist > EPS_SMALL) {
|
|
if ( dDist > 10 * EPS_SMALL) {
|
|
vVtTriaN.clear() ;
|
|
dTotDist += dDist ;
|
|
ptT += ( dDist - 5 * EPS_SMALL) * m_frMove.VersZ() ;
|
|
}
|
|
vVtTriaN.push_back( Tria.GetN()) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
return dTotDist ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CAvToolSurfTm::PrepareHashGrid( void)
|
|
{
|
|
// pulisco HashGrid 2d
|
|
m_HGrids.Clear() ;
|
|
// verifico esistenza superfici
|
|
if ( m_vSTM.empty() || m_vBaseInd.size() < m_vSTM.size() + 1)
|
|
return false ;
|
|
// creo HashGrid 2d
|
|
const int LIM_HG_TRIA = 256 ;
|
|
m_HGrids.SetActivationGrid( m_vBaseInd.back() > LIM_HG_TRIA) ;
|
|
// riempio HashGrid
|
|
for ( int i = 0 ; i < int( m_vSTM.size()) ; ++ i) {
|
|
Triangle3d Tria ;
|
|
int nT = m_vSTM[i]->GetFirstTriangle( Tria) ;
|
|
while ( nT != SVT_NULL) {
|
|
// calcolo il BBox del triangolo nel riferimento scelto
|
|
Tria.ToLoc( m_frMove) ;
|
|
BBox3d b3Tria ;
|
|
Tria.GetLocalBBox( b3Tria) ;
|
|
// inserisco nella griglia
|
|
if ( ! m_HGrids.Add( m_vBaseInd[i] + nT, b3Tria))
|
|
return false ;
|
|
// passo al prossimo triangolo
|
|
nT = m_vSTM[i]->GetNextTriangle( nT, Tria) ;
|
|
}
|
|
}
|
|
// aggiorno
|
|
return m_HGrids.Update() ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
int
|
|
CAvToolSurfTm::GetSurfInd( int nT) const
|
|
{
|
|
// verifico la presenza di almeno un intervallo
|
|
if ( m_vBaseInd.size() < 2)
|
|
return -1 ;
|
|
// ricerca binaria dell'intervallo contenente la posizione del triangolo
|
|
int nS = 0 ;
|
|
int nE = int( m_vBaseInd.size()) - 1 ;
|
|
while ( true) {
|
|
if ( nT < m_vBaseInd[nS] || nT >= m_vBaseInd[nE])
|
|
return -1 ;
|
|
if ( nE - nS == 1)
|
|
return nS ;
|
|
int nM = ( nS + nE) / 2 ;
|
|
if ( nT == m_vBaseInd[nM])
|
|
return nM ;
|
|
if ( nT < m_vBaseInd[nM])
|
|
nE = nM ;
|
|
else
|
|
nS = nM ;
|
|
}
|
|
return -1 ;
|
|
}
|