//---------------------------------------------------------------------------- // EgalTech 2018-2018 //---------------------------------------------------------------------------- // File : CAvToolTriangle.cpp Data : 27.04.18 Versione : 1.9e1 // Contenuto : Implementazione delle funzioni ToolTriangleCollisionAvoid. // // // // Modifiche : 10.03.18 LM Creazione modulo. // // //---------------------------------------------------------------------------- #include "stdafx.h" #include "CAvToolTriangle.h" #include "IntersLineSurfStd.h" #include "IntersLineTria.h" #include "/EgtDev/Include/ENkPolynomialRoots.h" #include "/EgtDev/Include/EgtNumUtils.h" using namespace std ; //---------------------------------------------------------------------------- // La funzione determina la minima distanza di allontanamento lungo una direzione fissata // per evitare la collisione tra un utensile ed un triangolo. // Se nella posizione iniziale non c'è gia collisione si restituisce il valore 0. // In caso di errore si restituisce il valore -1. double CAvToolTriangle( const Tool& tlTool, const Point3d& ptToolOrig, const Vector3d& vtToolAx, const Triangle3d& trTria, const Vector3d& vtMove) { // se utensile cilindrico if ( tlTool.GetType() == Tool::CYLMILL) { // parametri geometrici double dHeigth = tlTool.GetHeigth() ; double dRadius = tlTool.GetRadius() ; // distanza di allontanamento del cilindro double dDist = CAvCylinderTriangle( ptToolOrig, vtToolAx, dHeigth, dRadius, trTria, vtMove) ; return dDist ; } // se utensile sferico else if ( tlTool.GetType() == Tool::BALLMILL) { // parametri geometrici double dCylHeigth = tlTool.GetHeigth() - tlTool.GetTipHeigth() ; double dRadius = tlTool.GetRadius() ; // prima determino l'allontanamento della sfera Point3d ptSpheOrig = ptToolOrig - dCylHeigth * vtToolAx ; double dDist = CAvSphereTriangle( ptSpheOrig, dRadius, trTria, vtMove) ; if ( dDist < - EPS_SMALL) return dDist ; // poi verifico quello del cilindro (tenendo conto di quanto è stata allontanata la sfera) if ( dCylHeigth < EPS_SMALL) return dDist ; Point3d ptCylOrig = ptToolOrig + vtMove * max( dDist, 0.) ; double dDist2 = CAvCylinderTriangle( ptCylOrig, vtToolAx, dCylHeigth, dRadius, trTria, vtMove) ; if ( dDist2 < - EPS_SMALL) return dDist2 ; return ( dDist + dDist2) ; } // se utensile conico else if ( tlTool.GetType() == Tool::CONEMILL) { // parametri geometrici Point3d ptMinBase ; Vector3d vtConeAx ; if ( tlTool.GetRadius() > tlTool.GetTipRadius()) { ptMinBase = ptToolOrig - tlTool.GetHeigth() * vtToolAx ; vtConeAx = vtToolAx ; } else { ptMinBase = ptToolOrig - ( tlTool.GetHeigth() - tlTool.GetTipHeigth()) * vtToolAx ; vtConeAx = - vtToolAx ; } double dMinR = min( tlTool.GetRadius(), tlTool.GetTipRadius()) ; double dMaxR = max( tlTool.GetRadius(), tlTool.GetTipRadius()) ; double dCylHeigth = tlTool.GetHeigth() - tlTool.GetTipHeigth() ; // prima determino l'allontanamento del cono double dDist = CAvTrConeTriangle( ptMinBase, vtConeAx, dMinR, dMaxR, tlTool.GetTipHeigth(), trTria, vtMove) ; 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) ; if ( dDist2 < - EPS_SMALL) return dDist2 ; return ( dDist + dDist2) ; } // altrimenti utensile di tipo non gestito else return - 1. ; } //---------------------------------------------------------------------------- // **** SFERA **** //---------------------------------------------------------------------------- // La funzione determina la minima distanza di allontanamento lungo una direzione fissata // per evitare la collisione tra una sfera ed un triangolo. double CAvSphereTriangle( const Point3d& ptSpheCen, double dSpheRad, const Triangle3d& trTria, const Vector3d& vtMove) { // Se la sfera sta già tutta dalla parte del movimento rispetto al piano del triangolo non va allontanata Vector3d vtVert0 = ptSpheCen - trTria.GetP( 0) ; Vector3d vtVert1 = ptSpheCen - trTria.GetP( 1) ; Vector3d vtVert2 = ptSpheCen - trTria.GetP( 2) ; if ( vtVert0 * vtMove > dSpheRad - EPS_SMALL && vtVert1 * vtMove > dSpheRad - EPS_SMALL && vtVert2 * vtMove > dSpheRad - EPS_SMALL) return 0. ; // Valuto Tangenza col piano double dPlaneLeakDist = SpherePlaneLeakDist( ptSpheCen, dSpheRad, trTria.GetP( 0), trTria.GetN(), vtMove) ; // Se il punto di tangenza esiste, verifico se è interno al triangolo if ( dPlaneLeakDist >= 0) { Point3d ptTan = ptSpheCen + dPlaneLeakDist * vtMove - dSpheRad * trTria.GetN() ; // Se il punto è interno abbiamo finito if ( IsPointInsideTriangle( ptTan, trTria)) return dPlaneLeakDist ; } // Valuto le distanze di allontanamento dai segmenti (con i loro estremi) e prendo la maggiore double dLeakDist = 0. ; for ( int nSeg = 0 ; nSeg < 3 ; ++ nSeg) { Vector3d vtSeg = trTria.GetP( ( nSeg + 1) % 3) - trTria.GetP( nSeg) ; double dSegLen = vtSeg.Len() ; vtSeg /= dSegLen ; double dCurLeakDist = SphereSegmentLeakDist( ptSpheCen, dSpheRad, trTria.GetP( nSeg), vtSeg, dSegLen, vtMove) ; if ( dCurLeakDist > dLeakDist) dLeakDist = dCurLeakDist ; } return dLeakDist ; } //---------------------------------------------------------------------------- // Calcola la distanza di allontanamento lungo una direzione fissata di una sfera da un piano. double SpherePlaneLeakDist( const Point3d& ptSpheCen, double dSpheRad, const Point3d& ptPlane, const Vector3d& vtPlaneN, const Vector3d& vtMove) { // Se la direzione di allontanamento sta nel piano if ( abs( vtPlaneN * vtMove) < EPS_SMALL) { if ( abs( ( ptSpheCen - ptPlane) * vtPlaneN) > dSpheRad) return 0. ; else return -1. ; } // altrimenti ... else { double dLeakDist = ( dSpheRad - ( ptSpheCen - ptPlane) * vtPlaneN) / ( vtMove * vtPlaneN) ; return max( dLeakDist, 0.) ; } } //---------------------------------------------------------------------------- // Calcola la distanza di allontanamento lungo una direzione fissata di una sfera da un segmento. // Il segmento è descritto da punto iniziale, versore della direzione e lunghezza. double SphereSegmentLeakDist( const Point3d& ptSpheCen, double dSpheRad, const Point3d& ptSeg, const Vector3d& vtSegDir, double dSegLen, const Vector3d& vtMove) { // Controllo con l'interno del segmento double dU[2] ; int nTanPointNum = SphereLineTangentPoints( ptSpheCen, dSpheRad, ptSeg, vtSegDir, dSegLen, vtMove, dU[0], dU[1]) ; double dLeakDistIn = 0. ; for ( int nSol = 0 ; nSol < nTanPointNum && nTanPointNum != 3 ; ++ nSol) { if ( dU[nSol] < 0.) continue ; Point3d ptC = ptSpheCen + dU[nSol] * vtMove ; double dSegPar = ( ptC - ptSeg) * vtSegDir ; if ( dSegPar > 0 && dSegPar < dSegLen) dLeakDistIn = dU[nSol] ; } // Controllo con gli estremi Point3d ptSegEnd = ptSeg + dSegLen * vtSegDir ; double dDistStart = SpherePointLeakDist( ptSpheCen, dSpheRad, ptSeg, vtMove) ; double dDistEnd = SpherePointLeakDist( ptSpheCen, dSpheRad, ptSegEnd, vtMove) ; // Restituisco il massimo return max( dLeakDistIn, max( dDistStart, dDistEnd)) ; } //---------------------------------------------------------------------------- // Calcola la distanza di allontanamento lungo una direzione fissata di una sfera da un punto. double SpherePointLeakDist( const Point3d& ptSpheCen, double dSpheRad, const Point3d ptP, const Vector3d& vtMove) { Vector3d vtOR = ptP - ptSpheCen ; double dDotORMot = vtOR * vtMove ; Vector3d vtOROrt = vtOR - dDotORMot * vtMove ; double dSqOROrtLen = vtOROrt.SqLen() ; double dSqSphRad = dSpheRad * dSpheRad ; if ( dSqOROrtLen > dSqSphRad + 2 * dSpheRad * EPS_SMALL) return 0. ; else return max( dDotORMot + sqrt( max( dSpheRad * dSpheRad - dSqOROrtLen, 0.)), 0.) ; } //---------------------------------------------------------------------------- // **** CILINDRO **** //---------------------------------------------------------------------------- // La funzione determina la minima distanza di allontanamento lungo una direzione fissata // per evitare la collisione tra un cilindro ed un triangolo. double CAvCylinderTriangle( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad, const Triangle3d& trTria, const Vector3d& vtMove) { // Classificazione del moto int nMotionType = 0 ; if ( AreSameVectorApprox( vtCylAx, vtMove)) nMotionType = 1 ; else if ( AreOppositeVectorApprox( vtCylAx, vtMove)) nMotionType = 2 ; else if ( AreOrthoApprox( vtCylAx, vtMove)) nMotionType = 3 ; // Movimento lungo la direzione dell'asse del cilindro if ( nMotionType == 1 || nMotionType == 2) { // In questo caso il problema si riduce alla determinazione della distanza di allontanamento // del disco all'estremità opposta rispetto alla direzione di moto. // Il punto base di questo disco viene così calcolato Point3d ptBase = ptCylOrig ; if ( nMotionType == 1) ptBase -= dHeigth * vtCylAx ; // Valuto le distanze con segno dei vertici dal piano del disco : // se sono tutte negative non interferiscono. double dDistV[3] ; dDistV[0] = PointPlaneSignedDist( trTria.GetP( 0), ptBase, vtCylAx) ; dDistV[1] = PointPlaneSignedDist( trTria.GetP( 1), ptBase, vtCylAx) ; dDistV[2] = PointPlaneSignedDist( trTria.GetP( 2), ptBase, vtCylAx) ; double dMaxDistV = max( dDistV[0], max( dDistV[1], dDistV[2])) ; if ( dMaxDistV < 0.) return 0. ; // Se tutti i punti distano dall'asse di movimento meno del raggio, l'ultimo // punto di contatto deve essere un vertice del triangolo. double dSqRad = dRad * dRad ; bool bInV[3] ; bInV[0] = ( GetPointLineSqDist( trTria.GetP( 0), ptBase, vtMove) < dSqRad) ; bInV[1] = ( GetPointLineSqDist( trTria.GetP( 1), ptBase, vtMove) < dSqRad) ; bInV[2] = ( GetPointLineSqDist( trTria.GetP( 2), ptBase, vtMove) < dSqRad) ; if ( bInV[0] && bInV[1] && bInV[2]) return max( dMaxDistV, 0.) ; // Ciclo sui segmenti del triangolo e calcolo la loro distanza di allontanamento, calcolo // anche la distanza di allontanamento dai vertici distanti dall'asse meno del raggio. double dMaxDistVS = 0. ; for ( int nVS = 0 ; nVS < 3 ; ++ nVS) { // Vertici if ( bInV[nVS] && dDistV[nVS] > dMaxDistVS) dMaxDistVS = dDistV[nVS] ; // Se un lato del triangolo ha entrambi gli estremi con distanza negativa dal piano del disco, // non può interferire con esso. int nVE = ( nVS + 1) % 3 ; if ( dDistV[nVS] < 0 && dDistV[nVE] < 0) continue ; // Versore e lunghezza del segmento Vector3d vtSeg = trTria.GetP( nVE) - trTria.GetP( nVS) ; double dSegLen = vtSeg.Len() ; vtSeg /= dSegLen ; // Distanza dal piano del segmento corrente double dCurDist = DiskSegmentLeakDistLongMotion( ptBase, dRad, trTria.GetP( nVS), vtSeg, dSegLen, vtMove) ; if ( dCurDist > dMaxDistVS) dMaxDistVS = dCurDist ; } // Distanza di allontanamento dall'interno del triangolo double dMaxDistI = DiskTriaInteriorLeakDistLongMot( ptBase, dRad, trTria, vtMove) ; return max( dMaxDistVS, dMaxDistI) ; } // Movimento perpendicolare all'asse del cilindro else if ( nMotionType == 3) { // 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) ; if ( dLeakDist < dCurDist) dLeakDist = dCurDist ; } // FINE PROVE PUNTI // INIZIO PROVA SEGMENTI for ( int nVrtS = 0 ; nVrtS < 3 ; ++ nVrtS) { int nVrtE = ( nVrtS + 1) % 3 ; Vector3d vtSeg = trTria.GetP( nVrtE) - trTria.GetP( nVrtS) ; double dSegLen = vtSeg.Len() ; vtSeg /= dSegLen ; double dCurDist = CylSegmentLeakDistOrtMotion( ptCylOrig, vtCylAx, dHeigth, dRad, trTria.GetP( nVrtS), vtSeg, dSegLen, vtMove) ; if ( dLeakDist < dCurDist) dLeakDist = dCurDist ; } // FINE PROVA SEGMENTI // PROVA INTERNO Point3d ptBaseCont, ptBottomCont ; double dBaseLeak = DiskPlaneLeakDistOrtMot( ptCylOrig, vtCylAx, dRad, trTria.GetP( 0), trTria.GetN(), vtMove, ptBaseCont) ; double dBottomLeak = DiskPlaneLeakDistOrtMot( ptCylOrig - dHeigth * vtCylAx, vtCylAx, dRad, trTria.GetP( 0), trTria.GetN(), vtMove, ptBottomCont) ; if ( ! IsPointInsideTriangle( ptBaseCont, trTria)) dBaseLeak = 0. ; if ( ! IsPointInsideTriangle( ptBottomCont, trTria)) dBottomLeak = 0. ; double dInnerLeak = max( dBaseLeak, dBottomLeak) ; if ( dLeakDist < dInnerLeak) dLeakDist = dInnerLeak ; // FINE PROVA INTERNO return dLeakDist ; } // Movimento generico else return -1. ; } //---------------------------------------------------------------------------- double DiskSegmentLeakDistLongMotion( const Point3d& ptDiskCen, double dDiscRad, 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) return 0. ; // Imposto l'equazione: la distanza quadrata del generico punto della retta associata al // segmento dall'asse lungo cui si sposta il disco deve valere raggio quadrato del disco. Vector3d vtLineSegOrt = ( ptSeg - ptDiskCen) - ( ptSeg - ptDiskCen) * vtMove * vtMove ; Vector3d vtSegOrt = vtSeg - vtSeg * vtMove * vtMove ; DBLVECTOR vdCoef(3) ; DBLVECTOR vdRoots ; vdCoef[0] = vtLineSegOrt.SqLen() - dDiscRad * dDiscRad ; 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) { double dLenSt = PointPlaneSignedDist( ptSeg, ptDiskCen, vtMove) ; double dLenEn = PointPlaneSignedDist( ptSeg + dSegLen * vtSeg, ptDiskCen, vtMove) ; return max( max( dLenSt, dLenEn), 0.) ; } else return 0. ; } // Soluzione dell'equazione else { int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ; // Ciclo sulle soluzioni per trovare la distanza di allontanamento double dSegDist = 0. ; for ( int nSol = 0 ; nSol < nRoot ; ++ nSol) { // Soluzione interna al segmento if ( vdRoots[nSol] > 0. && vdRoots[nSol] < dSegLen) { Point3d ptC = ptSeg + vdRoots[nSol] * vtSeg ; // Distanza del punto soluzione dal piano del disco nella posizione iniziale double dCurDist = PointPlaneSignedDist( ptC, ptDiskCen, vtMove) ; if ( dCurDist > dSegDist) dSegDist = dCurDist ; } } return dSegDist ; } } //---------------------------------------------------------------------------- // Determina la distanza di allontanamento di un disco, che trasla lungo il suo asse di simmetria, // dall'interno di un triangolo. Il disco è descritto dal suo raggio e il moto dal // centro nella posizione iniziale e dal versore del suo asse di simmetria // (COINCIDENTE CON IL VERSORE DELLA DIREZIONE DI ALLONTANAMENTO). double DiskTriaInteriorLeakDistLongMot( const Point3d& ptDiskCen, double dDiskRad, const Triangle3d& trTria, const Vector3d& vtMove) { // Se disco e triangolo sono complanari if ( AreSameVectorApprox( vtMove, trTria.GetN())) { // verifico solo il centro, se centro esterno non più del raggio sicuramente toccherebbe i lati double dDist = PointPlaneSignedDist( ptDiskCen, trTria.GetP( 0), trTria.GetN()) ; if ( IsPointInsideTriangle( ptDiskCen - dDist * trTria.GetN(), trTria)) return max( -dDist, 0.) ; } // Cerco un punto di contatto nell'interno del triangolo. Se tale punto // esiste, la retta intersezione fra il piano del triangolo e quello del // disco è tangente alla circonferenza. // Vettore tangente alla circonferenza e vettore radiale Vector3d vtRadLine = vtMove * ( trTria.GetN() * vtMove) - trTria.GetN() ; vtRadLine.Normalize() ; // Punti delle due rette candidate all'intersezione col triangolo Point3d ptStPlus = ptDiskCen + dDiskRad * vtRadLine ; Point3d ptStMinus = ptDiskCen - dDiskRad * vtRadLine ; // Intersezioni con la prima retta double dDistPlus = 0. ; Point3d ptIntPlus1, ptIntPlus2 ; int nIntPlus = IntersLineTria( ptStPlus, vtMove, 10, trTria, ptIntPlus1, ptIntPlus2, false) ; if ( nIntPlus != ILTT_NO) { double dDist1 = PointPlaneSignedDist( ptIntPlus1, ptDiskCen, vtMove) ; // Se l'intersezione è doppia, ha senso anche dDist2. if ( nIntPlus == ILTT_SEGM || nIntPlus == ILTT_SEGM_ON_EDGE) { double dDist2 = PointPlaneSignedDist( ptIntPlus2, ptDiskCen, vtMove) ; dDistPlus = max( dDist1, dDist2) ; } // Altrimenti ha senso solo dDist1 else dDistPlus = dDist1 ; } // Intersezioni con la seconda retta double dDistMinus = 0. ; Point3d ptIntMinus1, ptIntMinus2 ; int nIntMinus = IntersLineTria( ptStMinus, vtMove, 10, trTria, ptIntMinus1, ptIntMinus2, false) ; if ( nIntMinus != ILTT_NO) { double dDist1 = PointPlaneSignedDist( ptIntMinus1, ptDiskCen, vtMove) ; // Se l'intersezione è doppia, ha senso anche dDist2. if ( nIntMinus == ILTT_SEGM || nIntMinus == ILTT_SEGM_ON_EDGE) { double dDist2 = PointPlaneSignedDist( ptIntMinus2, ptDiskCen, vtMove) ; dDistMinus = max( dDist1, dDist2) ; } // Altrimenti ha senso solo dDist1 else dDistMinus = dDist1 ; } return max( dDistPlus, dDistMinus) ; } //---------------------------------------------------------------------------- // Valuta l'allontanamento di un disco, che trasla lungo il proprio asse di simmetria, // da un piano. Il disco è descritto dal suo centro nella posizione iniziale e dal raggio. // Il piano è descritto da un suo punto e dal suo versore normale. Il moto è descritto // dal versore di traslazione. // La funzione restituisce un intero che descrive la situazione di allontanamento e, // nel caso che abbia senso, per referenza restituisce il punto in cui avviene // l'estremo contatto. // 0: Nessun contatto // 1: Un contatto // 2: Disco e piano ortogonali, il contatto è una semiretta // 3: Disco e piano paralleli, la superficie di contatto è il disco intero // Nel caso 0 il parametro ptTouch non ha senso, così come nel caso 2. // Nel caso 3 ptTouch è il centro del disco nella posizione finale. int DiskPlaneLastContactLongMot( const Point3d& ptDiskCen, double dDiskRad, const Point3d& ptPlane, const Vector3d& vtPlane, const Vector3d& vtMove, Point3d& ptTouch) { // Se disco e piano sono paralleli if ( AreSameOrOppositeVectorApprox( vtMove, vtPlane)) { double dDist = PointPlaneSignedDist( ptPlane, ptDiskCen, vtMove) ; if ( dDist > - EPS_SMALL) { ptTouch = ptDiskCen + dDist * vtMove ; return 3 ; } return 0 ; } // Vettore radiale Vector3d vtRadLine = vtMove * ( vtPlane * vtMove) - vtPlane ; // Disco e piano ortogonali if ( vtRadLine.IsNormalized()) { if ( abs( PointPlaneSignedDist( ptPlane, ptDiskCen, vtMove) - dDiskRad) < EPS_SMALL) return 2 ; return 0 ; } // Cerco un punto di contatto nell'interno del triangolo. Se tale punto // esiste, la retta intersezione fra il piano del triangolo e quello del // disco è tangente alla circonferenza. vtRadLine.Normalize() ; // Punti delle due rette candidate all'intersezione col triangolo Point3d ptStPlus = ptDiskCen + dDiskRad * vtRadLine ; Point3d ptStMinus = ptDiskCen - dDiskRad * vtRadLine ; // Parametri d'intersezione delle rette col piano double dDistPlus = ( ( ptPlane - ptStPlus) * vtPlane) / ( vtMove * vtPlane) ; double dDistMinus = ( ( ptPlane - ptStMinus) * vtPlane) / ( vtMove * vtPlane) ; double dDist ; if ( dDistPlus > dDistMinus) { dDist = dDistPlus ; ptTouch = ptStPlus + dDist * vtMove ; } else { dDist = dDistMinus ; ptTouch = ptStMinus + dDist * vtMove ; } if ( dDist > - EPS_SMALL) return 1 ; return 0 ; } //---------------------------------------------------------------------------- // 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) { 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) 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 fuga di un disco da un segmento nel caso di // moto ortogonale all'asse di simmetria. // Il disco è descritto dal suo raggio. Il movimento è descritto dal versore // dell'asse del disco, dal centro nella posizione iniziale e dal // versore della direzione del moto. Il segmento è descritto da punto iniziale, // versore direzione e lunghezza. double DiskSegmentLeakDistOrtMot( const Point3d& ptDiscCen, const Vector3d& vtDiscAx, double dDiscRad, 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)) { Point3d ptSegEnd = ptSeg + dSegLen * vtSeg ; double dDotStart = ( ptSeg - ptDiscCen) * vtDiscAx ; double dDotEnd = ( ptSegEnd - ptDiscCen) * vtDiscAx ; if ( dDotStart * dDotEnd < 0.) { double dS = ( ptDiscCen - ptSeg) * vtDiscAx / ( vtSeg * vtDiscAx) ; Point3d ptInt = ptSeg + dS * vtSeg ; Vector3d vtOI = ( ptInt - ptDiscCen) ; double dLongCord = vtOI * vtMove ; Vector3d vtLong = dLongCord * vtMove ; Vector3d vtOrt = vtOI - vtLong ; double dSqOrtLen = vtOrt.SqLen() ; if ( dSqOrtLen > dDiscRad * dDiscRad + 2 * dDiscRad * EPS_SMALL) return 0. ; else return max( dLongCord + sqrt( max( dDiscRad * dDiscRad - dSqOrtLen, 0.)), 0.) ; } else return 0. ; } 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. // Il disco è descritto dal suo raggio. Il moto è descritto dal centro nella posizione // 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, 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 ; if ( vtRad * vtMove > 0) vtRad *= - 1 ; double dPar = ( ptPlane - ptDiscCen) * vtPlane / ( vtMove * vtPlane) ; double dDeltaPar = dDiscRad * dDiscRad / abs( vtRad * vtMove) ; double dDesplacement = max( dPar + dDeltaPar, 0.) ; ptContact = ptDiscCen + dDesplacement * vtMove + vtRad ; return dDesplacement ; } 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 //---------------------------------------------------------------------------- // Dati un punto e una retta, restituisce il quadrato della loro distanza. // La retta è descritta da un suo punto e dal suo versore tangente. double GetPointLineSqDist( const Point3d& ptP, const Point3d& ptLine, const Vector3d& vtLine) { return ( ( ptP - ptLine) - ( ptP - ptLine) * vtLine * vtLine).SqLen() ; } //---------------------------------------------------------------------------- // Dati un punto e un piano, ne restituisce la distanza con segno. // Il piano è descritto da un suo punto e dal suo versore normale. double PointPlaneSignedDist( const Point3d& ptP, const Point3d& ptPlane, const Vector3d& vtNorm) { return ( ptP - ptPlane) * vtNorm ; } //---------------------------------------------------------------------------- // Dati un sistema di tre punti e un piano, ne calcola le tre distanze con segno e ne restituisce la // maggiore. I tre punti sono passati mediante un triangolo, il piano è descritto da un suo punto e // dal suo versore normale. double ThreePointPlaneSignedDist( const Triangle3d& trTria, const Point3d& ptPlane, const Vector3d& vtNorm) { double dMaxSgnDist = - INFINITO ; for ( int nV = 0 ; nV < 3 ; ++ nV) { double dCurrSgnDist = PointPlaneSignedDist( trTria.GetP( nV), ptPlane, vtNorm) ; if ( dCurrSgnDist > dMaxSgnDist) dMaxSgnDist = dCurrSgnDist ; } return dMaxSgnDist ; } //---------------------------------------------------------------------------- // Dati un piano, descritto da un suo punto e dal versore normale, // e una retta, descritta da un suo punto e dal versore direzione, // e un numero reale d, determina se esiste un punto sulla retta che ha distanza con // segno d dal piano. I casi possibili sono: // Nessun punto dista d dal piano (retta parallela al piano con distanza diversa da d), // viene restituito 0 e il valore di dPar non ha senso. // Un solo punto dista d dal piano (retta non parallela al piano), viene restituito 1 // e il valore di dPar è il parametro del punto sulla retta. // Tutti i punti distano d dal piano (retta parallela al piano con distanza d), viene // restituito 2 e qualunque valore di dPar ha senso. int LinePlaneDDistPar( const Point3d& ptPlane, const Vector3d& vtPlane, const Point3d& ptLine, const Vector3d& vtLine, double dDist, double& dPar) { // Vettore congiungente il punto del piano con il punto iniziale della retta Vector3d vtPlLn = ptLine - ptPlane ; // Distanza con segno del punto iniziale della retta dal piano double dSgnDist0 = vtPlLn * vtPlane ; // Prodotto scalare fra versore della direzione della retta e versore normale al piano double dDotDirNorm = vtLine * vtPlane ; // Retta parallela al piano if ( abs( dDotDirNorm) < EPS_ZERO) { if ( abs( dSgnDist0 - dDist) < EPS_SMALL) return 2 ; else return 0 ; } // Caso generale else { dPar = ( dDist - dSgnDist0) / dDotDirNorm ; return 1 ; } } //---------------------------------------------------------------------------- // Date due rette nello spazio restituisce il quadrato della loro distanza. // Le rette vengono descritte da punto di passaggio e versore direzione. double LineLineSqDist( const Point3d& ptP1, const Vector3d& vtD1, const Point3d& ptP2, const Vector3d& vtD2) { // Caso di rette parallele if ( abs( 1. - abs( vtD1 * vtD2)) < EPS_SMALL) return ( ( ptP2 - ptP1) - ( ptP2 - ptP1) * vtD1 * vtD1).SqLen() ; // Caso generale else { Vector3d vtOrtD2 = vtD2 - vtD2 * vtD1 * vtD1 ; Vector3d vtOrtP1P2 = ( ptP2 - ptP1) - ( ptP2 - ptP1) * vtD1 * vtD1 ; double dD = ( vtOrtD2 * vtOrtP1P2) / vtOrtP1P2.SqLen() ; return vtOrtP1P2.SqLen() + dD * dD * vtOrtD2.SqLen() + 2 * dD * vtOrtD2 * vtOrtP1P2 ; } } //---------------------------------------------------------------------------- // Data una retta e un segmento, ne restituisce il quadrato della distanza. // La retta è descritta da un qualsiasi punto ad essa appartenente e un versore // che ne identifica la direzione. Il segmento è descritto da un suo estremo, // dal versore direzione diretto verso l'altro estremo e dalla sua lunghezza. double LineSegmentSqDist( const Point3d& ptPLn, const Vector3d& vtDLn, const Point3d& ptPSg, const Vector3d& vtDSg, double dSgLen) { // Caso di parallelismo if ( AreSameOrOppositeVectorApprox( vtDLn, vtDSg)) return ( ( ptPSg - ptPLn) - ( ptPSg - ptPLn) * vtDLn * vtDLn).SqLen() ; // Caso generale else { double dDLn = ( ptPSg - ptPLn) * vtDLn ; double dDSg = ( ptPSg - ptPLn) * vtDSg ; double dDLnSg = vtDLn * vtDSg ; // posizione parametrica del punto sul segmento double dSgPar = ( dDLn * dDLnSg - dDSg) / ( 1. - dDLnSg * dDLnSg) ; // se prima dell'inizio, è distanza dell'inizio dalla linea if ( dSgPar <= 0.) return GetPointLineSqDist( ptPSg, ptPLn, vtDLn) ; // se dopo fine, è distanza fine dalla linea else if ( dSgPar >= dSgLen) return GetPointLineSqDist( ptPSg + dSgLen * vtDSg, ptPLn, vtDLn) ; // altrimenti, determino i due punti a minima distanza else { double dLnPar = ( dDLn - dDSg * dDLnSg) / ( 1. - dDLnSg * dDLnSg) ; Point3d ptMinLn = ptPLn + dLnPar * vtDLn ; Point3d ptMinSg = ptPSg + dSgPar * vtDSg ; return SqDist( ptMinLn, ptMinSg) ; } } } //---------------------------------------------------------------------------- // Stabilisce se un punto appartiene a un triangolo interno o frontiera. bool IsPointInsideTriangle( const Point3d& ptP, const Triangle3d& trTria) { // Se il punto non sta sul piano del triangolo ho finito if ( abs( ( ptP - trTria.GetP( 0)) * trTria.GetN()) > EPS_SMALL) return false ; // Vettori dei lati normalizzati Vector3d vtV0 = trTria.GetP( 1) - trTria.GetP( 0) ; vtV0.Normalize() ; Vector3d vtV1 = trTria.GetP( 2) - trTria.GetP( 1) ; vtV1.Normalize() ; Vector3d vtV2 = trTria.GetP( 0) - trTria.GetP( 2) ; vtV2.Normalize() ; // E' sufficiente che il segno di un prodotto misto sia negativo per essere fuori double dProd0 = ( vtV0 ^ ( ptP - trTria.GetP( 0))) * trTria.GetN() ; if ( dProd0 < - EPS_SMALL) return false ; double dProd1 = ( vtV1 ^ ( ptP - trTria.GetP( 1))) * trTria.GetN() ; if ( dProd1 < - EPS_SMALL) return false ; double dProd2 = ( vtV2 ^ ( ptP - trTria.GetP( 2))) * trTria.GetN() ; if ( dProd2 < - EPS_SMALL) return false ; return true ; } //---------------------------------------------------------------------------- // Valuta l'interferenza di un disco e un triangolo complanari. bool CoplanarDiscTriangleInterferance( const Point3d& ptCen, double dRad, const Triangle3d& trTria) { // Se il centro del disco non sta sul piano del triangolo, non sono complanari if ( abs( PointPlaneSignedDist( ptCen, trTria.GetP( 0), trTria.GetN())) > EPS_SMALL) return false ; // 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)) ; // 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) return false ; // interferenza return true ; } //---------------------------------------------------------------------------- // Date due rette, descritte da un punto e un vettore direzione, // trova i parametri corrispondenti ai punti di minima distanza. // Se le rette non sono parallele abbiamo una singola coppia di // punti e viene restituito true, altrimenti ne abbiamo infinite // e viene restituito false. Si noti che i parametri dU1, dU2 // non hanno necessariamente il significato di lunghezza; // perché non è richiesto che i vettori siano normalizzati. bool FindMinDistPar( const Point3d& ptL1, const Point3d& ptL2, const Vector3d& vtV1, const Vector3d& vtV2, double& dU1, double& dU2) { // Se le rette sono parallele if ( abs( abs( vtV1 * vtV2) - 1) < EPS_ZERO) return false ; // Vettore congiungente i punti iniziali Vector3d vtD = ptL2 - ptL1 ; // Proiezioni di vtD sui versori direzione double dD1 = vtD * vtV1 ; double dD2 = vtD * vtV2 ; // Prodotto scalare fra i versori direzione double dV = vtV1 * vtV2 ; // Se le rette sono perpendicolari if ( abs( vtV1 * vtV2) < EPS_ZERO) { dU1 = dD1 ; dU2 = - dD2 ; } // Caso generale else { dU1 = ( dD1 - dD2 * dV) / ( 1 - dV * dV) ; dU2 = ( - dD2 + dD1 * dV) / ( 1 - dV * dV) ; } return true ; } //---------------------------------------------------------------------------- // Calcolo delle posizioni di tangenza di una sfera che si muove dalla posizione // originale lungo una direzione prefissata rispetto ad una retta. // Le posizioni sono rappresentate dalla distanza percorsa. // Il parametro di ritorno può assumere i seguenti valori : // 0 : la sfera non è mai tangente alla retta; dU1 e dU2 non hanno senso // 1 : la sfera è tangente alla retta in una posizione; solo dU1 ha senso // 2 : la sfera è tangente alla retta in due posizioni; dU1 e dU2 valide // 3 : la sfera è sempre tangente int SphereLineTangentPoints( const Point3d& ptSpheCen, double dSpheRad, const Point3d& ptSeg, const Vector3d& vtSegDir, double dSegLen, const Vector3d& vtMove, double& dU1, double& dU2) { // Definisco l'equazione Vector3d vtMotOrt = vtMove - vtMove * vtSegDir * vtSegDir ; Vector3d vtLineSeg = ptSeg - ptSpheCen ; Vector3d vtLineSegOrt = vtLineSeg - vtLineSeg * vtSegDir * vtSegDir ; // Setto i coefficienti dell'equazione DBLVECTOR vdCoeff( 3) ; vdCoeff[0] = vtLineSegOrt.SqLen() - dSpheRad * dSpheRad ; vdCoeff[1] = - 2 * vtMotOrt * vtLineSegOrt ; vdCoeff[2] = vtMotOrt.SqLen() ; if ( vdCoeff[2] < EPS_ZERO * EPS_ZERO) { if ( abs( vdCoeff[0]) < EPS_SMALL) return 3 ; else return 0 ; } // Risolvo l'equazione DBLVECTOR vdRoots ; int nRoots = PolynomialRoots( 2, vdCoeff, vdRoots) ; // Studio le soluzioni if ( nRoots == 1) dU1 = vdRoots[0] ; else if ( nRoots == 2) { dU1 = min( vdRoots[0], vdRoots[1]) ; dU2 = max( vdRoots[0], vdRoots[1]) ; } return nRoots ; }