EgtGeomKernel :
- migliorie a tori inversi di Tool Collision Avoidance.
This commit is contained in:
Dario Sassi
+51
-74
@@ -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 ;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user