From a38a2770892ffe6a4a3c7a2a2ff5c05039566ee9 Mon Sep 17 00:00:00 2001 From: Dario Sassi Date: Mon, 18 Jun 2018 15:23:39 +0000 Subject: [PATCH] EgtGeomKernel 1.9f1 : - inserita prima versione CollisionAvoiding per utensili toroidali. --- CAvToolTriangle.cpp | 990 ++++++++++++++++++++++++++++++------------ CAvToolTriangle.h | 62 ++- EgtGeomKernel.rc | Bin 11710 -> 11710 bytes GdbExecutor.cpp | 8 +- IntersLineSurfStd.cpp | 240 ++++------ IntersLineSurfStd.h | 31 +- 6 files changed, 851 insertions(+), 480 deletions(-) diff --git a/CAvToolTriangle.cpp b/CAvToolTriangle.cpp index 302b627..d1b1f27 100644 --- a/CAvToolTriangle.cpp +++ b/CAvToolTriangle.cpp @@ -35,7 +35,7 @@ CAvToolTriangle( const Tool& tlTool, const Point3d& ptToolOrig, const Vector3d& double dHeigth = tlTool.GetHeigth() ; double dRadius = tlTool.GetRadius() ; // distanza di allontanamento del cilindro - double dDist = CAvCylinderTriangle( ptToolOrig, vtToolAx, dHeigth, dRadius, trTria, vtMove) ; + double dDist = CAvCylinderTriangle( ptToolOrig, vtToolAx, dHeigth, dRadius, trTria, vtMove, false, false) ; return dDist ; } // se utensile sferico @@ -52,7 +52,25 @@ CAvToolTriangle( const Tool& tlTool, const Point3d& ptToolOrig, const Vector3d& if ( dCylHeigth < EPS_SMALL) return dDist ; Point3d ptCylOrig = ptToolOrig + vtMove * max( dDist, 0.) ; - double dDist2 = CAvCylinderTriangle( ptCylOrig, vtToolAx, dCylHeigth, dRadius, trTria, vtMove) ; + double dDist2 = CAvCylinderTriangle( ptCylOrig, vtToolAx, dCylHeigth, dRadius, trTria, vtMove, false, true) ; + if ( dDist2 < - EPS_SMALL) + return dDist2 ; + return ( dDist + dDist2) ; + } + else if ( tlTool.GetType() == Tool::GEN) { + // parametri geometrici + double dCylHeigth = tlTool.GetHeigth() - tlTool.GetTipHeigth() ; + Point3d ptTorusCen = ptToolOrig - dCylHeigth * vtToolAx ; + // prima determino l'allontanamento del toro + double dDist = CAvTorusTriangle( ptTorusCen, vtToolAx, tlTool.GetRadius() - tlTool.GetCornRadius(), tlTool.GetCornRadius(), + trTria, vtMove, true, false) ; + if ( dDist < - EPS_SMALL) + return dDist ; + // poi verifico quello del cilindro (tenendo conto di quanto è stata allontanato il toro) + if ( dCylHeigth < EPS_SMALL) + return dDist ; + Point3d ptCylOrig = ptToolOrig + vtMove * max( dDist, 0.) ; + double dDist2 = CAvCylinderTriangle( ptCylOrig, vtToolAx, dCylHeigth, tlTool.GetRadius(), trTria, vtMove, false, true) ; if ( dDist2 < - EPS_SMALL) return dDist2 ; return ( dDist + dDist2) ; @@ -74,14 +92,14 @@ CAvToolTriangle( const Tool& tlTool, const Point3d& ptToolOrig, const Vector3d& double dMaxR = max( tlTool.GetRadius(), tlTool.GetTipRadius()) ; double dCylHeigth = tlTool.GetHeigth() - tlTool.GetTipHeigth() ; // prima determino l'allontanamento del cono - double dDist = CAvTrConeTriangle( ptMinBase, vtConeAx, dMinR, dMaxR, tlTool.GetTipHeigth(), trTria, vtMove) ; + double dDist = CAvTrConeTriangle( ptMinBase, vtConeAx, dMinR, dMaxR, tlTool.GetTipHeigth(), trTria, vtMove, true, false) ; if ( dDist < - EPS_SMALL) return dDist ; // poi verifico quello del cilindro (tenendo conto di quanto è stata allontanato il cono) if ( dCylHeigth < EPS_SMALL) return dDist ; Point3d ptCylOrig = ptToolOrig + vtMove * max( dDist, 0.) ; - double dDist2 = CAvCylinderTriangle( ptCylOrig, vtToolAx, dCylHeigth, tlTool.GetRadius(), trTria, vtMove) ; + double dDist2 = CAvCylinderTriangle( ptCylOrig, vtToolAx, dCylHeigth, tlTool.GetRadius(), trTria, vtMove, false, true) ; if ( dDist2 < - EPS_SMALL) return dDist2 ; return ( dDist + dDist2) ; @@ -202,7 +220,7 @@ SpherePointLeakDist( const Point3d& ptSpheCen, double dSpheRad, const Point3d pt // 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) + const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot) { // Classificazione del moto int nMotionType = 0 ; @@ -256,7 +274,7 @@ CAvCylinderTriangle( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double d double dSegLen = vtSeg.Len() ; vtSeg /= dSegLen ; // Distanza dal piano del segmento corrente - double dCurDist = DiskSegmentLeakDistLongMotion( ptBase, dRad, trTria.GetP( nVS), vtSeg, dSegLen, vtMove) ; + double dCurDist = DiskSegmentLeakDistLongMot( ptBase, dRad, trTria.GetP( nVS), vtSeg, dSegLen, vtMove) ; if ( dCurDist > dMaxDistVS) dMaxDistVS = dCurDist ; } @@ -269,7 +287,7 @@ CAvCylinderTriangle( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double d // PROVE PUNTI E SEGMENTI double dLeakDist = 0. ; for ( int nVrt = 0 ; nVrt < 3 ; ++ nVrt) { - double dCurDist = CylPointLeakDistOrtMotion( ptCylOrig, vtCylAx, dHeigth, dRad, trTria.GetP( nVrt), vtMove) ; + double dCurDist = CylPointLeakDistOrtMotion( ptCylOrig, vtCylAx, dHeigth, dRad, trTria.GetP( nVrt), vtMove, bTop, bBot) ; if ( dLeakDist < dCurDist) dLeakDist = dCurDist ; } @@ -281,7 +299,7 @@ CAvCylinderTriangle( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double d double dSegLen = vtSeg.Len() ; vtSeg /= dSegLen ; double dCurDist = CylSegmentLeakDistOrtMotion( ptCylOrig, vtCylAx, dHeigth, dRad, - trTria.GetP( nVrtS), vtSeg, dSegLen, vtMove) ; + trTria.GetP( nVrtS), vtSeg, dSegLen, vtMove, bTop, bBot) ; if ( dLeakDist < dCurDist) dLeakDist = dCurDist ; } @@ -307,28 +325,662 @@ CAvCylinderTriangle( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double d 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 +CylPointLeakDistOrtMotion( 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 +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 dTopTol = bTop ? EPS_SMALL : - EPS_SMALL ; + double dBotTol = bBot ? - EPS_SMALL : EPS_SMALL ; + // Le variabili fanno rifermiento a un sistema di 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 ; + Vector3d vtSegStart1 = dCordStart1 * vtCylAx ; + Vector3d vtSegEnd1 = dCordEnd1 * vtCylAx ; + Vector3d vtSegStart23 = vtSegStart - vtSegStart1 ; + Vector3d vtSegEnd23 = vtSegEnd - vtSegEnd1 ; + // Se entrambi gli estremi del segmento sono a un lato + // del cilindro non vi può essere interferenza. + double dCordStart2 = vtSegStart23 * vtPlane ; + double dCordEnd2 = vtSegEnd23 * vtPlane ; + if ( ( dCordStart2 > dCylRad && dCordEnd2 > dCylRad) || + ( dCordStart2 < - dCylRad && dCordEnd2 < - dCylRad)) + return 0. ; + // Verifico la distanza di fuga di superficie laterale e dischi di base e fondo. + double dBaseLeakDist = DiskSegmentLeakDistOrtMot( ptCylOrig, vtCylAx, dCylRad, ptSeg, vtSeg, dSegLen, vtMove) ; + double dBottomLeakDist = DiskSegmentLeakDistOrtMot( ptCylOrig - dCylHei * vtCylAx, vtCylAx, dCylRad, + ptSeg, vtSeg, dSegLen, vtMove) ; + + double dSurfLeakDist = 0. ; + if ( ( dCordStart1 < dTopTol && dCordStart1 > - dCylHei + dBotTol) || + ( dCordEnd1 < dTopTol && dCordEnd1 > - dCylHei + dBotTol)) { + // 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()) { + dSurfLeakDist = max( ( ptSeg - ptCylOrig) * vtMove, 0.) ; + } + // 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 + // abbia distanza dal piano +/- dOrtLen. + Vector3d vtPlane = vtMove ^ vtCylAx ; + vtPlane.Normalize() ; + 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)) { + dSurfLeakDist = 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.) ; + dSurfLeakDist = max( dNewDist, dSurfLeakDist) ; + } + } + } + return max( max( dBaseLeakDist, dBottomLeakDist), max( dSurfLeakDist, 0.)) ; +} + + +//---------------------------------------------------------------------------- +// **** 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) +{ + // Allontanamento con direzione coincidente con l'asse del cono + if ( AreSameOrOppositeVectorApprox( vtTrConeAx, vtMove)) { + // Distanza di allontanamento dai vertici e lati + double dOutLeakDist = 0 ; + for ( int nV = 0 ; nV < 3 ; ++ nV) { + // dal vertice + double dCurVertDist = TrConePointLeakDistLongMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH, + trTria.GetP( nV), vtMove) ; + dOutLeakDist = max( dCurVertDist, dOutLeakDist) ; + // dal lato + Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ; + double dSegLen = vtSeg.Len() ; + vtSeg /= dSegLen ; + double dCurSegDist = TrConeSegmentLeakDistLongMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, + dTrConeH, trTria.GetP( nV), vtSeg, dSegLen, vtMove) ; + dOutLeakDist = max( dCurSegDist, dOutLeakDist) ; + } + double dInnLeakDist = TrConeTriangleInteriorLeakDistLongMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH, + trTria, vtMove) ; + return max( dOutLeakDist, dInnLeakDist) ; + } + // allontanamento in direzione ortogonale all'asse del cono + else if ( AreOrthoApprox( vtTrConeAx, vtMove)) { + double dOutLeakDist = 0. ; + // Distanza di allontanamento da vertici e lati + for ( int nV = 0; nV < 3; ++ nV) { + // Dai vertici + double dCurVertDist = TrConePointLeakDistOrtMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH, + trTria.GetP( nV), vtMove, bTop, bBot) ; + dOutLeakDist = max( dOutLeakDist, dCurVertDist) ; + // Dal lato + Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ; + double dSegLen = vtSeg.Len() ; + vtSeg /= dSegLen ; + double dCurSegDist = TrConeSegmentLeakDistOrtMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH, + trTria.GetP( nV), vtSeg, dSegLen, vtMove, bTop, bBot) ; + dOutLeakDist = max( dOutLeakDist, dCurSegDist) ; + } + double dInnLeakDist = TrConeTriangleInteriorLeakDistOrtMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH, + trTria, vtMove) ; + return max( dOutLeakDist, dInnLeakDist) ; + } + // altri casi + else + return 0. ; +} + //---------------------------------------------------------------------------- double -DiskSegmentLeakDistLongMotion( const Point3d& ptDiskCen, double dDiscRad, - const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) +TrConePointLeakDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, + double dTrConeH, const Point3d& ptP, const Vector3d& vtMove) +{ + double dPointAxisSqDist = GetPointLineSqDist( ptP, ptMinBase, vtTrConeAx) ; + if ( dPointAxisSqDist > dMaxBaseR * dMaxBaseR + 2 * dMaxBaseR * EPS_SMALL) + return 0. ; + else if ( dPointAxisSqDist > dMinBaseR * dMinBaseR + 2 * dMinBaseR * EPS_SMALL) { + double dMinBaseLeakDist = PointPlaneSignedDist( ptP, ptMinBase, vtMove) ; + double dMaxBaseLeakDist = PointPlaneSignedDist( ptP, ptMinBase + dTrConeH * vtTrConeAx, vtMove) ; + if ( dMinBaseLeakDist < - EPS_SMALL && dMaxBaseLeakDist < - EPS_SMALL) + return 0. ; + double dDistMinBaseV = dTrConeH * dMinBaseR / ( dMaxBaseR - dMinBaseR) ; + Point3d ptConeV = ptMinBase - dDistMinBaseV * vtTrConeAx ; + Vector3d vtVertP = ptP - ptConeV ; + double dVertPLongCord = vtMove * vtVertP ; + if ( vtMove * vtTrConeAx > 0.) { + double dPointAxisDist = sqrt( GetPointLineSqDist( ptP, ptMinBase, vtMove)) ; + double dLateralSurfLeakDist = dVertPLongCord - dPointAxisDist * dTrConeH / ( dMaxBaseR - dMinBaseR) ; + return max( dLateralSurfLeakDist, 0.) ; + } + else + return max( PointPlaneSignedDist( ptP, ptMinBase + dTrConeH * vtTrConeAx, vtMove), 0.) ; + } + else { + double dMinBaseLeakDist = PointPlaneSignedDist( ptP, ptMinBase, vtMove) ; + double dMaxBaseLeakDist = PointPlaneSignedDist( ptP, ptMinBase + dTrConeH * vtTrConeAx, vtMove) ; + return max( max( dMinBaseLeakDist, dMaxBaseLeakDist), 0.) ; + } +} + +//---------------------------------------------------------------------------- +double +TrConeSegmentLeakDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, + double dTrConeH, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, + const Vector3d& vtMove) +{ + // Distanza di allontanamento delle basi + Point3d ptMaxBase = ptMinBase + dTrConeH * vtTrConeAx ; + double dMinBaseLeakDist = DiskSegmentLeakDistLongMot( ptMinBase, dMinBaseR, ptSeg, vtSeg, dSegLen, vtMove) ; + double dMaxBaseLeakDist = DiskSegmentLeakDistLongMot( ptMaxBase, dMaxBaseR, ptSeg, vtSeg, dSegLen, vtMove) ; + double dBasesLeakDist = max( max( dMinBaseLeakDist, dMaxBaseLeakDist), 0.) ; + // Distanza di allontanamento della superficie laterale + double dDistMinBaseV = dTrConeH * dMinBaseR / ( dMaxBaseR - dMinBaseR) ; + Point3d ptConeV = ptMinBase - dDistMinBaseV * vtTrConeAx ; + Frame3d frConusFrame ; + frConusFrame.Set( ptConeV, vtTrConeAx) ; + Point3d ptSegNew = ptSeg ; + Vector3d vtSegNew = vtSeg ; + ptSegNew.ToLoc( frConusFrame) ; + vtSegNew.ToLoc( frConusFrame) ; + double dAngCoef = ( dDistMinBaseV + dTrConeH) / dMaxBaseR ; + double dAlpha = vtSegNew.z * vtSegNew.z - dAngCoef * dAngCoef * ( vtSegNew.x * vtSegNew.x + vtSegNew.y * vtSegNew.y) ; + double dBeta = ptSegNew.z * vtSegNew.z - dAngCoef * dAngCoef * ( ptSegNew.x * vtSegNew.x + ptSegNew.y * vtSegNew.y) ; + double dGamma = ptSegNew.z * ptSegNew.z - dAngCoef * dAngCoef * ( ptSegNew.x * ptSegNew.x + ptSegNew.y * ptSegNew.y) ; + // Se l'equazione è di primo grado, non serve risolvere (si trova altrimenti con i punti estremi del segmento) + if ( abs( dAlpha) < EPS_ZERO) + return 0. ; + // Impongo il determinante nullo, ne deriva una nuova equazione da risolvere + DBLVECTOR vdDeltaCoef( 3) ; + DBLVECTOR vdDeltaRoots ; + vdDeltaCoef[0] = dBeta * dBeta - dAlpha * dGamma ; + vdDeltaCoef[1] = 2 * ( dBeta * vtSegNew.z - dAlpha * ptSegNew.z) ; + vdDeltaCoef[2] = ( vtSegNew.z * vtSegNew.z - dAlpha) ; + int nRoot = PolynomialRoots( 2, vdDeltaCoef, vdDeltaRoots) ; + // Studio le soluzioni + double dLateralSurfLeakDist = 0. ; + for ( int nS = 0 ; nS < nRoot ; ++ nS) { + // Queste soluzioni sono gli spostamenti lungo z 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 = - ( dBeta + vtSegNew.z * vdDeltaRoots[nS]) / dAlpha ; + Point3d ptTan( ptSegNew.x, ptSegNew.y, ptSegNew.z + vdDeltaRoots[nS]) ; + ptTan += dT * vtSegNew ; + if ( ptTan.z > dDistMinBaseV - EPS_SMALL && + ptTan.z < dDistMinBaseV + dTrConeH + EPS_SMALL && + dT > - EPS_SMALL && dT < dSegLen + EPS_SMALL) { + if ( ( vtTrConeAx * vtMove > 0 && vdDeltaRoots[nS] < 0.) || + ( vtTrConeAx * vtMove < 0 && vdDeltaRoots[nS] > 0.)) + dLateralSurfLeakDist = abs( vdDeltaRoots[nS]) ; + } + } + return max( dBasesLeakDist, dLateralSurfLeakDist) ; +} + +//---------------------------------------------------------------------------- +double +TrConeTriangleInteriorLeakDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, + double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) +{ + // Distanza di allontanamento del disco minore + Point3d ptMinTouch ; + int nMinLeakType = DiskPlaneLastContactLongMot( ptMinBase, dMinBaseR, trTria.GetP( 0), trTria.GetN(), + vtMove, ptMinTouch) ; + double dMinLeakDist = ( ptMinTouch - ptMinBase) * vtMove ; + // Distanza di allontanamento del disco maggiore + Point3d ptMaxTouch ; + Point3d ptMaxBase = ptMinBase + dTrConeH * vtTrConeAx ; + int nMaxLeakType = DiskPlaneLastContactLongMot( ptMaxBase, dMaxBaseR, trTria.GetP( 0), trTria.GetN(), + vtMove, ptMaxTouch) ; + double dMaxLeakDist = ( ptMaxTouch - ptMaxBase) * vtMove ; + // se le due distanze sono uguali e tipo di contatto 1, allora superficie laterale cono tangente a piano + if ( abs( dMaxLeakDist - dMinLeakDist) < EPS_SMALL) { + Point3d ptInt1, ptInt2 ; + int nRes = IntersLineTria( ptMinTouch, ptMaxTouch, trTria, ptInt1, ptInt2) ; + if ( nRes != ILTT_NO) + return max( ( dMaxLeakDist + dMinLeakDist) / 2, 0.) ; + } + // altrimenti sono i dischi a determinare l'allontanamento + double dMove = 0 ; + if ( ( nMinLeakType == 1 && IsPointInsideTriangle( ptMinTouch, trTria)) || + ( nMinLeakType == 3 && CoplanarDiscTriangleInterferance( ptMinTouch, dMinBaseR, trTria))) + dMove = max( dMinLeakDist, 0.) ; + if ( ( nMaxLeakType == 1 && IsPointInsideTriangle( ptMaxTouch, trTria)) || + ( nMaxLeakType == 3 && CoplanarDiscTriangleInterferance( ptMaxTouch, dMaxBaseR, trTria))) + dMove = max( dMove, dMaxLeakDist) ; + return dMove ; +} + +//---------------------------------------------------------------------------- +double +TrConePointLeakDistOrtMot( 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 DiskPointLeakDistOrtMot( ptMinBase + dPntPosOnAx * vtTrConeAx, vtTrConeAx, dR, ptP, vtMove) ; +} + +//---------------------------------------------------------------------------- +double +TrConeSegmentLeakDistOrtMot( 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 ; + // Distanza di allontanamento delle basi + Point3d ptMaxBase = ptMinBase + dTrConeH * vtTrConeAx ; + double dMinBaseLeakDist = DiskSegmentLeakDistOrtMot( ptMinBase, vtTrConeAx, dMinBaseR, ptSeg, vtSeg, dSegLen, vtMove) ; + double dMaxBaseLeakDist = DiskSegmentLeakDistOrtMot( ptMaxBase, vtTrConeAx, dMaxBaseR, ptSeg, vtSeg, dSegLen, vtMove) ; + double dBasesLeakDist = max( max( dMinBaseLeakDist, dMaxBaseLeakDist), 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 dLateralSurfLeakDist = 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] > dLateralSurfLeakDist) + dLateralSurfLeakDist = - vdDeltaRoots[nS] ; + } + } + return max( dBasesLeakDist, dLateralSurfLeakDist) ; +} + +//---------------------------------------------------------------------------- +double +TrConeTriangleInteriorLeakDistOrtMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, + double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) +{ + // Se l'asse del cono è ortogonale al piano la distanza di + // allontanamento è determinata dai vetici e segmenti. + if ( AreSameOrOppositeVectorApprox( vtTrConeAx, trTria.GetN())) + return 0. ; + + // Distanza di allontanamento del disco minore + Point3d ptMinTouch ; + double dMinLeakDist = DiskPlaneLeakDistOrtMot( ptMinBase, vtTrConeAx, dMinBaseR, trTria.GetP( 0), trTria.GetN(), + vtMove, ptMinTouch) ; + // Distanza di allontanamento del disco maggiore + Point3d ptMaxTouch ; + Point3d ptMaxBase = ptMinBase + dTrConeH * vtTrConeAx ; + double dMaxLeakDist = DiskPlaneLeakDistOrtMot( ptMaxBase, vtTrConeAx, dMaxBaseR, trTria.GetP( 0), trTria.GetN(), + vtMove, ptMaxTouch) ; + + // se le due distanze sono uguali, allora superficie laterale cono tangente a piano + if ( abs( dMaxLeakDist - dMinLeakDist) < EPS_SMALL && + dMinLeakDist > 0. && dMaxLeakDist > 0.) { + Point3d ptInt1, ptInt2 ; + int nRes = IntersLineTria( ptMinTouch, ptMaxTouch, trTria, ptInt1, ptInt2) ; + if ( nRes != ILTT_NO) + return max( ( dMaxLeakDist + dMinLeakDist) / 2, 0.) ; + } + + // altrimenti sono i dischi a determinare l'allontanamento + double dMove = 0 ; + if ( IsPointInsideTriangle( ptMinTouch, trTria)) + dMove = max( dMinLeakDist, 0.) ; + if ( IsPointInsideTriangle( ptMaxTouch, trTria)) + dMove = max( dMove, dMaxLeakDist) ; + return dMove ; +} + +//---------------------------------------------------------------------------- +// **** 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 con direzione coincidente con l'asse del toro + if ( AreSameOrOppositeVectorApprox( vtTorusAx, vtMove)) { + // Distanza di allontanamento dai lati + double dOutLeakDist = 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 = TorusSegmentLeakDistLongMot( ptTorusCen, dMaxRad, dMinRad, + trTria.GetP( nV), vtSeg, dSegLen, vtMove) ; + dOutLeakDist = max( dCurDist, dOutLeakDist) ; + // 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 > dOutLeakDist) + dOutLeakDist = max( dOutLeakDist, dCurDist) ; + } + double dInLeakDist = TorusTriangleInteriorLeakDistLongMot( ptTorusCen, dMaxRad, dMinRad, + trTria, vtMove) ; + return max( dInLeakDist, dOutLeakDist) ; + } + // Allontanamento in direzione ortogonale all'asse del toro + else if ( AreOrthoApprox( vtTorusAx, vtMove)) { + // Distanza di allontanamento dai lati + double dOutLeakDist = 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 = TorusSegmentLeakDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, + trTria.GetP( nV), vtSeg, dSegLen, vtMove, bTop, bBot) ; + dOutLeakDist = max( dCurDist, dOutLeakDist) ; + } + double dInLeakDist = TorusTriangleInteriorLeakDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, trTria, vtMove) ; + return max( dOutLeakDist, dInLeakDist) ; + } + // Casi non gestiti + else + return 0. ; +} + +//---------------------------------------------------------------------------- +double +TorusSegmentLeakDistLongMot( 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.0 ; + + // Dati del segmento + Point3d ptMySeg = ptSeg ; + double dMySegLen = dSegLen ; + // Limito il segmento entro la corona del toro + double dLen1, dLen2 ; + int nInters = IntersLineInfiniteCylinder( ptSeg, vtSeg, ptTorusCen, vtMove, dMaxRad + dMinRad, dLen1, dLen2) ; + if ( nInters == CC_TWO_INT) { + if ( dLen1 > EPS_SMALL) { + ptMySeg += vtSeg * dLen1 ; + dMySegLen -= dLen1 ; + dLen2 -= dLen1 ; + } + if ( dLen2 < dMySegLen) + dMySegLen -= ( dMySegLen - dLen2) ; + } + else if ( nInters == CC_ONE_INT_TAN) { + ptMySeg += vtSeg * dLen1 ; + dMySegLen = 0 ; + } + + Vector3d vtR0 = ptMySeg - ptTorusCen ; + double dLongR0 = vtR0 * vtMove ; + Vector3d vtOrtR0 = vtR0 - dLongR0 * vtMove ; + Vector3d vtSegOrt = vtSeg - vtSeg * vtMove * vtMove ; + + // Cerco massima distanza di allontanamento lungo il segmento + double dStep = max( 0.1 * dMinRad, EPS_SMALL) ; // !!! USARE ALGORITMO GOLDEN SECTION !!! + int nStepNum = int( dMySegLen / dStep) + 1 ; + dStep = dMySegLen / nStepNum ; + + double dDotSegLine = vtSeg * vtMove ; + double dSqMinRad = dMinRad * dMinRad ; + double dSqMaxRad = dMaxRad * dMaxRad ; + double dSqLenOrt = vtOrtR0 * vtOrtR0 ; + double dSegOrtSqLen = vtSegOrt * vtSegOrt ; + double dSegOrtDotROrt = vtOrtR0 * vtSegOrt ; + double dMaxLeakDist = 0.0 ; + for ( int n = 0 ; n <= nStepNum ; ++ n) { + double dPar = n * dStep ; + double dPointAxSqLen = GetPointLineSqDist( ptMySeg + dPar * vtSeg, ptTorusCen, vtMove) ; + if ( dPointAxSqLen > dSqMaxRad && dPointAxSqLen < ( dMaxRad + dMinRad) * ( dMaxRad + dMinRad)) { + double dPolynomialSqRoot = sqrt( EvalSecondDegreePolynomial( dSqLenOrt, 2 * dSegOrtDotROrt, dSegOrtSqLen, dPar)) ; + double dNonLinearComp = sqrt( dSqMinRad - ( dMaxRad - dPolynomialSqRoot) * ( dMaxRad - dPolynomialSqRoot)) ; + double dLeakDist = dLongR0 + dPar * dDotSegLine + dNonLinearComp ; + if ( dLeakDist > dMaxLeakDist) + dMaxLeakDist = dLeakDist ; + } + } + return dMaxLeakDist ; +} + +//---------------------------------------------------------------------------- +double +TorusTriangleInteriorLeakDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad, + const Triangle3d& trTria, const Vector3d& vtMove) +{ + // Se piano del triangolo e asse del toro perpendicolari + if ( AreSameOrOppositeVectorApprox( trTria.GetN(), vtMove)) { + // Se tutti i punti del triangolo distano meno del raggio dMaxRad non c'è interferenza col toro + if ( GetPointLineSqDist( trTria.GetP( 0), ptTorusCen, vtMove) < dMaxRad * dMaxRad && + GetPointLineSqDist( trTria.GetP( 1), ptTorusCen, vtMove) < dMaxRad * dMaxRad && + GetPointLineSqDist( trTria.GetP( 2), ptTorusCen, vtMove) < dMaxRad * dMaxRad) + return 0. ; + // Calcolo distanza di allontanamento e verifico se positiva + double dLeakDist = max( dMinRad + ( ( trTria.GetP( 0) - ptTorusCen) * trTria.GetN()) / ( vtMove * trTria.GetN()), 0.) ; + if ( dLeakDist > 0) { + // Se triangolo interferisce con anello esterno del toro (bordo interno) + Point3d ptNewCen = ptTorusCen + ( dLeakDist - dMinRad) * vtMove ; + if ( CoplanarDiscTriangleInterferance( ptNewCen, dMaxRad, trTria)) + return dLeakDist ; + } + // Nessuna interferenza + return 0. ; + } + // Altrimenti individuo le due possibili circonferenzecon cui può avvenire il contatto + Vector3d vtRadial = trTria.GetN() - trTria.GetN() * vtMove * vtMove ; + vtRadial.Normalize() ; + Point3d ptDiskPlus = ptTorusCen + dMaxRad * vtRadial ; + Point3d ptDiskMinus = ptTorusCen - dMaxRad * vtRadial ; + Vector3d vtDiskAx = vtRadial ^ vtMove; + vtDiskAx.Normalize() ; + Point3d ptTouchPlus, ptTouchMinus ; + double dLeakDist = 0. ; + // Distanza di fuga dalla prima circonferenza + double dCurDist = DiskPlaneLeakDistOrtMot( ptDiskPlus, vtDiskAx, dMinRad, trTria.GetP( 0), trTria.GetN(), + vtMove, ptTouchPlus) ; + // Se distanza distanza di fuga aumento la aggiorno + if ( IsPointInsideTriangle( ptTouchPlus, trTria) && dCurDist > dLeakDist) + dLeakDist = max( dLeakDist, dCurDist) ; + // Distanza di fuga dalla seconda circonferenza + dCurDist = DiskPlaneLeakDistOrtMot( ptDiskMinus, vtDiskAx, dMinRad, trTria.GetP( 0), trTria.GetN(), + vtMove, ptTouchMinus) ; + // Se distanza distanza di fuga aumento la aggiorno + if ( IsPointInsideTriangle( ptTouchMinus, trTria) && dCurDist > dLeakDist) + dLeakDist = max( dLeakDist, dCurDist) ; + return dLeakDist ; +} + +//---------------------------------------------------------------------------- +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 dTopTol = bTop ? EPS_SMALL : - EPS_SMALL ; + double dBotTol = bBot ? - EPS_SMALL : EPS_SMALL ; + + Vector3d vtR0 = ptSeg - ptTorusCen ; + + if ( LineSegmentSqDist( ptTorusCen, vtMove, ptSeg, vtSeg, dSegLen) > ( dMaxRad + dMinRad) * ( dMaxRad + dMinRad)) + return 0. ; + if ( vtR0 * vtTorusAx < - dMinRad && ( vtR0 + dSegLen * vtSeg) * vtTorusAx < - dMinRad) + return 0. ; + + double dSqR0 = vtR0 * vtR0 ; + double dH0 = vtR0 * vtTorusAx ; + Vector3d vtR0Ax = vtR0 - dH0 * vtTorusAx ; + double dL0 = vtR0 * vtMove ; + Vector3d vtR0Mot = vtR0 - dL0 * vtMove ; + double dSegDotAx = vtSeg * vtTorusAx ; + double dSegDotMove = vtSeg * vtMove ; + double dSegDotR0 = vtSeg * vtR0 ; + + double dStep = max( 0.1 * dMinRad, EPS_SMALL) ; // !!! USARE ALGORITMO GOLDEN SECTION !!! + int nStepNum = int( dSegLen / dStep) ; + + double dMaxLeakDist = - DBL_MAX ; + for ( int n = 0 ; n <= nStepNum + 1 ; ++ n) { + double dT = ( n <= nStepNum ? n * dStep : dSegLen) ; + double dHt = dH0 + dSegDotAx * dT ; + // Se non vi è interferenza fra punto e toro salto iterazione + if ( dHt > dTopTol || dHt < - dMinRad + dBotTol) + continue ; + double dSqR0LatT = dSqR0 - dH0 * dH0 - dL0 * dL0 + 2 * ( dSegDotR0 - dH0 * dSegDotAx - dL0 * dSegDotMove) * dT + + ( 1 - dSegDotMove * dSegDotMove - dSegDotAx * dSegDotAx) * dT * dT ; + // Raggio della circonferenza di quota Ht + double dHtCircRad = dMaxRad + sqrt( dMinRad * dMinRad - dHt * dHt) ; + // Se non vi è interferenza fra punto e toro salto iterazione + if ( dSqR0LatT > dHtCircRad * dHtCircRad) + continue ; + double dCurDist = dL0 + dSegDotMove * dT + sqrt( dHtCircRad * dHtCircRad - dSqR0LatT) ; + if ( dCurDist > dMaxLeakDist) + dMaxLeakDist = dCurDist ; + } + return dMaxLeakDist ; +} + +//---------------------------------------------------------------------------- +double +TorusTriangleInteriorLeakDistOrtMot( 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. ; + // 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 dLeakDist = max( ( ( trTria.GetP( 0) - ptTouch) * trTria.GetN()) / ( vtMove * trTria.GetN()), 0.) ; + // Se il punto di contatto è interno al triangolo restituisco distanza di fuga non negativa + if ( IsPointInsideTriangle( ptTouch + dLeakDist * vtMove, trTria)) + return dLeakDist ; + return 0. ; +} + +// DISTANZA DI ALLONTANAMENTO PER DISCHI + +//---------------------------------------------------------------------------- +double +DiskSegmentLeakDistLongMot( 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) > dDiscRad * dDiscRad) + if ( LineSegmentSqDist( ptDiskCen, vtMove, ptSeg, vtSeg, dSegLen) > dDiskRad * dDiskRad) return 0. ; - // Imposto l'equazione: la distanza quadrata del generico punto della retta associata al + // 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() - dDiscRad * dDiscRad ; + 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) { - if ( abs( vdCoef[0]) < 2 * dDiscRad * EPS_SMALL) { + if ( abs( vdCoef[0]) < 2 * dDiskRad * EPS_SMALL) { double dLenSt = PointPlaneSignedDist( ptSeg, ptDiskCen, vtMove) ; double dLenEn = PointPlaneSignedDist( ptSeg + dSegLen * vtSeg, ptDiskCen, vtMove) ; return max( max( dLenSt, dLenEn), 0.) ; @@ -439,7 +1091,7 @@ DiskPlaneLastContactLongMot( const Point3d& ptDiskCen, double dDiskRad, double dDist = PointPlaneSignedDist( ptPlane, ptDiskCen, vtMove) ; if ( dDist > - EPS_SMALL) { ptTouch = ptDiskCen + dDist * vtMove ; - return 3 ; + return 4 ; } return 0 ; } @@ -447,8 +1099,11 @@ DiskPlaneLastContactLongMot( const Point3d& ptDiskCen, double dDiskRad, Vector3d vtRadLine = vtMove * ( vtPlane * vtMove) - vtPlane ; // Disco e piano ortogonali if ( vtRadLine.IsNormalized()) { - if ( abs( PointPlaneSignedDist( ptPlane, ptDiskCen, vtMove) - dDiskRad) < EPS_SMALL) + double dDist = abs( PointPlaneSignedDist(ptPlane, ptDiskCen, vtMove)) ; + if ( abs( dDist - dDiskRad) < EPS_SMALL) return 2 ; + else if ( dDist < dDiskRad) + return 3 ; return 0 ; } // Cerco un punto di contatto nell'interno del triangolo. Se tale punto @@ -476,32 +1131,24 @@ DiskPlaneLastContactLongMot( const Point3d& ptDiskCen, double dDiskRad, } //---------------------------------------------------------------------------- -// 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 -CylPointLeakDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dCylHei, double dCylRad, - const Point3d& ptP, const Vector3d& vtMove) +DiskPointLeakDistOrtMot( const Point3d& ptDisc, const Vector3d& vtDiskAx, double dDiskRad, + const Point3d& ptP, const Vector3d& vtMove) { - 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 > EPS_SMALL || dDotRPCylAx < - dCylHei - EPS_SMALL) + // 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. ; - Vector3d vtPlaneRP = vtRP - dDotRPCylAx * vtCylAx ; - // Se il punto sta dietro al cilindro abbiamo finito - double dDotPlaneRPRemDir = vtPlaneRP * vtMove ; - if ( dDotPlaneRPRemDir < 0. && vtPlaneRP.SqLen() > dSqRad) + // 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. ; - 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.)) ; + return max( dLenLong + sqrt( max( dDiskRad * dDiskRad - dSqLenOrt, 0.)), 0.) ; } //---------------------------------------------------------------------------- @@ -512,28 +1159,28 @@ CylPointLeakDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, do // versore della direzione del moto. Il segmento è descritto da punto iniziale, // versore direzione e lunghezza. double -DiskSegmentLeakDistOrtMot( const Point3d& ptDiscCen, const Vector3d& vtDiscAx, double dDiscRad, +DiskSegmentLeakDistOrtMot( 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 - if ( ! ( abs( vtSeg * vtDiscAx) < EPS_ZERO)) { + if ( ! ( abs( vtSeg * vtDiskAx) < EPS_ZERO)) { Point3d ptSegEnd = ptSeg + dSegLen * vtSeg ; - double dDotStart = ( ptSeg - ptDiscCen) * vtDiscAx ; - double dDotEnd = ( ptSegEnd - ptDiscCen) * vtDiscAx ; + double dDotStart = ( ptSeg - ptDiskCen) * vtDiskAx ; + double dDotEnd = ( ptSegEnd - ptDiskCen) * vtDiskAx ; if ( dDotStart * dDotEnd < 0.) { - double dS = ( ptDiscCen - ptSeg) * vtDiscAx / ( vtSeg * vtDiscAx) ; + double dS = ( ptDiskCen - ptSeg) * vtDiskAx / ( vtSeg * vtDiskAx) ; Point3d ptInt = ptSeg + dS * vtSeg ; - Vector3d vtOI = ( ptInt - ptDiscCen) ; + Vector3d vtOI = ( ptInt - ptDiskCen) ; double dLongCord = vtOI * vtMove ; Vector3d vtLong = dLongCord * vtMove ; Vector3d vtOrt = vtOI - vtLong ; double dSqOrtLen = vtOrt.SqLen() ; - if ( dSqOrtLen > dDiscRad * dDiscRad + 2 * dDiscRad * EPS_SMALL) + if ( dSqOrtLen > dDiskRad * dDiskRad + 2 * dDiskRad * EPS_SMALL) return 0. ; else - return max( dLongCord + sqrt( max( dDiscRad * dDiscRad - dSqOrtLen, 0.)), 0.) ; + return max( dLongCord + sqrt( max( dDiskRad * dDiskRad - dSqOrtLen, 0.)), 0.) ; } else return 0. ; @@ -541,88 +1188,6 @@ DiskSegmentLeakDistOrtMot( const Point3d& ptDiscCen, const Vector3d& vtDiscAx, d return 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 -CylSegmentLeakDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dCylHei, double dCylRad, - const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) -{ - // Le variabili fanno rifermiento a un sistema di 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 ; - Vector3d vtSegStart1 = dCordStart1 * vtCylAx ; - Vector3d vtSegEnd1 = dCordEnd1 * vtCylAx ; - Vector3d vtSegStart23 = vtSegStart - vtSegStart1 ; - Vector3d vtSegEnd23 = vtSegEnd - vtSegEnd1 ; - // Se entrambi gli estremi del segmento sono a un lato - // del cilindro non vi può essere interferenza. - double dCordStart2 = vtSegStart23 * vtPlane ; - double dCordEnd2 = vtSegEnd23 * vtPlane ; - if ( ( dCordStart2 > dCylRad && dCordEnd2 > dCylRad) || - ( dCordStart2 < - dCylRad && dCordEnd2 < - dCylRad)) - return 0. ; - // Verifico la distanza di fuga di superficie laterale e dischi di base e fondo. - double dBaseLeakDist = DiskSegmentLeakDistOrtMot( ptCylOrig, vtCylAx, dCylRad, ptSeg, vtSeg, dSegLen, vtMove) ; - double dBottomLeakDist = DiskSegmentLeakDistOrtMot( ptCylOrig - dCylHei * vtCylAx, vtCylAx, dCylRad, - ptSeg, vtSeg, dSegLen, vtMove) ; - - double dSurfLeakDist = 0. ; - if ( ( dCordStart1 < 0. && dCordStart1 > - dCylHei) || - ( dCordEnd1 < 0. && dCordEnd1 > - dCylHei)) { - // 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()) { - dSurfLeakDist = max( ( ptSeg - ptCylOrig) * vtMove, 0.) ; - } - // 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 - // abbia distanza dal piano +/- dOrtLen. - Vector3d vtPlane = vtMove ^ vtCylAx ; - vtPlane.Normalize() ; - 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 < 0 && dTanCordPlus1 > - dCylHei)) { - dSurfLeakDist = max( ( ( ptSeg - ptCylOrig) + dPlusU * vtSeg) * vtMove + dDotRemRad, 0.) ; - } - if ( dMinusU > - EPS_SMALL && dMinusU < dSegLen + EPS_SMALL && - ( dTanCordMinus1 < 0 && dTanCordMinus1 > - dCylHei)) { - double dNewDist = max( ( ( ptSeg - ptCylOrig) + dMinusU * vtSeg) * vtMove + dDotRemRad, 0.) ; - dSurfLeakDist = max( dNewDist, dSurfLeakDist) ; - } - } - } - return max( max( dBaseLeakDist, dBottomLeakDist), max( dSurfLeakDist, 0.)) ; -} - //---------------------------------------------------------------------------- // Restituisce la distanza di allontanamento di un disco, che trasla ortogonalmente al suo asse // di simmetria, da un piano. @@ -630,17 +1195,22 @@ CylSegmentLeakDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, // iniziale, dal versore (NORMA UNITARIA) dell'asse di simmetria del disco e dal versore // della direzione del moto. double -DiskPlaneLeakDistOrtMot( const Point3d& ptDiscCen, const Vector3d& vtDiscAx, double dDiscRad, +DiskPlaneLeakDistOrtMot( const Point3d& ptDiscCen, const Vector3d& vtDiskAx, double dDiskRad, const Point3d& ptPlane, const Vector3d& vtPlane, const Vector3d& vtMove, Point3d& ptContact) { - Vector3d vtLine = vtPlane ^ vtDiscAx ; - if ( vtLine.Normalize() && ! AreSameOrOppositeVectorApprox( vtMove, vtLine)) { - Vector3d vtRad = dDiscRad * vtLine ^ vtDiscAx ; + Vector3d vtLine = vtPlane ^ vtDiskAx ; + if ( vtLine.Normalize()) { + Vector3d vtRad = dDiskRad * vtLine ^ vtDiskAx ; if ( vtRad * vtMove > 0) vtRad *= - 1 ; double dPar = ( ptPlane - ptDiscCen) * vtPlane / ( vtMove * vtPlane) ; - double dDeltaPar = dDiscRad * dDiscRad / abs( vtRad * vtMove) ; + double dRadCos = abs( vtRad * vtMove) ; + // Se la retta intersezione fra i piani è parallela alla direzione di + // allontanamento la distanza di fuga è determinata da punti e segmenti. + if ( dRadCos < EPS_SMALL) + return 0. ; + double dDeltaPar = dDiskRad * dDiskRad / dRadCos ; double dDesplacement = max( dPar + dDeltaPar, 0.) ; ptContact = ptDiscCen + dDesplacement * vtMove + vtRad ; return dDesplacement ; @@ -648,160 +1218,6 @@ DiskPlaneLeakDistOrtMot( const Point3d& ptDiscCen, const Vector3d& vtDiscAx, dou return 0. ; } -//---------------------------------------------------------------------------- -// **** CONO **** -//---------------------------------------------------------------------------- -double -CAvTrConeTriangle( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, - double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) -{ - // Allontanamento con direzione coincidente con l'asse del cono - if ( AreSameOrOppositeVectorApprox( vtTrConeAx, vtMove)) { - // Distanza di allontanamento dai vertici e lati - double dOutLeakDist = 0 ; - for ( int nV = 0 ; nV < 3 ; ++ nV) { - // dal vertice - double dCurVertDist = TrConePointLeakDistLongMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH, - trTria.GetP( nV), vtMove) ; - dOutLeakDist = max( dCurVertDist, dOutLeakDist) ; - // dal lato - Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ; - double dSegLen = vtSeg.Len() ; - vtSeg /= dSegLen ; - double dCurSegDist = TrConeSegmentLeakDistLongMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, - dTrConeH, trTria.GetP( nV), vtSeg, dSegLen, vtMove) ; - dOutLeakDist = max( dCurSegDist, dOutLeakDist) ; - } - double dInnLeakDist = TrConeTriangleInteriorLeakDistLongMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH, - trTria, vtMove) ; - return max( dOutLeakDist, dInnLeakDist) ; - } - // altri casi - else - return 0. ; -} - -//---------------------------------------------------------------------------- -double -TrConePointLeakDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, - double dTrConeH, const Point3d& ptP, const Vector3d& vtMove) -{ - double dPointAxisSqDist = GetPointLineSqDist( ptP, ptMinBase, vtTrConeAx) ; - if ( dPointAxisSqDist > dMaxBaseR * dMaxBaseR + 2 * dMaxBaseR * EPS_SMALL) - return 0. ; - else if ( dPointAxisSqDist > dMinBaseR * dMinBaseR + 2 * dMinBaseR * EPS_SMALL) { - double dMinBaseLeakDist = PointPlaneSignedDist( ptP, ptMinBase, vtMove) ; - double dMaxBaseLeakDist = PointPlaneSignedDist( ptP, ptMinBase + dTrConeH * vtTrConeAx, vtMove) ; - if ( dMinBaseLeakDist < - EPS_SMALL && dMaxBaseLeakDist < - EPS_SMALL) - return 0. ; - double dDistMinBaseV = dTrConeH * dMinBaseR / ( dMaxBaseR - dMinBaseR) ; - Point3d ptConeV = ptMinBase - dDistMinBaseV * vtTrConeAx ; - Vector3d vtVertP = ptP - ptConeV ; - double dVertPLongCord = vtMove * vtVertP ; - if ( vtMove * vtTrConeAx > 0.) { - double dPointAxisDist = sqrt( GetPointLineSqDist( ptP, ptMinBase, vtMove)) ; - double dLateralSurfLeakDist = dVertPLongCord - dPointAxisDist * dTrConeH / ( dMaxBaseR - dMinBaseR) ; - return max( dLateralSurfLeakDist, 0.) ; - } - else - return max( PointPlaneSignedDist( ptP, ptMinBase + dTrConeH * vtTrConeAx, vtMove), 0.) ; - } - else { - double dMinBaseLeakDist = PointPlaneSignedDist( ptP, ptMinBase, vtMove) ; - double dMaxBaseLeakDist = PointPlaneSignedDist( ptP, ptMinBase + dTrConeH * vtTrConeAx, vtMove) ; - return max( max( dMinBaseLeakDist, dMaxBaseLeakDist), 0.) ; - } -} - -//---------------------------------------------------------------------------- -double -TrConeSegmentLeakDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, - double dTrConeH, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, - const Vector3d& vtMove) -{ - // Distanza di allontanamento delle basi - Point3d ptMaxBase = ptMinBase + dTrConeH * vtTrConeAx ; - double dMinBaseLeakDist = DiskSegmentLeakDistLongMotion( ptMinBase, dMinBaseR, ptSeg, vtSeg, dSegLen, vtMove) ; - double dMaxBaseLeakDist = DiskSegmentLeakDistLongMotion( ptMaxBase, dMaxBaseR, ptSeg, vtSeg, dSegLen, vtMove) ; - double dBasesLeakDist = max( max( dMinBaseLeakDist, dMaxBaseLeakDist), 0.) ; - // Distanza di allontanamento della superficie laterale - double dDistMinBaseV = dTrConeH * dMinBaseR / ( dMaxBaseR - dMinBaseR) ; - Point3d ptConeV = ptMinBase - dDistMinBaseV * vtTrConeAx ; - Frame3d frConusFrame ; - frConusFrame.Set( ptConeV, vtTrConeAx) ; - Point3d ptSegNew = ptSeg ; - Vector3d vtSegNew = vtSeg ; - ptSegNew.ToLoc( frConusFrame) ; - vtSegNew.ToLoc( frConusFrame) ; - double dAngCoef = ( dDistMinBaseV + dTrConeH) / dMaxBaseR ; - double dAlpha = vtSegNew.z * vtSegNew.z - dAngCoef * dAngCoef * ( vtSegNew.x * vtSegNew.x + vtSegNew.y * vtSegNew.y) ; - double dBeta = ptSegNew.z * vtSegNew.z - dAngCoef * dAngCoef * ( ptSegNew.x * vtSegNew.x + ptSegNew.y * vtSegNew.y) ; - double dGamma = ptSegNew.z * ptSegNew.z - dAngCoef * dAngCoef * ( ptSegNew.x * ptSegNew.x + ptSegNew.y * ptSegNew.y) ; - // Se l'equazione è di primo grado, non serve risolvere (si trova altrimenti con i punti estremi del segmento) - if ( abs( dAlpha) < EPS_ZERO) - return 0. ; - // Impongo il determinante nullo, ne deriva una nuova equazione da risolvere - DBLVECTOR vdDeltaCoef( 3) ; - DBLVECTOR vdDeltaRoots ; - vdDeltaCoef[0] = dBeta * dBeta - dAlpha * dGamma ; - vdDeltaCoef[1] = 2 * ( dBeta * vtSegNew.z - dAlpha * ptSegNew.z) ; - vdDeltaCoef[2] = ( vtSegNew.z * vtSegNew.z - dAlpha) ; - int nRoot = PolynomialRoots( 2, vdDeltaCoef, vdDeltaRoots) ; - // Studio le soluzioni - double dLateralSurfLeakDist = 0. ; - for ( int nS = 0 ; nS < nRoot ; ++ nS) { - // Queste soluzioni sono gli spostamenti lungo z 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 = - ( dBeta + vtSegNew.z * vdDeltaRoots[nS]) / dAlpha ; - Point3d ptTan( ptSegNew.x, ptSegNew.y, ptSegNew.z + vdDeltaRoots[nS]) ; - ptTan += dT * vtSegNew ; - if ( ptTan.z > dDistMinBaseV - EPS_SMALL && - ptTan.z < dDistMinBaseV + dTrConeH + EPS_SMALL && - dT > - EPS_SMALL && dT < dSegLen + EPS_SMALL) { - if ( ( vtTrConeAx * vtMove > 0 && vdDeltaRoots[nS] < 0.) || - ( vtTrConeAx * vtMove < 0 && vdDeltaRoots[nS] > 0.)) - dLateralSurfLeakDist = abs( vdDeltaRoots[nS]) ; - } - } - return max( dBasesLeakDist, dLateralSurfLeakDist) ; -} - -//---------------------------------------------------------------------------- -double -TrConeTriangleInteriorLeakDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, - double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) -{ - // Distanza di allontanamento del disco minore - Point3d ptMinTouch ; - int nMinLeakType = DiskPlaneLastContactLongMot( ptMinBase, dMinBaseR, trTria.GetP( 0), trTria.GetN(), - vtMove, ptMinTouch) ; - double dMinLeakDist = ( ptMinTouch - ptMinBase) * vtMove ; - // Distanza di allontanamento del disco maggiore - Point3d ptMaxTouch ; - Point3d ptMaxBase = ptMinBase + dTrConeH * vtTrConeAx ; - int nMaxLeakType = DiskPlaneLastContactLongMot( ptMaxBase, dMaxBaseR, trTria.GetP( 0), trTria.GetN(), - vtMove, ptMaxTouch) ; - double dMaxLeakDist = ( ptMaxTouch - ptMaxBase) * vtMove ; - // se le due distanze sono uguali e tipo di contatto 1, allora superficie laterale cono tangente a piano - if ( abs( dMaxLeakDist - dMinLeakDist) < EPS_SMALL) { - Point3d ptInt1, ptInt2 ; - int nRes = IntersLineTria( ptMinTouch, ptMaxTouch, trTria, ptInt1, ptInt2) ; - if ( nRes != ILTT_NO) - return max( ( dMaxLeakDist + dMinLeakDist) / 2, 0.) ; - } - // altrimenti sono i dischi a determinare l'allontanamento - double dMove = 0 ; - if ( ( nMinLeakType == 1 && IsPointInsideTriangle( ptMinTouch, trTria)) || - ( nMinLeakType == 3 && CoplanarDiscTriangleInterferance( ptMinTouch, dMinBaseR, trTria))) - dMove = max( dMinLeakDist, 0.) ; - if ( ( nMaxLeakType == 1 && IsPointInsideTriangle( ptMaxTouch, trTria)) || - ( nMaxLeakType == 3 && CoplanarDiscTriangleInterferance( ptMaxTouch, dMaxBaseR, trTria))) - dMove = max( dMove, dMaxLeakDist) ; - return dMove ; -} - // FUNZIONI GEOMETRICHE DI BASE PER IL CALCOLO DELLA DISTANZA DI ALLONTANAMENTO diff --git a/CAvToolTriangle.h b/CAvToolTriangle.h index 9657888..db31696 100644 --- a/CAvToolTriangle.h +++ b/CAvToolTriangle.h @@ -20,6 +20,7 @@ double CAvToolTriangle( const Tool& tlTool, const Point3d& ptToolOrig, const Vec const Triangle3d& trTria, const Vector3d& vtMove) ; // Componenti: + // Sfera double CAvSphereTriangle( const Point3d& ptSpheCen, double dSpheRad, const Triangle3d& trTria, const Vector3d& vtMove) ; double SpherePlaneLeakDist( const Point3d& ptSpheCen, double dSpheRad, @@ -28,25 +29,19 @@ double SphereSegmentLeakDist( const Point3d& ptSpheCen, double dSpheRad, const Point3d& ptSeg, const Vector3d& vtSegDir, double dSegLen, const Vector3d& vtMove) ; double SpherePointLeakDist( const Point3d& ptSpheCen, double dSpheRad, const Point3d ptP, const Vector3d& vtMove) ; + // Cilindro double CAvCylinderTriangle( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad, - const Triangle3d& trTria, const Vector3d& vtMove) ; -double DiskSegmentLeakDistLongMotion( const Point3d& ptDiskCen, double dDiscRad, - const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ; -double DiskTriaInteriorLeakDistLongMot( const Point3d& ptDiskCen, double dDiskRad, - const Triangle3d& trTria, const Vector3d& vtMove) ; + const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot) ; double CylPointLeakDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dCylHei, double dCylRad, - const Point3d& ptP, const Vector3d& vtMove) ; -double DiskSegmentLeakDistOrtMot( const Point3d& ptDiscCen, const Vector3d& vtDiscAx, double dDiscRad, - const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ; + const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot) ; double CylSegmentLeakDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dCylHei, double dCylRad, - const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ; -double DiskPlaneLeakDistOrtMot( const Point3d& ptDiscCen, const Vector3d& vtDiscAx, double dDiscRad, - const Point3d& ptPlane, const Vector3d& vtPlane, - const Vector3d& vtMotion, Point3d& ptContact) ; + const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove, + bool bTop, bool bBot) ; + // Cono double CAvTrConeTriangle( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, - double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) ; + double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot) ; double TrConePointLeakDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Point3d& ptP, const Vector3d& vtMove) ; double TrConeSegmentLeakDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, @@ -54,6 +49,43 @@ double TrConeSegmentLeakDistLongMot( const Point3d& ptMinBase, const Vector3d& v const Vector3d& vtMove) ; double TrConeTriangleInteriorLeakDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) ; +double TrConePointLeakDistOrtMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, + double dTrConeH, const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot) ; +double TrConeSegmentLeakDistOrtMot( 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 TrConeTriangleInteriorLeakDistOrtMot( 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, + const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot) ; +double TorusSegmentLeakDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad, + const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ; +double TorusTriangleInteriorLeakDistLongMot( const Point3d& ptTorusCen, 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 TorusTriangleInteriorLeakDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, + const Triangle3d& trTria, const Vector3d& vtMove) ; + +// Dischi +double DiskSegmentLeakDistLongMot( const Point3d& ptDiskCen, double dDiskRad, + const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ; +int DiskPlaneLastContactLongMot( const Point3d& ptDiskCen, double dDiskRad, + const Point3d& ptPlane, const Vector3d& vtPlane, + const Vector3d& vtMove, Point3d& ptTouch) ; +double DiskTriaInteriorLeakDistLongMot( const Point3d& ptDiskCen, double dDiskRad, + const Triangle3d& trTria, const Vector3d& vtMove) ; +double DiskSegmentLeakDistOrtMot( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad, + const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ; +double DiskPlaneLeakDistOrtMot( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad, + const Point3d& ptPlane, const Vector3d& vtPlane, + const Vector3d& vtMove, Point3d& ptContact) ; +double DiskPointLeakDistOrtMot( const Point3d& ptDisc, const Vector3d& vtDiskAx, double dDiscRad, + const Point3d& ptP, const Vector3d& vtMove) ; + // Funzioni geometriche di base double GetPointLineSqDist( const Point3d& ptP, const Point3d& ptLine, const Vector3d& vtLine) ; double PointPlaneSignedDist( const Point3d& ptP, const Point3d& ptPlane, const Vector3d& vtNorm) ; @@ -71,3 +103,7 @@ bool FindMinDistPar( const Point3d& ptL1, const Point3d& ptL2, int SphereLineTangentPoints( const Point3d& ptSpheCen, double dSpheRad, const Point3d& ptSeg, const Vector3d& vtSegDir, double dSegLen, const Vector3d& vtMove, double& dU1, double& dU2) ; + +// Altre funzioni +inline double EvalSecondDegreePolynomial( double dCoeff0, double dCoeff1, double dCoeff2, double dVariable) + { return (( dCoeff2 * dVariable + dCoeff1) * dVariable + dCoeff0) ; } diff --git a/EgtGeomKernel.rc b/EgtGeomKernel.rc index f73ed6d078f67cd2360406d72d4db23d96248180..9c116631c3c8dab36f603f6cd74848423325d9ce 100644 GIT binary patch delta 118 zcmdlNy)SyhH#Sx?1|0^&&A-{?nVHiV3@0mc>u&bq>S2M3-{!MM7I$OZ+#~G9j8n2h Q8ZL=uq6g#VEaea`09rgA5&!@I delta 118 zcmdlNy)SyhH#SyN1|0^o&A-{?nVC}=%qA;x>u&bq>S2M3-{!MM7I$OZ+#~G9j8n2h Q8ZL=uq6g#VEaea`0Abu7G5`Po diff --git a/GdbExecutor.cpp b/GdbExecutor.cpp index 242b298..a9c914e 100644 --- a/GdbExecutor.cpp +++ b/GdbExecutor.cpp @@ -3664,7 +3664,7 @@ GdbExecutor::LineInfiniteCylinderInters( const STRVECTOR& vsParams) return false ; double dU1, dU2 ; - int nTypeInt = LineInfiniteCylinder( ptCyl, ptLine, vtCyl, vtLine, dRadius, dU1, dU2) ; + int nTypeInt = IntersLineInfiniteCylinder( ptLine, vtLine, ptCyl, vtCyl, dRadius, dU1, dU2) ; Point3d ptPS = ptLine + dU1 * vtLine ; Point3d ptPE = ptLine + dU2 * vtLine ; @@ -3735,7 +3735,7 @@ GdbExecutor::RayInfiniteCylinderInters( const STRVECTOR& vsParams) return false ; double dU1, dU2 ; - int nTypeInt = LineInfiniteCylinder( ptCyl, ptLine, vtCyl, vtLine, dRadius, dU1, dU2) ; + int nTypeInt = IntersLineInfiniteCylinder( ptLine, vtLine, ptCyl, vtCyl, dRadius, dU1, dU2) ; Point3d ptPS = ptLine + dU1 * vtLine ; Point3d ptPE = ptLine + dU2 * vtLine ; @@ -3810,7 +3810,7 @@ GdbExecutor::SegmentInfiniteCylinderInters( const STRVECTOR& vsParams) return false ; double dU1, dU2 ; - int nTypeInt = SegmentInfiniteCylinder( ptCyl, ptLine, vtCyl, vtLine, dRadius, dLen, dU1, dU2) ; + int nTypeInt = IntersSegmentInfiniteCylinder( ptLine, vtLine, dLen, ptCyl, vtCyl, dRadius, dU1, dU2) ; Point3d ptPS = ptLine + dU1 * vtLine ; Point3d ptPE = ptLine + dU2 * vtLine ; @@ -3888,7 +3888,7 @@ GdbExecutor::SegmentCylinderInters( const STRVECTOR& vsParams) return false ; double dU1, dU2 ; - int nTypeInt = SegmentCylinder( ptCyl, ptLine, vtCyl, vtLine, dRadius, dHeigth, dLen, dU1, dU2) ; + int nTypeInt = IntersSegmentCylinder( ptLine, vtLine, dLen, ptCyl, vtCyl, dRadius, dHeigth, dU1, dU2) ; Point3d ptPS = ptLine + dU1 * vtLine ; Point3d ptPE = ptLine + dU2 * vtLine ; diff --git a/IntersLineSurfStd.cpp b/IntersLineSurfStd.cpp index 4a8674a..40d6c51 100644 --- a/IntersLineSurfStd.cpp +++ b/IntersLineSurfStd.cpp @@ -15,6 +15,7 @@ #include "/EgtDev/Include/EgtNumUtils.h" #include "IntersLineSurfStd.h" +using namespace std ; //---------------------------------------------------------------------------- int LineDisc( const Point3d& ptPDisc, const Point3d& ptPLine, const Vector3d& vtVDisc, const Vector3d& vtVLine, @@ -33,8 +34,8 @@ int LineDisc( const Point3d& ptPDisc, const Point3d& ptPLine, const Vector3d& vt return nIntType ; } - std::vector vdCoef(3) ; - std::vector vdRoots ; + vector vdCoef(3) ; + vector vdRoots ; // Setto i coefficienti dell'equazione di itersezione fra retta e sfera, // dallo studio di queste intersezioni e di quella col piano si del disco si deduce @@ -72,7 +73,7 @@ int LineDisc( const Point3d& ptPDisc, const Point3d& ptPLine, const Vector3d& vt dU1 = vdRoots[0] ; dU2 = vdRoots[1] ; if ( dU1 > dU2) - std::swap( dU1, dU2) ; + swap( dU1, dU2) ; // Soluzioni ben distinte if ( dU2 - dU1 > EPS_SMALL) { // Se la retta è nel piano parte di essa è nel disco @@ -484,43 +485,36 @@ int LinCompSemiSphere( const Point3d& ptSphC, const Point3d& ptPLine, } //---------------------------------------------------------------------------- -int LineInfiniteCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const Vector3d& vtVCyl, const Vector3d& vtVLine, - double dCylRad, double& dU1, double& dU2) +int +IntersLineInfiniteCylinder( const Point3d& ptPLine, const Vector3d& vtVLine, + const Point3d& ptPCyl, const Vector3d& vtVCyl, double dCylRad, + double& dU1, double& dU2) { - int nIntType ; - // Se il raggio non è significativamente - // maggiore di zero, vi è un errore. - if ( dCylRad < EPS_SMALL) { - nIntType = CC_ERROR_INT ; - return nIntType ; - } + // Se il raggio non è significativamente maggiore di zero, vi è un errore. + if ( dCylRad < EPS_SMALL) + return CC_ERROR_INT ; // Si richiede che i vettori siano normalizzati - if ( ! ( vtVCyl.IsNormalized() && vtVLine.IsNormalized())) { - nIntType = S_ERROR_INT ; - return nIntType ; - } + if ( ! vtVCyl.IsNormalized() || ! vtVLine.IsNormalized()) + return CC_ERROR_INT ; - // Caso in cui i vettori della retta e del cilindro sono paralleli - if ( ( ( vtVLine * vtVCyl) * vtVCyl).SqLen() > 1 - EPS_ZERO * EPS_ZERO) { - + // La retta e l'asse del cilindro sono paralleli + if ( AreSameOrOppositeVectorExact( vtVLine, vtVCyl)) { double dRadialDist = ( ( ptPLine - ptPCyl) - ( ( ptPLine - ptPCyl) * vtVCyl) * vtVCyl).Len() ; // La retta appartiene al cilindro if ( abs( dRadialDist - dCylRad) < EPS_SMALL) { - - nIntType = CC_INF_INT ; dU1 = - INFINITO ; dU2 = INFINITO ; - return nIntType ; + return CC_INF_INT ; } // non vi è intersezione - else { - nIntType = CC_NO_INTERS ; - return nIntType ; - } + else + return CC_NO_INTERS ; } - std::vector vdCoef(3) ; - std::vector vdRoots ; + // Caso generico + int nIntType ; + vector vdCoef(3) ; + vector vdRoots ; // Setto i coefficienti dell'equazione vdCoef[0] = ( ptPLine - ptPCyl).SqLen() - ( ( ptPLine - ptPCyl) * vtVCyl) * ( ( ptPLine - ptPCyl) * vtVCyl) - dCylRad * dCylRad ; @@ -528,7 +522,7 @@ int LineInfiniteCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const V vdCoef[2] = 1 - ( vtVCyl * vtVLine) * ( vtVCyl * vtVLine) ; // Risolvo l'equazione int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ; - // Non vi sono intersezioni + // Non ci sono intersezioni if ( nRoot == 0) nIntType = CC_NO_INTERS ; // C'è un punto di tangenza @@ -536,19 +530,16 @@ int LineInfiniteCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const V dU1 = vdRoots[0] ; nIntType = CC_ONE_INT_TAN ; } - // L'equazione ha due soluzioni, bisogna - // valutare se sono den distinte. + // L'equazione ha due soluzioni, bisogna valutare se sono den distinte. else if ( nRoot == 2) { dU1 = vdRoots[0] ; dU2 = vdRoots[1] ; if ( dU1 > dU2) - std::swap( dU1, dU2) ; - // Soluzioni sufficientemente lontane: - // ci sono due intersezioni + swap( dU1, dU2) ; + // Soluzioni sufficientemente lontane: ci sono due intersezioni if ( dU2 - dU1 > EPS_SMALL) nIntType = CC_TWO_INT ; - // Soluzioni non sufficientemente lontane: - // c'è un punto di tangenza. + // Soluzioni non sufficientemente lontane: c'è un punto di tangenza. else nIntType = CC_ONE_INT_TAN ; } @@ -556,10 +547,12 @@ int LineInfiniteCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const V } //---------------------------------------------------------------------------- -int RayInfiniteCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const Vector3d& vtVCyl, const Vector3d& vtVLine, - double dCylRad, double& dU1, double& dU2) +int +IntersRayInfiniteCylinder( const Point3d& ptPLine, const Vector3d& vtVLine, + const Point3d& ptPCyl, const Vector3d& vtVCyl, double dCylRad, + double& dU1, double& dU2) { - int nIntType = LineInfiniteCylinder( ptPCyl, ptPLine, vtVCyl, vtVLine, dCylRad, dU1, dU2) ; + int nIntType = IntersLineInfiniteCylinder( ptPLine, vtVLine, ptPCyl, vtVCyl, dCylRad, dU1, dU2) ; if ( nIntType == CC_ERROR_INT) return nIntType ; @@ -591,10 +584,12 @@ int RayInfiniteCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const Ve } //---------------------------------------------------------------------------- -int SegmentInfiniteCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const Vector3d& vtVCyl, const Vector3d& vtVLine, - double dCylRad, double dLen, double& dU1, double& dU2) +int +IntersSegmentInfiniteCylinder( const Point3d& ptPLine, const Vector3d& vtVLine, double dLen, + const Point3d& ptPCyl, const Vector3d& vtVCyl, double dCylRad, + double& dU1, double& dU2) { - int nIntType = LineInfiniteCylinder( ptPCyl, ptPLine, vtVCyl, vtVLine, dCylRad, dU1, dU2) ; + int nIntType = IntersLineInfiniteCylinder( ptPLine, vtVLine, ptPCyl, vtVCyl, dCylRad, dU1, dU2) ; if ( nIntType == CC_ERROR_INT) return nIntType ; @@ -642,10 +637,12 @@ int SegmentInfiniteCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, cons } //---------------------------------------------------------------------------- -int LineSemiFiniteCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const Vector3d& vtVCyl, const Vector3d& vtVLine, - double dCylRad, double& dU1, double& dU2) +int +IntersLineSemiFiniteCylinder( const Point3d& ptPLine, const Vector3d& vtVLine, + const Point3d& ptPCyl, const Vector3d& vtVCyl, double dCylRad, + double& dU1, double& dU2) { - int nIntType = LineInfiniteCylinder( ptPCyl, ptPLine, vtVCyl, vtVLine, dCylRad, dU1, dU2) ; + int nIntType = IntersLineInfiniteCylinder( ptPLine, vtVLine, ptPCyl, vtVCyl, dCylRad, dU1, dU2) ; if ( nIntType == CC_ERROR_INT) return nIntType ; @@ -692,10 +689,11 @@ int LineSemiFiniteCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const } //---------------------------------------------------------------------------- -int RaySemiFiniteCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const Vector3d& vtVCyl, const Vector3d& vtVLine, - double dCylRad, double& dU1, double& dU2) +int +RaySemiFiniteCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const Vector3d& vtVCyl, const Vector3d& vtVLine, + double dCylRad, double& dU1, double& dU2) { - int nIntType = LineSemiFiniteCylinder( ptPCyl, ptPLine, vtVCyl, vtVLine, dCylRad, dU1, dU2) ; + int nIntType = IntersLineSemiFiniteCylinder( ptPLine, vtVLine, ptPCyl, vtVCyl, dCylRad, dU1, dU2) ; if ( nIntType == CC_ERROR_INT) return nIntType ; @@ -737,10 +735,11 @@ int RaySemiFiniteCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const } //---------------------------------------------------------------------------- -int SegmentSemiFiniteCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const Vector3d& vtVCyl, const Vector3d& vtVLine, - double dCylRad, double dLen, double& dU1, double& dU2) +int +SegmentSemiFiniteCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const Vector3d& vtVCyl, const Vector3d& vtVLine, + double dCylRad, double dLen, double& dU1, double& dU2) { - int nIntType = LineSemiFiniteCylinder( ptPCyl, ptPLine, vtVCyl, vtVLine, dCylRad, dU1, dU2) ; + int nIntType = IntersLineSemiFiniteCylinder( ptPLine, vtVLine, ptPCyl, vtVCyl, dCylRad, dU1, dU2) ; if ( nIntType == CC_ERROR_INT) return nIntType ; @@ -807,10 +806,12 @@ int SegmentSemiFiniteCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, co } //---------------------------------------------------------------------------- -int LineCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const Vector3d& vtVCyl, const Vector3d& vtVLine, - double dCylRad, double dCylHeigth, double& dU1, double& dU2) +int +IntersLineCylinder( const Point3d& ptPLine, const Vector3d& vtVLine, + const Point3d& ptPCyl, const Vector3d& vtVCyl, double dCylRad, double dCylHeigth, + double& dU1, double& dU2) { - int nIntType = LineInfiniteCylinder( ptPCyl, ptPLine, vtVCyl, vtVLine, dCylRad, dU1, dU2) ; + int nIntType = IntersLineInfiniteCylinder( ptPLine, vtVLine, ptPCyl, vtVCyl, dCylRad, dU1, dU2) ; if ( nIntType == CC_ERROR_INT) return nIntType ; @@ -859,10 +860,11 @@ int LineCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const Vector3d& } //---------------------------------------------------------------------------- -int RayCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const Vector3d& vtVCyl, const Vector3d& vtVLine, - double dCylRad, double dCylHeigth, double& dU1, double& dU2) +int +RayCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const Vector3d& vtVCyl, const Vector3d& vtVLine, + double dCylRad, double dCylHeigth, double& dU1, double& dU2) { - int nIntType = LineCylinder( ptPCyl, ptPLine, vtVCyl, vtVLine, dCylRad, dCylHeigth, dU1, dU2) ; + int nIntType = IntersLineCylinder( ptPLine, vtVLine, ptPCyl, vtVCyl, dCylRad, dCylHeigth, dU1, dU2) ; if ( nIntType == CC_ERROR_INT) return nIntType ; @@ -901,10 +903,12 @@ int RayCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const Vector3d& } //---------------------------------------------------------------------------- -int SegmentCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const Vector3d& vtVCyl, const Vector3d& vtVLine, - double dCylRad, double dCylHeigth, double dLen, double& dU1, double& dU2) +int +IntersSegmentCylinder( const Point3d& ptPLine, const Vector3d& vtVLine, double dLen, + const Point3d& ptPCyl, const Vector3d& vtVCyl, double dCylRad, double dCylHeigth, + double& dU1, double& dU2) { - int nIntType = LineCylinder( ptPCyl, ptPLine, vtVCyl, vtVLine, dCylRad, dCylHeigth, dU1, dU2) ; + int nIntType = IntersLineCylinder( ptPLine, vtVLine, ptPCyl, vtVCyl, dCylRad, dCylHeigth, dU1, dU2) ; if ( nIntType == CC_ERROR_INT) return nIntType ; @@ -967,8 +971,9 @@ int SegmentCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const Vector } //---------------------------------------------------------------------------- -int LineInfiniteCone( const Point3d& ptVCone, const Point3d& ptPLine, const Vector3d& vtDCone, const Vector3d& vtDLine, - double dConeRad, double dConeHeigth, double& dU1, double& dU2) +int +LineInfiniteCone( const Point3d& ptVCone, const Point3d& ptPLine, const Vector3d& vtDCone, const Vector3d& vtDLine, + double dConeRad, double dConeHeigth, double& dU1, double& dU2) { int nIntType ; // Raggio e altezza del cono devono essere maggiori di zero @@ -1034,8 +1039,8 @@ int LineInfiniteCone( const Point3d& ptVCone, const Point3d& ptPLine, const Vect } } - std::vector vdCoef(3) ; - std::vector vdRoots ; + vector vdCoef(3) ; + vector vdRoots ; // Setto i coefficienti dell'equazione vdCoef[0] = ( ptPLine - ptVCone) * ( ptPLine - ptVCone) - ( 1 + dTanTheta * dTanTheta) * ( ( ptPLine - ptVCone) * vtDCone) * ( ( ptPLine - ptVCone) * vtDCone) ; @@ -1068,7 +1073,7 @@ int LineInfiniteCone( const Point3d& ptVCone, const Point3d& ptPLine, const Vect dU1 = vdRoots[0] ; dU2 = vdRoots[1] ; if ( dU1 > dU2) - std::swap( dU1, dU2) ; + swap( dU1, dU2) ; // Se le soluzioni non sono sufficientemente lontane, // la soluzione è di tangenza if ( dU2 - dU1 < EPS_SMALL * EPS_SMALL) { @@ -1579,7 +1584,7 @@ int SegmentTruncatedCone( const Point3d& ptMinBase, const Point3d& ptPLine, cons //---------------------------------------------------------------------------- int LineTorus( const Point3d& ptOTorus, const Point3d& ptLine, const Vector3d& vtAxTorus, const Vector3d& vtLine, - double dMinRad, double dMaxRad, std::vector& vdPar, std::vector& vbType) + double dMinRad, double dMaxRad, vector& vdPar, vector& vbType) { int nIntType = T_ERROR ; // Si richiede che i vettori siano normalizzati @@ -1598,8 +1603,8 @@ int LineTorus( const Point3d& ptOTorus, const Point3d& ptLine, const Vector3d& v ptLn.ToLoc( frTorusFrame) ; vtLn.ToLoc( frTorusFrame) ; - std::vector vdCoef(5) ; - std::vector vdRoots ; + vector vdCoef(5) ; + vector vdRoots ; // Setto i coefficienti dell'equazione double dSqDistPO = ptLn.x * ptLn.x + ptLn.y * ptLn.y + ptLn.z * ptLn.z ; double dPtVt = ptLn.x * vtLn.x + ptLn.y * vtLn.y + ptLn.z * vtLn.z ; @@ -1621,7 +1626,7 @@ int LineTorus( const Point3d& ptOTorus, const Point3d& ptLine, const Vector3d& v for ( int ni = 0 ; ni < int( vdPar.size()) - 1 ; ++ ni) { for ( int nj = ni ; nj < int( vdPar.size()) ; ++ nj) { if( vdPar[ni] > vdPar[nj]) { - std::swap( vdPar[ni], vdPar[nj]) ; + swap( vdPar[ni], vdPar[nj]) ; } } } @@ -1730,7 +1735,7 @@ int LineTorus( const Point3d& ptOTorus, const Point3d& ptLine, const Vector3d& v //---------------------------------------------------------------------------- int RayTorus( const Point3d& ptOTorus, const Point3d& ptLine, const Vector3d& vtAxTorus, const Vector3d& vtLine, - double dMinRad, double dMaxRad, std::vector& vdPar, std::vector& vbType) + double dMinRad, double dMaxRad, vector& vdPar, vector& vbType) { int nIntType = LineTorus( ptOTorus, ptLine, vtAxTorus, vtLine, dMinRad, dMaxRad, vdPar, vbType) ; if ( nIntType == T_ERROR || nIntType == T_NO_INT) @@ -1814,7 +1819,7 @@ int RayTorus( const Point3d& ptOTorus, const Point3d& ptLine, const Vector3d& vt //---------------------------------------------------------------------------- int SegmentTorus( const Point3d& ptOTorus, const Point3d& ptLine, const Vector3d& vtAxTorus, const Vector3d& vtLine, - double dMinRad, double dMaxRad, double dSgLen, std::vector& vdPar, std::vector& vbType) + double dMinRad, double dMaxRad, double dSgLen, vector& vdPar, vector& vbType) { int nIntType = LineTorus( ptOTorus, ptLine, vtAxTorus, vtLine, dMinRad, dMaxRad, vdPar, vbType) ; if ( nIntType == T_ERROR || nIntType == T_NO_INT) @@ -1902,7 +1907,7 @@ int SegmentTorus( const Point3d& ptOTorus, const Point3d& ptLine, const Vector3d int LinCompTorusExtInt( const Point3d& ptOTorus, const Point3d& ptLine, const Vector3d& vtAxTorus, const Vector3d& vtLine, double dMinRad, double dMaxRad, double dSgLen, int nLinType, - std::vector& vdPar, std::vector& vbType) + vector& vdPar, vector& vbType) { int nIntType ; // Retta @@ -2000,94 +2005,3 @@ int LinCompTorusExtInt( const Point3d& ptOTorus, const Point3d& ptLine, return nIntType ; } - -//int RayTorus( const Point3d& ptOTorus, const Point3d& ptLine, const Vector3d& vtAxTorus, const Vector3d& vtLine, -// double dMinRad, double dMaxRad, std::vector& vdPar, std::vector& vbType) -//{ -// int nIntType = LineTorus( ptOTorus, ptLine, vtAxTorus, vtLine, dMinRad, dMaxRad, vdPar, vbType) ; -// if ( nIntType == T_ERROR || nIntType == T_NO_INT) -// return nIntType ; -// -// // Retta tangente in un punto -// if ( nIntType == T_ONE_TAN) { -// // La semiretta non interagise -// if ( vdPar[0] < - EPS_SMALL) { -// vdPar.resize( 0) ; -// vbType.resize( 0) ; -// nIntType = T_NO_INT ; -// } -// } -// // Retta tangente in due punti -// else if ( nIntType == T_TWO_TAN) { -// // Semi-retta non interagisce -// if ( vdPar[1] < - EPS_SMALL) { -// vdPar.resize( 0) ; -// vbType.resize( 0) ; -// nIntType = T_NO_INT ; -// } -// // Semi-retta tangente in un punto -// else if ( vdPar[0] < - EPS_SMALL) { -// vdPar[0] = vdPar[1] ; -// vdPar.resize( 1) ; -// vbType.resize( 1) ; -// nIntType = T_ONE_TAN ; -// } -// } -// else if ( nIntType == T_TWO_SEC) { -// // Semi-retta non interagisce -// if ( vdPar[1] < - EPS_SMALL) { -// vdPar.resize( 0) ; -// vbType.resize( 0) ; -// nIntType = T_NO_INT ; -// } -// // Semi-retta secante in un punto -// else if ( vdPar[0] < - EPS_SMALL) { -// vdPar[0] = vdPar[1] ; -// vdPar.resize( 1) ; -// vbType.resize( 1) ; -// nIntType = T_ONE_SEC ; -// } -// } -// else if ( nIntType == T_THREE_ONE_TAN_TWO_SEC) { -// // Semi-retta non interagente -// if ( vdPar[2] < - EPS_SMALL) { -// vdPar.resize( 0) ; -// vbType.resize( 0) ; -// nIntType = T_NO_INT ; -// } -// // Semi-retta tangente in un punto -// else if ( vdPar[1] < - EPS_ZERO) { -// vdPar[0] = vdPar[2] ; -// vbType[0] = vbType[2] ; -// vdPar.resize( 1) ; -// vbType.resize( 1) ; -// if ( vbType[0]) -// T_ONE_SEC ; -// else -// T_ONE_TAN ; -// } -// else { -// // Semi-retta interagente in due punti -// if ( vdPar[0] < - EPS_SMALL) { -// vdPar[0] = vdPar[1] ; -// vdPar[1] = vdPar[2] ; -// vbType[0] = vbType[1] ; -// vbType[1] = vbType[2] ; -// vdPar.resize( 2) ; -// vbType.resize( 2) ; -// if ( vbType[0] || vbType[1]) { -// if ( vbType[0] == vbType[1]) -// nIntType = T_TWO_SEC ; -// else -// nIntType = T_TWO_TAN_SEC ; -// } -// } -// } -// } -// // Retta secante in quattro punti -// else if ( nIntType == T_FOUR_SEC) { -// ; -// } -// -// return nIntType ; -//} \ No newline at end of file diff --git a/IntersLineSurfStd.h b/IntersLineSurfStd.h index 431b15a..c41a494 100644 --- a/IntersLineSurfStd.h +++ b/IntersLineSurfStd.h @@ -115,29 +115,33 @@ enum LinCompCCIntersType { CC_ERROR_INT = - 1, CC_NO_INTERS = 0, CC_ONE_INT_SEC // Valuta la posizione reciproca fra un cilindro infinito e una retta. // Nel caso in cui la retta giaccia sul cilindro, nIntType vale INF_INT e dU1 e dU2 valgono // rispettivamente - INFINITO e INFINITO. -int LineInfiniteCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const Vector3d& vtVCyl, const Vector3d& vtVLine, - double dCylRad, double& dU1, double& dU2) ; +int IntersLineInfiniteCylinder( const Point3d& ptPLine, const Vector3d& vtVLine, + const Point3d& ptPCyl, const Vector3d& vtVCyl, double dCylRad, + double& dU1, double& dU2) ; //---------------------------------------------------------------------------- // Valuta la posizione reciproca fra un cilindro infinito e una semi-retta. // Nel caso in cui la semi-retta giaccia sul cilindro, nIntType vale INF_INT e dU1 e dU2 // valgono rispettivamente 0 e INFINITO. -int RayInfiniteCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const Vector3d& vtVCyl, const Vector3d& vtVLine, - double dCylRad, double& dU1, double& dU2) ; +int IntersRayInfiniteCylinder( const Point3d& ptPLine, const Vector3d& vtVLine, + const Point3d& ptPCyl, const Vector3d& vtVCyl, double dCylRad, + double& dU1, double& dU2) ; //---------------------------------------------------------------------------- // Valuta la posizione reciproca fra un cilindro infinito e un segmento. // Nel caso in cui il sgmento giaccia sul cilindro, nIntType vale INF_INT e dU1 e dU2 // valgono rispettivamente 0 e dLen (lunghezza del segmento). -int SegmentInfiniteCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const Vector3d& vtVCyl, const Vector3d& vtVLine, - double dCylRad, double dLen, double& dU1, double& dU2) ; +int IntersSegmentInfiniteCylinder( const Point3d& ptPLine, const Vector3d& vtVLine, double dLen, + const Point3d& ptPCyl, const Vector3d& vtVCyl, double dCylRad, + double& dU1, double& dU2) ; //---------------------------------------------------------------------------- // Valuta la posizione reciproca fra una retta e un cilindro semi-finito. // Nel caso in cui parte della retta giaccia sul cilindro, nIntType vale INF_INT e un // parametro ha valore finito mentre l'altro ha valore infinito. -int LineSemiFiniteCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const Vector3d& vtVCyl, const Vector3d& vtVLine, - double dCylRad, double& dU1, double& dU2) ; +int IntersLineSemiFiniteCylinder( const Point3d& ptPLine, const Vector3d& vtVLine, + const Point3d& ptPCyl, const Vector3d& vtVCyl, double dCylRad, + double& dU1, double& dU2) ; //---------------------------------------------------------------------------- // Valuta la posizione reciproca fra una semi-retta e un cilindro semi-finito. @@ -157,9 +161,9 @@ int SegmentSemiFiniteCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, co // Valuta la posizione reciproca fra una retta e un cilindro. // Nel caso in cui parte della retta giaccia sul cilindro, nIntType vale INF_INT e entrambi i // i parametri dU1 e dU2 sono finiti. -int LineCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const Vector3d& vtVCyl, const Vector3d& vtVLine, - double dCylRad, double dCylHeigth, double& dU1, double& dU2) ; - +int IntersLineCylinder( const Point3d& ptPLine, const Vector3d& vtVLine, + const Point3d& ptPCyl, const Vector3d& vtVCyl, double dCylRad, double dCylHeigth, + double& dU1, double& dU2) ; //---------------------------------------------------------------------------- // Valuta la posizione reciproca fra una semi-retta e un cilindro. @@ -172,8 +176,9 @@ int RayCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const Vector3d& // Valuta la posizione reciproca fra un segmento e un cilindro. // Nel caso in cui parte del segmento giaccia sul cilindro, nIntType vale INF_INT e entrambi i // i parametri dU1 e dU2 hanno senso. -int SegmentCylinder( const Point3d& ptPCyl, const Point3d& ptPLine, const Vector3d& vtVCyl, const Vector3d& vtVLine, - double dCylRad, double dCylHeigth, double dLen, double& dU1, double& dU2) ; +int IntersSegmentCylinder( const Point3d& ptPLine, const Vector3d& vtVLine, double dLen, + const Point3d& ptPCyl, const Vector3d& vtVCyl, double dCylRad, double dCylHeigth, + double& dU1, double& dU2) ; //---------------------------------------------------------------------------- // Valuta la posizione reciproca fra una retta e un cono infinito.