//---------------------------------------------------------------------------- // 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) ; // poi verifico quello del cilindro (tenendo conto di quanto è stata allontanata la sfera Point3d ptCylOrig = ptToolOrig + vtMove * max( dDist, 0.) ; double dDist2 = CAvCylinderTriangle( ptCylOrig, vtToolAx, dCylHeigth, dRadius, trTria, vtMove) ; return ( max( dDist, 0.) + max( dDist2, 0.)) ; } // 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 ; // Moto 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 ; // La sua normale vale // Nel Caso 1 coincide con quella dell'utensile, nel caso 2 è opposta // !!!!! DA FARE !!!!! // 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, vtCylAx, dRad, trTria.GetP( nVS), vtSeg, dSegLen) ; if ( dCurDist > dMaxDistVS) dMaxDistVS = dCurDist ; } // Distanza di allontanamento dall'interno del triangolo double dMaxDistI = DiskTriaInteriorLeakDistLongMot( ptBase, vtCylAx, dRad, trTria) ; return max( dMaxDistVS, dMaxDistI) ; } // Moto perpendicolare all'asse del cilindro else if ( nMotionType == 3) { // PROVE PUNTI E SEGMENTI double dLeakDist = - DBL_MAX ; for ( int nVrt = 0 ; nVrt < 3 ; ++ nVrt) { double dCurDist = CylPointLeakDistOrtMotion( trTria.GetP( nVrt), ptCylOrig, vtCylAx, vtMove, dHeigth, dRad) ; 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( trTria.GetP( nVrtS), ptCylOrig, vtSeg, vtCylAx, vtMove, dSegLen, dHeigth, dRad) ; if ( dLeakDist < dCurDist) dLeakDist = dCurDist ; } // FINE PROVA SEGMENTI // PROVA INTERNO Point3d ptBaseCont, ptBottomCont ; double dBaseLeak = DiskPlaneLeakDistOrtMot( trTria, ptCylOrig, vtCylAx, vtMove, dRad, ptBaseCont) ; double dBottomLeak = DiskPlaneLeakDistOrtMot( trTria, ptCylOrig - dHeigth * vtCylAx, vtCylAx, vtMove, dRad, 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 ; } // Moto generico else return -1. ; } //---------------------------------------------------------------------------- // Determina la distanza di fuga di un disco, che trasla lungo il suo asse di simmetria, da // un segmento. Il disco è descritto dal quadrato del suo raggio, il moto dal centro nella // posizione di partenza e il versore di traslazione. Il segmento è // descritto da un punto suo estremo, dal versore direzione diretto verso // l'altro estremo e dalla sua lunghezza. //double //DiskSegmentLeakDistLongMotion( const Point3d& ptDisc, const Point3d& ptSeg, // const Vector3d& vtDiscLeak, const Vector3d& vtSeg, // double dSegLen, double dDiscSqRad) //{ // // Il disco non può interferire col segmento nel suo moto, se la distanza del // // segmento dall'asse di traslazione è maggiore del raggio. // if ( LineSegmentSqDist( ptDisc, ptSeg, vtDiscLeak, vtSeg, dSegLen) > dDiscSqRad) // 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 - ptDisc) - ( ptSeg - ptDisc) * vtDiscLeak * vtDiscLeak ; // Vector3d vtSegOrt = vtSeg - vtSeg * vtDiscLeak * vtDiscLeak ; // DBLVECTOR vdCoef( 3) ; // DBLVECTOR vdRoots ; // vdCoef[0] = vtLineSegOrt.SqLen() - dDiscSqRad ; // vdCoef[1] = 2 * vtLineSegOrt * vtSegOrt ; // vdCoef[2] = vtSegOrt.SqLen() ; // // Soluzione dell'equazione // int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ; // // Ciclo sulle soluzioni per trovare la distanza di allontanamento // double dSegDist = - DBL_MAX ; // 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, ptDisc, vtDiscLeak) ; // if ( dSegDist < dCurDist) // dSegDist = dCurDist ; // } // } // return dSegDist ; //} ///// ALTRA VERSIONE SPERIMENTALE DELLA PRECEDNTE double DiskSegmentLeakDistLongMotion( const Point3d& ptDisc, const Vector3d& vtDiscLeak, double dDiscRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen) { // Il disco non può interferire col segmento nel suo moto, se la distanza del // segmento dall'asse di traslazione è maggiore del raggio. if ( LineSegmentSqDist( ptDisc, vtDiscLeak, 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 - ptDisc) - ( ptSeg - ptDisc) * vtDiscLeak * vtDiscLeak ; Vector3d vtSegOrt = vtSeg - vtSeg * vtDiscLeak * vtDiscLeak ; 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, ptDisc, vtDiscLeak) ; double dLenEn = PointPlaneSignedDist( ptSeg + dSegLen * vtSeg, ptDisc, vtDiscLeak) ; 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, ptDisc, vtDiscLeak) ; 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& ptDisk, const Vector3d& vtDiskAx, double dDiskRad, const Triangle3d& trTria) { // Se disco e triangolo sono complanari if ( AreSameVectorApprox( vtDiskAx, trTria.GetN())) { // verifico solo il centro, se centro esterno non più del raggio sicuramente toccherebbe i lati double dDist = PointPlaneSignedDist( ptDisk, trTria.GetP( 0), trTria.GetN()) ; if ( IsPointInsideTriangle( ptDisk - 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 = vtDiskAx * ( trTria.GetN() * vtDiskAx) - trTria.GetN() ; vtRadLine.Normalize() ; // Punti delle due rette candidate all'intersezione col triangolo Point3d ptStPlus = ptDisk + dDiskRad * vtRadLine ; Point3d ptStMinus = ptDisk - dDiskRad * vtRadLine ; // Intersezioni con la prima retta double dDistPlus = 0. ; Point3d ptIntPlus1, ptIntPlus2 ; int nIntPlus = IntersLineTria( ptStPlus, vtDiskAx, 10, trTria, ptIntPlus1, ptIntPlus2, false) ; if ( nIntPlus != ILTT_NO) { double dDist1 = PointPlaneSignedDist( ptIntPlus1, ptDisk, vtDiskAx) ; // Se l'intersezione è doppia, ha senso anche dDist2. if ( nIntPlus == ILTT_SEGM || nIntPlus == ILTT_SEGM_ON_EDGE) { double dDist2 = PointPlaneSignedDist( ptIntPlus2, ptDisk, vtDiskAx) ; 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, vtDiskAx, 10, trTria, ptIntMinus1, ptIntMinus2, false) ; if ( nIntMinus != ILTT_NO) { double dDist1 = PointPlaneSignedDist( ptIntMinus1, ptDisk, vtDiskAx) ; // Se l'intersezione è doppia, ha senso anche dDist2. if ( nIntMinus == ILTT_SEGM || nIntMinus == ILTT_SEGM_ON_EDGE) { double dDist2 = PointPlaneSignedDist( ptIntMinus2, ptDisk, vtDiskAx) ; dDistMinus = max( dDist1, dDist2) ; } // Altrimenti ha senso solo dDist1 else dDistMinus = dDist1 ; } return max( dDistPlus, dDistMinus) ; } //---------------------------------------------------------------------------- // Restituisce la distanza di allontanamento di un cilindro, che trasla lungo il suo asse // di simmetria, da un triangolo. // Il cilindro è descritto da raggio e altezza. Il suo moto è descritto dalla // posizione iniziale, dal versore dell'asse di simmetria // e dal versore della direzione del moto. Il segmento è descritto da punto // iniziale, versore direzione e lunghezza. double CylTriaSegmentLeakDistLongMot( const Triangle3d& trTria, const Vector3d& vtMotion, const Point3d& ptCylBase, double dCylRad) { // Cerco i punti di contatto sui segmenti double dSegDist = - DBL_MIN ; // Ciclo sui segmenti for ( int nVS = 0 ; nVS < 3 ; ++ nVS) { int nVE = ( nVS + 1) % 3 ; // Versore del segmento Vector3d vtSeg = trTria.GetP( nVE) - trTria.GetP( nVS) ; double dSegLen = vtSeg.Len() ; vtSeg /= dSegLen ; // Vettore punto retta Vector3d vtPointLine = trTria.GetP( nVS) - ptCylBase ; // Componenti dei vettori ortogonali all'asse del cilindro Vector3d vtSegOrt = vtSeg - vtSeg * vtMotion * vtMotion ; Vector3d vtPointLineOrt = vtPointLine - vtPointLine * vtMotion * vtMotion ; DBLVECTOR vdCoef(3) ; DBLVECTOR vdRoots ; vdCoef[0] = vtPointLineOrt.SqLen() - dCylRad * dCylRad ; vdCoef[1] = 2 * vtPointLineOrt * vtSegOrt ; vdCoef[2] = vtSegOrt.SqLen() ; // Soluzione dell'equazione int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ; for ( int nSol = 0 ; nSol < nRoot ; ++ nSol) { Point3d ptInt = trTria.GetP( nVS) + vdRoots[nSol] * vtSegOrt ; if ( ( ptInt - trTria.GetP( nVS)) * vtSeg > 0 && ( ptInt - trTria.GetP( nVS)) * vtSeg < dSegLen) { double dCurDist = ( ptInt - ptCylBase) * vtMotion ; if ( dSegDist < dCurDist) dSegDist = dCurDist ; } } } return dSegDist ; } //---------------------------------------------------------------------------- // 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& ptP, const Point3d& ptCylOrig, const Vector3d& vtCylAx, const Vector3d& vtMove, double dCylHei, double dCylRad) { 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& ptSeg, const Point3d& ptDiscOrig, const Vector3d& vtSeg, const Vector3d& vtDiscAx, const Vector3d& vtMove, double dSegLen, double dDiscRad) { // 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 - ptDiscOrig) * vtDiscAx ; double dDotEnd = ( ptSegEnd - ptDiscOrig) * vtDiscAx ; if ( dDotStart * dDotEnd < 0.) { double dS = ( ptDiscOrig - ptSeg) * vtDiscAx / ( vtSeg * vtDiscAx) ; Point3d ptInt = ptSeg + dS * vtSeg ; Vector3d vtOI = ( ptInt - ptDiscOrig) ; 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& ptSeg, const Point3d& ptCylOrig, const Vector3d& vtSeg, const Vector3d& vtCylAx, const Vector3d& vtMove, double dSegLen, double dCylHei, double dCylRad) { // 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( ptSeg, ptCylOrig, vtSeg, vtCylAx, vtMove, dSegLen, dCylRad) ; double dBottomLeakDist = DiskSegmentLeakDistOrtMot( ptSeg, ptCylOrig - dCylHei * vtCylAx, vtSeg, vtCylAx, vtMove, dSegLen, dCylRad) ; double dSurfLeakDist = - DBL_MAX ; 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 Triangle3d& trTria, const Point3d& ptDiscOrig, const Vector3d& vtDiscAx, const Vector3d& vtMotion, double dCylRad, Point3d& ptContact) { Vector3d vtLine = trTria.GetN() ^ vtDiscAx ; if ( vtLine.Normalize() && abs( vtMotion * vtLine) < 1 - EPS_ZERO) { // Definisco l'equazione DBLVECTOR vdCoef( 3) ; DBLVECTOR vdRoots ; double dP = ( trTria.GetP( 0) - ptDiscOrig) * trTria.GetN() / ( vtMotion * trTria.GetN()) ; Point3d ptLine = ptDiscOrig + dP * vtMotion ; Vector3d vtMotOrt = vtMotion - vtMotion * vtLine * vtLine ; Vector3d vtDist = ptLine - ptDiscOrig ; Vector3d vtDistOrt = vtDist - vtDist * vtLine * vtLine ; // Setto i coefficienti dell'equazione vdCoef[0] = vtDistOrt.SqLen() - dCylRad * dCylRad ; vdCoef[1] = - 2 * vtMotOrt * vtDistOrt ; vdCoef[2] = vtMotOrt.SqLen() ; // Risolvo l'equazione int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ; // Studio le soluzioni double dLeakDist = - DBL_MAX ; for ( int nS = 0 ; nS < nRoot ; ++ nS) { if ( dLeakDist < vdRoots[nS]) { double dLinePar = vdRoots[nS] * vtMotion * vtLine - vtDist * vtLine ; ptContact = ptLine + dLinePar * vtLine ; dLeakDist = vdRoots[nS] ; } } return max( dLeakDist, 0.) ; } return 0. ; } // 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 ; } //---------------------------------------------------------------------------- // 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 ; }