diff --git a/CAvToolTriangle.cpp b/CAvToolTriangle.cpp index 7af19c7..1868ebb 100644 --- a/CAvToolTriangle.cpp +++ b/CAvToolTriangle.cpp @@ -1,7 +1,7 @@ //---------------------------------------------------------------------------- // EgalTech 2018-2018 //---------------------------------------------------------------------------- -// File : CAvToolTriangle.cpp Data : 19.07.18 Versione : 1.9h1 +// File : CAvToolTriangle.cpp Data : 25.10.18 Versione : 1.9j4 // Contenuto : Implementazione delle funzioni ToolTriangleCollisionAvoid. // // @@ -17,6 +17,7 @@ #include "CAvToolTriangle.h" #include "IntersLineSurfStd.h" #include "IntersLineTria.h" +#include "DistPointLine.h" #include "/EgtDev/Include/ENkPolynomialRoots.h" #include "/EgtDev/Include/EgtNumUtils.h" #include "/EgtDev/Include/EGkIntervals.h" @@ -90,7 +91,7 @@ CAvToolTriangle( const Tool& tlTool, const Point3d& ptToolOrig, const Vector3d& double dHeigth = tlTool.GetHeigth() ; double dRadius = tlTool.GetRadius() ; // prima determino l'allontanamento del disco inferiore - double dDist = CAvDiskTriangle( ptToolOrig - dHeigth * vtToolAx, vtToolAx, dRadius, trTria, vtMove) ; + double dDist = CAvDiskTriangle( ptToolOrig - dHeigth * vtToolAx, vtToolAx, dRadius, trTria, vtMove) ; if ( dDist < - EPS_SMALL) return dDist ; // poi verifico quello del cilindro (tenendo conto di quanto è stata allontanato il disco) @@ -131,7 +132,7 @@ CAvToolTriangle( const Tool& tlTool, const Point3d& ptToolOrig, const Vector3d& Point3d ptTorusCen = ptToolOrig - dCylHeigth * vtToolAx ; double dDist2 = CAvTorusTriangle( ptTorusCen + dDist * vtMove, vtToolAx, tlTool.GetRadius() - tlTool.GetCornRadius(), tlTool.GetCornRadius(), - trTria, vtMove, true, false) ; + trTria, vtMove, true, false) ; if ( dDist2 < - EPS_SMALL) return dDist2 ; // infine verifico quello del cilindro (tenendo conto dei precedenti allontanamenti) @@ -492,8 +493,38 @@ CAvCylinderTriangle( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double d return dLeakDist ; } // Movimento generico - else if ( vtMove * vtCylAx > 0) - return 0. ; + 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 dLeakDist = 0. ; + // distanza di allontanamento dall'interno + dLeakDist = max( dLeakDist, CylTriaInteriorLeakDistGenMot( ptCylOrig, vtCylAx, dHeigth, dRad, trTria, vtMove)) ; + if ( dLeakDist > EPS_SMALL) + return dLeakDist ; + // 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 ; + dLeakDist = max( dLeakDist, CylPointLeakDistGenMot( ptCylOrig, vtCylAx, dHeigth, dRad, + trTria.GetP( nV), vtMove, bTop, bBot)) ; + dLeakDist = max( dLeakDist, CylSegmentLeakDistGenMot( ptCylOrig, vtCylAx, dHeigth, dRad, + trTria.GetP( nV), vtSeg, dSegLen, vtMove, bTop, bBot)) ; + } + return dLeakDist ; + } // Errore else return -1. ; @@ -614,6 +645,150 @@ CylSegmentLeakDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, return max( max( dBaseLeakDist, dBottomLeakDist), max( dSurfLeakDist, 0.)) ; } +//---------------------------------------------------------------------------- +double +CylPointLeakDistGenMot( 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 + 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 - dRad * dRad ; + 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 dLeakDist = 0. ; + for ( int n = 0 ; n < nRoot ; ++ n) { + double dDotIntAx = ( vtR0 - vdRoots[n] * vtMove) * vtCylAx ; + if ( vdRoots[n] > dLeakDist && dDotIntAx < dTopTol && dDotIntAx > - dHeigth + dBotTol) + dLeakDist = vdRoots[n] ; + } + return dLeakDist ; +} + +//---------------------------------------------------------------------------- +double +CylSegmentLeakDistGenMot( 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 + 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 - dRad * dRad ; + vdCoef[1] = - 2 * vtR0Ort * vtMoveOrt ; + vdCoef[2] = vtMoveOrt * vtMoveOrt ; + // Risoluzione dell'equazione e studio delle soluzioni + int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ; + double dLeakDist = 0. ; + for ( int n = 0 ; n < nRoot ; ++ n) { + // Se abbiamo trovato una potenziale nuova distanza di allontanamento + if ( vdRoots[n] > dLeakDist) { + // 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)) + dLeakDist = vdRoots[n] ; + } + } + return dLeakDist ; + } + // 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 - dRad * dRad ; + // 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 dLeakDist = 0. ; + for ( int n = 0 ; n < nDeltaRoot ; ++ n) { + if ( vdDeltaRoots[n] > dLeakDist) { + 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) + dLeakDist = vdDeltaRoots[n] ; + } + } + } + return dLeakDist ; +} + +//---------------------------------------------------------------------------- +double +CylTriaInteriorLeakDistGenMot( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad, + const Triangle3d& trTria, const Vector3d& vtMove) +{ + double dLeakDist = 0. ; + if ( ! ( AreOrthoApprox( vtCylAx, trTria.GetN()))) + return dLeakDist ; + // 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 dLeakDist ; + // 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)) + dLeakDist = dU ; + return dLeakDist ; +} //---------------------------------------------------------------------------- // **** CONO **** @@ -690,8 +865,40 @@ CAvTrConeTriangle( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double return dLeakDist ; } // Movimento generico - else - return 0. ; + else { + // Controlli preliminari sull'interferenza fra ctronco 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 dLeakDist = 0. ; + // Allontanamento di allontanamento dall'interno del triangolo + dLeakDist = max( dLeakDist, TrConeTriaInteriorLeakDistGenMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, + dTrConeH, trTria, vtMove)) ; + // Se positiva abbiamo terminato + if ( dLeakDist > EPS_SMALL) + return dLeakDist ; + // 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 ; + dLeakDist = max( dLeakDist, TrConePointLeakDistGenMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH, + trTria.GetP( n), vtMove, bTop, bBot)) ; + dLeakDist = max( dLeakDist, TrConeSegmentLeakDistGenMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH, + trTria.GetP( n), vtSeg, dSegLen, vtMove, bTop, bBot)) ; + } + return dLeakDist ; + } } //---------------------------------------------------------------------------- @@ -949,6 +1156,174 @@ TrConeTriangleInteriorLeakDistOrtMot( const Point3d& ptMinBase, const Vector3d& return dLeakDist ; } +//---------------------------------------------------------------------------- +double +TrConePointLeakDistGenMot( 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 dLeakDist = 0. ; + for ( int n = 0 ; n < nRoot ; ++ n) { + if ( vdRoots[n] > dLeakDist) { + Point3d ptTouch = ptP - vdRoots[n] * vtMove ; + double dLongComp = ( ptTouch - ptMinBase) * vtTrConeAx ; + if ( dLongComp > dBotTol && dLongComp < dTrConeH + dTopTol) + dLeakDist = vdRoots[n] ; + } + } + return dLeakDist ; +} + +//---------------------------------------------------------------------------- +double +TrConeSegmentLeakDistGenMot( 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 dLeakDist = 0. ; + for ( int n = 0 ; n < nRoot ; ++ n) { + if ( vdRoots[n] > dLeakDist) { + 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))) + dLeakDist = vdRoots[n] ; + } + } + return dLeakDist ; + } + + // 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 dLeakDist = 0. ; + for ( int n = 0 ; n < nDeltaRoot ; ++ n) { + if ( vdDeltaRoots[n] > dLeakDist) { + 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) + dLeakDist = vdDeltaRoots[n] ; + } + } + } + return dLeakDist ; +} + +//---------------------------------------------------------------------------- +double +TrConeTriaInteriorLeakDistGenMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, + double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) +{ + double dLeakDist = 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 dLeakDist ; + // 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 dLeakDist ; + // 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)) + dLeakDist = dU ; + return dLeakDist ; +} + //---------------------------------------------------------------------------- // **** TORO **** // In questi algoritmi per toro intendiamo la corona torica esterna @@ -1028,8 +1403,40 @@ CAvTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double d } // Movimento generico - else - return -1 ; + 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 dInnLeakDist = TorusTriangleInteriorLeakDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, trTria, vtMove) ; + // Se positiva abbiamo finito + if ( dInnLeakDist > EPS_SMALL) + return dInnLeakDist ; + // Distanza di allontanamento da frontiera + double dBordLeakDist = 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 = TorusSegmentLeakDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, + trTria.GetP( n), vtSeg, dSegLen, vtMove) ; + dBordLeakDist = max( dBordLeakDist, dCurDist) ; + } + return dBordLeakDist ; + } } //---------------------------------------------------------------------------- @@ -1288,6 +1695,97 @@ TorusTriangleInteriorLeakDistOrtMot( const Point3d& ptTorusCen, const Vector3d& return 0. ; } +//---------------------------------------------------------------------------- +double +TorusPointLeakDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, + const Point3d& ptP, const Vector3d& vtMove) +{ + std::vector vbType ; + std::vector vdPar ; + // Intersezione fra semi-retta e corona torica + 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 +TorusSegmentLeakDistGenMot( 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 ptMySegP = ptSeg ; + Vector3d vtPlane = vtTorusAx ^ vtMove ; + if ( ! vtPlane.Normalize()) + return - 1. ; + Point3d ptPlaneLatPlus = ptTorusCen + ( dMaxRad + dMinRad) * vtPlane ; + Plane3d plPlane ; + plPlane.Set( ( ORIG - ptPlaneLatPlus) * vtPlane, - vtPlane) ; + ClampSegmentOutPlane( plPlane, ptMySegP, vtSeg, dSegLen) ; + Point3d ptPlanLateMinus = ptTorusCen - ( dMaxRad + dMinRad) * vtPlane ; + plPlane.Set( ( ptPlanLateMinus - ORIG) * vtPlane, vtPlane) ; + ClampSegmentOutPlane( plPlane, ptMySegP, vtSeg, dSegLen) ; + double dDotAxMv = vtMove * vtTorusAx ; + Vector3d vtOrtMv = vtMove - dDotAxMv * vtTorusAx ; + if ( ! vtOrtMv.Normalize()) + return - 1. ; + Point3d ptPlaneBehind = ptTorusCen - ( dMaxRad + dMinRad) * vtOrtMv ; + plPlane.Set( ( ptPlaneBehind - ORIG) * vtOrtMv, vtOrtMv) ; + ClampSegmentOutPlane( plPlane, ptMySegP, vtSeg, dSegLen) ; + Point3d ptUpDw = ptTorusCen - ( dDotAxMv > 0. ? dMinRad : 0.) * vtTorusAx ; + Vector3d vtUPDw = dDotAxMv > 0. ? vtTorusAx : - vtTorusAx ; + plPlane.Set( ( ptUpDw - ORIG) * vtUPDw, vtUPDw) ; + ClampSegmentOutPlane( plPlane, ptMySegP, vtSeg, dSegLen) ; + // Cerco il punto del segmento ridotto che ha distanza di allontanamento massima + double dLeakDist = 0. ; + double dStep = max( 0.05 * dMinRad, EPS_SMALL) ; + int nStepNum = int( dSegLen / dStep) ; + for ( int n = 0 ; n <= nStepNum ; ++ n) { + Point3d ptCurPoint = ptMySegP + ( n * dStep) * vtSeg ; + double dCurLeakDist = TorusPointLeakDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ; + if ( dLeakDist < dCurLeakDist) + dLeakDist = dCurLeakDist ; + } + return dLeakDist ; +} + +//---------------------------------------------------------------------------- +double +TorusTriangleInteriorLeakDistGenMot( 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 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 ( ! IsPointInsideOpenTriangle( ptTouch, trTria)) + dDist = 0. ; + return dDist ; +} + //---------------------------------------------------------------------------- double CAvConcaveTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, @@ -1634,8 +2132,52 @@ CAvDiskTriangle( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDis } // Direzione di allontanamento generica else if ( vtDiskAx * vtMove > 0.) { - // Non trattato al momento - return - 1. ; + Point3d ptV0 = trTria.GetP( 0) ; + Point3d ptV1 = trTria.GetP( 1) ; + Point3d ptV2 = trTria.GetP( 2) ; + // Distanze di allontanamento dei vertici + double dVertLeakDist[3] ; + dVertLeakDist[0] = - ( ( ptDiskCen - ptV0) * vtDiskAx) / ( vtMove * vtDiskAx) ; + dVertLeakDist[1] = - ( ( ptDiskCen - ptV1) * vtDiskAx) / ( vtMove * vtDiskAx) ; + dVertLeakDist[2] = - ( ( ptDiskCen - ptV2) * vtDiskAx) / ( vtMove * vtDiskAx) ; + // Triangolo proiettato sul piano del disco + Triangle3d trTriaOnPlane ; + trTriaOnPlane.Set( ptV0 - dVertLeakDist[0] * vtMove, + ptV1 - dVertLeakDist[1] * vtMove, + ptV2 - dVertLeakDist[2] * vtMove) ; + // Se non c'è interferenza fra triangolo proiettato e disco la distanza di fuga è nulla + if ( ! CoplanarDiscTriangleInterferance( ptDiskCen, dDiskRad, trTriaOnPlane)) + 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( dVertLeakDist[0], dVertLeakDist[1]), max( dVertLeakDist[2], 0.)) ; + // Caso generale + // Allontanamento dall'interno + double dLeakDist = max( DiskTriaInteriorLeakDistGenMot( ptDiskCen, vtDiskAx, dDiskRad, trTria, vtMove), 0.) ; + // Allontanamento dalla frontiera + Vector3d vtMoveOrt = vtMove - vtMove * vtDiskAx * vtDiskAx ; + vtMoveOrt.Normalize() ; + Frame3d DiskFrame ; + Vector3d vtJ = vtDiskAx ^ vtMoveOrt ; + vtJ.Normalize() ; + DiskFrame.Set( ptDiskCen, vtMoveOrt, vtJ, vtDiskAx) ; + 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 ; + dLeakDist = max( max( DiskSegmentLeakDistGenMot( dDiskRad, trTriaLoc.GetP( n), vtSeg, dSegLen, vtMoveLoc), + bInside[n] ? max( dVertLeakDist[n], 0.) : 0.), dLeakDist) ; + } + return dLeakDist ; } // Errore else @@ -1708,6 +2250,7 @@ DiskTriaInteriorLeakDistLongMot( const Point3d& ptDiskCen, double dDiskRad, double dDist = max( PointPlaneSignedDist( ptDiskCen, trTria.GetP( 0), trTria.GetN()), 0.) ; if ( CoplanarDiscTriangleInterferance( ptDiskCen + dDist * vtMove, dDiskRad, trTria)) return dDist ; + return 0. ; } // Se disco e triangolo sono ortogonali non può esserci contatto con interno if ( AreOrthoApprox( vtMove, trTria.GetN())) @@ -1717,20 +2260,12 @@ DiskTriaInteriorLeakDistLongMot( const Point3d& ptDiskCen, double dDiskRad, // Vettore radiale Vector3d vtRad = trTria.GetN() - ( trTria.GetN() * vtMove) * vtMove ; vtRad.Normalize() ; - // Punti delle due rette candidate all'intersezione col triangolo - Point3d ptStPlus = ptDiskCen + dDiskRad * vtRad ; - Point3d ptStMinus = ptDiskCen - dDiskRad * vtRad ; - - // Intersezioni con le rette - double dDistPlus = ( ( trTria.GetP( 0) - ptStPlus) * trTria.GetN()) / ( vtMove * trTria.GetN()) ; - double dDistMinus = ( ( trTria.GetP( 0) - ptStMinus) * trTria.GetN()) / ( vtMove * trTria.GetN()) ; - if ( dDistPlus > dDistMinus) { - if ( IsPointInsideTriangle( ptStPlus + dDistPlus * vtMove, trTria)) - dLeakDist = max( dLeakDist, dDistPlus) ; - } - else if ( IsPointInsideTriangle( ptStMinus + dDistMinus * vtMove, trTria)) - dLeakDist = max( dLeakDist, dDistMinus) ; - + // Punto candidato all'essere di estremo contatto + Point3d ptStLine = ptDiskCen - dDiskRad * vtRad ; + // Intersezioni con retta + double dDist = ( ( trTria.GetP( 0) - ptStLine) * trTria.GetN()) / ( vtMove * trTria.GetN()) ; + if ( IsPointInsideTriangle( ptStLine + dDist * vtMove, trTria)) + dLeakDist = max( dLeakDist, dDist) ; return dLeakDist ; } @@ -1856,6 +2391,83 @@ DiskPlaneLeakDistOrtMot( const Point3d& ptDiscCen, const Vector3d& vtDiskAx, dou return 0. ; } +//---------------------------------------------------------------------------- +// 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 +DiskSegmentLeakDistGenMot( double dDiskRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, + const Vector3d& vtMove) +{ + // Quadrato del raggio + double dSqRad = dDiskRad * dDiskRad ; + // 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. ; + + vector vdCoef(3) ; + vector vdRoots ; + + // 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 ; + // Setto i coeficienti dell'equazione + 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 + double dLenCoef = sqrt( 1 + dSqCoef) ; + int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ; + + double dLeakDist = 0. ; + for ( int n = 0 ; n < nRoot ; ++ n) { + if ( vdRoots[n] > 0 && vdRoots[n] < dSegLen) { + Point3d ptTouch = ptSeg + vdRoots[n] * vtSeg ; + dLeakDist = max( dLeakDist, dLenCoef * ptTouch.z) ; + } + } + return dLeakDist ; +} + +//---------------------------------------------------------------------------- +double +DiskTriaInteriorLeakDistGenMot( 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 ( CoplanarDiscTriangleInterferance( ptDiskCen + dDist * vtMove, dDiskRad, trTria)) + return dDist ; + return 0. ; + } + // Se disco e triangolo sono ortogonali non può esserci contatto con interno + if ( AreOrthoApprox( vtDiskAx, trTria.GetN())) + return 0. ; + // Cerco un punto di contatto nell'interno del triangolo. + double dLeakDist = 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)) + dLeakDist = max( dLeakDist, dDist) ; + return dLeakDist ; +} + // FUNZIONI GEOMETRICHE DI BASE PER IL CALCOLO DELLA DISTANZA DI ALLONTANAMENTO //---------------------------------------------------------------------------- @@ -2059,6 +2671,10 @@ IsPointInsideOpenTriangle( const Point3d& ptP, const Triangle3d& trTria) //---------------------------------------------------------------------------- // Valuta l'interferenza di un disco e un triangolo complanari. +// Si assume che disco e piano siano paralleli, ovvero che valga a n = 1 con +// a versore dell'asse del disco e n versore normale al triangolo. +// L'informazione su n è contenuta nell'oggetto triangolo; quella su a non è passata +// in quanto inutile sotto le nostre assunzioni. bool CoplanarDiscTriangleInterferance( const Point3d& ptCen, double dRad, const Triangle3d& trTria) { @@ -2068,19 +2684,13 @@ CoplanarDiscTriangleInterferance( const Point3d& ptCen, double dRad, const Trian // Se il centro giace nel triangolo if ( IsPointInsideTriangle( ptCen, trTria)) return true ; - double dSqMinDist = dRad * dRad + 2 * dRad * EPS_SMALL ; - Vector3d vtSeg0 = ( trTria.GetP(1) - trTria.GetP(0)) / Dist( trTria.GetP(1), trTria.GetP(0)) ; - Vector3d vtSeg1 = ( trTria.GetP(2) - trTria.GetP(1)) / Dist( trTria.GetP(2), trTria.GetP(1)) ; - Vector3d vtSeg2 = ( trTria.GetP(0) - trTria.GetP(2)) / Dist( trTria.GetP(0), trTria.GetP(2)) ; + double dSqMinDist = dRad * dRad + 2 * dRad * EPS_SMALL ; // Se il centro non si avvicina più del raggio dalle rette associate ai segmenti, non c'è interferenza - if ( GetPointLineSqDist( ptCen, trTria.GetP(0), vtSeg0) > dSqMinDist && - GetPointLineSqDist( ptCen, trTria.GetP(1), vtSeg1) > dSqMinDist && - GetPointLineSqDist( ptCen, trTria.GetP(2), vtSeg2) > dSqMinDist) - return false ; - // Se il centro non si avvicina più del raggio ai vertici, non c'è interferenza - if ( SqDist( ptCen, trTria.GetP(0)) > dSqMinDist && - SqDist( ptCen, trTria.GetP(1)) > dSqMinDist && - SqDist( ptCen, trTria.GetP(2)) > dSqMinDist) + double dSegPointDist0, dSegPointDist1, dSegPointDist2 ; + ( DistPointLine( ptCen, trTria.GetP( 0), trTria.GetP( 1))).GetSqDist( dSegPointDist0) ; + ( DistPointLine( ptCen, trTria.GetP( 1), trTria.GetP( 2))).GetSqDist( dSegPointDist1) ; + ( DistPointLine( ptCen, trTria.GetP( 2), trTria.GetP( 0))).GetSqDist( dSegPointDist2) ; + if ( dSegPointDist0 > dSqMinDist && dSegPointDist1 > dSqMinDist && dSegPointDist2 > dSqMinDist) return false ; // interferenza return true ; @@ -2227,3 +2837,31 @@ DiskPlaneLastContactLongMot( const Point3d& ptDiskCen, double dDiskRad, return 1 ; return 0 ; } + +//---------------------------------------------------------------------------- +// Si limita il segmento alla parte di spazio con distanza positiva dal piano. +// Se si modifica il segmento si restituisce true, altrimenti false. +bool +ClampSegmentOutPlane( const Plane3d& plPlane, Point3d& ptSegP, const Vector3d& vtSegV, double& dSegLen) +{ + // Distanze degli estremi del segmento dal piano + double dDistStart = DistPointPlane( ptSegP, plPlane) ; + double dDistEnd = DistPointPlane( ptSegP + dSegLen * vtSegV, plPlane) ; + // Se il segmento sta nel piano o è tutto esterno, va bene + if ( dDistStart > -EPS_ZERO && dDistEnd > -EPS_ZERO) + return false ; + // Se il segmento è tutto interno, va eliminato + if ( dDistStart < 0 && dDistEnd < 0) { + dSegLen = 0 ; + return true ; + } + // Altrimenti il segmento attraversa il piano e va limitato + double dIntersLen = dSegLen * abs( dDistStart) / ( abs( dDistStart) + abs( dDistEnd)) ; + if ( dDistStart > 0) + dSegLen = dIntersLen ; + else { + ptSegP += dIntersLen * vtSegV ; + dSegLen -= dIntersLen ; + } + return true ; +} diff --git a/CAvToolTriangle.h b/CAvToolTriangle.h index 59c48cc..4237f74 100644 --- a/CAvToolTriangle.h +++ b/CAvToolTriangle.h @@ -39,6 +39,13 @@ double CylPointLeakDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCy double CylSegmentLeakDistOrtMotion( 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 CylPointLeakDistGenMot( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad, + const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot) ; +double CylSegmentLeakDistGenMot( 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) ; +double CylTriaInteriorLeakDistGenMot( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad, + const Triangle3d& trTria, const Vector3d& vtMove) ; // Cono double CAvTrConeTriangle( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, @@ -57,6 +64,13 @@ double TrConeSegmentLeakDistOrtMot( const Point3d& ptMinBase, const Vector3d& vt const Vector3d& vtMove, bool bTop, bool bBot) ; double TrConeTriangleInteriorLeakDistOrtMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) ; +double TrConePointLeakDistGenMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, + double dTrConeH, const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot) ; +double TrConeSegmentLeakDistGenMot( 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 TrConeTriaInteriorLeakDistGenMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, + double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) ; // Toro double CAvTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, @@ -70,6 +84,15 @@ double TorusSegmentLeakDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vt bool bTop, bool bBot) ; double TorusTriangleInteriorLeakDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Triangle3d& trTria, const Vector3d& vtMove) ; +double TorusSegmentLeakDistOrtMot( 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) ; +double TorusPointLeakDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, + const Point3d& ptP, const Vector3d& vtMove) ; +double TorusSegmentLeakDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, + const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ; +double TorusTriangleInteriorLeakDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, + const Triangle3d& trTria, const Vector3d& vtMove) ; double CAvConcaveTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot) ; @@ -103,6 +126,11 @@ double DiskSegmentLeakDistOrtMot( const Point3d& ptDiskCen, const Vector3d& vtDi double DiskPlaneLeakDistOrtMot( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad, const Point3d& ptPlane, const Vector3d& vtPlane, const Vector3d& vtMove, Point3d& ptContact) ; +double DiskSegmentLeakDistGenMot( double dDiskRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, + const Vector3d& vtMove) ; +double DiskTriaInteriorLeakDistGenMot( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad, + const Triangle3d& trTria, const Vector3d& vtMove) ; + // Funzioni geometriche di base double GetPointLineSqDist( const Point3d& ptP, const Point3d& ptLine, const Vector3d& vtLine) ; @@ -125,6 +153,7 @@ int SphereLineTangentPoints( const Point3d& ptSpheCen, double dSpheRad, int DiskPlaneLastContactLongMot( const Point3d& ptDiskCen, double dDiskRad, const Point3d& ptPlane, const Vector3d& vtPlane, const Vector3d& vtMove, Point3d& ptTouch) ; +bool ClampSegmentOutPlane( const Plane3d& plPlane, Point3d& ptSegP, const Vector3d& vtSegV, double& dSegLen) ; // Altre funzioni inline double EvalSecondDegreePolynomial( double dCoeff0, double dCoeff1, double dCoeff2, double dVariable) diff --git a/EgtGeomKernel.rc b/EgtGeomKernel.rc index 7f2fac4..ea3ec15 100644 Binary files a/EgtGeomKernel.rc and b/EgtGeomKernel.rc differ diff --git a/IntersLineSurfStd.cpp b/IntersLineSurfStd.cpp index e652376..098af6a 100644 --- a/IntersLineSurfStd.cpp +++ b/IntersLineSurfStd.cpp @@ -1610,8 +1610,8 @@ LineTorus( const Point3d& ptLine, const Vector3d& vtLine, const Point3d& ptOTorus, const Vector3d& vtAxTorus, double dMinRad, double dMaxRad, BOOLVECTOR& vbType, DBLVECTOR& vdPar) { - // I Raggi devono essere positivi e sufficientemente diversi - if ( dMinRad < EPS_SMALL || dMaxRad < dMinRad + EPS_SMALL) + // I Raggi devono essere positiv + if ( dMinRad < EPS_SMALL || dMaxRad < EPS_SMALL) return T_ERROR ; // Si richiede che i vettori siano normalizzati if ( ! vtAxTorus.IsNormalized() || ! vtLine.IsNormalized())