From 371c00d13b563659f080619d0a4f1a0457f593df Mon Sep 17 00:00:00 2001 From: Dario Sassi Date: Fri, 20 Jul 2018 16:38:51 +0000 Subject: [PATCH] EgtGeomKernel : - migliorie a tori inversi di Tool Collision Avoidance. --- CAvToolTriangle.cpp | 125 ++++++++++++++++++-------------------------- 1 file changed, 51 insertions(+), 74 deletions(-) diff --git a/CAvToolTriangle.cpp b/CAvToolTriangle.cpp index 54febf1..6ea1ffd 100644 --- a/CAvToolTriangle.cpp +++ b/CAvToolTriangle.cpp @@ -1103,10 +1103,10 @@ TorusSegmentLeakDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double d Point3d ptCurPoint = ptStart + dPar * vtSeg ; double dPointAxSqLen = GetPointLineSqDist( ptCurPoint, ptTorusCen, vtMove) ; if ( dPointAxSqLen > dSqMaxRad && dPointAxSqLen < dSqTotRad) { - double dTmp = dMaxRad - sqrt( dPointAxSqLen) ; - double dTmp2 = dSqMinRad - dTmp * dTmp ; - double dNonLinearPart = ( dTmp2 > 0. ? sqrt( dTmp2) : 0.) ; - double dLeakDist = ( ptCurPoint - ptTorusCen) * vtMove + dNonLinearPart ; + double dDeltaRad = sqrt( dPointAxSqLen) - dMaxRad ; + double dSQAddLeakDist = dSqMinRad - dDeltaRad * dDeltaRad ; + double dAddLeakDist = ( dSQAddLeakDist > 0. ? sqrt( dSQAddLeakDist) : 0.) ; + double dLeakDist = ( ptCurPoint - ptTorusCen) * vtMove + dAddLeakDist ; if ( dLeakDist > dMaxLeakDist) dMaxLeakDist = dLeakDist ; } @@ -1342,87 +1342,64 @@ ConcaveTorusSegmentLeakDistLongMot( const Point3d& ptTorusCen, double dMaxRad, d // Se segmento dista dall'asse del toro pił del raggio massimo, posso uscire subito double dLinSegSqDist = LineSegmentSqDist( ptTorusCen, vtMove, ptSeg, vtSeg, dSegLen) ; if ( dLinSegSqDist > dMaxRad * dMaxRad) - return 0.0 ; + return 0. ; + // Se estremi del segmento pił vicini della corona torica, posso uscire subito + double dStartSqDist = GetPointLineSqDist( ptSeg, ptTorusCen, vtMove) ; + double dEndSqDist = GetPointLineSqDist( ptSeg + vtSeg * dSegLen, ptTorusCen, vtMove) ; + if ( dStartSqDist < ( dMaxRad - dMinRad) * ( dMaxRad - dMinRad) && + dEndSqDist < ( dMaxRad - dMinRad) * ( dMaxRad - dMinRad)) + return 0. ; - // Individuo le parti del segmento che attraversano il toro - Point3d ptMyFirstSeg = ptSeg ; - Point3d ptMySecondSeg = ptSeg ; - double dMyFirstSegLen = dSegLen ; - double dMySecondSegLen = dSegLen ; - // Limito il segmento entro la corona del toro - double dFirstLen1 = 0 ; - double dFirstLen2 = dMyFirstSegLen ; - double dSecondLen1, dSecondLen2 ; - Intervals SegLimits ; - SegLimits.Set( 0, dMyFirstSegLen) ; - Intervals ChordLimits ; - int nFirstInters = IntersLineInfiniteCylinder( ptSeg, vtSeg, ptTorusCen, vtMove, dMaxRad, - dFirstLen1, dFirstLen2) ; - int nSecondInters = CC_NO_INTERS ; - if ( nFirstInters == CC_TWO_INT) - ChordLimits.Set( dFirstLen1, dFirstLen2) ; - else if ( nFirstInters == CC_ONE_INT_TAN) - ChordLimits.Set( dFirstLen1, dFirstLen1) ; - SegLimits.Intersect( ChordLimits) ; - double dMin, dMax ; - SegLimits.GetMinMax( dMin, dMax) ; - if ( dMax - dMin > EPS_SMALL) { - nSecondInters = IntersLineInfiniteCylinder( ptSeg, vtSeg, ptTorusCen, vtMove, dMaxRad - dMinRad, - dSecondLen1, dSecondLen2) ; - if ( nSecondInters == CC_TWO_INT) - SegLimits.Subtract( dSecondLen1, dSecondLen2) ; + // Intervallo del segmento non limitato + Intervals SegLimits ; + SegLimits.Set( 0, dSegLen) ; + // Limito il segmento entro il cilindro esterno della corona del toro + double dOutLen1, dOutLen2 ; + int nOutInters = IntersLineInfiniteCylinder( ptSeg, vtSeg, ptTorusCen, vtMove, dMaxRad, + dOutLen1, dOutLen2) ; + if ( nOutInters == CC_TWO_INT || nOutInters == CC_ONE_INT_TAN) + SegLimits.Intersect( dOutLen1, dOutLen2) ; + if ( SegLimits.IsEmpty()) + return 0. ; + // Tolgo eventuale cilindro interno alla corona del toro + if ( dLinSegSqDist < ( dMaxRad - dMinRad) * ( dMaxRad - dMinRad)) { + double dIntLen1, dIntLen2 ; + int nIntInters = IntersLineInfiniteCylinder( ptSeg, vtSeg, ptTorusCen, vtMove, dMaxRad - dMinRad, + dIntLen1, dIntLen2) ; + if ( nIntInters == CC_TWO_INT) + SegLimits.Subtract( dIntLen1, dIntLen2) ; } - // Cerco massima distanza di allontanamento lungo il segmento - double dFirstStep = max( 0.1 * dMinRad, EPS_SMALL) ; // !!! USARE ALGORITMO GOLDEN SECTION !!! - double dSecondStep = dFirstStep ; - int nFirstStepNum = 0 ; - int nSecondStepNum = 0 ; - // Primo segmento - if ( SegLimits.GetFirst( dFirstLen1, dFirstLen2)) { - ptMyFirstSeg += dFirstLen1 * vtSeg ; - dMyFirstSegLen = dFirstLen2 - dFirstLen1 ; - nFirstStepNum = int( dMyFirstSegLen / dFirstStep) + 1 ; - dFirstStep = dMyFirstSegLen / nFirstStepNum ; - } - // Secondo segmento - if ( SegLimits.GetNext( dSecondLen1, dSecondLen2)) { - ptMySecondSeg += dSecondLen1 * vtSeg ; - dMySecondSegLen = dSecondLen2 - dSecondLen1 ; - nSecondStepNum = int( dMySecondSegLen / dSecondStep) + 1 ; - dSecondStep = dMySecondSegLen / nSecondStepNum ; - } - - double dSqMinRad = dMinRad * dMinRad ; + // Eseguo controllo sugli intervalli + double dSqMinRad = dMinRad * dMinRad ; double dSqMaxRad = dMaxRad * dMaxRad ; - // Cerco massimo allontanamento nel primo segmento + double dSqInnRad = ( dMaxRad - dMinRad) * ( dMaxRad - dMinRad) ; double dMaxLeakDist = 0.0 ; - for ( int n = 0 ; n <= nFirstStepNum ; ++ n) { - double dPar = n * dFirstStep ; - Point3d ptCurPoint = ptMyFirstSeg + dPar * vtSeg ; - double dPointAxSqLen = GetPointLineSqDist( ptCurPoint, ptTorusCen, vtMove) ; - if ( dPointAxSqLen > ( dMaxRad - dMinRad) * ( dMaxRad - dMinRad) && dPointAxSqLen < dSqMaxRad) { - double dNonLinearPart = sqrt( max( dSqMinRad - ( dMaxRad - sqrt( dPointAxSqLen)) * ( dMaxRad - sqrt( dPointAxSqLen)), 0.)) ; - double dLeakDist = ( ptCurPoint - ptTorusCen) * vtMove - dNonLinearPart ; - if ( dLeakDist > dMaxLeakDist) - dMaxLeakDist = dLeakDist ; - } - } - // Cerco nel secondo segmento un allontanamento ancor maggiore - if ( nSecondStepNum > 0) { - for ( int n = 0 ; n <= nSecondStepNum ; ++ n) { - double dPar = n * dSecondStep ; - Point3d ptCurPoint = ptMySecondSeg + dPar * vtSeg ; + double dLen1, dLen2 ; + bool bFound = SegLimits.GetFirst( dLen1, dLen2) ; + while ( bFound) { + // verifico l'intervallo + Point3d ptStart = ptSeg + vtSeg * dLen1 ; + double dLen = dLen2 - dLen1 ; + double dStep = max( 0.1 * dMinRad, EPS_SMALL) ; // !!! USARE ALGORITMO GOLDEN SECTION !!! + int nStepNum = int( dLen / dStep) + 1 ; + dStep = dLen / nStepNum ; + for ( int n = 0 ; n <= nStepNum ; ++ n) { + double dPar = n * dStep ; + Point3d ptCurPoint = ptStart + dPar * vtSeg ; double dPointAxSqLen = GetPointLineSqDist( ptCurPoint, ptTorusCen, vtMove) ; - if ( dPointAxSqLen > ( dMaxRad - dMinRad) * ( dMaxRad - dMinRad) && dPointAxSqLen < dSqMaxRad) { - double dNonLinearPart = sqrt( max( dSqMinRad - ( dMaxRad - sqrt( dPointAxSqLen)) * ( dMaxRad - sqrt( dPointAxSqLen)), 0.)) ; - double dLeakDist = ( ptCurPoint - ptTorusCen) * vtMove - dNonLinearPart ; + if ( dPointAxSqLen > dSqInnRad && dPointAxSqLen < dSqMaxRad) { + double dDeltaRad = dMaxRad - sqrt( dPointAxSqLen) ; + double dSqDeltaLeakDist = dSqMinRad - dDeltaRad * dDeltaRad ; + double dDeltaLeakDist = ( dSqDeltaLeakDist > 0. ? sqrt( dSqDeltaLeakDist) : 0.) ; + double dLeakDist = ( ptCurPoint - ptTorusCen) * vtMove - dDeltaLeakDist ; if ( dLeakDist > dMaxLeakDist) dMaxLeakDist = dLeakDist ; } } + // passo al successivo intervallo + bFound = SegLimits.GetNext( dLen1, dLen2) ; } - return dMaxLeakDist ; }