DarioS
2d8c815032
- a BBox3d aggiunto 2° metodo Overlaps con box aventi diverso orientamento - nelle funzioni di Collision Detection migliorato controllo non interne - nele funzioni Avoid di Zmap si utilizza confronto box con diverso orientamento.
398 lines
14 KiB
C++
398 lines
14 KiB
C++
//----------------------------------------------------------------------------
|
|
// EgalTech 2018-2018
|
|
//----------------------------------------------------------------------------
|
|
// File : CAToolSurfTm.cpp Data : 08.05.18 Versione : 1.9e2
|
|
// Contenuto : Implementazione della classe CAToolSurfTm.
|
|
//
|
|
//
|
|
//
|
|
// Modifiche : 27.04.18 DS Creazione modulo.
|
|
//
|
|
//
|
|
//----------------------------------------------------------------------------
|
|
|
|
#include "stdafx.h"
|
|
#include "CAvToolTriangle.h"
|
|
#include "CAvToolSurfTm.h"
|
|
#include "DistPointLine.h"
|
|
#include "DllMain.h"
|
|
#include "/EgtDev/Include/EGnStringUtils.h"
|
|
#include <thread>
|
|
#include <future>
|
|
|
|
using namespace std ;
|
|
|
|
//----------------------------------------------------------------------------
|
|
const int STEP_PE = 50 ;
|
|
|
|
//----------------------------------------------------------------------------
|
|
ICAvToolSurfTm*
|
|
CreateCAvToolSurfTm( void)
|
|
{
|
|
return static_cast<ICAvToolSurfTm*> ( new(nothrow) CAvToolSurfTm) ;
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
// CAvToolSurfTm
|
|
//----------------------------------------------------------------------------
|
|
CAvToolSurfTm::CAvToolSurfTm( void)
|
|
: m_Tool( false)
|
|
{
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
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) ;
|
|
// 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)
|
|
{
|
|
// Se utensile non definito, errore
|
|
if ( m_Tool.GetType() == Tool::UNDEF)
|
|
return false ;
|
|
// Imposto il riferimento di movimento
|
|
if ( ! AreSameOrOppositeVectorApprox( vtDir, vtMove))
|
|
m_frMove.Set( ORIG, vtMove, vtDir) ;
|
|
else
|
|
m_frMove.Set( ORIG, vtMove) ;
|
|
// Eseguo controllo
|
|
Point3d ptCurr = ptT ;
|
|
dTotDist = MyTestPosition( ptCurr, vtDir) ;
|
|
return ( dTotDist > - EPS_SMALL) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CAvToolSurfTm::TestPath( PNTULIST& lPntM, const Vector3d& vtDir, const Vector3d& vtMove, double dLinTol)
|
|
{
|
|
// Se utensile non definito, errore
|
|
if ( m_Tool.GetType() == Tool::UNDEF)
|
|
return false ;
|
|
// Se lista vuota, non devo fare alcunché
|
|
if ( lPntM.empty())
|
|
return true ;
|
|
// Imposto il riferimento di movimento
|
|
if ( ! AreSameOrOppositeVectorApprox( vtDir, vtMove))
|
|
m_frMove.Set( ORIG, vtMove, vtDir) ;
|
|
else
|
|
m_frMove.Set( ORIG, vtMove) ;
|
|
// Predispongo Hash Grid
|
|
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) {
|
|
bOk = TestSubPath( -1, lPntM, vtDir, dLinTol) ;
|
|
ProcessEvents( 100, 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) ;
|
|
// 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), 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( 100, 0) ;
|
|
}
|
|
// pulisco HashGrid 2d
|
|
m_HGrids.Clear() ;
|
|
return bOk ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CAvToolSurfTm::TestSubPath( int nId, PNTULIST& lPntM, const Vector3d& vtDir, double dLinTol)
|
|
{
|
|
// 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 ;
|
|
itPntMCurr->second = MyTestPositionHG( itPntMCurr->first, vtDir) ;
|
|
if ( itPntMCurr->second < - EPS_SMALL)
|
|
return false ;
|
|
// se esiste il punto precedente devo verificare il medio
|
|
if ( itPntMPrev != lPntM.end()) {
|
|
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), 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)
|
|
{
|
|
// 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 ;
|
|
double dMidMove = MyTestPositionHG( ptNewMid, vtDir) ;
|
|
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)
|
|
{
|
|
double dTotDist = 0 ;
|
|
for ( auto pStm : m_vSTM) {
|
|
Triangle3d Tria ;
|
|
for ( int nTria = pStm->GetFirstTriangle( Tria) ;
|
|
nTria != SVT_NULL ;
|
|
nTria = pStm->GetNextTriangle( nTria, Tria)) {
|
|
double dDist = CAvToolTriangle( m_Tool, ptT, vtDir, Tria, m_frMove.VersZ()) ;
|
|
if ( dDist < - EPS_SMALL)
|
|
return -1 ;
|
|
if ( dDist > EPS_SMALL) {
|
|
dTotDist += dDist ;
|
|
ptT += dDist * m_frMove.VersZ() ;
|
|
}
|
|
}
|
|
}
|
|
return dTotDist ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
double
|
|
CAvToolSurfTm::MyTestPositionHG( Point3d& ptT, const Vector3d& vtDir)
|
|
{
|
|
// 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) {
|
|
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() ;
|
|
}
|
|
else if ( dDist < -EPS_SMALL)
|
|
return -1 ;
|
|
}
|
|
}
|
|
return dTotDist ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CAvToolSurfTm::PrepareHashGrid( void)
|
|
{
|
|
// pulisco HashGrid 2d
|
|
m_HGrids.Clear() ;
|
|
// verifico esistenza superfici
|
|
if ( m_vSTM.size() == 0 || 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)
|
|
{
|
|
// 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 ;
|
|
}
|