//---------------------------------------------------------------------------- // EgalTech 2018-2020 //---------------------------------------------------------------------------- // File : CAvToolTriangle.cpp Data : 03.12.18 Versione : 2.2k1 // Contenuto : Implementazione delle funzioni ToolTriangleCollisionAvoid. // // // // Modifiche : 10.03.18 LM Creazione modulo. // 28.10.20 LM Funzioni ausiliarie spostate nel module CDeUtility.cpp. // //---------------------------------------------------------------------------- #include "stdafx.h" #include "CurveLine.h" #include "CurveArc.h" #include "CAvToolTriangle.h" #include "IntersLineSurfStd.h" #include "IntersLineTria.h" #include "CDeUtility.h" #include "/EgtDev/Include/EGkDistPointLine.h" #include "/EgtDev/Include/EGkIntervals.h" #include "/EgtDev/Include/ENkPolynomialRoots.h" #include "/EgtDev/Include/EgtNumUtils.h" using namespace std ; //---------------------------------------------------------------------------- static bool GetTopTapFromPrevCurve( const ICurve* pPrevCurve) ; static bool GetBotTapFromNextCurve( const ICurve* pNextCurve) ; // Sfera static double CAvSphereTriangle( const Point3d& ptSpheCen, double dSpheRad, const Triangle3d& trTria, const Vector3d& vtMove) ; static double SpherePointEscapeDist( const Point3d& ptSpheCen, double dSpheRad, const Point3d ptP, const Vector3d& vtMove) ; static double SphereSegmentEscapeDist( const Point3d& ptSpheCen, double dSpheRad, const Point3d& ptSeg, const Vector3d& vtSegDir, double dSegLen, const Vector3d& vtMove) ; static double SphereTriaInteriorEscapeDist( const Point3d& ptSpheCen, double dSpheRad, const Triangle3d& trTria, const Vector3d& vtMove) ; // Cilindro static double CAvCylinderTriangle( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad, const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot) ; static double CylPointEscapeDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dCylHei, double dCylRad, const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot) ; static double CylSegmentEscapeDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dCylHei, double dCylRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove, bool bTop, bool bBot) ; static double CylTriaInteriorEscapeDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dCylHei, double dCylRad, const Triangle3d& trTria, const Vector3d& vtMove) ; static double CylPointEscapeDistGenMot( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad, const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot) ; static double CylSegmentEscapeDistGenMot( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove, bool bTop, bool bBot) ; static double CylTriaInteriorEscapeDistGenMot( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad, const Triangle3d& trTria, const Vector3d& vtMove) ; // Tronco di cono static double CAvTrConeTriangle( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot) ; static double TrConePointEscapeDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Point3d& ptP, const Vector3d& vtMove) ; static double TrConeSegmentEscapeDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ; static double TrConeTriaInteriorEscapeDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) ; static double TrConePointEscapeDistOrtMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot) ; static double TrConeSegmentEscapeDistOrtMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove, bool bTop, bool bBot) ; static double TrConeTriaInteriorEscapeDistOrtMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) ; static double TrConePointEscapeDistGenMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot) ; static double TrConeSegmentEscapeDistGenMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove, bool bTop, bool bBot) ; static double TrConeTriaInteriorEscapeDistGenMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) ; // Toro convesso static double CAvTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot) ; static double TorusPointEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad, const Point3d& ptP, const Vector3d& vtMove) ; static double TorusSegmentEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ; static double TorusTriaInteriorEscapeDistLongMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Triangle3d& trTria, const Vector3d& vtMove) ; static double TorusPointEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Point3d& ptP, const Vector3d& vtMove) ; static double TorusSegmentEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove, bool bTop, bool bBot) ; static double TorusTriaInteriorEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Triangle3d& trTria, const Vector3d& vtMove) ; static double TorusPointEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Point3d& ptP, const Vector3d& vtMove) ; static double TorusSegmentEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ; static double TorusTriaInteriorEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Triangle3d& trTria, const Vector3d& vtMove) ; // Toro concavo static double CAvConcaveTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot) ; static double ConcaveTorusPointEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad, const Point3d& ptP, const Vector3d& vtMove) ; static double ConcaveTorusSegmentEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ; static double ConcaveTorusPointEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Point3d& ptP, const Vector3d& vtMove) ; static double ConcaveTorusSegmentEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove, bool bTop, bool bBot) ; static double ConcaveTorusPointEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Point3d& ptP, const Vector3d& vtMove) ; static double ConcaveTorusSegmentEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ; // Disco static double CAvDiskTriangle( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad, const Triangle3d& trTria, const Vector3d& vtMove) ; static double DiskPointEscapeDistLongMot( const Point3d& ptDiskCen, double dDiskRad, const Point3d& ptP, const Vector3d& vtMove) ; static double DiskSegmentEscapeDistLongMot( const Point3d& ptDiskCen, double dDiskRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ; static double DiskTriaInteriorEscapeDistLongMot( const Point3d& ptDiskCen, double dDiskRad, const Triangle3d& trTria, const Vector3d& vtMove) ; static double DiskPointEscapeDistOrtMot( const Point3d& ptDisc, const Vector3d& vtDiskAx, double dDiscRad, const Point3d& ptP, const Vector3d& vtMove) ; static double DiskSegmentEscapeDistOrtMot( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ; static double DiskTriaInteriorEscapeDistOrtMot( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad, const Triangle3d& trTria, const Vector3d& vtMove) ; static double DiskSegmentEscapeDistGenMot( double dDiskRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ; static double DiskTriaInteriorEscapeDistGenMot( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad, const Triangle3d& trTria, const Vector3d& vtMove) ; //---------------------------------------------------------------------------- // La funzione determina la minima distanza di allontanamento lungo una direzione fissata // per evitare la collisione tra un utensile ed un triangolo. // Se nella posizione iniziale non c'è gia collisione si restituisce il valore 0. // In caso di errore si restituisce il valore -1. double CAvToolTriangle( const Tool& tlTool, const Point3d& ptToolOrig, const Vector3d& vtToolAx, const Triangle3d& trTria, const Vector3d& vtMove) { // Non ha senso che il movimento sia in direzione "opposta" a quella dell'asse utensile if ( vtMove * vtToolAx < - 10 * EPS_ZERO) return -1. ; // Se avvicinamento non devo fare nulla if ( vtMove * trTria.GetN() < - EPS_ZERO) return 0. ; // Tolleranza su dischi (parti piatte perpendicolari all'asse utensile) double dEpsilon = ( vtMove * vtToolAx > 0.5 ? 0 : EPS_SMALL) ; // se utensile cilindrico if ( tlTool.GetType() == Tool::CYLMILL) { // parametri geometrici double dHeigth = tlTool.GetHeigth() ; double dRadius = tlTool.GetRadius() ; // prima determino l'allontanamento del disco inferiore double dDist = CAvDiskTriangle( ptToolOrig - ( dHeigth - dEpsilon) * vtToolAx, vtToolAx, dRadius, trTria, vtMove) ; if ( dDist < - EPS_SMALL) return dDist ; // poi verifico quello del cilindro (tenendo conto di quanto è stata allontanato il disco) double dDist2 = CAvCylinderTriangle( ptToolOrig + dDist * vtMove, vtToolAx, dHeigth, dRadius, trTria, vtMove, false, false) ; if ( dDist2 < - EPS_SMALL) return dDist2 ; // verifico quello del disco sopra double dDist3 = CAvDiskTriangle( ptToolOrig - dEpsilon * vtToolAx + ( dDist + dDist2) * vtMove, vtToolAx, dRadius, trTria, vtMove) ; if ( dDist3 < - EPS_SMALL) return dDist3 ; return ( dDist + dDist2 + dDist3) ; } // se utensile sferico else if ( tlTool.GetType() == Tool::BALLMILL) { // parametri geometrici double dCylHeigth = tlTool.GetHeigth() - tlTool.GetTipHeigth() ; double dRadius = tlTool.GetRadius() ; // prima determino l'allontanamento della sfera Point3d ptSpheOrig = ptToolOrig - dCylHeigth * vtToolAx ; double dDist = CAvSphereTriangle( ptSpheOrig, dRadius, trTria, vtMove) ; if ( dDist < - EPS_SMALL) return dDist ; // poi verifico quello del cilindro (tenendo conto di quanto è stata allontanata la sfera) if ( dCylHeigth < EPS_SMALL) return dDist ; Point3d ptCylOrig = ptToolOrig + vtMove * dDist ; double dDist2 = CAvCylinderTriangle( ptCylOrig, vtToolAx, dCylHeigth, dRadius, trTria, vtMove, false, true) ; if ( dDist2 < - EPS_SMALL) return dDist2 ; // verifico quello del disco sopra Point3d ptDiskUpOrig = ptCylOrig + dDist2 * vtMove ; double dDist3 = CAvDiskTriangle( ptDiskUpOrig - dEpsilon * vtToolAx, vtToolAx, dRadius, trTria, vtMove) ; if ( dDist3 < - EPS_SMALL) return dDist3 ; return ( dDist + dDist2 + dDist3) ; } // se utensile a naso di toro else if ( tlTool.GetType() == Tool::BULLNOSEMILL) { // parametri geometrici double dCylHeigth = tlTool.GetHeigth() - tlTool.GetTipHeigth() ; // prima determino l'allontanamento del disco inferiore double dDist = CAvDiskTriangle( ptToolOrig - ( tlTool.GetHeigth() - dEpsilon) * vtToolAx, vtToolAx, tlTool.GetTipRadius(), trTria, vtMove) ; if ( dDist < - EPS_SMALL) return dDist ; // poi verifico quello del toro (tenendo conto di quanto è stata allontanato il disco) Point3d ptTorusCen = ptToolOrig - dCylHeigth * vtToolAx ; double dDist2 = CAvTorusTriangle( ptTorusCen + dDist * vtMove, vtToolAx, tlTool.GetRadius() - tlTool.GetCornRadius(), tlTool.GetCornRadius(), trTria, vtMove, true, false) ; if ( dDist2 < - EPS_SMALL) return dDist2 ; // infine verifico quello del cilindro (tenendo conto dei precedenti allontanamenti) if ( dCylHeigth < EPS_SMALL) return ( dDist + dDist2) ; Point3d ptCylOrig = ptToolOrig + vtMove * ( dDist + dDist2) ; double dDist3 = CAvCylinderTriangle( ptCylOrig, vtToolAx, dCylHeigth, tlTool.GetRadius(), trTria, vtMove, false, true) ; if ( dDist3 < - EPS_SMALL) return dDist3 ; // verifico quello del disco sopra Point3d ptDiskUpOrig = ptCylOrig + vtMove * dDist3 ; double dDist4 = CAvDiskTriangle( ptDiskUpOrig - dEpsilon * vtToolAx, vtToolAx, tlTool.GetRadius(), trTria, vtMove) ; if ( dDist4 < - EPS_SMALL) return dDist4 ; return ( dDist + dDist2 + dDist3 + dDist4) ; } // se utensile conico else if ( tlTool.GetType() == Tool::CONEMILL) { // parametri geometrici Point3d ptMinBase ; Vector3d vtConeAx ; bool bTop, bBot ; if ( tlTool.GetRadius() > tlTool.GetTipRadius()) { ptMinBase = ptToolOrig - tlTool.GetHeigth() * vtToolAx ; vtConeAx = vtToolAx ; bTop = true ; bBot = false ; } else { ptMinBase = ptToolOrig - ( tlTool.GetHeigth() - tlTool.GetTipHeigth()) * vtToolAx ; vtConeAx = - vtToolAx ; bTop = false ; bBot = true ; } double dMinR = min( tlTool.GetRadius(), tlTool.GetTipRadius()) ; double dMaxR = max( tlTool.GetRadius(), tlTool.GetTipRadius()) ; double dCylHeigth = tlTool.GetHeigth() - tlTool.GetTipHeigth() ; // prima determino l'allontanamento del disco inferiore double dDist = CAvDiskTriangle( ptToolOrig - ( tlTool.GetHeigth() - dEpsilon) * vtToolAx, vtToolAx, tlTool.GetTipRadius(), trTria, vtMove) ; if ( dDist < - EPS_SMALL) return dDist ; // poi verifico quello del cono (tenendo conto di quanto è stata allontanato il disco) double dDist2 = CAvTrConeTriangle( ptMinBase + dDist * vtMove, vtConeAx, dMinR, dMaxR, tlTool.GetTipHeigth(), trTria, vtMove, bTop, bBot) ; if ( dDist2 < - EPS_SMALL) return dDist2 ; // infine verifico quello del cilindro (tenendo conto dei precedenti allontanamenti) if ( dCylHeigth < EPS_SMALL) return ( dDist + dDist2) ; Point3d ptCylOrig = ptToolOrig + vtMove * ( dDist + dDist2) ; double dDist3 = CAvCylinderTriangle( ptCylOrig, vtToolAx, dCylHeigth, tlTool.GetRadius(), trTria, vtMove, false, true) ; if ( dDist3 < - EPS_SMALL) return dDist3 ; // verifico quello del disco sopra Point3d ptDiskUpOrig = ptCylOrig + vtMove * dDist3 ; double dDist4 = CAvDiskTriangle( ptDiskUpOrig - dEpsilon * vtToolAx, vtToolAx, tlTool.GetRadius(), trTria, vtMove) ; if ( dDist4 < - EPS_SMALL) return dDist4 ; return ( dDist + dDist2 + dDist3 + dDist4) ; } // se utensile generico else if ( tlTool.GetType() == Tool::GEN) { // distanza di allontanamento double dDist = 0 ; // riferimento per componenti Point3d ptCompOrig = ptToolOrig - tlTool.GetHeigth() * vtToolAx ; // analizzo le curve del profilo a partire dall'ultima const CurveComposite& ToolProfile = tlTool.GetApproxOutline() ; int nCrv = ToolProfile.GetCurveCount() - 1 ; const ICurve* pCurve = ToolProfile.GetCurve( nCrv) ; const ICurve* pNextCurve = nullptr ; while ( pCurve != nullptr) { // distanza di allontanamento del tratto corrente double dDist2 = 0 ; // curva precedente const ICurve* pPrevCurve = ToolProfile.GetCurve( -- nCrv) ; // Se segmento if ( pCurve->GetType() == CRV_LINE) { // Recupero gli estremi const ICurveLine* pLine = GetCurveLine( pCurve) ; const Point3d& ptStart = pLine->GetStart() ; const Point3d& ptEnd = pLine->GetEnd() ; // Ne determino l'altezza double dHeight = abs( ptStart.y - ptEnd.y) ; if ( dHeight <= EPS_SMALL) { double dRadius = max( ptStart.x, ptEnd.x) ; // Se disco verso il basso dell'utensile if ( ptStart.x > ptEnd.x) dDist2 = CAvDiskTriangle( ptCompOrig + dEpsilon * vtToolAx, vtToolAx, dRadius, trTria, vtMove) ; // Se disco verso l'alto else dDist2 = CAvDiskTriangle( ptCompOrig - dEpsilon * vtToolAx, vtToolAx, dRadius, trTria, vtMove) ; } else { // Verifiche curva precedente per eventuale tappo sopra bool bTop = GetTopTapFromPrevCurve( pPrevCurve) ; // Verifiche curva successiva per eventuale tappo sotto bool bBot = GetBotTapFromNextCurve( pNextCurve) ; // Calcolo distanza di allontanamento del componente corrente if ( abs( ptStart.x - ptEnd.x) < EPS_SMALL) { double dRadius = ptStart.x ; // riferimento del cilindro in alto ptCompOrig += dHeight * vtToolAx ; dDist2 = CAvCylinderTriangle( ptCompOrig, vtToolAx, dHeight, dRadius, trTria, vtMove, bTop, bBot) ; } else if ( ptStart.x > ptEnd.x) { double dMaxRad = ptStart.x ; double dMinRad = ptEnd.x ; // riferimento del cono in basso (base più piccola) dDist2 = CAvTrConeTriangle( ptCompOrig, vtToolAx, dMinRad, dMaxRad, dHeight, trTria, vtMove, bTop, bBot) ; // sposto riferimento in alto ptCompOrig += dHeight * vtToolAx ; } else if ( ptStart.x < ptEnd.x) { double dMaxRad = ptEnd.x ; double dMinRad = ptStart.x ; // riferimento del cono in alto (base più piccola) ptCompOrig += dHeight * vtToolAx ; dDist2 = CAvTrConeTriangle( ptCompOrig, - vtToolAx, dMinRad, dMaxRad, dHeight, trTria, vtMove, bTop, bBot) ; } } } // Se arco else if ( pCurve->GetType() == CRV_ARC) { // Recupero estremi, centro e raggio const ICurveArc* pArc = GetCurveArc( pCurve) ; Point3d ptStart ; pArc->GetStartPoint( ptStart) ; Point3d ptEnd ; pArc->GetEndPoint( ptEnd) ; const Point3d& ptCen = pArc->GetCenter() ; double dRadius = pArc->GetRadius() ; double dAngToCen = pArc->GetAngCenter() ; // Calcolo della distanza di allontanamento del componente corrente // Se sfera if ( abs( ptCen.x) < EPS_SMALL) { // riferimento sul centro sfera ptCompOrig += ( ptCen.y - ptEnd.y) * vtToolAx ; dDist2 = CAvSphereTriangle( ptCompOrig, dRadius, trTria, vtMove) ; // sposto riferimento in alto ptCompOrig += ( ptStart.y - ptCen.y) * vtToolAx ; } // altrimenti toro else { // Verifiche curva precedente per eventuale tappo sopra bool bTop = GetTopTapFromPrevCurve( pPrevCurve) ; // Verifiche curva successiva per eventuale tappo sotto bool bBot = GetBotTapFromNextCurve( pNextCurve) ; // riferimento sul centro toro ptCompOrig += ( ptCen.y - ptEnd.y) * vtToolAx ; // verifica presenza semitori bool bHalfTorusDown = ( ptEnd.y < ptCen.y - EPS_SMALL) ; bool bHalfTorusUp = ( ptStart.y > ptCen.y + EPS_SMALL) ; // semi-toro sotto if ( bHalfTorusDown) { double dMyDist ; if ( dAngToCen < 0) dMyDist = CAvTorusTriangle( ptCompOrig, vtToolAx, ptCen.x, dRadius, trTria, vtMove, ( bHalfTorusUp ? true : bTop), bBot) ; else dMyDist = CAvConcaveTorusTriangle( ptCompOrig, - vtToolAx, ptCen.x, dRadius, trTria, vtMove, ( bHalfTorusUp ? true : bTop), bBot) ; if ( dMyDist < - EPS_SMALL) return dMyDist ; dDist2 = max( dDist2, dMyDist) ; } // semi-toro sopra if ( bHalfTorusUp) { double dMyDist ; if ( dAngToCen < 0) dMyDist = CAvTorusTriangle( ptCompOrig, - vtToolAx, ptCen.x, dRadius, trTria, vtMove, bTop, ( bHalfTorusDown ? true : bBot)) ; else dMyDist = CAvConcaveTorusTriangle( ptCompOrig, vtToolAx, ptCen.x, dRadius, trTria, vtMove, bTop, ( bHalfTorusDown ? true : bBot)) ; if ( dMyDist < - EPS_SMALL) return dMyDist ; dDist2 = max( dDist2, dMyDist) ; } // sposto riferimento in alto ptCompOrig += ( ptStart.y - ptCen.y) * vtToolAx ; } } // Aggiornamento distanza allontanamento if ( dDist2 < - EPS_SMALL) return dDist2 ; dDist += dDist2 ; ptCompOrig += dDist2 * vtMove ; // passo alla curva precedente pNextCurve = pCurve ; pCurve = pPrevCurve ; } return dDist ; } // altrimenti utensile di tipo non gestito else return - 1. ; } //---------------------------------------------------------------------------- bool GetTopTapFromPrevCurve( const ICurve* pPrevCurve) { if ( pPrevCurve == nullptr) return false ; if ( pPrevCurve->GetType() == CRV_LINE) { const ICurveLine* pPrevLine = GetCurveLine( pPrevCurve) ; const Point3d& ptPrevStart = pPrevLine->GetStart() ; const Point3d& ptPrevEnd = pPrevLine->GetEnd() ; if ( abs( ptPrevStart.y - ptPrevEnd.y) > EPS_SMALL || ptPrevStart.x > ptPrevEnd.x) return true ; } else if ( pPrevCurve->GetType() == CRV_ARC) { const ICurveArc* pPrevArc = GetCurveArc( pPrevCurve) ; const Point3d& ptPrevCen = pPrevArc->GetCenter() ; if ( abs( ptPrevCen.x) > EPS_SMALL) return true ; } return false ; } //---------------------------------------------------------------------------- bool GetBotTapFromNextCurve( const ICurve* pNextCurve) { if ( pNextCurve == nullptr) return false ; if ( pNextCurve->GetType() == CRV_LINE) { const ICurveLine* pNextLine = GetCurveLine( pNextCurve) ; const Point3d& ptNextStart = pNextLine->GetStart() ; const Point3d& ptNextEnd = pNextLine->GetEnd() ; if ( abs( ptNextStart.y - ptNextEnd.y) > EPS_SMALL || ptNextStart.x < ptNextEnd.x) return true ; } else if ( pNextCurve->GetType() == CRV_ARC) { const ICurveArc* pNextArc = GetCurveArc( pNextCurve) ; const Point3d& ptNextCen = pNextArc->GetCenter() ; if ( abs( ptNextCen.x) > EPS_SMALL) return true ; } return false ; } //---------------------------------------------------------------------------- // **** SFERA **** //---------------------------------------------------------------------------- // La funzione determina la minima distanza di allontanamento lungo una direzione fissata // per evitare la collisione tra una sfera ed un triangolo. double CAvSphereTriangle( const Point3d& ptSpheCen, double dSpheRad, const Triangle3d& trTria, const Vector3d& vtMove) { // Se la sfera sta già tutta dalla parte esterna del triangolo, non va ulteriormente allontanata // Posizione del centro sfera rispetto al piano del triangolo Vector3d vtPCen = ptSpheCen - trTria.GetP( 0) ; if ( vtPCen * trTria.GetN() > dSpheRad - EPS_SMALL) return 0. ; // Se la fetta di spazio delimitata dal piano sotto alla sfera rispetto alla direzione // di allontanamento non sia già sopra al triangolo (tutti i vertici del triangolo sono sotto). Point3d ptBase = ptSpheCen - dSpheRad * vtMove ; double dMaxDistV = PointPlaneSignedDist( trTria.GetP( 0), ptBase, vtMove) ; dMaxDistV = max( dMaxDistV, PointPlaneSignedDist( trTria.GetP( 1), ptBase, vtMove)) ; dMaxDistV = max( dMaxDistV, PointPlaneSignedDist( trTria.GetP( 2), ptBase, vtMove)) ; if ( dMaxDistV < EPS_SMALL) return 0. ; // Valuto allontanamento da interno triangolo double dEscapeDist = SphereTriaInteriorEscapeDist( ptSpheCen, dSpheRad, trTria, vtMove) ; if ( dEscapeDist > 0.) return dEscapeDist ; // Valuto le distanze di allontanamento dai segmenti (con i loro estremi) e prendo la maggiore for ( int nSeg = 0 ; nSeg < 3 ; ++ nSeg) { Vector3d vtSeg = trTria.GetP( ( nSeg + 1) % 3) - trTria.GetP( nSeg) ; double dSegLen = vtSeg.Len() ; vtSeg /= dSegLen ; double dCurEscapeDist = max( SpherePointEscapeDist( ptSpheCen, dSpheRad, trTria.GetP( nSeg), vtMove), SphereSegmentEscapeDist( ptSpheCen, dSpheRad, trTria.GetP( nSeg), vtSeg, dSegLen, vtMove)) ; if ( dCurEscapeDist > dEscapeDist) dEscapeDist = dCurEscapeDist ; } return dEscapeDist ; } //---------------------------------------------------------------------------- // Calcola la distanza di allontanamento lungo una direzione fissata di una sfera da un punto. double SpherePointEscapeDist( const Point3d& ptSpheCen, double dSpheRad, const Point3d ptP, const Vector3d& vtMove) { Vector3d vtOR = ptP - ptSpheCen ; double dDotORMot = vtOR * vtMove ; Vector3d vtOROrt = vtOR - dDotORMot * vtMove ; double dSqOROrtLen = vtOROrt.SqLen() ; double dSqSphRad = dSpheRad * dSpheRad ; if ( dSqOROrtLen > dSqSphRad - 2 * dSpheRad * EPS_SMALL) return 0. ; else return max( dDotORMot + sqrt( max( dSqSphRad - dSqOROrtLen, 0.)), 0.) ; } //---------------------------------------------------------------------------- // Calcola la distanza di allontanamento lungo una direzione fissata di una sfera da un segmento. // Il segmento è descritto da punto iniziale, versore della direzione e lunghezza. double SphereSegmentEscapeDist( const Point3d& ptSpheCen, double dSpheRad, const Point3d& ptSeg, const Vector3d& vtSegDir, double dSegLen, const Vector3d& vtMove) { // Controllo con l'interno del segmento double dU[2] ; int nTanPointNum = SphereLineTangentPoints( ptSpheCen, dSpheRad - EPS_SMALL, ptSeg, vtSegDir, dSegLen, vtMove, dU[0], dU[1]) ; double dEscapeDist = 0. ; for ( int nSol = 0 ; nSol < nTanPointNum && nTanPointNum != 3 ; ++ nSol) { if ( dU[nSol] < dEscapeDist) continue ; Point3d ptC = ptSpheCen + dU[nSol] * vtMove ; double dSegPar = ( ptC - ptSeg) * vtSegDir ; if ( dSegPar > 0 && dSegPar < dSegLen) dEscapeDist = dU[nSol] ; } return dEscapeDist ; } //---------------------------------------------------------------------------- // Calcola la distanza di allontanamento lungo una direzione fissata di una sfera da un piano. double SphereTriaInteriorEscapeDist( const Point3d& ptSpheCen, double dSpheRad, const Triangle3d& trTria, const Vector3d& vtMove) { double dCosNM = trTria.GetN() * vtMove ; // Se la direzione di allontanamento sta nel piano if ( abs( dCosNM) < EPS_ZERO) return 0. ; // altrimenti ... double dEscapeDist = ( dSpheRad - ( ptSpheCen - trTria.GetP( 0)) * trTria.GetN()) / dCosNM ; if ( dEscapeDist < 0.) return 0. ; Point3d ptTan = ptSpheCen + dEscapeDist * vtMove - dSpheRad * trTria.GetN() ; if ( IsPointInsideTriangle( ptTan, trTria, TriangleType::EXACT)) return dEscapeDist ; return 0. ; } //---------------------------------------------------------------------------- // **** CILINDRO **** //---------------------------------------------------------------------------- // La funzione determina la minima distanza di allontanamento lungo una direzione fissata // per evitare la collisione tra un cilindro ed un triangolo. double CAvCylinderTriangle( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad, const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot) { double dDiskEscapeDist = 0 ; if ( bTop) dDiskEscapeDist = CAvDiskTriangle( ptCylOrig, vtCylAx, dRad, trTria, vtMove) ; // Movimento lungo la direzione dell'asse del cilindro if ( AreSameVectorApprox( vtCylAx, vtMove)) { // la distanza di fuga è determinata dal disco // o dai componenti sotto il cilindro. return dDiskEscapeDist ; } // Movimento perpendicolare all'asse del cilindro else if ( AreOrthoApprox( vtCylAx, vtMove)) { double dEscapeDist = CylTriaInteriorEscapeDistOrtMotion( ptCylOrig, vtCylAx, dHeigth, dRad, trTria, vtMove) ; // Se distanza da interno positiva abbiamo finito if ( dEscapeDist > EPS_SMALL) return dEscapeDist ; // PUNTI for ( int nVrt = 0 ; nVrt < 3 ; ++ nVrt) { double dCurDist = CylPointEscapeDistOrtMotion( ptCylOrig, vtCylAx, dHeigth, dRad, trTria.GetP( nVrt), vtMove, bTop, bBot) ; if ( dEscapeDist < dCurDist) dEscapeDist = dCurDist ; } // SEGMENTI for ( int nVrtS = 0 ; nVrtS < 3 ; ++ nVrtS) { int nVrtE = ( nVrtS + 1) % 3 ; Vector3d vtSeg = trTria.GetP( nVrtE) - trTria.GetP( nVrtS) ; double dSegLen = vtSeg.Len() ; vtSeg /= dSegLen ; double dCurDist = CylSegmentEscapeDistOrtMotion( ptCylOrig, vtCylAx, dHeigth, dRad, trTria.GetP( nVrtS), vtSeg, dSegLen, vtMove, bTop, bBot) ; if ( dEscapeDist < dCurDist) dEscapeDist = dCurDist ; } return max( dEscapeDist, dDiskEscapeDist) ; } // Movimento generico else if ( vtMove * vtCylAx > 0.) { // Controlli preliminari sull'interferenza fra cilindro e triangolo Vector3d vtOrtMv = vtMove - ( vtMove * vtCylAx) * vtCylAx ; if ( ! vtOrtMv.Normalize()) return - 1. ; Vector3d vtR0 = trTria.GetP( 0) - ptCylOrig ; Vector3d vtR1 = trTria.GetP( 1) - ptCylOrig ; Vector3d vtR2 = trTria.GetP( 2) - ptCylOrig ; if ( vtR0 * vtOrtMv < - dRad && vtR1 * vtOrtMv < - dRad && vtR2 * vtOrtMv < - dRad) return 0. ; Vector3d vtLat = vtCylAx ^ vtOrtMv ; if ( ( vtR0 * vtLat < - dRad && vtR1 * vtLat < - dRad && vtR2 * vtLat < - dRad) || ( vtR0 * vtLat > dRad && vtR1 * vtLat > dRad && vtR2 * vtLat > dRad)) return 0. ; // Calcolo la distanza di allontanamento double dEscapeDist = 0. ; // distanza di allontanamento dall'interno dEscapeDist = max( dEscapeDist, CylTriaInteriorEscapeDistGenMot( ptCylOrig, vtCylAx, dHeigth, dRad, trTria, vtMove)) ; if ( dEscapeDist > EPS_SMALL) return dEscapeDist ; // Distanza di allontanamento dalla frontiera for ( int nV = 0 ; nV < 3 ; ++ nV) { Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ; double dSegLen = vtSeg.Len() ; vtSeg /= dSegLen ; dEscapeDist = max( dEscapeDist, CylPointEscapeDistGenMot( ptCylOrig, vtCylAx, dHeigth, dRad, trTria.GetP( nV), vtMove, bTop, bBot)) ; dEscapeDist = max( dEscapeDist, CylSegmentEscapeDistGenMot( ptCylOrig, vtCylAx, dHeigth, dRad, trTria.GetP( nV), vtSeg, dSegLen, vtMove, bTop, bBot)) ; } return max( dEscapeDist, dDiskEscapeDist) ; ; } // Errore else return -1. ; } //---------------------------------------------------------------------------- // Restituisce la distanza di fuga di un cilindro da un punto nel caso di moto // ortogonale all'asse di simmetria. Il cilindro è descritto da raggio e altezza. // Il suo moto è descritto dalla posizione iniziale, dal versore (NORMA UNITARIA) // dell'asse di simmetria e dal versore della direzione del moto. double CylPointEscapeDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dCylHei, double dCylRad, const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot) { double dTopTol = bTop ? EPS_SMALL : - EPS_SMALL ; double dBotTol = bBot ? - EPS_SMALL : EPS_SMALL ; double dSqRad = dCylRad * dCylRad ; Vector3d vtRP = ptP - ptCylOrig ; // Se il punto sta al di sopra o al di sotto rispetto al cilindro abbiamo finito double dDotRPCylAx = vtRP * vtCylAx ; if ( dDotRPCylAx > dTopTol || dDotRPCylAx < - dCylHei + dBotTol) return 0. ; Vector3d vtPlaneRP = vtRP - dDotRPCylAx * vtCylAx ; // Se il punto sta dietro al cilindro abbiamo finito double dDotPlaneRPRemDir = vtPlaneRP * vtMove ; if ( dDotPlaneRPRemDir < 0. && vtPlaneRP.SqLen() > dSqRad) return 0. ; Vector3d vtPlaneOrtRP = vtPlaneRP - vtPlaneRP * vtMove * vtMove ; // Se il punto è a destra o sinistra del cilindro abbiamo finito double dSqOrtLen = vtPlaneOrtRP.SqLen() ; if ( dSqOrtLen > dSqRad - 2 * dCylRad * EPS_SMALL) return 0. ; // Calcolo e restituisco la distanza return dDotPlaneRPRemDir + sqrt( max( dSqRad - dSqOrtLen, 0.)) ; } //---------------------------------------------------------------------------- // Restituisce la distanza di allontanamento di un cilindro da un segmento. // Il cilindro è descritto da raggio e altezza. // Il suo moto è descritto dalla posizione iniziale, dal versore (NORMA UNITARIA) // dell'asse di simmetria e dal versore della direzione del moto. double CylSegmentEscapeDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dCylHei, double dCylRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove, bool bTop, bool bBot) { double dTopTol = ( bTop ? EPS_SMALL : - EPS_SMALL) ; double dBotTol = ( bBot ? - EPS_SMALL : EPS_SMALL) ; // Riferimento con origine nel centro del disco nella posizione iniziale. // X := vtCylAx, Y:= vtMove, Z:= vtCylAx ^ vtMove. Vector3d vtPlane = vtCylAx ^ vtMove ; vtPlane.Normalize() ; Point3d ptSegEnd = ptSeg + dSegLen * vtSeg ; Vector3d vtSegStart = ptSeg - ptCylOrig ; Vector3d vtSegEnd = ptSegEnd - ptCylOrig ; double dCordStart1 = vtSegStart * vtCylAx ; double dCordEnd1 = vtSegEnd * vtCylAx ; // Se entrambi gli estremi sono sopra o sotto, non ci può essere interferenza if ( ( dCordStart1 > dTopTol && dCordEnd1 > dTopTol) || ( dCordStart1 < - dCylHei + dBotTol && dCordEnd1 < - dCylHei + dBotTol)) return 0. ; Vector3d vtSegStart1 = dCordStart1 * vtCylAx ; Vector3d vtSegEnd1 = dCordEnd1 * vtCylAx ; Vector3d vtSegStart23 = vtSegStart - vtSegStart1 ; Vector3d vtSegEnd23 = vtSegEnd - vtSegEnd1 ; double dCordStart3 = vtSegStart23 * vtPlane ; double dCordEnd3 = vtSegEnd23 * vtPlane ; // Se entrambi gli estremi del segmento sono a un lato, non ci può essere interferenza if ( ( dCordStart3 > dCylRad && dCordEnd3 > dCylRad) || ( dCordStart3 < - dCylRad && dCordEnd3 < - dCylRad)) return 0. ; double dEscapeDist = 0. ; // Se il vettore del segmento è parallelo all'asse del cilindro, // il suo prodotto vettoriale con il versore dell'asse è nullo. Vector3d vtRad = vtCylAx ^ vtSeg ; if ( ! vtRad.Normalize()) { if ( dCordStart3 > - dCylRad + EPS_SMALL && dCordStart3 < dCylRad - EPS_SMALL) dEscapeDist = max( dEscapeDist, ( ptSeg - ptCylOrig) * vtMove) ; } // Se il versore radiale NON è ortogonale a quello di movimento può esserci tangenza, // altrimenti il versore del segmento non ha componenti ortogonali al piano // generato da asse cilindro e moto e non può esserci tangenza. else if ( abs( vtRad * vtMove) > EPS_SMALL) { // Nella posizione finale il cilindro e la retta del segmento sono tangenti. // Vettore che spicca dal punto di tangenza fra cilindro e retta // associata al segmento e termina sull'asse del cilindro. vtRad *= dCylRad ; // Lunghezza della componente del vettore radiale ortogonale alla linea di movimento double dDotRemRad = abs( vtRad * vtMove) ; double dOrtLen = sqrt( max( dCylRad * dCylRad - dDotRemRad * dDotRemRad, 0.)) ; // Cerco lungo la retta un punto che stia nel segmento e abbia distanza dal piano +/- dOrtLen Vector3d vtD = ptSeg - ptCylOrig ; double dDotPlaneD = - vtD * vtPlane ; double dDotPlaneSeg = - vtSeg * vtPlane ; double dPlusU = ( dOrtLen - dDotPlaneD) / dDotPlaneSeg ; double dMinusU = ( - dOrtLen - dDotPlaneD) / dDotPlaneSeg ; Point3d ptTanPlus = ptSeg + dPlusU * vtSeg ; Point3d ptTanMinus = ptSeg + dMinusU * vtSeg ; double dTanCordPlus1 = ( ptTanPlus - ptCylOrig) * vtCylAx ; double dTanCordMinus1 = ( ptTanMinus - ptCylOrig) * vtCylAx ; if ( ( dPlusU > - EPS_SMALL && dPlusU < dSegLen + EPS_SMALL) && ( dTanCordPlus1 < dTopTol && dTanCordPlus1 > - dCylHei + dBotTol)) { dEscapeDist = max( ( ( ptSeg - ptCylOrig) + dPlusU * vtSeg) * vtMove + dDotRemRad, 0.) ; } if ( dMinusU > - EPS_SMALL && dMinusU < dSegLen + EPS_SMALL && ( dTanCordMinus1 < dTopTol && dTanCordMinus1 > - dCylHei + dBotTol)) { double dNewDist = max( ( ( ptSeg - ptCylOrig) + dMinusU * vtSeg) * vtMove + dDotRemRad, 0.) ; dEscapeDist = max( dNewDist, dEscapeDist) ; } } return max( dEscapeDist, 0.) ; } //---------------------------------------------------------------------------- double CylTriaInteriorEscapeDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dCylHei, double dCylRad, const Triangle3d& trTria, const Vector3d& vtMove) { // La superficie laterale può trovarsi in estremo contatto con l'interno solo in tangenza. // Se non ci può essere tangenza guardo il disco sopra per le giunzioni if ( ! AreOrthoExact( vtCylAx, trTria.GetN())) return 0. ; // Ci può essere tangenza cerco segmento candidato a toccare il triangolo double dDist = 0. ; Point3d ptTouch0 = ptCylOrig - dCylRad * trTria.GetN() ; double dU = ( ( trTria.GetP( 0) - ptTouch0) * trTria.GetN()) / ( vtMove * trTria.GetN()) ; // Se avvicinamento, distanza nulla if ( dU < dDist) dDist = 0. ; ptTouch0 += ( dU * vtMove) ; Point3d ptTouch1 = ptTouch0 - dCylHei * vtCylAx ; Point3d ptInt0, ptInt1 ; int nIntType = IntersLineTria( ptTouch0, ptTouch1, trTria, ptInt0, ptInt1) ; // Il segmento interseca triangolo aggiorno la distanza di allontanamento if ( ! ( nIntType == ILTT_NO || nIntType == ILTT_VERT || nIntType == ILTT_IN || nIntType == ILTT_EDGE)) dDist = dU ; return dDist ; } //---------------------------------------------------------------------------- double CylPointEscapeDistGenMot( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad, const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot) { // Setto le tolleranze sui limiti del cilindro double dTopTol = ( bTop ? EPS_SMALL : - EPS_SMALL) ; double dBotTol = ( bBot ? - EPS_SMALL : EPS_SMALL) ; // Grandezze da cui dipendono i parametri double dMyRad = dRad - EPS_SMALL ; Vector3d vtR0 = ptP - ptCylOrig ; double dDotAxR0 = vtR0 * vtCylAx ; double dDotMoveAx = vtMove * vtCylAx ; // Definisco i parametri dell'equazione DBLVECTOR vdCoef( 3) ; DBLVECTOR vdRoots ; vdCoef[0] = vtR0 * vtR0 - dDotAxR0 * dDotAxR0 - dMyRad * dMyRad ; vdCoef[1] = 2 * ( dDotMoveAx * dDotAxR0 - vtR0 * vtMove) ; vdCoef[2] = 1 - dDotMoveAx * dDotMoveAx ; // Risolvo l'equazione int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ; // Ciclo sulle soluzioni per cercare la distanza di allontanamento double dEscapeDist = 0. ; for ( int n = 0 ; n < nRoot ; ++ n) { double dDotIntAx = ( vtR0 - vdRoots[n] * vtMove) * vtCylAx ; if ( vdRoots[n] > dEscapeDist && dDotIntAx < dTopTol && dDotIntAx > - dHeigth + dBotTol) dEscapeDist = vdRoots[n] ; } return dEscapeDist ; } //---------------------------------------------------------------------------- double CylSegmentEscapeDistGenMot( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove, bool bTop, bool bBot) { // Setto le tolleranze sui limiti del cilindro double dTopTol = ( bTop ? EPS_SMALL : - EPS_SMALL) ; double dBotTol = ( bBot ? - EPS_SMALL : EPS_SMALL) ; // Grandezze da cui dipendono i parametri double dMyRad = dRad - EPS_SMALL ; Vector3d vtR0 = ptSeg - ptCylOrig ; double dDotR0Ax = vtR0 * vtCylAx ; double dDotMoveAx = vtMove * vtCylAx ; double dDotSegAx = vtSeg * vtCylAx ; // Se l'asse del cilindro e il segmento hanno la medesima direzione if ( AreSameOrOppositeVectorApprox( vtCylAx, vtSeg)) { // Valuto distanze di allontanamento degli estremi del segmento; ovviamente esse, // a causa del parallelismo fra segmento e asse del cilindro, sono uguali tra loro. // Calcolo quindi solo quella del punto di partenza. Vector3d vtMoveOrt = vtMove - dDotMoveAx * vtCylAx ; Vector3d vtR0Ort = vtR0 - dDotR0Ax * vtCylAx ; DBLVECTOR vdCoef( 3) ; DBLVECTOR vdRoots ; vdCoef[0] = vtR0Ort * vtR0Ort - dMyRad * dMyRad ; vdCoef[1] = - 2 * vtR0Ort * vtMoveOrt ; vdCoef[2] = vtMoveOrt * vtMoveOrt ; // Risoluzione dell'equazione e studio delle soluzioni int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ; double dEscapeDist = 0. ; for ( int n = 0 ; n < nRoot ; ++ n) { // Se abbiamo trovato una potenziale nuova distanza di allontanamento if ( vdRoots[n] > dEscapeDist) { // Posizioni di contatto degli estremi e del punto medio del segmento Point3d ptTouch0 = ptSeg - vdRoots[n] * vtMove ; Point3d ptTouch1 = ptTouch0 + dSegLen * vtSeg ; Point3d ptTouchM = ( ptTouch0 + ptTouch1) / 2 ; double dDot0 = ( ptTouch0 - ptCylOrig) * vtCylAx ; double dDot1 = ( ptTouch1 - ptCylOrig) * vtCylAx ; double dDotM = ( ptTouchM - ptCylOrig) * vtCylAx ; // Se almeno uno dei tre punti di contatto è sul cilindro, la potenziale distanza è valida if ( ( dDot0 < dTopTol && dDot0 > - dHeigth + dBotTol) || ( dDot1 < dTopTol && dDot1 > - dHeigth + dBotTol) || ( dDotM < dTopTol && dDotM > - dHeigth + dBotTol)) dEscapeDist = vdRoots[n] ; } } return dEscapeDist ; } // L'equazione nel parametro del segmento è di secondo grado double dAlpha = 1 - dDotSegAx * dDotSegAx ; double dBeta1 = dDotSegAx * dDotMoveAx - vtSeg * vtMove ; double dBeta2 = vtR0 * vtSeg - dDotR0Ax * dDotSegAx ; double dGamma = 1 - dDotMoveAx * dDotMoveAx ; double dLambda = dDotR0Ax * dDotMoveAx - vtR0 * vtMove ; double dEta = vtR0 * vtR0 - dDotR0Ax * dDotR0Ax - dMyRad * dMyRad ; // Abbiamo un'equazione di secondo grado in due incognite. // Un'incognita rappresenta il parametro del segmento, l'altra // lo spostamento nella direzione di traslazione. // Se consideriamo come variabile il parametro del segmento e lo // spostamento come parametro dell'equazione, dobbiamo imporre // Delta = 0 per ottenere una nuova equazione nella sola variabile // spostamento. Le soluzioni di quest'ultima equazione sono candidate // a essere distanze di fuga. // Equazione Delta = 0 DBLVECTOR vdDeltaCoef( 3) ; DBLVECTOR vdDeltaRoots ; vdDeltaCoef[0] = 4 * ( dBeta2 * dBeta2 - dAlpha * dEta) ; vdDeltaCoef[1] = 8 * ( dBeta1 * dBeta2 - dAlpha * dLambda) ; vdDeltaCoef[2] = 4 * ( dBeta1 * dBeta1 - dAlpha * dGamma) ; // Risolvo l'equazione int nDeltaRoot = PolynomialRoots( 2, vdDeltaCoef, vdDeltaRoots) ; // Ciclo sulle soluzioni e per ogni soluzione valida valuto se la tangenza è // situata sul cilindro. double dEscapeDist = 0. ; for ( int n = 0 ; n < nDeltaRoot ; ++ n) { if ( vdDeltaRoots[n] > dEscapeDist) { double dS = - ( dBeta1 * vdDeltaRoots[n] + dBeta2) / dAlpha ; if ( dS > - EPS_SMALL && dS < dSegLen + EPS_SMALL) { Point3d ptTan = ptSeg + dS * vtSeg - vdDeltaRoots[n] * vtMove ; Vector3d vtTan = ptTan - ptCylOrig ; double dDotTanAx = vtTan * vtCylAx ; if ( dDotTanAx < dTopTol && dDotTanAx > - dHeigth + dBotTol) dEscapeDist = vdDeltaRoots[n] ; } } } return dEscapeDist ; } //---------------------------------------------------------------------------- double CylTriaInteriorEscapeDistGenMot( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad, const Triangle3d& trTria, const Vector3d& vtMove) { double dEscapeDist = 0. ; if ( ! AreOrthoApprox( vtCylAx, trTria.GetN()) || AreOrthoApprox( vtMove, trTria.GetN())) return dEscapeDist ; // Un punto candidato a essere di estremo contatto è ptP = ptCylOrig - dRad * trTria.GetN(), // dU è il parametro a cui la retta ptP + dU * vtMove interseca il piano del triangolo. double dU = ( dRad - ( ptCylOrig - trTria.GetP(0)) * trTria.GetN()) / ( vtMove * trTria.GetN()) ; if ( dU < EPS_SMALL) return dEscapeDist ; // Estremi del segmento, facente parte del cilindro, candidato ad // essere di estremo contatto, traslato sul piano del triangolo. Point3d ptTouch0 = ptCylOrig - dRad * trTria.GetN() + dU * vtMove ; Point3d ptTouch1 = ptTouch0 - dHeigth * vtCylAx ; // Studio l'intersezione del segmento col triangolo Point3d ptInt0, ptInt1 ; int nIntType = IntersLineTria( ptTouch0, ptTouch1, trTria, ptInt0, ptInt1) ; if ( ! ( nIntType == ILTT_NO || nIntType == ILTT_VERT || nIntType == ILTT_IN)) dEscapeDist = dU ; return dEscapeDist ; } //---------------------------------------------------------------------------- // **** CONO **** //---------------------------------------------------------------------------- double CAvTrConeTriangle( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot) { double dDiskDist ; if ( vtTrConeAx * vtMove < - EPS_ZERO) dDiskDist = CAvDiskTriangle( ptMinBase, - vtTrConeAx, dMinBaseR, trTria, vtMove) ; else dDiskDist = CAvDiskTriangle( ptMinBase + dTrConeH * vtTrConeAx, vtTrConeAx, dMaxBaseR, trTria, vtMove) ; // Allontanamento con direzione coincidente con l'asse del cono if ( AreSameOrOppositeVectorApprox( vtTrConeAx, vtMove)) { // Verifica preliminare che la fetta di spazio delimitata dal piano sotto del cono rispetto alla direzione // di allontanamento non sia già sopra al triangolo (tutti i vertici del triangolo sono sotto). Point3d ptInfLim = ptMinBase ; if ( AreOppositeVectorApprox( vtTrConeAx, vtMove)) ptInfLim += dTrConeH * vtTrConeAx ; double dMaxDistV = PointPlaneSignedDist( trTria.GetP( 0), ptInfLim, vtMove) ; dMaxDistV = max( dMaxDistV, PointPlaneSignedDist( trTria.GetP( 1), ptInfLim, vtMove)) ; dMaxDistV = max( dMaxDistV, PointPlaneSignedDist( trTria.GetP( 2), ptInfLim, vtMove)) ; if ( dMaxDistV < 0.) return 0. ; bool bInside[3] = { GetPointLineSqDist( trTria.GetP( 0), ptMinBase, vtMove) < dMinBaseR * dMinBaseR, GetPointLineSqDist( trTria.GetP( 1), ptMinBase, vtMove) < dMinBaseR * dMinBaseR, GetPointLineSqDist( trTria.GetP( 2), ptMinBase, vtMove) < dMinBaseR * dMinBaseR} ; if ( bInside[0] && bInside[1] && bInside[2]) return 0. ; // Distanza di allontanamento dall'interno double dInnEscapeDist = TrConeTriaInteriorEscapeDistLongMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH, trTria, vtMove) ; if ( dInnEscapeDist > EPS_SMALL) return dInnEscapeDist ; // Distanza di allontanamento dai vertici e lati double dOutEscapeDist = 0 ; for ( int nV = 0 ; nV < 3 ; ++ nV) { if ( bInside[nV] && bInside[( nV + 1) % 3]) continue ; // dal vertice double dCurVertDist = TrConePointEscapeDistLongMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH, trTria.GetP( nV), vtMove) ; dOutEscapeDist = max( dCurVertDist, dOutEscapeDist) ; // dal lato Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ; double dSegLen = vtSeg.Len() ; vtSeg /= dSegLen ; double dCurSegDist = TrConeSegmentEscapeDistLongMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH, trTria.GetP( nV), vtSeg, dSegLen, vtMove) ; dOutEscapeDist = max( dCurSegDist, dOutEscapeDist) ; } return max( dOutEscapeDist, dDiskDist) ; } // Allontanamento in direzione ortogonale all'asse del cono else if ( AreOrthoApprox( vtTrConeAx, vtMove)) { double dEscapeDist = 0. ; // Distanza di allontanamento dall'interno del trianolo: se positiva, abbiamo finito double dInnEscapeDist = TrConeTriaInteriorEscapeDistOrtMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH, trTria, vtMove) ; if ( dInnEscapeDist > EPS_SMALL) return dInnEscapeDist ; else dEscapeDist = max( dEscapeDist, dInnEscapeDist) ; // Distanza di allontanamento da vertici e lati for ( int nV = 0; nV < 3; ++ nV) { // Dai vertici double dCurVertDist = TrConePointEscapeDistOrtMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH, trTria.GetP( nV), vtMove, bTop, bBot) ; dEscapeDist = max( dEscapeDist, dCurVertDist) ; // Dal lato Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ; double dSegLen = vtSeg.Len() ; vtSeg /= dSegLen ; double dCurSegDist = TrConeSegmentEscapeDistOrtMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH, trTria.GetP( nV), vtSeg, dSegLen, vtMove, bTop, bBot) ; dEscapeDist = max( dEscapeDist, dCurSegDist) ; } return max( dEscapeDist, dDiskDist) ; } // Movimento generico else { // Controlli preliminari sull'interferenza fra tronco di cono e triangolo Vector3d vtOrtMv = vtMove - ( vtMove * vtTrConeAx) * vtTrConeAx ; if ( ! vtOrtMv.Normalize()) return - 1. ; Vector3d vtR0 = trTria.GetP( 0) - ptMinBase ; Vector3d vtR1 = trTria.GetP( 1) - ptMinBase ; Vector3d vtR2 = trTria.GetP( 2) - ptMinBase ; if ( vtR0 * vtOrtMv < - dMaxBaseR && vtR1 * vtOrtMv < - dMaxBaseR && vtR2 * vtOrtMv < - dMaxBaseR) return 0. ; Vector3d vtLat = vtTrConeAx ^ vtOrtMv ; if ( ( vtR0 * vtLat < - dMaxBaseR && vtR1 * vtLat < - dMaxBaseR && vtR2 * vtLat < - dMaxBaseR) || ( vtR0 * vtLat > dMaxBaseR && vtR1 * vtLat > dMaxBaseR && vtR2 * vtLat > dMaxBaseR)) return 0. ; // Calcolo la distanza di allontanamento double dEscapeDist = 0. ; // Allontanamento di allontanamento dall'interno del triangolo dEscapeDist = max( dEscapeDist, TrConeTriaInteriorEscapeDistGenMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH, trTria, vtMove)) ; // Se positiva abbiamo terminato if ( dEscapeDist > EPS_SMALL) return dEscapeDist ; // Allontanamento dalla frontiera for ( int n = 0 ; n < 3 ; ++ n) { Vector3d vtSeg = trTria.GetP( ( n + 1) % 3) - trTria.GetP( n) ; double dSegLen = vtSeg.Len() ; vtSeg /= dSegLen ; dEscapeDist = max( dEscapeDist, TrConePointEscapeDistGenMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH, trTria.GetP( n), vtMove, bTop, bBot)) ; dEscapeDist = max( dEscapeDist, TrConeSegmentEscapeDistGenMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH, trTria.GetP( n), vtSeg, dSegLen, vtMove, bTop, bBot)) ; } return max( dEscapeDist, dDiskDist) ; } } //---------------------------------------------------------------------------- double TrConePointEscapeDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Point3d& ptP, const Vector3d& vtMove) { double dPointAxisSqDist = GetPointLineSqDist( ptP, ptMinBase, vtTrConeAx) ; // Non c'è interferenza fra punto e superficie se la distanza del punto dall'asse non è compresa fra i raggi if ( dPointAxisSqDist > dMaxBaseR * dMaxBaseR - 2 * dMaxBaseR * EPS_SMALL || dPointAxisSqDist < dMinBaseR * dMinBaseR) return 0. ; double dMinBaseEscapeDist = PointPlaneSignedDist( ptP, ptMinBase, vtMove) ; double dMaxBaseEscapeDist = PointPlaneSignedDist( ptP, ptMinBase + dTrConeH * vtTrConeAx, vtMove) ; // Non vi è interferenza se il cono è già oltre il punto if ( dMinBaseEscapeDist < - EPS_SMALL && dMaxBaseEscapeDist < - EPS_SMALL) return 0. ; double dAddDist = ( sqrt( dPointAxisSqDist) - dMinBaseR) * dTrConeH / ( dMaxBaseR - dMinBaseR) ; return dMinBaseEscapeDist + ( vtMove * vtTrConeAx > 0. ? - dAddDist : dAddDist) ; } //---------------------------------------------------------------------------- double TrConeSegmentEscapeDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) { // Tangente dell'angolo di semi-apertura del cono e vertice del cono double dTanTheta = ( dMaxBaseR - dMinBaseR) / dTrConeH ; double dDistMinBaseV = dMinBaseR / dTanTheta ; Point3d ptConeV = ptMinBase - dDistMinBaseV * vtTrConeAx ; // Grandezze da cui dipendono i parametri Vector3d vtR0 = ptSeg - ptConeV ; double dDotR0Ax = vtR0 * vtTrConeAx ; double dDotMoveAx = vtMove * vtTrConeAx ; double dDotSegAx = vtSeg * vtTrConeAx ; double dB = 1 + dTanTheta * dTanTheta ; // Coseno e seno dell'angolo di semi-apertura del cono double dCosAlpha = 1 / sqrt( 1 + dTanTheta * dTanTheta) ; double dSinAlpha = ( 1 - dCosAlpha * dCosAlpha > 0 ? sqrt( 1 - dCosAlpha * dCosAlpha) : 0.) ; // Caso in cui il segmento è parallelo a una generatrice if ( abs( dDotSegAx - dCosAlpha) < EPS_ANG_ZERO * dSinAlpha) { // Definizione dell'equazione DBLVECTOR vdCoef( 3) ; DBLVECTOR vdRoots ; vdCoef[0] = vtR0 * vtR0 - dB * dDotR0Ax * dDotR0Ax ; vdCoef[1] = 2 * ( dB * dDotR0Ax * dDotMoveAx - vtR0 * vtMove) ; vdCoef[2] = 1 - dB * dDotMoveAx * dDotMoveAx ; // Soluzione dell'equazione int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ; // Analisi delle soluzioni e calcolo della distanza di allontanamento double dEscapeDist = 0. ; for ( int n = 0 ; n < nRoot ; ++ n) { if ( vdRoots[n] > dEscapeDist) { Point3d ptTouch0 = ptSeg - vdRoots[n] * vtMove ; Point3d ptTouch1 = ptTouch0 + dSegLen * vtSeg ; Point3d ptTouchM = ( ptTouch0 + ptTouch1) / 2 ; double dLongComp0 = ( ptTouch0 - ptMinBase) * vtTrConeAx ; double dLongComp1 = ( ptTouch1 - ptMinBase) * vtTrConeAx ; double dLongCompM = ( ptTouchM - ptMinBase) * vtTrConeAx ; // Se almeno uno dei tre punti di contatto è sul cilindro, la potenziale distanza è valida if ( ( ptTouch0 - ptConeV) * vtTrConeAx > - EPS_SMALL && ( ( dLongComp0 > 0. && dLongComp0 < dTrConeH) || ( dLongComp1 > 0. && dLongComp1 < dTrConeH) || ( dLongCompM > 0. && dLongCompM < dTrConeH))) dEscapeDist = vdRoots[n] ; } } return dEscapeDist ; } // Caso generale double dAlpha = 1 - dB * dDotSegAx * dDotSegAx ; double dBeta1 = dB * dDotMoveAx * dDotSegAx - vtMove * vtSeg ; double dBeta2 = vtR0 * vtSeg - dB * dDotR0Ax * dDotSegAx ; double dGamma = 1 - dB * dDotMoveAx * dDotMoveAx ; double dLambda = dB * dDotR0Ax * dDotMoveAx - vtR0 * vtMove ; double dEta = vtR0 * vtR0 - dB * dDotR0Ax * dDotR0Ax ; // Abbiamo un'equazione di secondo grado in due incognite. // Un'incognita rappresenta il parametro del segmento, l'altra // lo spostamento nella direzione di traslazione. // Se consideriamo come variabile il parametro del segmento e lo // spostamento come parametro dell'equazione, dobbiamo imporre // Delta = 0 per ottenere una nuova equazione nella sola variabile // spostamento. Le soluzioni di quest'ultima equazione sono candidate // a essere distanze di fuga. // Equazione Delta = 0 DBLVECTOR vdDeltaCoef( 3) ; DBLVECTOR vdDeltaRoots ; vdDeltaCoef[0] = 4 * ( dBeta2 * dBeta2 - dAlpha * dEta) ; vdDeltaCoef[1] = 8 * ( dBeta1 * dBeta2 - dAlpha * dLambda) ; vdDeltaCoef[2] = 4 * ( dBeta1 * dBeta1 - dAlpha * dGamma) ; // Risolvo l'equazione int nDeltaRoot = PolynomialRoots( 2, vdDeltaCoef, vdDeltaRoots) ; // Ciclo sulle soluzioni e per ogni soluzione valida valuto se la tangenza è // situata sul cilindro. double dEscapeDist = 0. ; for ( int n = 0 ; n < nDeltaRoot ; ++ n) { if ( vdDeltaRoots[n] > dEscapeDist) { double dS = - ( dBeta1 * vdDeltaRoots[n] + dBeta2) / dAlpha ; if ( dS > - EPS_SMALL && dS < dSegLen + EPS_SMALL) { Point3d ptTan = ptSeg + dS * vtSeg - vdDeltaRoots[n] * vtMove ; Vector3d vtTan = ptTan - ptMinBase ; double dDotTanAx = vtTan * vtTrConeAx ; if ( dDotTanAx > - EPS_SMALL && dDotTanAx < dTrConeH - EPS_SMALL) dEscapeDist = vdDeltaRoots[n] ; } } } return dEscapeDist ; } //---------------------------------------------------------------------------- double TrConeTriaInteriorEscapeDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) { double dEscapeDist = 0. ; // Affinché ci possa essere tangenza, l'angolo fra il versore dell'asse A // e il versore normale del triangolo N deve essere uguale a beta = Pi / 2 - alpha // dove alpha è l'angolo di semi-apertura del cono. Poiché Tan beta = 1 / Tan alpha e // cos beta = 1 / sqrt( 1 + ( tan beta)^2), abbiamo che cos beta = tan alpha / sqrt( 1 + ( tan alpha)^2). // Quindi affinché possa esserci tangenza si deve avere N * A - tan alpha / sqrt( 1 + ( tan alpha)^2) = 0. double dTanAlpha = ( dMaxBaseR - dMinBaseR) / dTrConeH ; double dCosBeta = dTanAlpha / sqrt( 1 + dTanAlpha * dTanAlpha) ; double dSinBeta = ( 1 - dCosBeta * dCosBeta > 0 ? sqrt( 1 - dCosBeta * dCosBeta) : 0.) ; if ( abs( trTria.GetN() * vtTrConeAx - dCosBeta) > dSinBeta * EPS_ANG_ZERO) return dEscapeDist ; // Calcolo componente ortogonale di N ad A Vector3d vtOrtN = trTria.GetN() - ( trTria.GetN() * vtTrConeAx) * vtTrConeAx ; vtOrtN.Normalize() ; // Un punto candidato a essere di estremo contatto è ptP = ptMinBase - dMinRad * vtOrtN, // dU è il parametro a cui la retta ptP + dU * vtMove interseca il piano del triangolo. double dU = ( dMinBaseR * trTria.GetN() * vtOrtN - ( ptMinBase - trTria.GetP(0)) * trTria.GetN()) / ( vtMove * trTria.GetN()) ; if ( dU < EPS_SMALL) return dEscapeDist ; // Estremi del segmento, facente parte del cilindro, candidato ad // essere di estremo contatto, traslato sul piano del triangolo. Point3d ptTouch0 = ptMinBase - dMinBaseR * vtOrtN + dU * vtMove ; Point3d ptTouch1 = ptTouch0 - ( dMaxBaseR - dMinBaseR) * vtOrtN + dTrConeH * vtTrConeAx ; // Studio l'intersezione del segmento col triangolo Point3d ptInt0, ptInt1 ; int nIntType = IntersLineTria( ptTouch0, ptTouch1, trTria, ptInt0, ptInt1) ; if ( ! ( nIntType == ILTT_NO || nIntType == ILTT_VERT || nIntType == ILTT_IN)) dEscapeDist = dU ; return dEscapeDist ; } //---------------------------------------------------------------------------- double TrConePointEscapeDistOrtMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot) { double dTopTol = bTop ? EPS_SMALL : - EPS_SMALL ; double dBotTol = bBot ? - EPS_SMALL : EPS_SMALL ; Vector3d vtP = ptP - ptMinBase ; double dPntPosOnAx = vtP * vtTrConeAx ; if ( dPntPosOnAx < dBotTol || dPntPosOnAx > dTrConeH + dTopTol) return 0. ; dPntPosOnAx = Clamp( dPntPosOnAx, 0., dTrConeH) ; double dR = dMinBaseR + ( dMaxBaseR - dMinBaseR) * dPntPosOnAx / dTrConeH ; return DiskPointEscapeDistOrtMot( ptMinBase + dPntPosOnAx * vtTrConeAx, vtTrConeAx, dR, ptP, vtMove) ; } //---------------------------------------------------------------------------- double TrConeSegmentEscapeDistOrtMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove, bool bTop, bool bBot) { double dTopTol = bTop ? EPS_SMALL : - EPS_SMALL ; double dBotTol = bBot ? - EPS_SMALL : EPS_SMALL ; double dStH = ( ptSeg - ptMinBase) * vtTrConeAx ; double dEnH = ( ptSeg + dSegLen * vtSeg - ptMinBase) * vtTrConeAx ; if ( ( dStH < dBotTol && dEnH < dBotTol) || ( dStH > dTrConeH + dTopTol && dEnH > dTrConeH + dTopTol)) return 0. ; // Distanza di allontanamento della superficie laterale: ci interessa solo la tangenza double dDistMinBaseV = dTrConeH * dMinBaseR / ( dMaxBaseR - dMinBaseR) ; Point3d ptConeV = ptMinBase - dDistMinBaseV * vtTrConeAx ; Frame3d frConusFrame ; frConusFrame.Set( ptConeV, vtMove, vtTrConeAx ^ vtMove, vtTrConeAx) ; Point3d ptSegNew = ptSeg ; Vector3d vtSegNew = vtSeg ; ptSegNew.ToLoc( frConusFrame) ; vtSegNew.ToLoc( frConusFrame) ; double dAngCoef = ( dDistMinBaseV + dTrConeH) / dMaxBaseR ; double dD0 = ptSegNew.z * ptSegNew.z - dAngCoef * dAngCoef * ( ptSegNew.x * ptSegNew.x + ptSegNew.y * ptSegNew.y) ; double dD1 = ptSegNew.z * vtSegNew.z - dAngCoef * dAngCoef * ( ptSegNew.x * vtSegNew.x + ptSegNew.y * vtSegNew.y) ; double dD2 = vtSegNew.z * vtSegNew.z - dAngCoef * dAngCoef * ( vtSegNew.x * vtSegNew.x + vtSegNew.y * vtSegNew.y) ; // Se l'equazione è di primo grado, non serve risolvere (si trova altrimenti con i punti estremi del segmento) if ( abs( dD2) < EPS_ZERO) return 0. ; // Impongo il determinante nullo, ne deriva una nuova equazione da risolvere DBLVECTOR vdDeltaCoef( 3) ; DBLVECTOR vdDeltaRoots ; vdDeltaCoef[0] = dD1 * dD1 - dD0 * dD2 ; vdDeltaCoef[1] = 2 * dAngCoef * dAngCoef * ( dD2 * ptSegNew.x - dD1 * vtSegNew.x) ; vdDeltaCoef[2] = dAngCoef * dAngCoef * ( dAngCoef * dAngCoef * vtSegNew.x * vtSegNew.x + dD2) ; int nRoot = PolynomialRoots( 2, vdDeltaCoef, vdDeltaRoots) ; // Studio le soluzioni double dEscapeDist = 0. ; for ( int nS = 0 ; nS < nRoot ; ++ nS) { // Queste soluzioni sono gli spostamenti lungo x del segmento che, lo portano in // una configurazione di tangenza col cono (delta = 0). Ora risolviamo l'equazione // dell'intersezione in corrispondenza del parametro di spostamento trovato. // Essendo il delta nullo la sola soluzione è - b/2a. double dT = ( dAngCoef * dAngCoef * vtSegNew.x * vdDeltaRoots[nS] - dD1) / dD2 ; Point3d ptTan( ptSegNew.x + vdDeltaRoots[nS], ptSegNew.y, ptSegNew.z) ; ptTan += dT * vtSegNew ; if ( ptTan.z > dDistMinBaseV + dBotTol && ptTan.z < dDistMinBaseV + dTrConeH + dTopTol && dT > - EPS_SMALL && dT < dSegLen + EPS_SMALL) { if ( - vdDeltaRoots[nS] > dEscapeDist) dEscapeDist = - vdDeltaRoots[nS] ; } } return dEscapeDist ; } //---------------------------------------------------------------------------- double TrConeTriaInteriorEscapeDistOrtMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) { double dEscapeDist = 0. ; // Affinché ci possa essere tangenza, l'angolo fra il versore dell'asse A // e il versore normale del triangolo N deve essere uguale a beta = Pi / 2 - alpha // dove alpha è l'angolo di semi-apertura del cono. Poiché Tan beta = 1 / Tan alpha e // cos beta = 1 / sqrt( 1 + ( tan beta)^2), abbiamo che cos beta = tan alpha / sqrt( 1 + ( tan alpha)^2). // Quindi affinché possa esserci tangenza si deve avere N * A - tan alpha / sqrt( 1 + ( tan alpha)^2) = 0. double dTanAlpha = ( dMaxBaseR - dMinBaseR) / dTrConeH ; double dCosBeta = dTanAlpha / sqrt( 1 + dTanAlpha * dTanAlpha) ; double dSinBeta = ( 1 - dCosBeta * dCosBeta > 0 ? sqrt( 1 - dCosBeta * dCosBeta) : 0.) ; if ( abs( trTria.GetN() * vtTrConeAx - dCosBeta) > dSinBeta * EPS_ANG_ZERO) return dEscapeDist ; // Calcolo componente ortogonale di N ad A Vector3d vtOrtN = trTria.GetN() - ( trTria.GetN() * vtTrConeAx) * vtTrConeAx ; vtOrtN.Normalize() ; // Un punto candidato a essere di estremo contatto è ptP = ptMinBase - dMinRad * vtOrtN, // dU è il parametro a cui la retta ptP + dU * vtMove interseca il piano del triangolo. double dU = ( dMinBaseR * trTria.GetN() * vtOrtN - ( ptMinBase - trTria.GetP(0)) * trTria.GetN()) / ( vtMove * trTria.GetN()) ; if ( dU < EPS_SMALL) return dEscapeDist ; // Estremi del segmento, facente parte del cilindro, candidato ad // essere di estremo contatto, traslato sul piano del triangolo. Point3d ptTouch0 = ptMinBase - dMinBaseR * vtOrtN + dU * vtMove ; Point3d ptTouch1 = ptTouch0 - ( dMaxBaseR - dMinBaseR) * vtOrtN + dTrConeH * vtTrConeAx ; // Studio l'intersezione del segmento col triangolo Point3d ptInt0, ptInt1 ; int nIntType = IntersLineTria( ptTouch0, ptTouch1, trTria, ptInt0, ptInt1) ; if ( ! ( nIntType == ILTT_NO || nIntType == ILTT_VERT || nIntType == ILTT_IN)) dEscapeDist = dU ; return dEscapeDist ; } //---------------------------------------------------------------------------- double TrConePointEscapeDistGenMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot) { double dTopTol = bTop ? EPS_SMALL : - EPS_SMALL ; double dBotTol = bBot ? - EPS_SMALL : EPS_SMALL ; // Tangente dell'angolo di semi-apertura del cono e vertice del cono double dTanAlpha = ( dMaxBaseR - dMinBaseR) / dTrConeH ; double dDistMinBaseV = dMinBaseR / dTanAlpha ; Point3d ptConeV = ptMinBase - dDistMinBaseV * vtTrConeAx ; // Grandezze da cui dipendono i parametri dell'equazione Vector3d vtR0 = ptP - ptConeV ; double dDotMoveAx = vtMove * vtTrConeAx ; double dDotR0Ax = vtR0 * vtTrConeAx ; double dB = 1 + dTanAlpha * dTanAlpha ; // Definizione dell'equazione DBLVECTOR vdCoef( 3) ; DBLVECTOR vdRoots ; vdCoef[0] = vtR0 * vtR0 - dB * dDotR0Ax * dDotR0Ax ; vdCoef[1] = 2 * ( dB * dDotR0Ax * dDotMoveAx - vtR0 * vtMove) ; vdCoef[2] = 1 - dB * dDotMoveAx * dDotMoveAx ; // Soluzione dell'equazione int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ; // Analisi delle soluzioni e calcolo della distanza di allontanamento double dEscapeDist = 0. ; for ( int n = 0 ; n < nRoot ; ++ n) { if ( vdRoots[n] > dEscapeDist) { Point3d ptTouch = ptP - vdRoots[n] * vtMove ; double dLongComp = ( ptTouch - ptMinBase) * vtTrConeAx ; if ( dLongComp > dBotTol && dLongComp < dTrConeH + dTopTol) dEscapeDist = vdRoots[n] ; } } return dEscapeDist ; } //---------------------------------------------------------------------------- double TrConeSegmentEscapeDistGenMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove, bool bTop, bool bBot) { // Setto le tolleranze sui limiti del cono double dTopTol = ( bTop ? EPS_SMALL : - EPS_SMALL) ; double dBotTol = ( bBot ? - EPS_SMALL : EPS_SMALL) ; // Tangente dell'angolo di semi-apertura del cono e vertice del cono double dTanTheta = ( dMaxBaseR - dMinBaseR) / dTrConeH ; double dDistMinBaseV = dMinBaseR / dTanTheta ; Point3d ptConeV = ptMinBase - dDistMinBaseV * vtTrConeAx ; // Grandezze da cui dipendono i parametri Vector3d vtR0 = ptSeg - ptConeV ; double dDotR0Ax = vtR0 * vtTrConeAx ; double dDotMoveAx = vtMove * vtTrConeAx ; double dDotSegAx = vtSeg * vtTrConeAx ; double dB = 1 + dTanTheta * dTanTheta ; // Coseno e seno dell'angolo di semi-apertura del cono double dCosAlpha = 1 / sqrt( 1 + dTanTheta * dTanTheta) ; double dSinAlpha = ( 1 - dCosAlpha * dCosAlpha > 0 ? sqrt( 1 - dCosAlpha * dCosAlpha) : 0.) ; // Caso in cui il segmento è parallelo a una generatrice if ( abs( dDotSegAx - dCosAlpha) < EPS_ANG_ZERO * dSinAlpha) { // Definizione dell'equazione DBLVECTOR vdCoef( 3) ; DBLVECTOR vdRoots ; vdCoef[0] = vtR0 * vtR0 - dB * dDotR0Ax * dDotR0Ax ; vdCoef[1] = 2 * ( dB * dDotR0Ax * dDotMoveAx - vtR0 * vtMove) ; vdCoef[2] = 1 - dB * dDotMoveAx * dDotMoveAx ; // Soluzione dell'equazione int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ; // Analisi delle soluzioni e calcolo della distanza di allontanamento double dEscapeDist = 0. ; for ( int n = 0 ; n < nRoot ; ++ n) { if ( vdRoots[n] > dEscapeDist) { Point3d ptTouch0 = ptSeg - vdRoots[n] * vtMove ; Point3d ptTouch1 = ptTouch0 + dSegLen * vtSeg ; Point3d ptTouchM = ( ptTouch0 + ptTouch1) / 2 ; double dLongComp0 = ( ptTouch0 - ptMinBase) * vtTrConeAx ; double dLongComp1 = ( ptTouch1 - ptMinBase) * vtTrConeAx ; double dLongCompM = ( ptTouchM - ptMinBase) * vtTrConeAx ; // Se almeno uno dei tre punti di contatto è sul cilindro, la potenziale distanza è valida if ( ( ptTouch0 - ptConeV) * vtTrConeAx > - EPS_SMALL && ( ( dLongComp0 > dBotTol && dLongComp0 < dTrConeH + dTopTol) || ( dLongComp1 > dBotTol && dLongComp1 < dTrConeH + dTopTol) || ( dLongCompM > dBotTol && dLongCompM < dTrConeH + dTopTol))) dEscapeDist = vdRoots[n] ; } } return dEscapeDist ; } // Caso generale double dAlpha = 1 - dB * dDotSegAx * dDotSegAx ; double dBeta1 = dB * dDotMoveAx * dDotSegAx - vtMove * vtSeg ; double dBeta2 = vtR0 * vtSeg - dB * dDotR0Ax * dDotSegAx ; double dGamma = 1 - dB * dDotMoveAx * dDotMoveAx ; double dLambda = dB * dDotR0Ax * dDotMoveAx - vtR0 * vtMove ; double dEta = vtR0 * vtR0 - dB * dDotR0Ax * dDotR0Ax ; // Abbiamo un'equazione di secondo grado in due incognite. // Un'incognita rappresenta il parametro del segmento, l'altra // lo spostamento nella direzione di traslazione. // Se consideriamo come variabile il parametro del segmento e lo // spostamento come parametro dell'equazione, dobbiamo imporre // Delta = 0 per ottenere una nuova equazione nella sola variabile // spostamento. Le soluzioni di quest'ultima equazione sono candidate // a essere distanze di fuga. // Equazione Delta = 0 DBLVECTOR vdDeltaCoef( 3) ; DBLVECTOR vdDeltaRoots ; vdDeltaCoef[0] = 4 * ( dBeta2 * dBeta2 - dAlpha * dEta) ; vdDeltaCoef[1] = 8 * ( dBeta1 * dBeta2 - dAlpha * dLambda) ; vdDeltaCoef[2] = 4 * ( dBeta1 * dBeta1 - dAlpha * dGamma) ; // Risolvo l'equazione int nDeltaRoot = PolynomialRoots( 2, vdDeltaCoef, vdDeltaRoots) ; // Ciclo sulle soluzioni e per ogni soluzione valida valuto se la tangenza è // situata sul cilindro. double dEscapeDist = 0. ; for ( int n = 0 ; n < nDeltaRoot ; ++ n) { if ( vdDeltaRoots[n] > dEscapeDist) { double dS = - ( dBeta1 * vdDeltaRoots[n] + dBeta2) / dAlpha ; if ( dS > - EPS_SMALL && dS < dSegLen + EPS_SMALL) { Point3d ptTan = ptSeg + dS * vtSeg - vdDeltaRoots[n] * vtMove ; Vector3d vtTan = ptTan - ptMinBase ; double dDotTanAx = vtTan * vtTrConeAx ; if ( dDotTanAx > dBotTol && dDotTanAx < dTrConeH + dTopTol) dEscapeDist = vdDeltaRoots[n] ; } } } return dEscapeDist ; } //---------------------------------------------------------------------------- double TrConeTriaInteriorEscapeDistGenMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) { double dEscapeDist = 0. ; // Affinché ci possa essere tangenza, l'angolo fra il versore dell'asse A // e il versore normale del triangolo N deve essere uguale a beta = Pi / 2 - alpha // dove alpha è l'angolo di semi-apertura del cono. Poiché Tan beta = 1 / Tan alpha e // cos beta = 1 / sqrt( 1 + ( tan beta)^2), abbiamo che cos beta = tan alpha / sqrt( 1 + ( tan alpha)^2). // Quindi affinché possa esserci tangenza si deve avere N * A - tan alpha / sqrt( 1 + ( tan alpha)^2) = 0. double dTanAlpha = dTrConeH / ( dMaxBaseR - dMinBaseR) ; double dCosBeta = dTanAlpha / sqrt( 1 + dTanAlpha * dTanAlpha) ; double dSinBeta = ( 1 - dCosBeta * dCosBeta > 0 ? sqrt( 1 - dCosBeta * dCosBeta) : 0.) ; if ( abs( trTria.GetN() * vtTrConeAx - dCosBeta) > dSinBeta * EPS_ANG_ZERO) return dEscapeDist ; // Calcolo componente ortogonale di N ad A Vector3d vtOrtN = trTria.GetN() - ( trTria.GetN() * vtTrConeAx) * vtTrConeAx ; vtOrtN.Normalize() ; // Un punto candidato a essere di estremo contatto è ptP = ptMinBase - dMinRad * vtOrtN, // dU è il parametro a cui la retta ptP + dU * vtMove interseca il piano del triangolo. double dU = ( dMinBaseR * trTria.GetN() * vtOrtN - ( ptMinBase - trTria.GetP(0)) * trTria.GetN()) / ( vtMove * trTria.GetN()) ; if ( dU < EPS_SMALL) return dEscapeDist ; // Estremi del segmento, facente parte del cilindro, candidato ad // essere di estremo contatto, traslato sul piano del triangolo. Point3d ptTouch0 = ptMinBase - dMinBaseR * vtOrtN + dU * vtMove ; Point3d ptTouch1 = ptTouch0 - ( dMaxBaseR - dMinBaseR) * vtOrtN + dTrConeH * vtTrConeAx ; // Studio l'intersezione del segmento col triangolo Point3d ptInt0, ptInt1 ; int nIntType = IntersLineTria( ptTouch0, ptTouch1, trTria, ptInt0, ptInt1) ; if ( ! ( nIntType == ILTT_NO || nIntType == ILTT_VERT || nIntType == ILTT_IN)) dEscapeDist = dU ; return dEscapeDist ; } //---------------------------------------------------------------------------- // **** TORO **** // In questi algoritmi per toro intendiamo la corona torica esterna //---------------------------------------------------------------------------- double CAvTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot) { // Allontanamento in direzione coincidente con l'asse del toro if ( AreSameVectorApprox( vtTorusAx, vtMove)) { // Verifica preliminare che la fetta di spazio delimitata dal piano sotto del toro rispetto alla direzione // di allontanamento non sia già sopra al triangolo (tutti i vertici del triangolo sono sotto). Point3d ptBase = ptTorusCen - dMinRad * vtMove ; double dMaxDistV = PointPlaneSignedDist( trTria.GetP( 0), ptBase, vtMove) ; dMaxDistV = max( dMaxDistV, PointPlaneSignedDist( trTria.GetP( 1), ptBase, vtMove)) ; dMaxDistV = max( dMaxDistV, PointPlaneSignedDist( trTria.GetP( 2), ptBase, vtMove)) ; if ( dMaxDistV < 0.) return 0. ; // Distanza di allontanamento dall'interno double dInEscapeDist = TorusTriaInteriorEscapeDistLongMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, trTria, vtMove) ; if ( dInEscapeDist > EPS_SMALL) return dInEscapeDist ; // Distanza di allontanamento dai lati double dOutEscapeDist = 0. ; for ( int nV = 0 ; nV < 3 ; ++ nV) { Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ; double dSegLen = vtSeg.Len() ; vtSeg /= dSegLen ; double dCurDist = TorusSegmentEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad, trTria.GetP( nV), vtSeg, dSegLen, vtMove) ; dOutEscapeDist = max( dCurDist, dOutEscapeDist) ; // Valuto contatto del disco con i vertici dCurDist = PointPlaneSignedDist( trTria.GetP( nV), ptTorusCen - dMinRad * vtTorusAx, vtMove) ; if ( GetPointLineSqDist( trTria.GetP( nV), ptTorusCen, vtTorusAx) < dMaxRad * dMaxRad && dCurDist > dOutEscapeDist) dOutEscapeDist = max( dOutEscapeDist, dCurDist) ; } return dOutEscapeDist ; } // Allontanamento in direzione ortogonale all'asse del toro else if ( AreOrthoApprox( vtTorusAx, vtMove)) { // Verifico che il triangolo intersechi il volume spazzato dal oro nel suo possibile movimento double dTopTol = bTop ? EPS_SMALL : - EPS_SMALL ; double dBotTol = bBot ? - EPS_SMALL : EPS_SMALL ; double dRadSum = dMaxRad + dMinRad ; Vector3d vtTrasv = vtTorusAx ^ vtMove ; Vector3d vtR0 = trTria.GetP( 0) - ptTorusCen ; Vector3d vtR1 = trTria.GetP( 1) - ptTorusCen ; Vector3d vtR2 = trTria.GetP( 2) - ptTorusCen ; if ( ( vtR0 * vtMove < - dRadSum && vtR1 * vtMove < - dRadSum && vtR2 * vtMove < - dRadSum) || ( vtR0 * vtTrasv < - dRadSum && vtR1 * vtTrasv < - dRadSum && vtR2 * vtTrasv < - dRadSum) || ( vtR0 * vtTrasv > dRadSum && vtR1 * vtTrasv > dRadSum && vtR2 * vtTrasv > dRadSum) || ( vtR0 * vtTorusAx > dTopTol && vtR1 * vtTorusAx > dTopTol && vtR2 * vtTorusAx > dTopTol) || ( vtR0 * vtTorusAx < - dMinRad + dBotTol && vtR1 * vtTorusAx < - dMinRad + dBotTol && vtR2 * vtTorusAx < - dMinRad + dBotTol)) return 0. ; // Distanza di allontanamento dall'interno del triangolo: se positivo abbiamo finito double dInEscapeDist = TorusTriaInteriorEscapeDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, trTria, vtMove) ; if ( dInEscapeDist > EPS_SMALL) return dInEscapeDist ; // Distanza di allontanamento dai lati double dOutEscapeDist = 0. ; for ( int nV = 0 ; nV < 3 ; ++ nV) { Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ; double dSegLen = vtSeg.Len() ; vtSeg /= dSegLen ; double dCurDist = TorusSegmentEscapeDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, trTria.GetP( nV), vtSeg, dSegLen, vtMove, bTop, bBot) ; dOutEscapeDist = max( dCurDist, dOutEscapeDist) ; } return max( dOutEscapeDist, dInEscapeDist) ; } // Allontanamento in direzione generica else { // Controlli preliminari sull'interferenza del triangolo con il toro Vector3d vtOrtMv = vtMove - ( vtMove * vtTorusAx) * vtTorusAx ; vtOrtMv.Normalize( /*EPS_ZERO*/) ; Point3d ptBehind = ptTorusCen - dMaxRad * vtOrtMv ; double dRadSum = dMaxRad + dMinRad ; if ( ( trTria.GetP( 0) - ptBehind) * vtOrtMv < - dRadSum && ( trTria.GetP( 1) - ptBehind) * vtOrtMv < - dRadSum && ( trTria.GetP( 2) - ptBehind) * vtOrtMv < - dRadSum) return 0. ; Vector3d vtLat = vtTorusAx ^ vtOrtMv ; double dDot0 = ( trTria.GetP( 0) - ptTorusCen) * vtLat ; double dDot1 = ( trTria.GetP( 1) - ptTorusCen) * vtLat ; double dDot2 = ( trTria.GetP( 2) - ptTorusCen) * vtLat ; if ( ( dDot0 < - dRadSum && dDot1 < - dRadSum && dDot2 < - dRadSum) || ( dDot0 > dRadSum && dDot1 > dRadSum && dDot2 > dRadSum)) return 0. ; // Distanza di allontanamento dall'interno double dInnEscapeDist = TorusTriaInteriorEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, trTria, vtMove) ; // Se positiva abbiamo finito if ( dInnEscapeDist > EPS_SMALL) return dInnEscapeDist ; // Distanza di allontanamento da frontiera double dBordEscapeDist = 0. ; for ( int n = 0 ; n < 3 ; ++ n) { Vector3d vtSeg = trTria.GetP( ( n + 1) % 3) - trTria.GetP( n) ; double dSegLen = vtSeg.Len() ; vtSeg /= dSegLen ; double dCurDist = TorusSegmentEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, trTria.GetP( n), vtSeg, dSegLen, vtMove) ; dBordEscapeDist = max( dBordEscapeDist, dCurDist) ; } return dBordEscapeDist ; } } //---------------------------------------------------------------------------- double TorusPointEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad, const Point3d& ptP, const Vector3d& vtMove) { double dPointAxSqLen = GetPointLineSqDist( ptP, ptTorusCen, vtMove) ; double dDeltaRad = sqrt( dPointAxSqLen) - dMaxRad ; double dSQAddEscapeDist = dMinRad * dMinRad - dDeltaRad * dDeltaRad ; if ( dSQAddEscapeDist <= 0.) return 0. ; double dAddEscapeDist = sqrt( dSQAddEscapeDist) ; double dEscapeDist = ( ptP - ptTorusCen) * vtMove + dAddEscapeDist ; return dEscapeDist ; } //---------------------------------------------------------------------------- double TorusSegmentEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) { // Se segmento dista dall'asse del toro più del raggio esterno, posso uscire subito double dLinSegSqDist = LineSegmentSqDist( ptTorusCen, vtMove, ptSeg, vtSeg, dSegLen) ; if ( dLinSegSqDist > ( dMaxRad + dMinRad) * ( dMaxRad + dMinRad)) 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 * dMaxRad && dEndSqDist < dMaxRad * dMaxRad) return 0. ; // 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 + dMinRad, dOutLen1, dOutLen2) ; if ( nOutInters == CC_TWO_INT) SegLimits.Intersect( dOutLen1, dOutLen2) ; else if ( nOutInters == CC_NO_INTERS || nOutInters == CC_ONE_INT_TAN) SegLimits.Reset() ; if ( SegLimits.IsEmpty()) return 0. ; // Tolgo eventuale cilindro interno alla corona del toro if ( dLinSegSqDist < dMaxRad * dMaxRad) { double dIntLen1, dIntLen2 ; int nIntInters = IntersLineInfiniteCylinder( ptSeg, vtSeg, ptTorusCen, vtMove, dMaxRad, dIntLen1, dIntLen2) ; if ( nIntInters == CC_TWO_INT) SegLimits.Subtract( dIntLen1, dIntLen2) ; } // Eseguo controllo sugli intervalli double dMaxEscapeDist = 0. ; double dLen1, dLen2 ; bool bFound = SegLimits.GetFirst( dLen1, dLen2) ; while ( bFound) { // verifico l'intervallo Point3d ptStart = ptSeg + vtSeg * dLen1 ; double dLen = dLen2 - dLen1 ; const double INV_GOLDEN_RATIO = ( sqrt( 5.) - 1.) / 2. ; double dTol = max( 0.01 * dMinRad, EPS_SMALL) ; double dPSt = 0 ; double dPEn = dLen ; double dPIn1 = dPEn - ( dPEn - dPSt) * INV_GOLDEN_RATIO ; double dPIn2 = dPSt + ( dPEn - dPSt) * INV_GOLDEN_RATIO ; double dEscapeDist1 = TorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad, ptStart + dPIn1 * vtSeg, vtMove) ; double dEscapeDist2 = TorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad, ptStart + dPIn2 * vtSeg, vtMove) ; while ( dPEn - dPSt > dTol) { if ( dEscapeDist1 > dEscapeDist2) { dPEn = dPIn2 ; dPIn2 = dPIn1 ; dEscapeDist2 = dEscapeDist1 ; dPIn1 = dPEn - ( dPEn - dPSt) * INV_GOLDEN_RATIO ; dEscapeDist1 = TorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad, ptStart + dPIn1 * vtSeg, vtMove) ; } else { dPSt = dPIn1 ; dPIn1 = dPIn2 ; dEscapeDist1 = dEscapeDist2 ; dPIn2 = dPSt + ( dPEn - dPSt) * INV_GOLDEN_RATIO ; dEscapeDist2 = TorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad, ptStart + dPIn2 * vtSeg, vtMove) ; } } double dPMax = 0.5 * ( dPSt + dPEn) ; dMaxEscapeDist = max( dMaxEscapeDist, TorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad, ptStart + dPMax * vtSeg, vtMove)) ; // passo al successivo intervallo bFound = SegLimits.GetNext( dLen1, dLen2) ; } return dMaxEscapeDist ; } //---------------------------------------------------------------------------- double TorusTriaInteriorEscapeDistLongMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Triangle3d& trTria, const Vector3d& vtMove) { // Siamo interessati solo ai casi di tangenza, poiché il punto di contatto // estremo con l'interno di un triangolo non può che essere di tangenza. double dCompLong = trTria.GetN() * vtTorusAx ; Vector3d vtNormOrt = trTria.GetN() - dCompLong * vtTorusAx ; // Se il triangolo è orientato in modo che non possa esserci un punto di tangenza oppure la // direzione di allontanamento è nel piano del triangolo abbiamo finito if ( ( dCompLong < 0. || dCompLong >= 1.) || ( ! vtNormOrt.Normalize()) || abs( vtMove * trTria.GetN()) < EPS_ZERO) return 0. ; // Punto del toro candidato a toccare l'interno del triangolo Point3d ptTouch = ptTorusCen - dMaxRad * vtNormOrt - dMinRad * trTria.GetN() ; // Distanza percorsa dal punto per posizionarsi sul piano del triangolo double dDist = ( ( trTria.GetP( 0) - ptTouch) * trTria.GetN()) / ( vtMove * trTria.GetN()) ; // Se distanza negativa abbiamo finito if ( dDist < 0.) return 0. ; // Sposto il punto sul piano del triangolo ptTouch += ( dDist * vtMove) ; // Se ora il punto non giace dentro il triangolo distanza nulla if ( ! IsPointInsideTriangle( ptTouch, trTria, TriangleType::OPEN)) dDist = 0. ; return dDist ; } //---------------------------------------------------------------------------- double TorusPointEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Point3d& ptP, const Vector3d& vtMove) { Vector3d vtR = ptP - ptTorusCen ; // Quota del piano di movimento del punto double dH = vtR * vtTorusAx ; if ( dH > EPS_SMALL) return 0. ; // Raggio della circonferenza alla quota dH double dHRad = dMaxRad + sqrt( max( dMinRad * dMinRad - dH * dH, 0.)) ; // Vettore movimento nel piano e sua lunghezza Vector3d vtPlaneR = vtR - dH * vtTorusAx ; double dL = vtPlaneR * vtMove ; if ( dL <= - dHRad) return 0. ; // Distanza della linea di movimento dall'asse del toro Vector3d vtPlaneOrtR = vtPlaneR - dL * vtMove ; double dSqDist = vtPlaneOrtR.SqLen() ; if ( dSqDist >= dHRad * dHRad) return 0. ; // Distanza di movimento return ( dL + sqrt( dHRad * dHRad - dSqDist)) ; } //---------------------------------------------------------------------------- double TorusSegmentEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove, bool bTop, bool bBot) { // Dati segmento modificabli Point3d ptMySeg = ptSeg ; double dMySegLen = dSegLen ; // Versore ortogonale a asse utensile e movimento Vector3d vtTrasv = vtTorusAx ^ vtMove ; vtTrasv.Normalize() ; // Piano a destra Plane3d plLeft ; plLeft.Set( ptTorusCen - ( dMaxRad + dMinRad) * vtTrasv, vtTrasv) ; if ( ! ClampSegmentOutPlane( plLeft, ptMySeg, vtSeg, dMySegLen)) return 0. ; // Piano a sinistra Plane3d plRight ; plRight.Set( ptTorusCen + ( dMaxRad + dMinRad) * vtTrasv, -vtTrasv) ; if ( ! ClampSegmentOutPlane( plRight, ptMySeg, vtSeg, dMySegLen)) return 0. ; // Piano dietro Plane3d plBack ; plBack.Set( ptTorusCen - ( dMaxRad + dMinRad) * vtMove, vtMove) ; if ( ! ClampSegmentOutPlane( plBack, ptMySeg, vtSeg, dMySegLen)) return 0. ; // Piano sotto double dBotTol = ( bBot ? - EPS_SMALL : EPS_SMALL) ; Plane3d plBot ; plBot.Set( ptTorusCen - dMinRad * vtTorusAx, vtTorusAx) ; if ( ! ClampSegmentOutPlane( plBot, ptMySeg, vtSeg, dMySegLen, dBotTol)) return 0. ; // Piano sopra double dTopTol = ( bTop ? -EPS_SMALL : EPS_SMALL) ; Plane3d plTop ; plTop.Set( ptTorusCen, -vtTorusAx) ; if ( ! ClampSegmentOutPlane( plTop, ptMySeg, vtSeg, dMySegLen, dTopTol)) return 0. ; // Cerco distanza di massimo allontanamento lungo il segmento ridotto double dEscapeDist = 0. ; double dStep = max( 0.1 * dMinRad, EPS_SMALL) ; int nStepNum = max( int( dMySegLen / dStep), 1) ; dStep = dMySegLen / nStepNum ; int nMax = -1 ; for ( int i = 0 ; i <= nStepNum ; ++ i) { Point3d ptP = ptMySeg + ( i * dStep) * vtSeg ; // Minima distanza per evitare collisione double dDist = TorusPointEscapeDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptP, vtMove) ; if ( dDist > dEscapeDist) { dEscapeDist = dDist ; nMax = i ; } } // affino la ricerca (curva parabolica nell'intorno del massimo trovato) if ( nMax != -1) { // nuovo passo e punto di partenza const int SUB_STEPS = 10 ; double dNewStep = dStep / SUB_STEPS ; Point3d ptStart = ptMySeg + ( nMax * dStep) * vtSeg ; // cerco dopo if ( nMax < nStepNum) { for ( int j = 1 ; j < SUB_STEPS ; ++ j) { Point3d ptP = ptStart + ( j * dNewStep) * vtSeg ; // Minima distanza per evitare collisione double dDist = TorusPointEscapeDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptP, vtMove) ; if ( dDist > dEscapeDist) dEscapeDist = dDist ; else break ; } } // cerco prima if ( nMax > 0) { for ( int j = 1 ; j < SUB_STEPS ; ++ j) { Point3d ptP = ptStart - ( j * dNewStep) * vtSeg ; // Minima distanza per evitare collisione double dDist = TorusPointEscapeDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptP, vtMove) ; if ( dDist > dEscapeDist) dEscapeDist = dDist ; else break ; } } } return dEscapeDist ; } //---------------------------------------------------------------------------- double TorusTriaInteriorEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Triangle3d& trTria, const Vector3d& vtMove) { // Componente ortogonale all'asse del toro del vettore del piano del triangolo Vector3d vtPlaneOrtToAx = trTria.GetN() - ( trTria.GetN() * vtTorusAx) * vtTorusAx ; // Se piano ortogonale all'asse del toro non ci può essere tangenza if ( ! vtPlaneOrtToAx.Normalize( EPS_ZERO)) return 0. ; // Se la componente del versore normale al triangolo nella direzione // del moto è nulla, non può esserci contatto con l'interno double dDotMoveN = vtMove * trTria.GetN() ; if ( abs( dDotMoveN) < EPS_ZERO) return 0. ; // Scarto il primo contatto if ( vtPlaneOrtToAx * vtMove < 0.) return 0. ; vtPlaneOrtToAx *= dMaxRad ; // Trovo il punto che toccherà il piano Point3d ptTouch = ptTorusCen - vtPlaneOrtToAx - dMinRad * trTria.GetN() ; double dEscapeDist = max( ( ( trTria.GetP( 0) - ptTouch) * trTria.GetN()) / dDotMoveN, 0.) ; // Se il punto di contatto è interno al triangolo restituisco distanza di fuga non negativa if ( IsPointInsideTriangle( ptTouch + dEscapeDist * vtMove, trTria, TriangleType::OPEN)) return dEscapeDist ; return 0. ; } //---------------------------------------------------------------------------- double TorusPointEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Point3d& ptP, const Vector3d& vtMove) { // Intersezione fra semi-retta e corona torica BOOLVECTOR vbType ; DBLVECTOR vdPar ; int nIntType = LinCompTorusExtInt( ptP, - vtMove, 1, Ray, ptTorusCen, vtTorusAx, dMinRad, dMaxRad, vbType, vdPar) ; if ( nIntType == T_ERROR) return nIntType ; // Cerco il maggior parametro corrispondente a un contatto secante double dDist = 0. ; for ( int n = 0 ; n < int( vdPar.size()) ; ++ n) { // Se secante e maggiore della distanza corrente aggiorno la distanza if ( vbType[n] && dDist < vdPar[n]) dDist = vdPar[n] ; } return dDist ; } //---------------------------------------------------------------------------- double TorusSegmentEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) { Point3d ptMySeg = ptSeg ; double dMySegLen = dSegLen ; // Restringo il segmento nella striscia di spazio in cui si sposta il toro Vector3d vtTrasv = vtTorusAx ^ vtMove ; if ( ! vtTrasv.Normalize()) return - 1. ; // Piano a sinistra Plane3d plPlane ; plPlane.Set( ptTorusCen + ( dMaxRad + dMinRad) * vtTrasv, - vtTrasv) ; if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen)) return 0. ; // Piano a destra plPlane.Set( ptTorusCen - ( dMaxRad + dMinRad) * vtTrasv, vtTrasv) ; if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen)) return 0. ; double dDotAxMv = vtMove * vtTorusAx ; Vector3d vtOrtMv = vtMove - dDotAxMv * vtTorusAx ; if ( ! vtOrtMv.Normalize()) return - 1. ; // Piano dietro plPlane.Set( ptTorusCen - ( dMaxRad + dMinRad) * vtOrtMv, vtOrtMv) ; if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen)) return 0. ; // Piano orizzontale Point3d ptUpDw = ptTorusCen - ( dDotAxMv > 0. ? dMinRad : 0.) * vtTorusAx ; Vector3d vtUPDw = dDotAxMv > 0. ? vtTorusAx : - vtTorusAx ; plPlane.Set( ptUpDw, vtUPDw) ; if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen)) return 0. ; // Cerco il punto del segmento ridotto che ha distanza di allontanamento massima double dEscapeDist = 0. ; double dStep = max( 0.1 * dMinRad, EPS_SMALL) ; int nStepNum = max( int( dMySegLen / dStep), 1) ; dStep = dMySegLen / nStepNum ; int nMax = -1 ; for ( int n = 0 ; n <= nStepNum ; ++ n) { Point3d ptCurPoint = ptMySeg + ( n * dStep) * vtSeg ; double dCurrDist = TorusPointEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ; if ( dEscapeDist < dCurrDist) { dEscapeDist = dCurrDist ; nMax = n ; } } // affino la ricerca (curva parabolica nell'intorno del massimo trovato) if ( nMax != -1) { // nuovo passo e punto di partenza const int SUB_STEPS = 10 ; double dNewStep = dStep / SUB_STEPS ; Point3d ptStart = ptMySeg + ( nMax * dStep) * vtSeg ; // cerco dopo if ( nMax < nStepNum) { for ( int j = 1 ; j < SUB_STEPS ; ++ j) { Point3d ptCurPoint = ptStart + ( j * dNewStep) * vtSeg ; // Minima distanza per evitare collisione double dDist = TorusPointEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ; if ( dDist > dEscapeDist) dEscapeDist = dDist ; else break ; } } // cerco prima if ( nMax > 0) { for ( int j = 1 ; j < SUB_STEPS ; ++ j) { Point3d ptCurPoint = ptStart - ( j * dNewStep) * vtSeg ; // Minima distanza per evitare collisione double dDist = TorusPointEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ; if ( dDist > dEscapeDist) dEscapeDist = dDist ; else break ; } } } return dEscapeDist ; } //---------------------------------------------------------------------------- double TorusTriaInteriorEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Triangle3d& trTria, const Vector3d& vtMove) { // Siamo interessati solo ai casi di tangenza, poiché il punto di contatto // estremo con l'interno di un triangolo non può che essere di tangenza. double dCompLong = trTria.GetN() * vtTorusAx ; Vector3d vtNormOrt = trTria.GetN() - dCompLong * vtTorusAx ; // Se il triangolo è orientato in modo che non possa esserci un punto di tangenza oppure la // direzione di allontanamento è nel piano del triangolo abbiamo finito if ( ( dCompLong < 0. || dCompLong >= 1.) || ( ! vtNormOrt.Normalize()) || abs( vtMove * trTria.GetN()) < EPS_ZERO) return 0. ; // Punto del toro candidato a toccare l'interno del triangolo Point3d ptTouch = ptTorusCen - dMaxRad * vtNormOrt - dMinRad * trTria.GetN() ; // Distanza percorsa dal punto per posizionarsi sul piano del triangolo double dDist = ( ( trTria.GetP( 0) - ptTouch) * trTria.GetN()) / ( vtMove * trTria.GetN()) ; // Se distanza negativa abbiamo finito if ( dDist < 0.) return 0. ; // Sposto il punto sul piano del triangolo ptTouch += ( dDist * vtMove) ; // Se ora il punto non giace dentro il triangolo distanza nulla if ( ! IsPointInsideTriangle( ptTouch, trTria, TriangleType::OPEN)) dDist = 0. ; return dDist ; } //---------------------------------------------------------------------------- double CAvConcaveTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot) { double dDiskDist ; if ( vtTorusAx * vtMove < - EPS_ZERO) dDiskDist = CAvDiskTriangle( ptTorusCen, - vtTorusAx, dMaxRad - dMinRad, trTria, vtMove) ; else dDiskDist = CAvDiskTriangle( ptTorusCen + dMinRad * vtTorusAx, vtTorusAx, dMaxRad, trTria, vtMove) ; // Allontanamento con direzione coincidente con l'asse del toro if ( AreSameOrOppositeVectorApprox( vtTorusAx, vtMove)) { // Le corone toriche non possono effettuare un moto in direzione opposta // al proprio asse di simmetria: vedi documentazione. if ( vtTorusAx * vtMove < 0.) return 0. ; // Distanza di allontanamento dai lati double dOutEscapeDist = 0. ; for ( int nV = 0 ; nV < 3 ; ++ nV) { Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ; double dSegLen = vtSeg.Len() ; vtSeg /= dSegLen ; double dCurDist = ConcaveTorusSegmentEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad, trTria.GetP( nV), vtSeg, dSegLen, vtMove) ; dOutEscapeDist = max( dCurDist, dOutEscapeDist) ; // Valuto contatto del disco con i vertici dCurDist = PointPlaneSignedDist( trTria.GetP( nV), ptTorusCen, vtMove) ; if ( GetPointLineSqDist( trTria.GetP( nV), ptTorusCen, vtTorusAx) < ( dMaxRad - dMinRad) * ( dMaxRad - dMinRad) && dCurDist > dOutEscapeDist) dOutEscapeDist = max( dOutEscapeDist, dCurDist) ; } return max( dDiskDist, dOutEscapeDist) ; } // Allontanamento con direzione ortogonale a quella dell'asse del toro else if ( AreOrthoApprox( vtTorusAx, vtMove)) { double dEscapeDist = 0. ; // Distanza di allontanamento dei segmenti for ( int nV = 0 ; nV < 3 ; ++ nV) { Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ; double dSegLen = vtSeg.Len() ; vtSeg /= dSegLen ; double dCurDist = ConcaveTorusSegmentEscapeDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, trTria.GetP( nV), vtSeg, dSegLen, vtMove, bTop, bBot) ; dEscapeDist = max( dEscapeDist, dCurDist) ; } dEscapeDist = max( dEscapeDist, dDiskDist) ; return dEscapeDist ; } // Casi non gestiti else { double dEscapeDist = 0. ; // Distanza di allontanamento dei segmenti for ( int nV = 0 ; nV < 3 ; ++ nV) { Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ; double dSegLen = vtSeg.Len() ; vtSeg /= dSegLen ; double dCurDist = ConcaveTorusSegmentEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, trTria.GetP( nV), vtSeg, dSegLen, vtMove) ; dEscapeDist = max( dEscapeDist, dCurDist) ; } dEscapeDist = max( dEscapeDist, dDiskDist) ; return dEscapeDist ; } } //---------------------------------------------------------------------------- double ConcaveTorusPointEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad, const Point3d& ptP, const Vector3d& vtMove) { double dPointAxSqLen = GetPointLineSqDist( ptP, ptTorusCen, vtMove) ; double dDeltaRad = sqrt( dPointAxSqLen) - dMaxRad ; if ( dDeltaRad > 0 || dDeltaRad < - dMinRad) return 0. ; return ( ptP - ptTorusCen) * vtMove - sqrt( dMinRad * dMinRad - dDeltaRad * dDeltaRad) ; } //---------------------------------------------------------------------------- double ConcaveTorusSegmentEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) { // 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. ; // 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. ; // 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) ; else if ( nOutInters == CC_NO_INTERS) SegLimits.Reset() ; 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) ; } // Eseguo controllo sugli intervalli double dMaxEscapeDist = 0. ; double dLen1, dLen2 ; bool bFound = SegLimits.GetFirst( dLen1, dLen2) ; while ( bFound) { // verifico l'intervallo Point3d ptStart = ptSeg + vtSeg * dLen1 ; double dLen = dLen2 - dLen1 ; const double INV_GOLDEN_RATIO = ( sqrt( 5.) - 1.) / 2. ; double dTol = max( 0.01 * dMinRad, EPS_SMALL) ; double dPSt = 0 ; double dPEn = dLen ; double dPIn1 = dPEn - ( dPEn - dPSt) * INV_GOLDEN_RATIO ; double dPIn2 = dPSt + ( dPEn - dPSt) * INV_GOLDEN_RATIO ; double dEscapeDist1 = ConcaveTorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad, ptStart + dPIn1 * vtSeg, vtMove) ; double dEscapeDist2 = ConcaveTorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad, ptStart + dPIn2 * vtSeg, vtMove) ; while ( dPEn - dPSt > dTol) { if ( dEscapeDist1 > dEscapeDist2) { dPEn = dPIn2 ; dPIn2 = dPIn1 ; dEscapeDist2 = dEscapeDist1 ; dPIn1 = dPEn - ( dPEn - dPSt) * INV_GOLDEN_RATIO ; dEscapeDist1 = ConcaveTorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad, ptStart + dPIn1 * vtSeg, vtMove) ; } else { dPSt = dPIn1 ; dPIn1 = dPIn2 ; dEscapeDist1 = dEscapeDist2 ; dPIn2 = dPSt + ( dPEn - dPSt) * INV_GOLDEN_RATIO ; dEscapeDist2 = ConcaveTorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad, ptStart + dPIn2 * vtSeg, vtMove) ; } } double dPMax = 0.5 * ( dPSt + dPEn) ; dMaxEscapeDist = max( dMaxEscapeDist, ConcaveTorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad, ptStart + dPMax * vtSeg, vtMove)) ; // passo al successivo intervallo bFound = SegLimits.GetNext( dLen1, dLen2) ; } return dMaxEscapeDist ; } //---------------------------------------------------------------------------- double ConcaveTorusPointEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Point3d& ptP, const Vector3d& vtMove) { double dSqMinRad = dMinRad * dMinRad ; Vector3d vtR = ptP - ptTorusCen ; double dH = vtR * vtTorusAx ; Vector3d vtPlaneR = vtR - dH * vtTorusAx ; double dL = vtPlaneR * vtMove ; Vector3d vtPlaneOrtR = vtPlaneR - dL * vtMove ; double dDeltaRad = dSqMinRad - dH * dH > 0. ? sqrt( dSqMinRad - dH * dH) : 0. ; // Raggio della circonferenza alla quota dH double dHRad = dMaxRad - dDeltaRad ; double dSqDeltaL = dHRad * dHRad - vtPlaneOrtR.SqLen() ; double dEscapeDist = dSqDeltaL > 0. ? dL + sqrt( dSqDeltaL) : 0. ; return dEscapeDist ; } //---------------------------------------------------------------------------- double ConcaveTorusSegmentEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove, bool bTop, bool bBot) { // Dati segmento modificabli Point3d ptMySeg = ptSeg ; double dMySegLen = dSegLen ; // Versore ortogonale a asse utensile e movimento Vector3d vtTrasv = vtTorusAx ^ vtMove ; vtTrasv.Normalize() ; // Piano a destra Plane3d plLeft ; plLeft.Set( ptTorusCen - dMaxRad * vtTrasv, vtTrasv) ; if ( ! ClampSegmentOutPlane( plLeft, ptMySeg, vtSeg, dMySegLen)) return 0. ; // Piano a sinistra Plane3d plRight ; plRight.Set( ptTorusCen + dMaxRad * vtTrasv, -vtTrasv) ; if ( ! ClampSegmentOutPlane( plRight, ptMySeg, vtSeg, dMySegLen)) return 0. ; // Piano dietro Plane3d plBack ; plBack.Set( ptTorusCen - dMaxRad * vtMove, vtMove) ; if ( ! ClampSegmentOutPlane( plBack, ptMySeg, vtSeg, dMySegLen)) return 0. ; // Piano sotto double dBotTol = ( bBot ? - EPS_SMALL : EPS_SMALL) ; Plane3d plBot ; plBot.Set( ptTorusCen, vtTorusAx) ; if ( ! ClampSegmentOutPlane( plBot, ptMySeg, vtSeg, dMySegLen, dBotTol)) return 0. ; // Piano sopra double dTopTol = ( bTop ? -EPS_SMALL : EPS_SMALL) ; Plane3d plTop ; plTop.Set( ptTorusCen + dMinRad * vtTorusAx, -vtTorusAx) ; if ( ! ClampSegmentOutPlane( plTop, ptMySeg, vtSeg, dMySegLen, dTopTol)) return 0. ; // Cerco distanza di massimo allontanamento lungo il segmento ridotto double dEscapeDist = 0. ; double dStep = max( 0.1 * dMinRad, EPS_SMALL) ; int nStepNum = max( int( dMySegLen / dStep), 1) ; dStep = dMySegLen / nStepNum ; int nMax = -1 ; for ( int n = 0 ; n <= nStepNum ; ++ n) { Point3d ptCurPoint = ptMySeg + ( n * dStep) * vtSeg ; double dCurDist = ConcaveTorusPointEscapeDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ; if ( dCurDist > dEscapeDist) { dEscapeDist = dCurDist ; nMax = n ; } } // affino la ricerca (curva parabolica nell'intorno del massimo trovato) if ( nMax != -1) { // nuovo passo e punto di partenza const int SUB_STEPS = 10 ; double dNewStep = dStep / SUB_STEPS ; Point3d ptStart = ptMySeg + ( nMax * dStep) * vtSeg ; // cerco dopo if ( nMax < nStepNum) { for ( int j = 1 ; j < SUB_STEPS ; ++ j) { Point3d ptCurPoint = ptStart + ( j * dNewStep) * vtSeg ; double dCurDist = ConcaveTorusPointEscapeDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ; if ( dCurDist > dEscapeDist) dEscapeDist = dCurDist ; else break ; } } // cerco prima if ( nMax > 0) { for ( int j = 1 ; j < SUB_STEPS ; ++ j) { Point3d ptCurPoint = ptStart - ( j * dNewStep) * vtSeg ; double dCurDist = ConcaveTorusPointEscapeDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ; if ( dCurDist > dEscapeDist) dEscapeDist = dCurDist ; else break ; } } } return dEscapeDist ; } //---------------------------------------------------------------------------- double ConcaveTorusPointEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Point3d& ptP, const Vector3d& vtMove) { BOOLVECTOR vbType ; DBLVECTOR vdPar ; // Intersezione fra semi-retta e corona torica int nIntType = LinCompTorusInnUpInt( ptP, - vtMove, 1, Ray, ptTorusCen, vtTorusAx, dMinRad, dMaxRad, vbType, vdPar) ; if ( nIntType == T_ERROR) return nIntType ; // Cerco il maggior parametro corrispondente a un contatto secante double dDist = 0. ; for ( int n = 0 ; n < int( vdPar.size()) ; ++ n) { // Se secante e maggiore della distanza corrente aggiorno la distanza if ( vbType[n] && dDist < vdPar[n]) dDist = vdPar[n] ; } return dDist ; } //---------------------------------------------------------------------------- double ConcaveTorusSegmentEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) { // Restringo il segmento nella striscia di spazio in cui si sposta il toro Point3d ptMySeg = ptSeg ; double dMySegLen = dSegLen ; Vector3d vtTrasv = vtTorusAx ^ vtMove ; if ( ! vtTrasv.Normalize()) return - 1. ; // Piano a sinistra Plane3d plPlane ; plPlane.Set( ptTorusCen + dMaxRad * vtTrasv, - vtTrasv) ; if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen)) return 0. ; // Piano a destra plPlane.Set( ptTorusCen - dMaxRad * vtTrasv, vtTrasv) ; if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen)) return 0. ; double dDotAxMv = vtMove * vtTorusAx ; Vector3d vtOrtMv = vtMove - dDotAxMv * vtTorusAx ; if ( ! vtOrtMv.Normalize()) return - 1. ; // Piano dietro plPlane.Set( ptTorusCen - dMaxRad * vtOrtMv, vtOrtMv) ; if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen)) return 0. ; // Piano orizzontale Point3d ptUpDw = ptTorusCen + ( dDotAxMv > 0. ? 0. : dMinRad) * vtTorusAx ; Vector3d vtUPDw = dDotAxMv > 0. ? vtTorusAx : - vtTorusAx ; plPlane.Set( ptUpDw, vtUPDw) ; if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen)) return 0. ; // Cerco il punto del segmento ridotto che ha distanza di allontanamento massima double dEscapeDist = 0. ; double dStep = max( 0.1 * dMinRad, EPS_SMALL) ; int nStepNum = max( int( dMySegLen / dStep), 1) ; dStep = dMySegLen / nStepNum ; int nMax = -1 ; for ( int n = 0 ; n <= nStepNum ; ++ n) { Point3d ptCurPoint = ptMySeg + ( n * dStep) * vtSeg ; double dCurEscapeDist = ConcaveTorusPointEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ; if ( dEscapeDist < dCurEscapeDist) { dEscapeDist = dCurEscapeDist ; nMax = n ; } } // affino la ricerca (curva parabolica nell'intorno del massimo trovato) if ( nMax != -1) { // nuovo passo e punto di partenza const int SUB_STEPS = 10 ; double dNewStep = dStep / SUB_STEPS ; Point3d ptStart = ptMySeg + ( nMax * dStep) * vtSeg ; // cerco dopo if ( nMax < nStepNum) { for ( int j = 1 ; j < SUB_STEPS ; ++ j) { Point3d ptCurPoint = ptStart + ( j * dNewStep) * vtSeg ; double dCurEscapeDist = ConcaveTorusPointEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ; if ( dEscapeDist < dCurEscapeDist) dEscapeDist = dCurEscapeDist ; else break ; } } // cerco prima if ( nMax > 0) { for ( int j = 1 ; j < SUB_STEPS ; ++ j) { Point3d ptCurPoint = ptStart - ( j * dNewStep) * vtSeg ; double dCurEscapeDist = ConcaveTorusPointEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ; if ( dEscapeDist < dCurEscapeDist) dEscapeDist = dCurEscapeDist ; else break ; } } } return dEscapeDist ; } // DISTANZA DI ALLONTANAMENTO PER DISCHI //---------------------------------------------------------------------------- double CAvDiskTriangle( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad, const Triangle3d& trTria, const Vector3d& vtMove) { // Direzione di allontanamento diretta come normale al disco if ( AreSameVectorApprox( vtDiskAx, vtMove)) { // Valuto le distanze con segno dei vertici dal piano del disco : // se sono tutte negative non interferiscono. double dDistV[3] ; dDistV[0] = PointPlaneSignedDist( trTria.GetP( 0), ptDiskCen, vtMove) ; dDistV[1] = PointPlaneSignedDist( trTria.GetP( 1), ptDiskCen, vtMove) ; dDistV[2] = PointPlaneSignedDist( trTria.GetP( 2), ptDiskCen, vtMove) ; double dMaxDistV = max( dDistV[0], max( dDistV[1], dDistV[2])) ; if ( dMaxDistV < 0.) return 0. ; // Se tutti i punti distano dall'asse di movimento meno del raggio, l'ultimo // punto di contatto deve essere un vertice del triangolo. double dSqRad = dDiskRad * dDiskRad ; bool bInV[3] ; bInV[0] = ( GetPointLineSqDist( trTria.GetP( 0), ptDiskCen, vtMove) < dSqRad) ; bInV[1] = ( GetPointLineSqDist( trTria.GetP( 1), ptDiskCen, vtMove) < dSqRad) ; bInV[2] = ( GetPointLineSqDist( trTria.GetP( 2), ptDiskCen, vtMove) < dSqRad) ; if ( bInV[0] && bInV[1] && bInV[2]) return max( dMaxDistV, 0.) ; // Distanza di allontanamento dall'interno del triangolo double dMaxDistI = DiskTriaInteriorEscapeDistLongMot( ptDiskCen, dDiskRad, trTria, vtMove) ; if ( dMaxDistI > EPS_SMALL) return dMaxDistI ; // Se disco è un punto, inutile fare controlli con i lati e i vertici del triangolo if ( dDiskRad < EPS_SMALL) return 0. ; // Ciclo sui segmenti del triangolo e calcolo la loro distanza di allontanamento, calcolo // anche la distanza di allontanamento dai vertici distanti dall'asse meno del raggio. double dMaxDistVS = 0. ; for ( int nVS = 0 ; nVS < 3 ; ++ nVS) { // Vertici if ( bInV[nVS] && dDistV[nVS] > dMaxDistVS) dMaxDistVS = dDistV[nVS] ; // Se un lato del triangolo ha entrambi gli estremi con distanza negativa dal piano del disco, // non può interferire con esso. int nVE = ( nVS + 1) % 3 ; if ( dDistV[nVS] < 0 && dDistV[nVE] < 0) continue ; // Versore e lunghezza del segmento Vector3d vtSeg = trTria.GetP( nVE) - trTria.GetP( nVS) ; double dSegLen = vtSeg.Len() ; vtSeg /= dSegLen ; // Distanza dal piano del segmento corrente double dCurDist = DiskSegmentEscapeDistLongMot( ptDiskCen, dDiskRad, trTria.GetP( nVS), vtSeg, dSegLen, vtMove) ; if ( dCurDist > dMaxDistVS) dMaxDistVS = dCurDist ; } return dMaxDistVS ; } // Direzione di allontanamento perpendicolare all'asse del disco else if ( AreOrthoApprox( vtDiskAx, vtMove)) { // Allontanamento dall'interno del triangolo double dDist = max( DiskTriaInteriorEscapeDistOrtMot( ptDiskCen, vtDiskAx, dDiskRad, trTria, vtMove), 0.) ; if ( dDist > EPS_SMALL) return dDist ; // Distanza di allontanamento dalla frontiera: non considero i punti for ( int nV = 0 ; nV < 3 ; ++ nV) { Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ; double dSegLen = vtSeg.Len() ; vtSeg /= dSegLen ; dDist = max( dDist, DiskSegmentEscapeDistOrtMot( ptDiskCen, vtDiskAx, dDiskRad, trTria.GetP( nV), vtSeg, dSegLen, vtMove)) ; } return dDist ; } // Direzione di allontanamento generica else if ( vtDiskAx * vtMove > 0.) { Point3d ptV0 = trTria.GetP( 0) ; Point3d ptV1 = trTria.GetP( 1) ; Point3d ptV2 = trTria.GetP( 2) ; // Distanze di allontanamento dei vertici double dVertEscapeDist[3] ; dVertEscapeDist[0] = - ( ( ptDiskCen - ptV0) * vtDiskAx) / ( vtMove * vtDiskAx) ; dVertEscapeDist[1] = - ( ( ptDiskCen - ptV1) * vtDiskAx) / ( vtMove * vtDiskAx) ; dVertEscapeDist[2] = - ( ( ptDiskCen - ptV2) * vtDiskAx) / ( vtMove * vtDiskAx) ; // Triangolo proiettato sul piano del disco Triangle3d trTriaOnPlane ; trTriaOnPlane.Set( ptV0 - dVertEscapeDist[0] * vtMove, ptV1 - dVertEscapeDist[1] * vtMove, ptV2 - dVertEscapeDist[2] * vtMove) ; // Se non c'è interferenza fra triangolo proiettato e disco la distanza di fuga è nulla if ( ! CoplanarDiscTriangleInterference( ptDiskCen, dDiskRad, trTriaOnPlane, TriangleType::CLOSED)) // OPEN EXACT O CLOSED ? return 0. ; double dSqSafeRad = dDiskRad * dDiskRad - 2 * dDiskRad * EPS_SMALL ; bool bInside[3] ; bInside[0] = SqDist( ptDiskCen, trTriaOnPlane.GetP( 0)) < dSqSafeRad ; bInside[1] = SqDist( ptDiskCen, trTriaOnPlane.GetP( 1)) < dSqSafeRad ; bInside[2] = SqDist( ptDiskCen, trTriaOnPlane.GetP( 2)) < dSqSafeRad ; // Tutte le immagini dei vertici cadono nel disco: basta valutare i vertici if ( bInside[0] && bInside[1] && bInside[2]) return max( max( dVertEscapeDist[0], dVertEscapeDist[1]), max( dVertEscapeDist[2], 0.)) ; // Caso generale // Allontanamento dall'interno double dEscapeDist = max( DiskTriaInteriorEscapeDistGenMot( ptDiskCen, vtDiskAx, dDiskRad, trTria, vtMove), 0.) ; // Allontanamento dalla frontiera Vector3d vtMoveOrt = OrthoCompo( vtMove, vtDiskAx) ; vtMoveOrt.Normalize() ; Frame3d DiskFrame ; DiskFrame.Set( ptDiskCen, vtDiskAx, vtMoveOrt) ; Triangle3d trTriaLoc = trTria ; Vector3d vtMoveLoc = vtMove ; trTriaLoc.ToLoc( DiskFrame) ; vtMoveLoc.ToLoc( DiskFrame) ; for ( int n = 0 ; n < 3 ; ++ n) { Vector3d vtSeg = trTriaLoc.GetP( ( n + 1) % 3) - trTriaLoc.GetP( n) ; double dSegLen = vtSeg.Len() ; vtSeg /= dSegLen ; dEscapeDist = max( max( DiskSegmentEscapeDistGenMot( dDiskRad, trTriaLoc.GetP( n), vtSeg, dSegLen, vtMoveLoc), bInside[n] ? max( dVertEscapeDist[n], 0.) : 0.), dEscapeDist) ; } return dEscapeDist ; } // Errore else return -1. ; } //---------------------------------------------------------------------------- double DiskPointEscapeDistLongMot( const Point3d& ptDiskCen, double dDiskRad, const Point3d& ptP, const Vector3d& vtMove) { if ( GetPointLineSqDist( ptP, ptDiskCen, vtMove) < dDiskRad * dDiskRad - 2 * dDiskRad * EPS_SMALL) return PointPlaneSignedDist( ptP, ptDiskCen, vtMove) ; return 0. ; } //---------------------------------------------------------------------------- double DiskSegmentEscapeDistLongMot( const Point3d& ptDiskCen, double dDiskRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) { // Il disco non può interferire col segmento nel suo moto, se la distanza del // segmento dall'asse di traslazione è maggiore del raggio. if ( LineSegmentSqDist( ptDiskCen, vtMove, ptSeg, vtSeg, dSegLen) > dDiskRad * dDiskRad - 2 * dDiskRad * EPS_SMALL) return 0. ; // Imposto l'equazione: la distanza quadrata del generico punto della retta associata al // segmento dall'asse lungo cui si sposta il disco deve valere raggio quadrato del disco. Vector3d vtLineSegOrt = ( ptSeg - ptDiskCen) - ( ptSeg - ptDiskCen) * vtMove * vtMove ; Vector3d vtSegOrt = vtSeg - vtSeg * vtMove * vtMove ; DBLVECTOR vdCoef(3) ; DBLVECTOR vdRoots ; vdCoef[0] = vtLineSegOrt.SqLen() - dDiskRad * dDiskRad ; vdCoef[1] = 2 * vtLineSegOrt * vtSegOrt ; vdCoef[2] = vtSegOrt.SqLen() ; // Segmento e asse paralleli if ( vdCoef[2] < SQ_EPS_ZERO) return 0. ; // Soluzione dell'equazione int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ; // Ciclo sulle soluzioni per trovare la distanza di allontanamento double dSegDist = 0. ; for ( int nSol = 0 ; nSol < nRoot ; ++ nSol) { // Soluzione interna al segmento if ( vdRoots[nSol] > 0. && vdRoots[nSol] < dSegLen + EPS_ZERO) { Point3d ptC = ptSeg + vdRoots[nSol] * vtSeg ; // Distanza del punto soluzione dal piano del disco nella posizione iniziale double dCurDist = PointPlaneSignedDist( ptC, ptDiskCen, vtMove) ; if ( dCurDist > dSegDist) dSegDist = dCurDist ; } } return dSegDist ; } //---------------------------------------------------------------------------- // Determina la distanza di allontanamento di un disco, che trasla lungo il suo asse di simmetria, // dall'interno di un triangolo. Il disco è descritto dal suo raggio e il moto dal // centro nella posizione iniziale e dal versore del suo asse di simmetria // (COINCIDENTE CON IL VERSORE DELLA DIREZIONE DI ALLONTANAMENTO). //---------------------------------------------------------------------------- double DiskTriaInteriorEscapeDistLongMot( const Point3d& ptDiskCen, double dDiskRad, const Triangle3d& trTria, const Vector3d& vtMove) { // Se disco e triangolo sono complanari if ( AreSameVectorApprox( vtMove, trTria.GetN())) { double dDist = max( - PointPlaneSignedDist( ptDiskCen, trTria.GetP( 0), trTria.GetN()), 0.) ; if ( CoplanarDiscTriangleInterference( ptDiskCen + dDist * vtMove, dDiskRad - EPS_SMALL, trTria, TriangleType::EXACT)) return dDist ; return 0. ; } // Se disco e triangolo sono ortogonali non può esserci contatto con interno if ( AreOrthoApprox( vtMove, trTria.GetN())) return 0. ; // Cerco un punto di contatto nell'interno del triangolo. double dEscapeDist = 0. ; // Vettore radiale Vector3d vtRad = trTria.GetN() - ( trTria.GetN() * vtMove) * vtMove ; vtRad.Normalize() ; // Punto candidato all'essere di estremo contatto Point3d ptStLine = ptDiskCen - ( dDiskRad - EPS_SMALL) * vtRad ; // Intersezioni con retta double dDist = ( ( trTria.GetP( 0) - ptStLine) * trTria.GetN()) / ( vtMove * trTria.GetN()) ; if ( IsPointInsideTriangle( ptStLine + dDist * vtMove, trTria, TriangleType::EXACT)) dEscapeDist = max( dEscapeDist, dDist) ; return dEscapeDist ; } //---------------------------------------------------------------------------- double DiskPointEscapeDistOrtMot( const Point3d& ptDisc, const Vector3d& vtDiskAx, double dDiskRad, const Point3d& ptP, const Vector3d& vtMove) { // Vettore congiungente il punto da cui si allontana il disco e il centro del medesimo Vector3d vtP = ptP - ptDisc ; // Se il punto non appartiene al piano del disco, la distanza di fuga è nulla Vector3d vtCompOutOfPlane = vtP * vtDiskAx * vtDiskAx ; if ( vtCompOutOfPlane.SqLen() > EPS_SMALL * EPS_SMALL) return 0. ; // Depuro da eventuali componenti fuori dal piano del disco vtP -= vtCompOutOfPlane ; double dLenLong = vtP * vtMove ; Vector3d vtPOrt = vtP - dLenLong * vtMove ; double dSqLenOrt = vtPOrt * vtPOrt ; if ( dSqLenOrt > dDiskRad * dDiskRad - 2 * dDiskRad * EPS_SMALL) return 0. ; return max( dLenLong + sqrt( max( dDiskRad * dDiskRad - dSqLenOrt, 0.)), 0.) ; } //---------------------------------------------------------------------------- // Restituisce la distanza di fuga di un disco da un segmento nel caso di // moto ortogonale all'asse di simmetria. // Il disco è descritto dal suo raggio. Il movimento è descritto dal versore // dell'asse del disco, dal centro nella posizione iniziale e dal // versore della direzione del moto. Il segmento è descritto da punto iniziale, // versore direzione e lunghezza. double DiskSegmentEscapeDistOrtMot( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) { // Se il segmento non è nel piano, il più remoto punto di contatto è, // se esiete, l'intersezione fra piano e segmento. Il caso in cui il // segmento giace nel piano non ci interessa Point3d ptMyDisk = ptDiskCen/* + EPS_SMALL * vtDiskAx*/ ; if ( ! ( abs( vtSeg * vtDiskAx) < EPS_ZERO)) { Point3d ptSegEnd = ptSeg + dSegLen * vtSeg ; double dDotStart = ( ptSeg - ptMyDisk) * vtDiskAx ; double dDotEnd = ( ptSegEnd - ptMyDisk) * vtDiskAx ; if ( dDotStart * dDotEnd < 0.) { double dS = ( ptMyDisk - ptSeg) * vtDiskAx / ( vtSeg * vtDiskAx) ; Point3d ptInt = ptSeg + dS * vtSeg ; Vector3d vtOI = ( ptInt - ptMyDisk) ; double dLongCord = vtOI * vtMove ; Vector3d vtLong = dLongCord * vtMove ; Vector3d vtOrt = vtOI - vtLong ; double dSqOrtLen = vtOrt.SqLen() ; if ( dSqOrtLen > dDiskRad * dDiskRad - 2 * dDiskRad * EPS_SMALL) return 0. ; else return max( dLongCord + sqrt( max( dDiskRad * dDiskRad - dSqOrtLen, 0.)), 0.) ; } else return 0. ; } return 0. ; } //---------------------------------------------------------------------------- double DiskTriaInteriorEscapeDistOrtMot( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad, const Triangle3d& trTria, const Vector3d& vtMove) { double dDist = 0. ; // Vettore direzione della retta intersezione fra il piano del triangolo e quello del disco Vector3d vtLine = vtDiskAx ^ trTria.GetN() ; // Se i piani sono paralleli, non ci può essere punto di estremo contatto nell'interno del triangolo if ( ! vtLine.Normalize()) return dDist ; // Nella configurazione di estremo contatto il disco deve essere tangente a questa retta; // Il vettore radiale congiungente il centro del disco con il potenziale punto di contatto Vector3d vtRad = vtLine ^ vtDiskAx ; // Se il vettore radiale è ortogonale al versore di movimento non può esserci configurazione di estremo contatto. if ( abs( vtRad * vtMove) < EPS_ZERO) return dDist ; // Il versore radiale deve sempre avere puntare contro la direzione di allontanamento if ( vtRad * vtMove > 0) vtRad *= - 1 ; // Punto candidato a essere di estremo contatto Point3d ptTouch = ptDiskCen + dDiskRad * vtRad ; // Parametro intersezione del punto candidato ad essere di estremo contatto col piano del triangolo double dU = ( ( trTria.GetP( 0) - ptTouch) * trTria.GetN()) / ( vtMove * trTria.GetN()) ; if ( dU < 0.) return dDist ; // Contatto all'interno del triangolo if ( IsPointInsideTriangle( ptTouch + dU * vtMove, trTria, TriangleType::EXACT)) dDist = max( dDist, dU) ; return dDist ; } //---------------------------------------------------------------------------- // La funzione restituisce la distanza di allontanamento di un disco da un segmento. // NB: L'origine del sistema di riferimento deve essere nel centro della circonferenza // di base, la cui traslazione obliqua genera il cilindro ellittico, e l'asse z deve // essere l'asse di simmetria di tale circonferenza. // NB: I parametri dLongMvLen e dOrtMvLen sono rispettivamente le lunghezze delle // proiezioni del movimento su z e x del sistema di riferimento CircFrame. double DiskSegmentEscapeDistGenMot( double dDiskRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) { // Il caso in cui il segmento è parallelo al versore del moto è gestito dalla // routine per il calcolo della distanza di allontanamento dei punti. if ( AreSameOrOppositeVectorApprox( vtMove, vtSeg)) return 0. ; // Siamo nel caso in cui il movimento è generico rispetto all'asse del disco, quindi // sicuramente vtMove.z > 0, quindi esiste il rapporto dObCoef double dObCoef = vtMove.x / vtMove.z ; double dSqCoef = dObCoef * dObCoef ; // Quadrato del raggio double dSqRad = ( dDiskRad - EPS_SMALL) * ( dDiskRad - EPS_SMALL) ; // Setto i coeficienti dell'equazione DBLVECTOR vdCoef(3) ; vdCoef[0] = dSqCoef * ptSeg.z * ptSeg.z + ptSeg.x * ptSeg.x + ptSeg.y * ptSeg.y - 2 * dObCoef * ptSeg.z * ptSeg.x - dSqRad ; vdCoef[1] = 2 * ( dSqCoef * vtSeg.z * ptSeg.z + vtSeg.x * ptSeg.x + vtSeg.y * ptSeg.y - dObCoef * ( vtSeg.z * ptSeg.x + vtSeg.x * ptSeg.z)) ; vdCoef[2] = dSqCoef * vtSeg.z * vtSeg.z + vtSeg.x * vtSeg.x + vtSeg.y * vtSeg.y - 2 * dObCoef * vtSeg.z * vtSeg.x ; // Numero di soluzioni: l'equazione ammette o due soluzioni (eventualmente // coincidenti) oppure nessuna o infinite se la la retta // appartiene alla superficie DBLVECTOR vdRoots ; int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ; double dLenCoef = sqrt( 1 + dSqCoef) ; double dEscapeDist = 0. ; for ( int n = 0 ; n < nRoot ; ++ n) { if ( vdRoots[n] > 0 && vdRoots[n] < dSegLen) { Point3d ptTouch = ptSeg + vdRoots[n] * vtSeg ; dEscapeDist = max( dEscapeDist, dLenCoef * ptTouch.z) ; } } return dEscapeDist ; } //---------------------------------------------------------------------------- double DiskTriaInteriorEscapeDistGenMot( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad, const Triangle3d& trTria, const Vector3d& vtMove) { // Se disco e triangolo sono complanari if ( AreSameVectorApprox( vtDiskAx, trTria.GetN())) { double dDist = max( ( ( trTria.GetP( 0) - ptDiskCen) * trTria.GetN()) / ( vtMove * trTria.GetN()), 0.) ; if ( CoplanarDiscTriangleInterference( ptDiskCen + dDist * vtMove, dDiskRad, trTria, TriangleType::EXACT)) return dDist ; return 0. ; } // Se il movimento è nel piano del triangolo non può esserci contatto con interno if ( AreOrthoApprox( vtMove, trTria.GetN())) return 0. ; // Cerco un punto di contatto nell'interno del triangolo. double dEscapeDist = 0. ; // Vettore radiale Vector3d vtRad = trTria.GetN() - ( trTria.GetN() * vtDiskAx) * vtDiskAx ; vtRad.Normalize() ; // Punti delle due rette candidate all'intersezione col triangolo Point3d ptStLine = ptDiskCen - dDiskRad * vtRad ; // Intersezioni con le rette double dDist = ( ( trTria.GetP( 0) - ptStLine) * trTria.GetN()) / ( vtMove * trTria.GetN()) ; if ( IsPointInsideTriangle( ptStLine + dDist * vtMove, trTria, TriangleType::EXACT)) dEscapeDist = max( dEscapeDist, dDist) ; return dEscapeDist ; }