From 7771cd8324ed7ee3d57f09f157d3f30b7abe1c03 Mon Sep 17 00:00:00 2001 From: Dario Sassi Date: Tue, 19 Jan 2021 10:19:13 +0000 Subject: [PATCH] EgtGeomKernel : - modifiche in VolZmap per migliorare e velocizzare verifiche di collisione. --- VolZmap.h | 7 +- VolZmapCalculus.cpp | 576 ++++++++++++++++++++++++++++++++------------ 2 files changed, 429 insertions(+), 154 deletions(-) diff --git a/VolZmap.h b/VolZmap.h index 8368772..8e16b51 100644 --- a/VolZmap.h +++ b/VolZmap.h @@ -20,6 +20,7 @@ #include #include #include +#include // ------------------------- STRUTTURE ----------------------------------------------------------- struct AppliedVector { @@ -382,7 +383,9 @@ class VolZmap : public IVolZmap, public IGeoObjRW bool AvoidSimpleRectPrismoid( const Frame3d& frPrismoid, double dLenghtBaseX, double dLenghtBaseY, double dLenghtTopX, double dLenghtTopY, double dHeight, bool bPrecise = false) const ; bool AvoidSimpleTorus( const Frame3d& frTorus, double dMaxRad, double dMinRad, bool bPrecise = false) const ; - + // Funzioni ausiliarie per metodi avoid + bool SingleMapDexelConeCollision( int nStI, int nEnI, int nStJ, int nEnJ, const Point3d& ptRefPoint, const Vector3d& vtRefAx, + double dMinRad, double dMaxRad, double dHeight, double dMinBoxH, double dMaxBoxH) const ; private : enum Status { ERR = 0, OK = 1, TO_VERIFY = 2} ; enum Shape { GENERIC = 0, BOX = 1, EXTRUSION = 2} ; @@ -439,6 +442,8 @@ class VolZmap : public IVolZmap, public IGeoObjRW mutable std::vector m_SliceYZ ; mutable std::mutex SliceMutex ; + mutable std::atomic m_bBreak ; + Tool m_Tool ; } ; diff --git a/VolZmapCalculus.cpp b/VolZmapCalculus.cpp index f3e7385..160029a 100644 --- a/VolZmapCalculus.cpp +++ b/VolZmapCalculus.cpp @@ -23,7 +23,10 @@ #include "/EgtDev/Include/EGkIntersPlaneTria.h" #include "/EgtDev/Include/EGkChainCurves.h" #include "/EgtDev/Include/ENkPolynomialRoots.h" +#include "/EgtDev/Include/EGnStringUtils.h" #include "/EgtDev/Include/EgtNumUtils.h" +#include +#include using namespace std ; @@ -604,6 +607,7 @@ VolZmap::AvoidSimpleBox( const Frame3d& frBox, const Vector3d& vtDiag, bool bPre int nEnI = Clamp( int( ptBoxSup.x / m_dStep), 0, m_nNx[nMap] - 1) ; int nStJ = Clamp( int( ptBoxInf.y / m_dStep), 0, m_nNy[nMap] - 1) ; int nEnJ = Clamp( int( ptBoxSup.y / m_dStep), 0, m_nNy[nMap] - 1) ; + // Ciclo sui dexel. for ( int i = nStI ; i <= nEnI ; ++ i) { for ( int j = nStJ ; j <= nEnJ ; ++ j) { int nPos = j * m_nNx[nMap] + i; @@ -612,10 +616,13 @@ VolZmap::AvoidSimpleBox( const Frame3d& frBox, const Vector3d& vtDiag, bool bPre continue ; Point3d ptT = ptO + ( i + 0.5) * m_dStep * vtX + ( j + 0.5) * m_dStep * vtY ; double dMinU, dMaxU ; + // La retta associata al dexel interseca il box. if ( IntersLineBox( ptT, vtK, ORIG, ORIG + vtDiag, dMinU, dMaxU)) { + // Ciclo sui segmenti del dexel for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { - if ( ! ( dMaxU < m_Values[nMap][nPos][nIndex].dMin - EPS_SMALL || - dMinU > m_Values[nMap][nPos][nIndex].dMax + EPS_SMALL)) + // Se il segmento è interno all'intervallo d'intersezione, ho finito. + if ( dMaxU > m_Values[nMap][nPos][nIndex].dMin - EPS_SMALL && + dMinU < m_Values[nMap][nPos][nIndex].dMax + EPS_SMALL) return false ; } } @@ -810,19 +817,23 @@ VolZmap::AvoidSimpleSphere( const Point3d& ptCenter, double dRad, bool bPrecise) vtY = X_AX ; vtK = Y_AX ; } - // Limiti su indici + // Limiti su indici int nStI = Clamp( int( ptBoxInf.x / m_dStep), 0, m_nNx[nMap] - 1) ; int nEnI = Clamp( int( ptBoxSup.x / m_dStep), 0, m_nNx[nMap] - 1) ; int nStJ = Clamp( int( ptBoxInf.y / m_dStep), 0, m_nNy[nMap] - 1) ; int nEnJ = Clamp( int( ptBoxSup.y / m_dStep), 0, m_nNy[nMap] - 1) ; + // Ciclo sui dexel. for ( int i = nStI ; i <= nEnI ; ++ i) { for ( int j = nStJ ; j <= nEnJ ; ++ j) { int nPos = j * m_nNx[nMap] + i ; int nSize = int( m_Values[nMap][nPos].size()) ; if ( nSize == 0) continue ; + if ( m_Values[nMap][nPos][nSize-1].dMax < b3Int.GetMin().z || m_Values[nMap][nPos][0].dMin > b3Int.GetMax().z) + continue ; Point3d ptT = ORIG + ( i + 0.5) * m_dStep * vtX + ( j + 0.5) * m_dStep * vtY ; Point3d ptI1, ptI2 ; + // La linea del dexel interseca la sfera. if ( ::IntersLineSphere( ptT, vtK, ptC, dRad, ptI1, ptI2) != ILST_NO) { double dMinU, dMaxU ; if ( nMap == 0) { @@ -837,9 +848,11 @@ VolZmap::AvoidSimpleSphere( const Point3d& ptCenter, double dRad, bool bPrecise) dMinU = min( ptI1.y, ptI2.y) ; dMaxU = max( ptI1.y, ptI2.y) ; } + // Ciclo sui segmenti del dexel. for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { - if ( ! ( dMaxU < m_Values[nMap][nPos][nIndex].dMin - EPS_SMALL || - dMinU > m_Values[nMap][nPos][nIndex].dMax + EPS_SMALL)) + // Se il segmento è interno all'intervallo d'intersezione, ho finito. + if ( dMaxU > m_Values[nMap][nPos][nIndex].dMin - EPS_SMALL && + dMinU < m_Values[nMap][nPos][nIndex].dMax + EPS_SMALL) return false ; } } @@ -967,6 +980,7 @@ VolZmap::AvoidSimpleCylinder( const Frame3d& frCyl, double dR, double dH, bool b int nEnI = Clamp( int( ptBoxSup.x / m_dStep), 0, m_nNx[nMap] - 1) ; int nStJ = Clamp( int( ptBoxInf.y / m_dStep), 0, m_nNy[nMap] - 1) ; int nEnJ = Clamp( int( ptBoxSup.y / m_dStep), 0, m_nNy[nMap] - 1) ; + // Ciclo sui dexel. for ( int i = nStI ; i <= nEnI ; ++ i) { for ( int j = nStJ ; j <= nEnJ ; ++ j) { if ( nMap == 2 && j == 75) @@ -975,9 +989,12 @@ VolZmap::AvoidSimpleCylinder( const Frame3d& frCyl, double dR, double dH, bool b int nSize = int( m_Values[nMap][nPos].size()) ; if ( nSize == 0) continue ; + if ( m_Values[nMap][nPos][nSize-1].dMax < b3Int.GetMin().z || m_Values[nMap][nPos][0].dMin > b3Int.GetMax().z) + continue ; Point3d ptT = ORIG + ( i + 0.5) * m_dStep * vtX + ( j + 0.5) * m_dStep * vtY ; Point3d ptI1, ptI2 ; Vector3d vtN1, vtN2 ; + // La linea del dexel interseca il cilindro. if ( IntersLineCylinder( ptT, vtK, frC, dH, dR, true, true, ptI1, vtN1, ptI2, vtN2)) { double dMinU, dMaxU ; if ( nMap == 0) { @@ -992,11 +1009,11 @@ VolZmap::AvoidSimpleCylinder( const Frame3d& frCyl, double dR, double dH, bool b dMinU = min( ptI1.y, ptI2.y) ; dMaxU = max( ptI1.y, ptI2.y) ; } + // Ciclo sui segmenti del dexel. for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { - double dMin = m_Values[nMap][nPos][nIndex].dMin ; - double dMax = m_Values[nMap][nPos][nIndex].dMax ; - if ( ! ( dMaxU < dMin - EPS_SMALL || - dMinU > dMax + EPS_SMALL)) + // Se il segmento è interno all'intervallo d'intersezione, ho finito. + if ( dMaxU > m_Values[nMap][nPos][nIndex].dMin - EPS_SMALL && + dMinU < m_Values[nMap][nPos][nIndex].dMax + EPS_SMALL) return false ; } } @@ -1046,6 +1063,101 @@ VolZmap::AvoidCylinder( const Frame3d& frCyl, double dR, double dH, double dSafe return true ; } +//---------------------------------------------------------------------------- +bool +VolZmap::SingleMapDexelConeCollision( int nStI, int nEnI, int nStJ, int nEnJ, const Point3d& ptRefPoint, const Vector3d& vtRefAx, + double dMinRad, double dMaxRad, double dHeight, double dMinBoxH, double dMaxBoxH) const +{ + // Ciclo di intersezione dei dexel con il cono (nel riferimento intrinseco) + for ( int i = nStI ; i <= nEnI ; ++ i) { + for ( int j = nStJ ; j <= nEnJ ; ++ j) { + int nPos = j * m_nNx[0] + i ; + int nSize = int( m_Values[0][nPos].size()) ; + if ( nSize == 0) + continue ; + double dParMin = m_Values[0][nPos][0].dMin ; + double dParMax = m_Values[0][nPos][nSize-1].dMax ; + if ( dParMax < dMinBoxH || dParMin > dMaxBoxH) + continue ; + for ( int k = 0 ; k < 5 ; ++ k) { + // se richiesta interruzione, esco + if ( m_bBreak) + return false ; + // Calcolo spillone + Point3d ptLineSt = ORIG + ( i + 0.5) * m_dStep * X_AX + ( j + 0.5) * m_dStep * Y_AX ; + switch ( k) { + case 0 : break ; + case 1 : ptLineSt += -0.4 * m_dStep * X_AX - 0.4 * m_dStep * Y_AX ; break ; + case 2 : ptLineSt += +0.4 * m_dStep * X_AX - 0.4 * m_dStep * Y_AX ; break ; + case 3 : ptLineSt += +0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ; break ; + case 4 : ptLineSt += -0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ; break ; + } + // Cono proprio + if ( dMinRad < EPS_SMALL) { + Point3d ptSegSt = ptLineSt + dParMin * Z_AX ; + double dU1, dU2 ; + int nIntType = SegmentCone( ptSegSt, Z_AX, dParMax - dParMin, ptRefPoint, vtRefAx, + dMaxRad, dHeight, dU1, dU2) ; + if ( nIntType == LinCompCCIntersType::CC_ERROR_INT) + return true ; + else if ( nIntType != LinCompCCIntersType::CC_NO_INTERS) { + for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { + if ( m_Values[0][nPos][nIndex].dMax >= dU1 && m_Values[0][nPos][nIndex].dMin <= dU2) + return true ; + } + } + nIntType = SegmentDisc( ptSegSt, Z_AX, dParMax - dParMin, ptRefPoint + dHeight * vtRefAx, + vtRefAx, dMaxRad, dU1, dU2) ; + if ( nIntType == LinCompDiscIntersType::D_ERROR_INT) + return true ; + else if ( nIntType != LinCompDiscIntersType::D_NO_INTERS) { + for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { + if ( m_Values[0][nPos][nIndex].dMax >= dU1 && m_Values[0][nPos][nIndex].dMin <= dU2) + return true ; + } + } + } + // Tronco di cono + else { + Point3d ptSegSt = ptLineSt + dParMin * Z_AX ; + double dU1, dU2 ; + int nIntType = SegmentConeFrustum( ptSegSt, Z_AX, dParMax - dParMin, ptRefPoint, vtRefAx, + dMinRad, dMaxRad, dHeight, dU1, dU2) ; + if ( nIntType == LinCompCCIntersType::CC_ERROR_INT) + return true ; + else if ( nIntType != LinCompCCIntersType::CC_NO_INTERS) { + for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { + if ( m_Values[0][nPos][nIndex].dMax >= dU1 && m_Values[0][nPos][nIndex].dMin <= dU2) + return true ; + } + } + nIntType = SegmentDisc( ptSegSt, Z_AX, dParMax - dParMin, ptRefPoint + dHeight * vtRefAx, + vtRefAx, dMaxRad, dU1, dU2) ; + if ( nIntType == LinCompDiscIntersType::D_ERROR_INT) + return true ; + else if ( nIntType != LinCompDiscIntersType::D_NO_INTERS) { + for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { + if ( m_Values[0][nPos][nIndex].dMax >= dU1 && m_Values[0][nPos][nIndex].dMin <= dU2) + return true ; + } + } + nIntType = SegmentDisc( ptSegSt, Z_AX, dParMax - dParMin, ptRefPoint, vtRefAx, dMinRad, dU1, dU2) ; + if ( nIntType == LinCompDiscIntersType::D_ERROR_INT) + return true ; + else if ( nIntType != LinCompDiscIntersType::D_NO_INTERS) { + for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { + if ( m_Values[0][nPos][nIndex].dMax >= dU1 && m_Values[0][nPos][nIndex].dMin <= dU2) + return true ; + } + } + } + } + } + } + + return false ; +} + //---------------------------------------------------------------------------- bool VolZmap::AvoidSimpleConeFrustum( const Frame3d& frCone, double dMinRad, double dMaxRad, double dHeight, bool bPrecise) const @@ -1087,89 +1199,63 @@ VolZmap::AvoidSimpleConeFrustum( const Frame3d& frCone, double dMinRad, double d int nEnI = Clamp( int( b3Int.GetMax().x / m_dStep), 0, m_nNx[0] - 1) ; int nStJ = Clamp( int( b3Int.GetMin().y / m_dStep), 0, m_nNy[0] - 1) ; int nEnJ = Clamp( int( b3Int.GetMax().y / m_dStep), 0, m_nNy[0] - 1) ; - // Ciclo di intersezione dei dexel con il cono (nel riferimento intrinseco) - for ( int i = nStI ; i <= nEnI ; ++ i) { - for ( int j = nStJ ; j <= nEnJ ; ++ j) { - int nPos = j * m_nNx[0] + i ; - int nSize = int( m_Values[0][nPos].size()) ; - if ( nSize == 0) - continue ; - double dParMin = m_Values[0][nPos][0].dMin ; - double dParMax = m_Values[0][nPos][nSize-1].dMax ; - if ( dParMax < b3Int.GetMin().z || dParMin > b3Int.GetMax().z) - continue ; - for ( int k = 0 ; k < 5 ; ++ k) { - Point3d ptLineSt = ORIG + ( i + 0.5) * m_dStep * X_AX + ( j + 0.5) * m_dStep * Y_AX ; - switch ( k) { - case 0 : break ; - case 1 : ptLineSt += -0.4 * m_dStep * X_AX - 0.4 * m_dStep * Y_AX ; break ; - case 2 : ptLineSt += +0.4 * m_dStep * X_AX - 0.4 * m_dStep * Y_AX ; break ; - case 3 : ptLineSt += +0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ; break ; - case 4 : ptLineSt += -0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ; break ; - } - // Cono proprio - if ( dMinRad < EPS_SMALL) { - Point3d ptSegSt = ptLineSt + dParMin * Z_AX ; - double dU1, dU2 ; - int nIntType = SegmentCone( ptSegSt, Z_AX, dParMax - dParMin, ptRefPoint, vtRefAx, - dMaxRad, dHeight, dU1, dU2) ; - if ( nIntType == LinCompCCIntersType::CC_ERROR_INT) - return false ; - else if ( nIntType != LinCompCCIntersType::CC_NO_INTERS) { - for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { - if ( m_Values[0][nPos][nIndex].dMax >= dU1 && m_Values[0][nPos][nIndex].dMin <= dU2) - return false ; - } - } - nIntType = SegmentDisc( ptSegSt, Z_AX, dParMax - dParMin, ptRefPoint + dHeight * vtRefAx, - vtRefAx, dMaxRad, dU1, dU2) ; - if ( nIntType == LinCompDiscIntersType::D_ERROR_INT) - return false ; - else if ( nIntType != LinCompDiscIntersType::D_NO_INTERS) { - for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { - if ( m_Values[0][nPos][nIndex].dMax >= dU1 && m_Values[0][nPos][nIndex].dMin <= dU2) - return false ; - } - } - } - // Tronco di cono - else { - Point3d ptSegSt = ptLineSt + dParMin * Z_AX ; - double dU1, dU2 ; - int nIntType = SegmentConeFrustum( ptSegSt, Z_AX, dParMax - dParMin, ptRefPoint, vtRefAx, - dMinRad, dMaxRad, dHeight, dU1, dU2) ; - if ( nIntType == LinCompCCIntersType::CC_ERROR_INT) - return false ; - else if ( nIntType != LinCompCCIntersType::CC_NO_INTERS) { - for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { - if ( m_Values[0][nPos][nIndex].dMax >= dU1 && m_Values[0][nPos][nIndex].dMin <= dU2) - return false ; - } - } - nIntType = SegmentDisc( ptSegSt, Z_AX, dParMax - dParMin, ptRefPoint + dHeight * vtRefAx, - vtRefAx, dMaxRad, dU1, dU2) ; - if ( nIntType == LinCompDiscIntersType::D_ERROR_INT) - return false ; - else if ( nIntType != LinCompDiscIntersType::D_NO_INTERS) { - for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { - if ( m_Values[0][nPos][nIndex].dMax >= dU1 && m_Values[0][nPos][nIndex].dMin <= dU2) - return false ; - } - } - nIntType = SegmentDisc( ptSegSt, Z_AX, dParMax - dParMin, ptRefPoint, vtRefAx, dMinRad, dU1, dU2) ; - if ( nIntType == LinCompDiscIntersType::D_ERROR_INT) - return false ; - else if ( nIntType != LinCompDiscIntersType::D_NO_INTERS) { - for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { - if ( m_Values[0][nPos][nIndex].dMax >= dU1 && m_Values[0][nPos][nIndex].dMin <= dU2) - return false ; - } + // Limiti su Z + double dZmin = b3Int.GetMin().z ; + double dZmax = b3Int.GetMax().z ; + // Numero massimo di thread + int nThreadMax = max( 1, int( thread::hardware_concurrency()) - 1) ; + // se un solo thread + if ( nThreadMax == 1) { + m_bBreak = false ; + bool bCollision = SingleMapDexelConeCollision( nStI, nEnI, nStJ, nEnJ, ptRefPoint, vtRefAx, + dMinRad, dMaxRad, dHeight, dZmin, dZmax) ; + return ( ! bCollision) ; + } + // altrimenti esecuzione in parallelo dei calcoli + else { + //string sOut = "I=" + ToString( nStI) + "," + ToString( nEnI) + " J=" + ToString( nStJ) + "," + ToString( nEnJ) ; + //LOG_INFO( GetEGkLogger(), sOut.c_str()) + m_bBreak = false ; + // lancio dei thread + int nSpanI = ( nEnI - nStI + 1) / nThreadMax + 1 ; + int nSpanJ = ( nEnJ - nStJ + 1) / nThreadMax + 1 ; + bool bOnI = ( nSpanI >= nSpanJ) ; + int nThreadTot = 0 ; + vector< future> vRes( nThreadMax) ; + for ( int nT = 0 ; nT < nThreadMax ; ++ nT) { + int nMyStI = ( bOnI ? nStI + nT * nSpanI : nStI) ; + int nMyEnI = ( bOnI ? min( nMyStI + nSpanI - 1, nEnI) : nEnI) ; + int nMyStJ = ( bOnI ? nStJ : nStJ + nT * nSpanJ) ; + int nMyEnJ = ( bOnI ? nEnJ : min( nMyStJ + nSpanJ - 1, nEnJ)) ; + if ( nMyStI > nEnI || nMyStJ > nEnJ) + break ; + //string sOut = "MyI=" + ToString( nMyStI) + "," + ToString( nMyEnI) + " MyJ=" + ToString( nMyStJ) + "," + ToString( nMyEnJ) ; + //LOG_INFO( GetEGkLogger(), sOut.c_str()) + vRes[nT] = async( launch::async, &VolZmap::SingleMapDexelConeCollision, this, + nMyStI, nMyEnI, nMyStJ, nMyEnJ, cref( ptRefPoint), cref( vtRefAx), + dMinRad, dMaxRad, dHeight, dZmin, dZmax) ; + ++ nThreadTot ; + } + // recupero i risultati dei thread alla loro terminazione + bool bCollision = false ; + int nTerminated = 0 ; + while ( nTerminated < nThreadTot) { + for ( int nT = 0 ; nT < nThreadTot ; ++ nT) { + // Async terminato + if ( vRes[nT].valid() && vRes[nT].wait_for( chrono::nanoseconds{ 1}) == future_status::ready) { + ++ nTerminated ; + // Se c'è collisione ... + if ( vRes[nT].get()) { + bCollision = true ; + m_bBreak = true ; } } } } + return ( ! bCollision) ; } } + // Uso tutte le mappe else { // Ciclo sulle mappe. @@ -1194,7 +1280,13 @@ VolZmap::AvoidSimpleConeFrustum( const Frame3d& frCone, double dMinRad, double d int nEnI = Clamp( int( ptSupIntBox.x / m_dStep), 0, m_nNx[nMap] - 1) ; int nStJ = Clamp( int( ptInfIntBox.y / m_dStep), 0, m_nNy[nMap] - 1) ; int nEnJ = Clamp( int( ptSupIntBox.y / m_dStep), 0, m_nNy[nMap] - 1) ; + // Ciclo sui dexel. for ( int nDex = 0 ; nDex < int( m_Values[nMap].size()) ; ++ nDex) { + int nDexSize = (int)m_Values[nMap][nDex].size() ; + if ( nDexSize == 0 || + m_Values[nMap][nDex][ nDexSize- 1].dMax < ptInfIntBox.z || + m_Values[nMap][nDex][0].dMin > ptSupIntBox.z) + continue ; // Indici del dexel. int nI = nDex % m_nNx[nMap] ; int nJ = nDex / m_nNx[nMap] ; @@ -1221,41 +1313,65 @@ VolZmap::AvoidSimpleConeFrustum( const Frame3d& frCone, double dMinRad, double d } // Cono proprio if ( dMinRad < EPS_SMALL) { - int nSize = int( m_Values[nMap][nDex].size()) ; - for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { - double dMinPar = m_Values[nMap][nDex][nIndex].dMin ; - double dMaxPar = m_Values[nMap][nDex][nIndex].dMax ; - Point3d ptSegSt = ptLineSt + dMinPar * vtLineDir ; - double dU1, dU2 ; - int nIntType = SegmentCone( ptSegSt, vtLineDir, dMaxPar - dMinPar, ptRefPoint, vtRefAx, - dMaxRad, dHeight, dU1, dU2) ; - if ( nIntType != LinCompCCIntersType::CC_NO_INTERS) - return false ; - nIntType = SegmentDisc( ptSegSt, vtLineDir, dMaxPar - dMinPar, ptRefPoint + dHeight * vtRefAx, - vtRefAx, dMaxRad, dU1, dU2) ; - if ( nIntType != LinCompDiscIntersType::D_NO_INTERS) - return false ; + double dMinPar = m_Values[nMap][nDex][0].dMin ; + double dMaxPar = m_Values[nMap][nDex][nDexSize - 1].dMax ; + Point3d ptSegSt = ptLineSt + dMinPar * vtLineDir ; + double dU1, dU2 ; + int nIntType = SegmentCone( ptSegSt, vtLineDir, dMaxPar - dMinPar, ptRefPoint, vtRefAx, + dMaxRad, dHeight, dU1, dU2) ; + if ( nIntType == LinCompCCIntersType::CC_ERROR_INT) + return false ; + else if ( nIntType != LinCompCCIntersType::CC_NO_INTERS) { + for ( int nIndex = 0 ; nIndex < nDexSize ; nIndex += 1) { + if ( m_Values[nMap][nDex][nIndex].dMax >= dU1 && m_Values[nMap][nDex][nIndex].dMin <= dU2) + return false ; + } + } + nIntType = SegmentDisc( ptSegSt, vtLineDir, dMaxPar - dMinPar, ptRefPoint + dHeight * vtRefAx, + vtRefAx, dMaxRad, dU1, dU2) ; + if ( nIntType == LinCompDiscIntersType::D_ERROR_INT) + return false ; + else if ( nIntType != LinCompDiscIntersType::D_NO_INTERS) { + for ( int nIndex = 0 ; nIndex < nDexSize ; nIndex += 1) { + if ( m_Values[nMap][nDex][nIndex].dMax >= dU1 && m_Values[nMap][nDex][nIndex].dMin <= dU2) + return false ; + } } } // Tronco di cono else { - int nSize = int( m_Values[nMap][nDex].size()) ; - for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { - double dMinPar = m_Values[nMap][nDex][nIndex].dMin ; - double dMaxPar = m_Values[nMap][nDex][nIndex].dMax ; - Point3d ptSegSt = ptLineSt + dMinPar * vtLineDir ; - double dU1, dU2 ; - int nIntType = SegmentConeFrustum( ptSegSt, vtLineDir, dMaxPar - dMinPar, ptRefPoint, vtRefAx, - dMinRad, dMaxRad, dHeight, dU1, dU2) ; - if ( nIntType != LinCompCCIntersType::CC_NO_INTERS) - return false ; - nIntType = SegmentDisc( ptSegSt, vtLineDir, dMaxPar - dMinPar, ptRefPoint + dHeight * vtRefAx, - vtRefAx, dMaxRad, dU1, dU2) ; - if ( nIntType != LinCompDiscIntersType::D_NO_INTERS) - return false ; - nIntType = SegmentDisc( ptSegSt, vtLineDir, dMaxPar - dMinPar, ptRefPoint, vtRefAx, dMinRad, dU1, dU2) ; - if ( nIntType != LinCompDiscIntersType::D_NO_INTERS) - return false ; + double dMinPar = m_Values[nMap][nDex][0].dMin ; + double dMaxPar = m_Values[nMap][nDex][nDexSize - 1].dMax ; + Point3d ptSegSt = ptLineSt + dMinPar * vtLineDir ; + double dU1, dU2 ; + int nIntType = SegmentConeFrustum( ptSegSt, vtLineDir, dMaxPar - dMinPar, ptRefPoint, vtRefAx, + dMinRad, dMaxRad, dHeight, dU1, dU2) ; + if ( nIntType == LinCompCCIntersType::CC_ERROR_INT) + return false ; + else if ( nIntType != LinCompCCIntersType::CC_NO_INTERS) { + for ( int nIndex = 0 ; nIndex < nDexSize ; nIndex += 1) { + if ( m_Values[nMap][nDex][nIndex].dMax >= dU1 && m_Values[nMap][nDex][nIndex].dMin <= dU2) + return false ; + } + } + nIntType = SegmentDisc( ptSegSt, vtLineDir, dMaxPar - dMinPar, ptRefPoint + dHeight * vtRefAx, + vtRefAx, dMaxRad, dU1, dU2) ; + if ( nIntType == LinCompDiscIntersType::D_ERROR_INT) + return false ; + else if ( nIntType != LinCompDiscIntersType::D_NO_INTERS) { + for ( int nIndex = 0 ; nIndex < nDexSize ; nIndex += 1) { + if ( m_Values[nMap][nDex][nIndex].dMax >= dU1 && m_Values[nMap][nDex][nIndex].dMin <= dU2) + return false ; + } + } + nIntType = SegmentDisc( ptSegSt, vtLineDir, dMaxPar - dMinPar, ptRefPoint, vtRefAx, dMinRad, dU1, dU2) ; + if ( nIntType == LinCompDiscIntersType::D_ERROR_INT) + return false ; + else if ( nIntType != LinCompDiscIntersType::D_NO_INTERS) { + for ( int nIndex = 0 ; nIndex < nDexSize ; nIndex += 1) { + if ( m_Values[nMap][nDex][nIndex].dMax >= dU1 && m_Values[nMap][nDex][nIndex].dMin <= dU2) + return false ; + } } } } @@ -1405,24 +1521,24 @@ RectPrismoidSegmentCollision( const Frame3d& frPrismoid, double dLenghtBaseX, do Triangle3d trFaceTria1, trFaceTria2 ; // Faccia base trFaceTria1.Set( Point3d( - dHalfBaseX, - dHalfBaseY, 0.), - Point3d( - dHalfBaseX, dHalfBaseY, 0.), - Point3d( dHalfBaseX, dHalfBaseY, 0.)) ; + Point3d( - dHalfBaseX, dHalfBaseY, 0.), + Point3d( dHalfBaseX, dHalfBaseY, 0.)) ; if ( trFaceTria1.Validate() && IntersLineTria( ptMySt, ptMyEn, trFaceTria1, Point3d(), Point3d()) != ILTT_NO) return true ; trFaceTria2.Set( Point3d( - dHalfBaseX, - dHalfBaseY, 0.), - Point3d( dHalfBaseX, dHalfBaseY, 0.), - Point3d( dHalfBaseX, - dHalfBaseY, 0.)) ; + Point3d( dHalfBaseX, dHalfBaseY, 0.), + Point3d( dHalfBaseX, - dHalfBaseY, 0.)) ; if ( trFaceTria2.Validate() && IntersLineTria( ptMySt, ptMyEn, trFaceTria2, Point3d(), Point3d()) != ILTT_NO) return true ; // Faccia top trFaceTria1.Set( Point3d( - dHalfTopX, - dHalfTopY, dHeight), - Point3d( dHalfTopX, dHalfTopY, dHeight), - Point3d( - dHalfTopX, dHalfTopY, dHeight)) ; + Point3d( dHalfTopX, dHalfTopY, dHeight), + Point3d( - dHalfTopX, dHalfTopY, dHeight)) ; if ( trFaceTria1.Validate() && IntersLineTria( ptMySt, ptMyEn, trFaceTria1, Point3d(), Point3d()) != ILTT_NO) return true ; trFaceTria2.Set( Point3d( - dHalfTopX, - dHalfTopY, dHeight), - Point3d( dHalfTopX, - dHalfTopY, dHeight), - Point3d( dHalfTopX, dHalfTopY, dHeight)) ; + Point3d( dHalfTopX, - dHalfTopY, dHeight), + Point3d( dHalfTopX, dHalfTopY, dHeight)) ; if ( trFaceTria2.Validate() && IntersLineTria( ptMySt, ptMyEn, trFaceTria2, Point3d(), Point3d()) != ILTT_NO) return true ; // Faccia laterale 1 @@ -1473,6 +1589,146 @@ RectPrismoidSegmentCollision( const Frame3d& frPrismoid, double dLenghtBaseX, do return false ; } +//---------------------------------------------------------------------------- +static bool +IntersSegmentTrianglePlus( const Point3d& ptTr1, const Point3d& ptTr2, const Point3d& ptTr3, + const Point3d& ptLnSt, const Vector3d& vtLnDir, double dLnLen, + double& dStU, double& dEnU) +{ + Triangle3d trFaceTria ; + trFaceTria.Set( ptTr1, ptTr2, ptTr3) ; + if ( trFaceTria.Validate()) { + Point3d ptMyIntSt, ptMyIntEn ; + int nIntType = IntersLineTria( ptLnSt, vtLnDir, dLnLen, trFaceTria, ptMyIntSt, ptMyIntEn, true) ; + if ( nIntType != ILTT_NO) { + if ( nIntType == ILTT_VERT || nIntType == ILTT_EDGE || nIntType == ILTT_IN) { + double dCurU = ( ptMyIntSt - ptLnSt) * vtLnDir ; + if ( dCurU < dStU) + dStU = dCurU ; + if ( dCurU > dEnU) + dEnU = dCurU ; + } + else { + double dCurStU = ( ptMyIntSt - ptLnSt) * vtLnDir ; + double dCurEnU = ( ptMyIntEn - ptLnSt) * vtLnDir ; + if ( dCurStU < dStU) + dStU = dCurStU ; + if ( dCurEnU > dEnU) + dEnU = dCurEnU ; + } + } + return true ; + } + + return false ; +} + +//---------------------------------------------------------------------------- +static bool +RectPrismoidSegmentCollisionPlus( const Frame3d& frPrismoid, double dLenghtBaseX, double dLenghtBaseY, + double dLenghtTopX, double dLenghtTopY, double dHeight, + const Point3d& ptSt, const Point3d& ptEn, + double& dStU, double& dEnU) +{ + // Se il solido non è ben definito, non ha senso continuare. + if ( max( dLenghtBaseX, dLenghtTopX) < EPS_SMALL || + max( dLenghtBaseY, dLenghtTopY) < EPS_SMALL || + dHeight < EPS_SMALL) + return false ; + + // Porto il segmento nel sistema del prismoide a base rettangolare + Point3d ptMySt = ptSt ; + ptMySt.ToLoc( frPrismoid) ; + Point3d ptMyEn = ptEn ; + ptMyEn.ToLoc( frPrismoid) ; + + // Se il segmento non è ben definito, non ha senso continuare. + Vector3d vtMySeg = ptMyEn - ptMySt ; + double dSegLen = vtMySeg.Len() ; + if ( dSegLen < EPS_SMALL) + return false ; + vtMySeg /= dSegLen ; + + // Semidimensioni delle basi + double dHalfBaseX = 0.5 * dLenghtBaseX ; + double dHalfBaseY = 0.5 * dLenghtBaseY ; + double dHalfTopX = 0.5 * dLenghtTopX ; + double dHalfTopY = 0.5 * dLenghtTopY ; + + // Inizializzo gli estremi della parte di retta che interseca il prismoide + dStU = INFINITO ; + dEnU = -INFINITO ; + // Interseco la retta con le facce e salvo i punti d'intersezione + // Faccia base + IntersSegmentTrianglePlus( Point3d( -dHalfBaseX, -dHalfBaseY, 0.), + Point3d( -dHalfBaseX, dHalfBaseY, 0.), + Point3d( dHalfBaseX, dHalfBaseY, 0.), + ptMySt, vtMySeg, dSegLen, + dStU, dEnU) ; + IntersSegmentTrianglePlus( Point3d( -dHalfBaseX, -dHalfBaseY, 0.), + Point3d( dHalfBaseX, dHalfBaseY, 0.), + Point3d( dHalfBaseX, -dHalfBaseY, 0.), + ptMySt, vtMySeg, dSegLen, + dStU, dEnU) ; + // Faccia top + IntersSegmentTrianglePlus( Point3d( -dHalfTopX, -dHalfTopY, dHeight), + Point3d( dHalfTopX, dHalfTopY, dHeight), + Point3d( -dHalfTopX, dHalfTopY, dHeight), + ptMySt, vtMySeg, dSegLen, + dStU, dEnU) ; + IntersSegmentTrianglePlus( Point3d( -dHalfTopX, -dHalfTopY, dHeight), + Point3d( dHalfTopX, -dHalfTopY, dHeight), + Point3d( dHalfTopX, dHalfTopY, dHeight), + ptMySt, vtMySeg, dSegLen, + dStU, dEnU) ; + // Faccia laterale 1 + IntersSegmentTrianglePlus( Point3d( -dHalfBaseX, -dHalfBaseY, 0.), + Point3d( dHalfTopX , -dHalfTopY , dHeight), + Point3d( -dHalfTopX , -dHalfTopY , dHeight), + ptMySt, vtMySeg, dSegLen, + dStU, dEnU) ; + IntersSegmentTrianglePlus( Point3d( -dHalfBaseX, -dHalfBaseY, 0.), + Point3d( dHalfBaseX, -dHalfBaseY, 0.), + Point3d( dHalfTopX, -dHalfTopY , dHeight), + ptMySt, vtMySeg, dSegLen, + dStU, dEnU) ; + // Faccia laterale 2 + IntersSegmentTrianglePlus( Point3d( dHalfBaseX, -dHalfBaseY, 0.), + Point3d( dHalfTopX , dHalfTopY , dHeight), + Point3d( dHalfTopX , -dHalfTopY , dHeight), + ptMySt, vtMySeg, dSegLen, + dStU, dEnU) ; + IntersSegmentTrianglePlus( Point3d( dHalfBaseX, -dHalfBaseY, 0.), + Point3d( dHalfBaseX, dHalfBaseY, 0.), + Point3d( dHalfTopX , dHalfTopY , dHeight), + ptMySt, vtMySeg, dSegLen, + dStU, dEnU) ; + // Faccia laterale 3 + IntersSegmentTrianglePlus( Point3d( dHalfBaseX, dHalfBaseY, 0.), + Point3d( -dHalfTopX , dHalfTopY , dHeight), + Point3d( dHalfTopX , dHalfTopY , dHeight), + ptMySt, vtMySeg, dSegLen, + dStU, dEnU) ; + IntersSegmentTrianglePlus( Point3d( dHalfBaseX, dHalfBaseY, 0.), + Point3d( -dHalfBaseX, dHalfBaseY, 0.), + Point3d( -dHalfTopX , dHalfTopY , dHeight), + ptMySt, vtMySeg, dSegLen, + dStU, dEnU) ; + // Faccia laterale 4 + IntersSegmentTrianglePlus( Point3d( -dHalfBaseX, dHalfBaseY, 0.), + Point3d( -dHalfTopX , -dHalfTopY , dHeight), + Point3d( -dHalfTopX , dHalfTopY , dHeight), + ptMySt, vtMySeg, dSegLen, + dStU, dEnU) ; + IntersSegmentTrianglePlus( Point3d( -dHalfBaseX, dHalfBaseY, 0.), + Point3d( -dHalfBaseX, -dHalfBaseY, 0.), + Point3d( -dHalfTopX , -dHalfTopY , dHeight), + ptMySt, vtMySeg, dSegLen, + dStU, dEnU) ; + + return ( dEnU > dStU - EPS_ZERO && dStU < dSegLen + EPS_SMALL && dEnU > -EPS_SMALL) ; +} + //---------------------------------------------------------------------------- bool VolZmap::AvoidSimpleRectPrismoid( const Frame3d& frPrismoid, double dLenghtBaseX, double dLenghtBaseY, @@ -1525,14 +1781,15 @@ VolZmap::AvoidSimpleRectPrismoid( const Frame3d& frPrismoid, double dLenghtBaseX case 3 : ptLineSt += +0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ; break ; case 4 : ptLineSt += -0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ; break ; } - // Ciclo sui segmenti di dexel. - for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { - Point3d ptSegSt = ptLineSt + m_Values[0][nPos][nIndex].dMin * Z_AX ; - Point3d ptSegEn = ptLineSt + m_Values[0][nPos][nIndex].dMax * Z_AX ; - // Se c'è intersezione, ho finito. - if ( RectPrismoidSegmentCollision( frMyFrame, dLenghtBaseX, dLenghtBaseY, dLenghtTopX, dLenghtTopY, - dHeight, ptSegSt, ptSegEn)) - return false ; + double dStU, dEnU ; + Point3d ptSegSt = ptLineSt + m_Values[0][nPos][0].dMin * Z_AX ; + Point3d ptSegEn = ptLineSt + m_Values[0][nPos][nSize-1].dMax * Z_AX ; + if ( RectPrismoidSegmentCollisionPlus( frMyFrame, dLenghtBaseX, dLenghtBaseY, dLenghtTopX, dLenghtTopY, + dHeight, ptSegSt, ptSegEn, dStU, dEnU)) { + for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { + if ( m_Values[0][nPos][nIndex].dMax >= dStU && m_Values[0][nPos][nIndex].dMin <= dEnU) + return false ; + } } } } @@ -1562,6 +1819,9 @@ VolZmap::AvoidSimpleRectPrismoid( const Frame3d& frPrismoid, double dLenghtBaseX int nStJ = Clamp( int( ptInfIntBox.y / m_dStep), 0, m_nNy[nMap] - 1) ; int nEnJ = Clamp( int( ptSupIntBox.y / m_dStep), 0, m_nNy[nMap] - 1) ; for ( int nDex = 0 ; nDex < int( m_Values[nMap].size()) ; ++ nDex) { + int nSize = int( m_Values[nMap][nDex].size()) ; + if ( nSize == 0) + continue ; // Indici del dexel int nI = nDex % m_nNx[nMap] ; int nJ = nDex / m_nNx[nMap] ; @@ -1586,14 +1846,15 @@ VolZmap::AvoidSimpleRectPrismoid( const Frame3d& frPrismoid, double dLenghtBaseX swap( vtLineDir.x, vtLineDir.y) ; swap( vtLineDir.y, vtLineDir.z) ; } - // Ciclo sui segmenti del dexel. - for ( int nInt = 0 ; nInt < int( m_Values[nMap][nDex].size()) ; ++ nInt) { - Point3d ptSegSt = ptLineSt + m_Values[nMap][nDex][nInt].dMin * vtLineDir ; - Point3d ptSegEn = ptLineSt + m_Values[nMap][nDex][nInt].dMax * vtLineDir ; - // Se c'è intersezione, ho finito. - if ( RectPrismoidSegmentCollision( frMyFrame, dLenghtBaseX, dLenghtBaseY, dLenghtTopX, dLenghtTopY, - dHeight, ptSegSt, ptSegEn)) - return false ; + double dStU, dEnU ; + Point3d ptSegSt = ptLineSt + m_Values[nMap][nDex][0].dMin * vtLineDir ; + Point3d ptSegEn = ptLineSt + m_Values[nMap][nDex][nSize-1].dMax * vtLineDir ; + if ( RectPrismoidSegmentCollisionPlus( frMyFrame, dLenghtBaseX, dLenghtBaseY, dLenghtTopX, dLenghtTopY, + dHeight, ptSegSt, ptSegEn, dStU, dEnU)) { + for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { + if ( m_Values[nMap][nDex][nIndex].dMax >= dStU && m_Values[nMap][nDex][nIndex].dMin <= dEnU) + return false ; + } } } } @@ -1831,6 +2092,13 @@ VolZmap::AvoidSimpleTorus( const Frame3d& frTorus, double dMaxRad, double dMinRa int nStJ = Clamp( int( ptInfIntBox.y / m_dStep), 0, m_nNy[nMap] - 1) ; int nEnJ = Clamp( int( ptSupIntBox.y / m_dStep), 0, m_nNy[nMap] - 1) ; for ( int nDex = 0 ; nDex < int( m_Values[nMap].size()) ; ++ nDex) { + int nSize = int( m_Values[nMap][nDex].size()) ; + if ( nSize == 0) + continue ; + double dParMin = m_Values[nMap][nDex][0].dMin ; + double dParMax = m_Values[nMap][nDex][nSize-1].dMax ; + if ( dParMax < ptInfIntBox.z || dParMin > ptSupIntBox.z) + continue ; // Indici del dexel int nI = nDex % m_nNx[nMap] ; int nJ = nDex / m_nNx[nMap] ; @@ -1855,18 +2123,20 @@ VolZmap::AvoidSimpleTorus( const Frame3d& frTorus, double dMaxRad, double dMinRa swap( vtLineDir.x, vtLineDir.y) ; swap( vtLineDir.y, vtLineDir.z) ; } - // Ciclo sui segmenti del dexel. - for ( int nInt = 0 ; nInt < int( m_Values[nMap][nDex].size()) ; ++ nInt) { - // Segmento - Point3d ptSegSt = ptLineSt + m_Values[nMap][nDex][nInt].dMin * vtLineDir ; - double dSegLen = m_Values[nMap][nDex][nInt].dMax - m_Values[nMap][nDex][nInt].dMin ; - // Intersezione segmento toro - int nIntersType = SegmentTorus( ptSegSt, vtLineDir, dSegLen, ptMyCen, vtMyAx, dMinRad, dMaxRad, - BOOLVECTOR(), DBLVECTOR()) ; - // Collisione - if ( ! ( nIntersType == LinCompTorusIntersType::T_ERROR || - nIntersType == LinCompTorusIntersType::T_NO_INT)) - return false ; + BOOLVECTOR vbType ; + DBLVECTOR vdPar ; + Point3d ptSegSt = ptLineSt + dParMin * vtLineDir ; + int nIntType = SegmentTorus( ptSegSt, vtLineDir, dParMax - dParMin, ptMyCen, vtMyAx, dMinRad, dMaxRad, + vbType, vdPar) ; + if ( nIntType == LinCompTorusIntersType::T_ERROR) + return false ; + else if ( nIntType != LinCompTorusIntersType::T_NO_INT) { + double dUmin = vdPar.front() ; + double dUmax = vdPar.back() ; + for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { + if ( m_Values[nMap][nDex][nIndex].dMax >= dUmin && m_Values[nMap][nDex][nIndex].dMin <= dUmax) + return false ; + } } } }