diff --git a/CAvToolTriangle.cpp b/CAvToolTriangle.cpp index 22936ed..7bb6afb 100644 --- a/CAvToolTriangle.cpp +++ b/CAvToolTriangle.cpp @@ -1,13 +1,13 @@ //---------------------------------------------------------------------------- -// EgalTech 2018-2018 +// EgalTech 2018-2020 //---------------------------------------------------------------------------- -// File : CAvToolTriangle.cpp Data : 03.12.18 Versione : 1.9l1 +// File : CAvToolTriangle.cpp Data : 03.12.18 Versione : 2.2k1 // Contenuto : Implementazione delle funzioni ToolTriangleCollisionAvoid. // // // // Modifiche : 10.03.18 LM Creazione modulo. -// +// 28.10.20 LM Funzioni ausiliarie spostate nel module CDeUtility.cpp. // //---------------------------------------------------------------------------- @@ -18,6 +18,7 @@ #include "IntersLineSurfStd.h" #include "IntersLineTria.h" #include "DistPointLine.h" +#include "CDeUtility.h" #include "/EgtDev/Include/EGkIntervals.h" #include "/EgtDev/Include/ENkPolynomialRoots.h" #include "/EgtDev/Include/EgtNumUtils.h" @@ -134,26 +135,6 @@ static double DiskSegmentEscapeDistGenMot( double dDiskRad, const Point3d& ptSeg const Vector3d& vtMove) ; static double DiskTriaInteriorEscapeDistGenMot( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad, const Triangle3d& trTria, const Vector3d& vtMove) ; -// Funzioni ausiliarie -static double GetPointLineSqDist( const Point3d& ptP, const Point3d& ptLine, const Vector3d& vtLine) ; -static double PointPlaneSignedDist( const Point3d& ptP, const Point3d& ptPlane, const Vector3d& vtNorm) ; -static double ThreePointPlaneSignedDist( const Triangle3d& trTria, const Point3d& ptPlane, const Vector3d& vtNorm) ; -static int LinePlaneDDistPar( const Point3d& ptPlane, const Vector3d& vtPlane, const Point3d& ptLine, const Vector3d& vtLine, - double dDist, double& dPar) ; -static double LineLineSqDist( const Point3d& ptP1, const Vector3d& vtD1, const Point3d& ptP2, const Vector3d& vtD2) ; -static double LineSegmentSqDist( const Point3d& ptPLn, const Vector3d& vtDLn, - const Point3d& ptPSg, const Vector3d& vtDSg, double dSgLen) ; -static bool CoplanarDiscTriangleInterference( const Point3d& ptCen, double dRad, const Triangle3d& trTria, int nTriType) ; -static bool FindLineLineMinDistPar( const Point3d& ptL1, const Vector3d& vtV1, - const Point3d& ptL2, const Vector3d& vtV2, - double& dU1, double& dU2) ; -static int SphereLineTangentPoints( const Point3d& ptSpheCen, double dSpheRad, - const Point3d& ptSeg, const Vector3d& vtSegDir, double dSegLen, - const Vector3d& vtMove, double& dU1, double& dU2) ; -static int DiskPlaneLastContactLongMot( const Point3d& ptDiskCen, double dDiskRad, - const Point3d& ptPlane, const Vector3d& vtPlane, - const Vector3d& vtMove, Point3d& ptTouch) ; -static bool ClampSegmentOutPlane( const Plane3d& plPlane, Point3d& ptSegP, const Vector3d& vtSegV, double& dSegLen, double dSignTol = 0) ; //---------------------------------------------------------------------------- // La funzione determina la minima distanza di allontanamento lungo una direzione fissata @@ -2732,338 +2713,3 @@ DiskTriaInteriorEscapeDistGenMot( const Point3d& ptDiskCen, const Vector3d& vtDi dEscapeDist = max( dEscapeDist, dDist) ; return dEscapeDist ; } - -// 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 ; -} - -//---------------------------------------------------------------------------- -// Calcola, se esiste, il punto di una retta a distanza con segno data da un piano. -// Il piano è descritto da punto e versore normale. -// La retta è descritta da punto e versore direzione. -// I casi possibili sono: -// - Nessun punto della retta 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 della retta 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 della retta 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 sono definite da un punto e dal versore direzione. -double -LineLineSqDist( const Point3d& ptP1, const Vector3d& vtD1, - const Point3d& ptP2, const Vector3d& vtD2) -{ - // Se 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 è definita da un punto e dal versore direzione. -// Il segmento è definito da un estremo, dal versore direzione verso l'altro estremo e dalla lunghezza. -double -LineSegmentSqDist( const Point3d& ptPLn, const Vector3d& vtDLn, - const Point3d& ptPSg, const Vector3d& vtDSg, double dSgLen) -{ - // Se paralleli - 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) ; - } - } -} - -//---------------------------------------------------------------------------- -// Valuta l'interferenza di un disco e un triangolo complanari. -// Si assume che disco e piano siano paralleli, ovvero che valga a n = 1 con -// a versore dell'asse del disco e n versore normale al triangolo. -// L'informazione su n è contenuta nell'oggetto triangolo; quella su a non è passata -// in quanto inutile sotto le nostre assunzioni. -bool -CoplanarDiscTriangleInterference( const Point3d& ptCen, double dRad, const Triangle3d& trTria, int nTriType) -{ - // 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, TriangleType::EXACT)) - return true ; - - double dSqMinDist = 0 ; - switch ( nTriType) { - case OPEN : - dSqMinDist = dRad * dRad - 2 * dRad * EPS_SMALL ; - break ; - case EXACT : - dSqMinDist = dRad * dRad ; - break ; - case CLOSED : - dSqMinDist = dRad * dRad + 2 * dRad * EPS_SMALL ; - break ; - } - - // Se il centro non si avvicina più del raggio dalle rette associate ai segmenti, non c'è interferenza - double dSegPointDist0, dSegPointDist1, dSegPointDist2 ; - ( DistPointLine( ptCen, trTria.GetP( 0), trTria.GetP( 1))).GetSqDist( dSegPointDist0) ; - ( DistPointLine( ptCen, trTria.GetP( 1), trTria.GetP( 2))).GetSqDist( dSegPointDist1) ; - ( DistPointLine( ptCen, trTria.GetP( 2), trTria.GetP( 0))).GetSqDist( dSegPointDist2) ; - if ( dSegPointDist0 > dSqMinDist && dSegPointDist1 > dSqMinDist && dSegPointDist2 > dSqMinDist) - return false ; - // interferenza - return true ; -} - -//---------------------------------------------------------------------------- -// 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 -FindLineLineMinDistPar( const Point3d& ptL1, const Vector3d& vtV1, - const Point3d& ptL2, const Vector3d& vtV2, - double& dU1, double& dU2) -{ - // Se le rette sono parallele - if ( AreSameOrOppositeVectorExact( vtV1, vtV2)) - 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 ; -} - -//---------------------------------------------------------------------------- -// Valuta l'allontanamento di un disco, lungo il proprio asse di simmetria, da un piano. -// Il disco è descritto dal centro nella posizione iniziale e dal raggio (la normale è il versore traslazione). -// Il piano è descritto da un suo punto e dal suo versore normale. -// Il moto è descritto dal versore di traslazione. -// Il valore restituito è un intero che descrive la situazione di allontanamento e, -// quando sensato, per referenza restituisce il punto di ultimo contatto. -// 0: Nessun contatto -// 1: Un contatto -// 2: Disco e piano ortogonali, con disco tg al piano, il contatto è una semiretta -// 3: Disco e piano ortogonali, con disco secante il piano, il contatto è una striscia -// 4: Disco e piano paralleli, come ultimo contatto si restituisce il centro. -// Nel caso 0 il parametro ptTouch non ha senso, così come nel caso 2 e nel caso 3 -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 4 ; - } - return 0 ; - } - // Vettore radiale - Vector3d vtRadLine = vtMove * ( vtPlane * vtMove) - vtPlane ; - // Disco e piano ortogonali - if ( vtRadLine.IsNormalized()) { - 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 - // 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 ; -} - -//---------------------------------------------------------------------------- -// Si limita il segmento alla parte di spazio con distanza positiva dal piano. -// Se segmento ancora valido si restituisce true, altrimenti false. -bool -ClampSegmentOutPlane( const Plane3d& plPlane, Point3d& ptSegP, const Vector3d& vtSegV, double& dSegLen, double dSignTol) -{ - // Distanze degli estremi del segmento dal piano - double dDistStart = DistPointPlane( ptSegP, plPlane) ; - double dDistEnd = DistPointPlane( ptSegP + dSegLen * vtSegV, plPlane) ; - // Se il segmento sta nel piano o è tutto esterno, va bene - if ( dDistStart > dSignTol - EPS_ZERO && dDistEnd > dSignTol - EPS_ZERO) - return true ; - // Se il segmento è tutto interno, va eliminato - if ( dDistStart < dSignTol && dDistEnd < dSignTol) { - dSegLen = 0 ; - return false ; - } - // Altrimenti il segmento attraversa il piano e va limitato - double dIntersLen = dSegLen * abs( dDistStart) / ( abs( dDistStart) + abs( dDistEnd)) ; - if ( dDistStart > 0) - dSegLen = dIntersLen ; - else { - ptSegP += dIntersLen * vtSegV ; - dSegLen -= dIntersLen ; - } - return true ; -} diff --git a/CDeConeTria.cpp b/CDeConeTria.cpp new file mode 100644 index 0000000..d8062d5 --- /dev/null +++ b/CDeConeTria.cpp @@ -0,0 +1,147 @@ +//---------------------------------------------------------------------------- +// EgalTech 2020-2020 +//---------------------------------------------------------------------------- +// File : CDeConTria.cpp Data : 27.10.20 Versione : 2.2k1 +// Contenuto : Implementazione della verifica di collisione tra +// Cono e Triangle3d. +// +// +// Modifiche : 27.10.20 LM Creazione modulo. +// +// +//---------------------------------------------------------------------------- + +//--------------------------- Include ---------------------------------------- +#include "stdafx.h" +#include "IntersLineSurfStd.h" +#include "CDeUtility.h" +#include "EgtDev/Include/EGkCDeConeTria.h" +#include "EgtDev/Include/EGkCDeCylTria.h" +#include "EgtDev/Include/EGkPlane3d.h" +#include "EgtDev/Include/EGkIntersLineTria.h" +#include "EgtDev/Include/EGkIntersPlanePlane.h" +#include "EgtDev/Include/EgtNumUtils.h" + +using namespace std ; + +//---------------------------------------------------------------------------- +// Il sistema di riferimento deve avere l'asse di simmetria del cono come asse Z e origine nel centro della base. +// La distanza di sicurezza ha effetto solo se maggiore di epsilon, altrimenti è ignorata ed è ininfluente. +bool +CDeConeTria( const Frame3d& frCone, double dBaseRad, double dTopRad, double dHeight, double dSafeDist, const Triangle3d& trTria) +{ + // Se è un cilindro chiamo la routine apposita. + if ( abs( dBaseRad - dTopRad) < EPS_SMALL) + return CDeCylTria( frCone, dHeight, max( dBaseRad, dTopRad), dSafeDist, trTria) ; + + // Se il tronco di cono o il trinagolo non è ben definito non procedo + if ( dBaseRad < EPS_SMALL || dTopRad < EPS_SMALL || dHeight < EPS_SMALL || ! trTria.IsValid()) + return false ; + + // Se il raggio di base è maggiore del raggio top cambio il sistema di riferimento. + double dMinRad = dBaseRad ; + double dMaxRad = dTopRad ; + Frame3d frMyFrame = frCone ; + if ( dBaseRad > dTopRad) { + swap( dMinRad, dMaxRad) ; + frMyFrame.Translate( dHeight * frMyFrame.VersZ()) ; + frMyFrame.Rotate( frMyFrame.Orig(), frMyFrame.VersX(), 180.) ; + } + + // Se la distanza di sicurezza è maggiore di epsilon aumento le dimensioni del tronco di cono. + if ( dSafeDist > EPS_SMALL) { + dBaseRad += dSafeDist ; + dTopRad += dSafeDist ; + dHeight += ( 2 * dSafeDist) ; + frMyFrame.Translate( - dSafeDist * frMyFrame.VersZ()) ; + } + + // Porto il triangolo nel sistema di riferimento del tronco di cono + Triangle3d trMyTria = trTria ; + trMyTria.ToLoc( frMyFrame) ; + + // Se almeno un vertice collide ho finito + for ( int nV = 0 ; nV < 3 ; ++ nV) { + Point3d ptVert = trMyTria.GetP( nV) ; + double dLenZ = ptVert.z ; + double dLenXY = sqrt( ptVert.x * ptVert.x + ptVert.y * ptVert.y) ; + double dRadAtHeight = Clamp( dMinRad + ( dMaxRad - dMinRad) * dLenZ / dHeight, dMinRad, dMaxRad) ; + if ( dLenZ > - EPS_SMALL && dLenZ < dHeight + EPS_SMALL && dLenXY < dRadAtHeight) + return true ; + } + + // Se almeno un segmento collide ho finito + for ( int nS = 0 ; nS < 3 ; ++ nS) { + Point3d ptSegSt = trMyTria.GetP( nS) ; + Point3d ptSegEn = trMyTria.GetP( ( nS + 1) % 3) ; + Vector3d vtDir = ptSegEn - ptSegSt ; + double dSegLen = vtDir.Len() ; + vtDir /= dSegLen ; + double dU1, dU2 ; + // Collisione con la superficie laterale + int nIndex = SegmentTruncatedCone( ptSegSt, vtDir, dSegLen, ORIG, Z_AX, dMinRad, dMaxRad, dHeight, dU1, dU2) ; + if ( nIndex != CC_ERROR_INT && nIndex != CC_NO_INTERS) + return true ; + // Collisione con le basi + nIndex = SegmentDisc( ptSegSt, vtDir, dSegLen, ORIG, -Z_AX, dMinRad, dU1, dU2) ; + if ( nIndex != D_ERROR_INT && nIndex != D_NO_INTERS) + return true ; + nIndex = SegmentDisc( ptSegSt, vtDir, dSegLen, Point3d( 0., 0., dHeight), Z_AX, dMaxRad, dU1, dU2) ; + if ( nIndex != D_ERROR_INT && nIndex != D_NO_INTERS) + return true ; + } + + // Contatti con l'interno del triangolo + Point3d ptP = trMyTria.GetP( 0) ; + Vector3d vtN = trMyTria.GetN() ; + // Se il segmento congiungente i centri delle basi interseca il triangolo ho finito + if ( abs( vtN.z) > EPS_ZERO) { + Point3d ptInt( 0., 0., ( ( ptP - ORIG) * vtN) / vtN.z) ; + if ( ptInt.z > - EPS_SMALL && ptInt.z < dHeight + EPS_SMALL && IsPointInsideTriangle( ptInt, trMyTria, TriangleType::CLOSED)) + return true ; + } + // Intersezione basi / interno triangolo + Point3d ptLine ; + Vector3d vtLine ; + Plane3d plPlaneTria ; + plPlaneTria.Set( ptP, vtN) ; + // Base minore + Plane3d plMinBase ; + plMinBase.Set( ORIG, - Z_AX) ; + int nMinBaseTriaIndex = IntersPlanePlane( plPlaneTria, plMinBase, ptLine, vtLine) ; + if ( nMinBaseTriaIndex == IPPT_OVERLAPS) { + if ( CoplanarDiscTriangleInterference( plMinBase.GetPoint(), dMinRad, trMyTria, TriangleType::CLOSED)) + return true ; + } + else if ( nMinBaseTriaIndex == IPPT_YES) { + double dU1, dU2 ; + if ( LineDisc( ptLine, vtLine, ORIG, -Z_AX, dMinRad, dU1, dU2) == D_INFINITE_INT_LINE_ON_PLANE) { + int nLineTriaIndex = IntersLineTria( ptLine + dU1 * vtLine, ptLine + dU2 * vtLine, trMyTria, Point3d(), Point3d()) ; + if ( nLineTriaIndex != ILTT_NO && nLineTriaIndex != ILTT_IN) + return true ; + } + } + // Base maggiore + Plane3d plMaxBase ; + plMaxBase.Set( Point3d( 0., 0., dHeight), Z_AX) ; + int nMaxBaseTriaIndex = IntersPlanePlane( plPlaneTria, plMaxBase, ptLine, vtLine) ; + if ( nMaxBaseTriaIndex == IPPT_OVERLAPS) { + if ( CoplanarDiscTriangleInterference( plMaxBase.GetPoint(), dMaxRad, trMyTria, TriangleType::CLOSED)) + return true ; + } + else if ( nMinBaseTriaIndex == IPPT_YES) { + double dU1, dU2 ; + if ( LineDisc( ptLine, vtLine, ORIG, -Z_AX, dMaxRad, dU1, dU2) == D_INFINITE_INT_LINE_ON_PLANE) { + int nLineTriaIndex = IntersLineTria( ptLine + dU1 * vtLine, ptLine + dU2 * vtLine, trMyTria, Point3d(), Point3d()) ; + if ( nLineTriaIndex != ILTT_NO && nLineTriaIndex != ILTT_IN) + return true ; + } + } + // Tronco di cono tangente al triangolo + Vector3d vtTriaNormXY( vtN.x, vtN.y, 0.) ; + vtTriaNormXY.Normalize() ; + Point3d ptContactSt( dMinRad * vtTriaNormXY.x, dMinRad * vtTriaNormXY.y, 0.) ; + Point3d ptContactEn( dMaxRad * vtTriaNormXY.x, dMaxRad * vtTriaNormXY.y, 0.) ; + int nContactIndex = IntersLineTria( ptContactSt, ptContactEn, trMyTria, Point3d(), Point3d()) ; + return ! ( nContactIndex == ILTT_NO || nContactIndex == ILTT_IN) ; +} diff --git a/CDePyramidTria.cpp b/CDePyramidTria.cpp new file mode 100644 index 0000000..509627b --- /dev/null +++ b/CDePyramidTria.cpp @@ -0,0 +1,137 @@ +//---------------------------------------------------------------------------- +// EgalTech 2020-2020 +//---------------------------------------------------------------------------- +// File : CDePyramidTria.cpp Data : 29.10.20 Versione : 2.2k1 +// Contenuto : Implementazione della verifica di collisione tra +// Pyramid e Triangle3d. +// +// +// Modifiche : 29.10.20 LM Creazione modulo. +// +// +//---------------------------------------------------------------------------- + +//--------------------------- Include ---------------------------------------- +#include "stdafx.h" +#include "EgtDev/Include/EGkCDePyramidTria.h" +#include "EgtDev/Include/EGkIntersTriaTria.h" + +//---------------------------------------------------------------------------- +// Il sistema di riferimento deve avere l'origine nel centro della base Base, asse X lungo +// un segmento della stessa e asse Z ortogonale alle basi e diretta verso la base Top. +bool +CDePyramidTria( const Frame3d& frPyrFrame, double dLenghtBaseX, double dLenghtBaseY, double dLenghtTopX, double dLenghtTopY, double dHeight, + double dSafeDist, const Triangle3d& trTria) +{ + // Se il tronco di piramide o il triangolo non sono ben definiti non procedo + if ( dLenghtBaseX < EPS_SMALL || dLenghtBaseY < EPS_SMALL || + dLenghtTopX < EPS_SMALL || dLenghtTopY < EPS_SMALL || + dHeight < EPS_SMALL || ! trTria.IsValid()) + return false ; + + Frame3d frMyFrame = frPyrFrame ; + // Se la distanza di sicurezza è maggiore di epsilon aumento le dimensioni del tronco di piramide. + if ( dSafeDist > EPS_SMALL) { + dLenghtBaseX += dSafeDist ; + dLenghtBaseY += dSafeDist ; + dLenghtTopX += dSafeDist ; + dLenghtTopY += dSafeDist ; + dHeight += ( 2 * dSafeDist) ; + frMyFrame.Translate( - dSafeDist * frMyFrame.VersZ()) ; + } + + // Porto il triangolo nel sistema di riferimento del tronco di cono + Triangle3d trMyTria = trTria ; + trMyTria.ToLoc( frMyFrame) ; + + // Se almeno un vertice collide ho finito + for ( int nV = 0 ; nV < 3 ; ++ nV) { + Point3d ptVert = trMyTria.GetP( nV) ; + if ( ptVert.z > - EPS_SMALL && ptVert.z < dHeight + EPS_SMALL && + ( ptVert.x + dLenghtBaseX + EPS_SMALL) * dHeight > ( dLenghtBaseX - dLenghtTopX) * ptVert.z && + ( ptVert.x - dLenghtBaseX - EPS_SMALL) * dHeight < ( dLenghtTopX - dLenghtBaseX) * ptVert.z && + ( ptVert.y + dLenghtBaseY + EPS_SMALL) * dHeight > ( dLenghtBaseY - dLenghtTopY) * ptVert.z && + ( ptVert.y - dLenghtBaseY - EPS_SMALL) * dHeight < ( dLenghtTopY - dLenghtBaseY) * ptVert.z) + return true ; + } + + // Se c'è collisione con almeno un triangolo delle facce ho finito + double dHalfBaseX = 0.5 * dLenghtBaseX ; + double dHalfBaseY = 0.5 * dLenghtBaseY ; + double dHalfTopX = 0.5 * dLenghtTopX ; + double dHalfTopY = 0.5 * dLenghtTopY ; + Triangle3d trFaceTria1, trFaceTria2 ; + // Faccia base + trFaceTria1.Set( Point3d( - dHalfBaseX, - dHalfBaseY, 0.), + Point3d( - dHalfBaseX, dHalfBaseY, 0.), + Point3d( dHalfBaseX, dHalfBaseY, 0.)) ; + trFaceTria1.Validate() ; + trFaceTria2.Set( Point3d( - dHalfBaseX, - dHalfBaseY, 0.), + Point3d( dHalfBaseX, dHalfBaseY, 0.), + Point3d( dHalfBaseX, - dHalfBaseY, 0.)) ; + trFaceTria2.Validate() ; + if ( IntersTriaTria( trMyTria, trFaceTria1, Point3d(), Point3d(), TRIA3DVECTOR()) != ITTT_NO || + IntersTriaTria( trMyTria, trFaceTria2, Point3d(), Point3d(), TRIA3DVECTOR()) != ITTT_NO) + return true ; + // Faccia top + trFaceTria1.Set( Point3d( - dHalfTopX, - dHalfTopY, dHeight), + Point3d( dHalfTopX, dHalfTopY, dHeight), + Point3d( - dHalfTopX, dHalfTopY, dHeight)) ; + trFaceTria1.Validate() ; + trFaceTria2.Set( Point3d( - dHalfTopX, - dHalfTopY, dHeight), + Point3d( dHalfTopX, - dHalfTopY, dHeight), + Point3d( dHalfTopX, dHalfTopY, dHeight)) ; + trFaceTria2.Validate() ; + if ( IntersTriaTria( trMyTria, trFaceTria1, Point3d(), Point3d(), TRIA3DVECTOR()) != ITTT_NO || + IntersTriaTria( trMyTria, trFaceTria2, Point3d(), Point3d(), TRIA3DVECTOR()) != ITTT_NO) + return true ; + // Faccia laterale 1 + trFaceTria1.Set( Point3d( - dHalfBaseX, - dHalfBaseY, 0.), + Point3d( dHalfTopX , - dHalfTopY , dHeight), + Point3d( - dHalfTopX , - dHalfTopY , dHeight)) ; + trFaceTria1.Validate() ; + trFaceTria2.Set( Point3d( - dHalfBaseX, - dHalfBaseY, 0.), + Point3d( dHalfBaseX, - dHalfBaseY, 0.), + Point3d( dHalfTopX, - dHalfTopY , dHeight)) ; + trFaceTria2.Validate() ; + if ( IntersTriaTria( trMyTria, trFaceTria1, Point3d(), Point3d(), TRIA3DVECTOR()) != ITTT_NO || + IntersTriaTria( trMyTria, trFaceTria2, Point3d(), Point3d(), TRIA3DVECTOR()) != ITTT_NO) + return true ; + // Faccia laterale 2 + trFaceTria1.Set( Point3d( dHalfBaseX, - dHalfBaseY, 0.), + Point3d( dHalfTopX , dHalfTopY , dHeight), + Point3d( dHalfTopX , - dHalfTopY , dHeight)) ; + trFaceTria1.Validate() ; + trFaceTria2.Set( Point3d( dHalfBaseX, - dHalfBaseY, 0.), + Point3d( dHalfBaseX, dHalfBaseY, 0.), + Point3d( dHalfTopX , dHalfTopY , dHeight)) ; + trFaceTria2.Validate() ; + if ( IntersTriaTria(trMyTria, trFaceTria1, Point3d(), Point3d(), TRIA3DVECTOR()) != ITTT_NO || + IntersTriaTria(trMyTria, trFaceTria2, Point3d(), Point3d(), TRIA3DVECTOR()) != ITTT_NO) + return true ; + // Faccia laterale 3 + trFaceTria1.Set( Point3d( dHalfBaseX, dHalfBaseY, 0.), + Point3d( - dHalfTopX , dHalfTopY , dHeight), + Point3d( dHalfTopX , dHalfTopY , dHeight)) ; + trFaceTria1.Validate() ; + trFaceTria2.Set( Point3d( dHalfBaseX, dHalfBaseY, 0.), + Point3d( - dHalfBaseX, dHalfBaseY, 0.), + Point3d( - dHalfTopX , dHalfTopY , dHeight)) ; + trFaceTria2.Validate() ; + if ( IntersTriaTria( trMyTria, trFaceTria1, Point3d(), Point3d(), TRIA3DVECTOR()) != ITTT_NO || + IntersTriaTria( trMyTria, trFaceTria2, Point3d(), Point3d(), TRIA3DVECTOR()) != ITTT_NO) + return true ; + // Faccia laterale 4 + trFaceTria1.Set( Point3d( - dHalfBaseX, dHalfBaseY, 0.), + Point3d( - dHalfTopX , - dHalfTopY , dHeight), + Point3d( - dHalfTopX , dHalfTopY , dHeight)) ; + trFaceTria1.Validate() ; + trFaceTria2.Set( Point3d( - dHalfBaseX, dHalfBaseY, 0.), + Point3d( - dHalfBaseX, - dHalfBaseY, 0.), + Point3d( - dHalfTopX , - dHalfTopY , dHeight)) ; + trFaceTria2.Validate() ; + if ( IntersTriaTria( trMyTria, trFaceTria1, Point3d(), Point3d(), TRIA3DVECTOR()) != ITTT_NO || + IntersTriaTria( trMyTria, trFaceTria2, Point3d(), Point3d(), TRIA3DVECTOR()) != ITTT_NO) + return true ; + return false ; +} diff --git a/CDeTriaTria.cpp b/CDeTriaTria.cpp new file mode 100644 index 0000000..cba7dce --- /dev/null +++ b/CDeTriaTria.cpp @@ -0,0 +1,185 @@ +//---------------------------------------------------------------------------- +// EgalTech 2020-2020 +//---------------------------------------------------------------------------- +// File : CDeTriaTria.cpp Data : 05.11.20 Versione : 2.2k1 +// Contenuto : Implementazione della verifica di collisione tra due Triangle3d. +// +// +// +// Modifiche : 05.11.20 LM Creazione modulo. +// +// +//---------------------------------------------------------------------------- + +#include "stdafx.h" +#include "DistLineLine.h" +#include "EgtDev/Include/EGkCDeTriaTria.h" +#include "EgtDev/Include/EGkIntersPlanePlane.h" +#include +#include + +using namespace std ; + +//---------------------------------------------------------------------------- +void +GetTwoGreater( int nVal0, int nVal1, int nVal2, int& nFirstMaxPos, int& nSecondMaxPos) +{ + if ( nVal0 > nVal1) { + if ( nVal0 > nVal2) { + nFirstMaxPos = 0 ; + if ( nVal1 > nVal2) + nSecondMaxPos = 1 ; + else + nSecondMaxPos = 2 ; + } + else { + nFirstMaxPos = 2 ; + nSecondMaxPos = 1 ; + } + } + else if ( nVal0 > nVal2) { + nFirstMaxPos = 1 ; + nSecondMaxPos = 0 ; + } + else if ( nVal1 > nVal2) { + nFirstMaxPos = 1 ; + nSecondMaxPos = 2 ; + } + else { + nFirstMaxPos = 2 ; + nSecondMaxPos = 1 ; + } +} + +//---------------------------------------------------------------------------- +bool +CDeTriaTria( const Triangle3d& trTriaA, const Triangle3d& trTriaB) +{ + // Piano del triangolo A + Plane3d plPlaneTriaA ; + plPlaneTriaA.Set( trTriaA.GetCentroid(), trTriaA.GetN()) ; + // Calcolo le distanze dei vertici del triangolo B dal piano del triangolo A + array< double, 3> vDistBA ; + for ( int i = 0 ; i < 3 ; ++ i) + vDistBA[i] = DistPointPlane( trTriaB.GetP( i), plPlaneTriaA) ; + // Verifico posizione del triangolo B rispetto al piano del triangolo A + int nVertPosB = 0 ; + int nVertNegB = 0 ; + for ( const auto& dDist : vDistBA) { + if ( dDist > EPS_SMALL) + ++ nVertPosB ; + else if ( dDist < - EPS_SMALL) + ++ nVertNegB ; + } + // Se il triangolo giace tutto da una parte del piano, nessuna intersezione + if ( nVertPosB == 3 || nVertNegB == 3) + return false ; + // Piano del triangolo B + Plane3d plPlaneTriB ; + plPlaneTriB.Set( trTriaB.GetCentroid(), trTriaB.GetN()) ; + // Calcolo le distanze dei vertici del triangolo A dal piano del triangolo B + array< double, 3> vDistAB ; + for ( int i = 0 ; i < 3 ; ++ i) + vDistAB[i] = DistPointPlane( trTriaA.GetP(i) , plPlaneTriB) ; + // Verifico posizione del triangolo A rispetto al piano del triangolo B + int nVertPosA = 0 ; + int nVertNegA = 0 ; + for ( const auto& dDist : vDistAB) { + if ( dDist > EPS_SMALL) + ++ nVertPosA ; + else if ( dDist < - EPS_SMALL) + ++ nVertNegA ; + } + // Se il triangolo giace tutto da una parte del piano, nessuna intersezione + if ( nVertPosA == 3 || nVertNegA == 3) + return false ; + // Interseco i piani + Point3d ptLineP ; + Vector3d vtLineV ; + int nResPP = IntersPlanePlane( plPlaneTriaA, plPlaneTriB, ptLineP, vtLineV) ; + if ( nResPP == IPPT_NO) + return false ; + else if ( nResPP == IPPT_OVERLAPS) { + for ( int nA = 0 ; nA < 3 ; ++ nA) { + Point3d ptStA = trTriaA.GetP( nA) ; + Point3d PtEnA = trTriaA.GetP( ( nA + 1) % 3); + for ( int nB = 0 ; nB < 3 ; ++ nB) { + Point3d ptStB = trTriaB.GetP( nB) ; + Point3d PtEnB = trTriaB.GetP( ( nB + 1) % 3) ; + DistLineLine LineLineDistCalc( ptStA, PtEnA, ptStB, PtEnB) ; + double dSqSegSegDist ; + LineLineDistCalc.GetSqDist( dSqSegSegDist) ; + if ( dSqSegSegDist < EPS_SMALL * EPS_SMALL) + return true ; + } + } + return ( IsPointInsideTriangle( trTriaA.GetP( 0), trTriaB, TriangleType::CLOSED) || + IsPointInsideTriangle( trTriaA.GetP( 1), trTriaB, TriangleType::CLOSED) || + IsPointInsideTriangle( trTriaA.GetP( 2), trTriaB, TriangleType::CLOSED) || + IsPointInsideTriangle( trTriaB.GetP( 0), trTriaA, TriangleType::CLOSED) || + IsPointInsideTriangle( trTriaB.GetP( 1), trTriaA, TriangleType::CLOSED) || + IsPointInsideTriangle( trTriaB.GetP( 2), trTriaA, TriangleType::CLOSED)) ; + } + // Interseco la retta col triangolo A + // Cerco i due segmenti del triangolo A che limitano la retta di intersezione fra i piani + int nSegExtrDistA[3] = { -1, -1, -1 } ; + for ( int nV = 0 ; nV < 3 ; ++ nV) { + int nStateSt = ( vDistAB[nV] > EPS_SMALL ? 1 : ( vDistAB[nV] > - EPS_SMALL ? 0 : - 1)) ; + int nStateEn = ( vDistAB[( nV + 1) % 3] > EPS_SMALL ? 1 : ( vDistAB[( nV + 1) % 3] > - EPS_SMALL ? 0 : - 1)) ; + nSegExtrDistA[nV] = abs( nStateSt - nStateEn) ; + } + int nFirstMaxPosA, nSecondMaxPosA ; + GetTwoGreater( nSegExtrDistA[0], nSegExtrDistA[1], nSegExtrDistA[2], nFirstMaxPosA, nSecondMaxPosA) ; + // Limito la retta col primo segmento trovato di A + Vector3d vtSegFirstA = trTriaA.GetP( ( nFirstMaxPosA + 1) % 3) - trTriaA.GetP( nFirstMaxPosA) ; + double dSegLenFirstA = vtSegFirstA.Len() ; + vtSegFirstA /= dSegLenFirstA ; + DistLineLine LineLineDistCalcFirstA( ptLineP, vtLineV, 100., trTriaA.GetP( nFirstMaxPosA), vtSegFirstA, dSegLenFirstA, false) ; + double dIntParStA, dOtherParFirstA ; + LineLineDistCalcFirstA.GetParamsAtMinDistPoints( dIntParStA, dOtherParFirstA) ; + // Limito la retta col secondo segmento trovato di A + Vector3d vtSegSecondA = trTriaA.GetP( ( nSecondMaxPosA + 1) % 3) - trTriaA.GetP( nSecondMaxPosA) ; + double dSegLenSecondA = vtSegSecondA.Len() ; + vtSegSecondA /= dSegLenSecondA ; + DistLineLine LineLineDistCalcSecondA( ptLineP, vtLineV, 100., trTriaA.GetP( nSecondMaxPosA), vtSegSecondA, dSegLenSecondA, false) ; + double dIntParEnA, dOtherParSecondA ; + LineLineDistCalcSecondA.GetParamsAtMinDistPoints( dIntParEnA, dOtherParSecondA) ; + // Ordino i parametri lungo la retta di intersezione fra i piani + if ( dIntParStA > dIntParEnA) { + swap( dIntParStA, dIntParEnA) ; + } + // Interseco la retta col triangolo B + int nSegExtrDistB[3] = { -1, -1, -1 } ; + for (int nV = 0; nV < 3; ++nV) { + int nStateSt = ( vDistBA[nV] > EPS_SMALL ? 1 : ( vDistBA[nV] > - EPS_SMALL ? 0 : - 1)) ; + int nStateEn = ( vDistBA[( nV + 1) % 3] > EPS_SMALL ? 1 : ( vDistBA[( nV + 1) % 3] > - EPS_SMALL ? 0 : - 1)) ; + nSegExtrDistB[nV] = abs( nStateSt - nStateEn) ; + } + int nFirstMaxPosB, nSecondMaxPosB ; + GetTwoGreater( nSegExtrDistB[0], nSegExtrDistB[1], nSegExtrDistB[2], nFirstMaxPosB, nSecondMaxPosB) ; + // Limito la retta col primo segmento trovato di B + Vector3d vtSegFirstB = trTriaB.GetP( ( nFirstMaxPosB + 1) % 3) - trTriaB.GetP( nFirstMaxPosB) ; + double dSegLenFirstB = vtSegFirstB.Len() ; + vtSegFirstB /= dSegLenFirstB ; + DistLineLine LineLineDistCalcFirstB( ptLineP, vtLineV, 100., trTriaB.GetP( nFirstMaxPosB), vtSegFirstB, dSegLenFirstB, false) ; + double dIntParStB, dOtherParFirstB ; + LineLineDistCalcFirstB.GetParamsAtMinDistPoints( dIntParStB, dOtherParFirstB) ; + // Limito la retta col secondo segmento trovato di B + Vector3d vtSegSecondB = trTriaB.GetP( ( nSecondMaxPosB + 1) % 3) - trTriaB.GetP( nSecondMaxPosB) ; + double dSegLenSecondB = vtSegSecondB.Len() ; + vtSegSecondB /= dSegLenSecondB ; + DistLineLine LineLineDistCalcSecondB( ptLineP, vtLineV, 100., trTriaB.GetP( nSecondMaxPosA), vtSegSecondB, dSegLenSecondB, false) ; + double dIntParEnB, dOtherParSecondB ; + LineLineDistCalcSecondB.GetParamsAtMinDistPoints( dIntParEnB, dOtherParSecondB) ; + // Ordino i parametri lungo la retta di intersezione fra i piani + if ( dIntParStB > dIntParEnB) { + swap( dIntParStB, dIntParEnB) ; + } + // Se gli intervalli si sovrappongono c'è collisione, altrimenti non c'è + if ( dIntParEnB < dIntParStA - EPS_SMALL) + return false ; + else if ( dIntParStB < dIntParEnA + EPS_SMALL) + return true ; + else + return false ; +} diff --git a/CDeUtility.cpp b/CDeUtility.cpp new file mode 100644 index 0000000..20774a3 --- /dev/null +++ b/CDeUtility.cpp @@ -0,0 +1,337 @@ +#include "stdafx.h" +#include "CDeUtility.h" +#include "DistPointLine.h" +#include "/EgtDev/Include/ENkPolynomialRoots.h" + +using namespace std ; + +//---------------------------------------------------------------------------- +// 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 ; +} + +//---------------------------------------------------------------------------- +// Calcola, se esiste, il punto di una retta a distanza con segno data da un piano. +// Il piano è descritto da punto e versore normale. +// La retta è descritta da punto e versore direzione. +// I casi possibili sono: +// - Nessun punto della retta 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 della retta 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 della retta 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 sono definite da un punto e dal versore direzione. +double +LineLineSqDist( const Point3d& ptP1, const Vector3d& vtD1, const Point3d& ptP2, const Vector3d& vtD2) +{ + // Se 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 è definita da un punto e dal versore direzione. +// Il segmento è definito da un estremo, dal versore direzione verso l'altro estremo e dalla lunghezza. +double +LineSegmentSqDist( const Point3d& ptPLn, const Vector3d& vtDLn, + const Point3d& ptPSg, const Vector3d& vtDSg, double dSgLen) +{ + // Se paralleli + 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) ; + } + } +} +//---------------------------------------------------------------------------- +// Valuta l'interferenza di un disco e un triangolo complanari. +// Si assume che disco e piano siano paralleli, ovvero che valga a n = 1 con +// a versore dell'asse del disco e n versore normale al triangolo. +// L'informazione su n è contenuta nell'oggetto triangolo; quella su a non è passata +// in quanto inutile sotto le nostre assunzioni. +bool +CoplanarDiscTriangleInterference( const Point3d& ptCen, double dRad, const Triangle3d& trTria, int nTriType) +{ + // 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, TriangleType::EXACT)) + return true ; + + double dSqMinDist = 0 ; + switch ( nTriType) { + case OPEN: + dSqMinDist = dRad * dRad - 2 * dRad * EPS_SMALL ; + break ; + case EXACT: + dSqMinDist = dRad * dRad ; + break ; + case CLOSED: + dSqMinDist = dRad * dRad + 2 * dRad * EPS_SMALL ; + break ; + } + + // Se il centro non si avvicina più del raggio dalle rette associate ai segmenti, non c'è interferenza + double dSegPointDist0, dSegPointDist1, dSegPointDist2 ; + ( DistPointLine( ptCen, trTria.GetP( 0), trTria.GetP( 1))).GetSqDist( dSegPointDist0) ; + ( DistPointLine( ptCen, trTria.GetP( 1), trTria.GetP( 2))).GetSqDist( dSegPointDist1) ; + ( DistPointLine( ptCen, trTria.GetP( 2), trTria.GetP( 0))).GetSqDist( dSegPointDist2) ; + if ( dSegPointDist0 > dSqMinDist && dSegPointDist1 > dSqMinDist && dSegPointDist2 > dSqMinDist) + return false ; + // interferenza + return true ; +} + +//---------------------------------------------------------------------------- +// 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 +FindLineLineMinDistPar( const Point3d& ptL1, const Vector3d& vtV1, + const Point3d& ptL2, const Vector3d& vtV2, + double& dU1, double& dU2) +{ + // Se le rette sono parallele + if ( AreSameOrOppositeVectorExact( vtV1, vtV2)) + 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 ; +} + +//---------------------------------------------------------------------------- +// Valuta l'allontanamento di un disco, lungo il proprio asse di simmetria, da un piano. +// Il disco è descritto dal centro nella posizione iniziale e dal raggio (la normale è il versore traslazione). +// Il piano è descritto da un suo punto e dal suo versore normale. +// Il moto è descritto dal versore di traslazione. +// Il valore restituito è un intero che descrive la situazione di allontanamento e, +// quando sensato, per referenza restituisce il punto di ultimo contatto. +// 0: Nessun contatto +// 1: Un contatto +// 2: Disco e piano ortogonali, con disco tg al piano, il contatto è una semiretta +// 3: Disco e piano ortogonali, con disco secante il piano, il contatto è una striscia +// 4: Disco e piano paralleli, come ultimo contatto si restituisce il centro. +// Nel caso 0 il parametro ptTouch non ha senso, così come nel caso 2 e nel caso 3 +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 4 ; + } + return 0 ; + } + // Vettore radiale + Vector3d vtRadLine = vtMove * ( vtPlane * vtMove) - vtPlane ; + // Disco e piano ortogonali + if ( vtRadLine.IsNormalized()) { + 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 + // 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 ; +} + +//---------------------------------------------------------------------------- +// Si limita il segmento alla parte di spazio con distanza positiva dal piano. +// Se segmento ancora valido si restituisce true, altrimenti false. +bool +ClampSegmentOutPlane( const Plane3d& plPlane, Point3d& ptSegP, const Vector3d& vtSegV, double& dSegLen, double dSignTol) +{ + // Distanze degli estremi del segmento dal piano + double dDistStart = DistPointPlane( ptSegP, plPlane) ; + double dDistEnd = DistPointPlane( ptSegP + dSegLen * vtSegV, plPlane) ; + // Se il segmento sta nel piano o è tutto esterno, va bene + if ( dDistStart > dSignTol - EPS_ZERO && dDistEnd > dSignTol - EPS_ZERO) + return true ; + // Se il segmento è tutto interno, va eliminato + if ( dDistStart < dSignTol && dDistEnd < dSignTol) { + dSegLen = 0 ; + return false ; + } + // Altrimenti il segmento attraversa il piano e va limitato + double dIntersLen = dSegLen * abs( dDistStart) / ( abs( dDistStart) + abs( dDistEnd)) ; + if ( dDistStart > 0) + dSegLen = dIntersLen ; + else { + ptSegP += dIntersLen * vtSegV ; + dSegLen -= dIntersLen ; + } + return true ; +} \ No newline at end of file diff --git a/CDeUtility.h b/CDeUtility.h new file mode 100644 index 0000000..ea7cbf4 --- /dev/null +++ b/CDeUtility.h @@ -0,0 +1,124 @@ +//---------------------------------------------------------------------------- +// EgalTech 2020-2020 +//---------------------------------------------------------------------------- +// File : CoplanarDiscTriaInterferance.h Data : 27.10.20 Versione : +// Contenuto : Dichiarazione funzione verifica interferenza tra +// Disco e Triangle3d complanari +// +// Modifiche : 27.10.20 LM Creazione modulo. +// +// +//---------------------------------------------------------------------------- + +#pragma once + +#include "/EgtDev/Include/EGkPoint3d.h" +#include "/EgtDev/Include/EGkTriangle3d.h" + +//---------------------------------------------------------------------------- +// 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) ; + +//---------------------------------------------------------------------------- +// 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) ; + +//---------------------------------------------------------------------------- +// 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) ; + +//---------------------------------------------------------------------------- +// Calcola, se esiste, il punto di una retta a distanza con segno data da un piano. +// Il piano è descritto da punto e versore normale. +// La retta è descritta da punto e versore direzione. +// I casi possibili sono: +// - Nessun punto della retta 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 della retta 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 della retta 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) ; + +//---------------------------------------------------------------------------- +// Date due rette nello spazio restituisce il quadrato della loro distanza. +// Le rette sono definite da un punto e dal versore direzione. +double +LineLineSqDist( const Point3d& ptP1, const Vector3d& vtD1, const Point3d& ptP2, const Vector3d& vtD2) ; + +//---------------------------------------------------------------------------- +// Data una retta e un segmento, ne restituisce il quadrato della distanza. +// La retta è definita da un punto e dal versore direzione. +// Il segmento è definito da un estremo, dal versore direzione verso l'altro estremo e dalla lunghezza. +double +LineSegmentSqDist( const Point3d& ptPLn, const Vector3d& vtDLn, + const Point3d& ptPSg, const Vector3d& vtDSg, double dSgLen) ; + +//---------------------------------------------------------------------------- +// Valuta l'interferenza di un disco e un triangolo complanari. +// Si assume che disco e piano siano paralleli, ovvero che valga a n = 1 con +// a versore dell'asse del disco e n versore normale al triangolo. +// L'informazione su n è contenuta nell'oggetto triangolo; quella su a non è passata +// in quanto inutile sotto le nostre assunzioni. +bool +CoplanarDiscTriangleInterference( const Point3d& ptCen, double dRad, const Triangle3d& trTria, int nTriType) ; + +//---------------------------------------------------------------------------- +// 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 +FindLineLineMinDistPar( const Point3d& ptL1, const Vector3d& vtV1, + const Point3d& ptL2, const Vector3d& vtV2, + double& dU1, double& dU2) ; + +//---------------------------------------------------------------------------- +// 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) ; + +//---------------------------------------------------------------------------- +// Valuta l'allontanamento di un disco, lungo il proprio asse di simmetria, da un piano. +// Il disco è descritto dal centro nella posizione iniziale e dal raggio (la normale è il versore traslazione). +// Il piano è descritto da un suo punto e dal suo versore normale. +// Il moto è descritto dal versore di traslazione. +// Il valore restituito è un intero che descrive la situazione di allontanamento e, +// quando sensato, per referenza restituisce il punto di ultimo contatto. +// 0: Nessun contatto +// 1: Un contatto +// 2: Disco e piano ortogonali, con disco tg al piano, il contatto è una semiretta +// 3: Disco e piano ortogonali, con disco secante il piano, il contatto è una striscia +// 4: Disco e piano paralleli, come ultimo contatto si restituisce il centro. +// Nel caso 0 il parametro ptTouch non ha senso, così come nel caso 2 e nel caso 3 +int +DiskPlaneLastContactLongMot( const Point3d& ptDiskCen, double dDiskRad, + const Point3d& ptPlane, const Vector3d& vtPlane, + const Vector3d& vtMove, Point3d& ptTouch) ; + +//---------------------------------------------------------------------------- +// Si limita il segmento alla parte di spazio con distanza positiva dal piano. +// Se segmento ancora valido si restituisce true, altrimenti false. +bool +ClampSegmentOutPlane( const Plane3d& plPlane, Point3d& ptSegP, const Vector3d& vtSegV, double& dSegLen, double dSignTol = 0.) ; \ No newline at end of file diff --git a/DistLineLine.cpp b/DistLineLine.cpp index 8451373..a9081dc 100644 --- a/DistLineLine.cpp +++ b/DistLineLine.cpp @@ -1,9 +1,12 @@ -// File : DistLineLine.h Data : Versione : -// Contenuto : Dichiarazione della classe distanza fra elementi lineari. +//---------------------------------------------------------------------------- +// EgalTech 2020-2020 +//---------------------------------------------------------------------------- +// File : DistLineLine.h Data : 06.11.20 Versione : 2.2k1 +// Contenuto : Implementazione della classe distanza fra elementi lineari. // // // -// Modifiche : +// Modifiche : 06.11.20 LM Creazione modulo. // // //---------------------------------------------------------------------------- @@ -22,11 +25,16 @@ DistLineLine::DistLineLine( const Point3d& ptSt1, const Point3d& ptEn1, { Vector3d vtD1 = ptEn1 - ptSt1 ; double dLen1 = vtD1.Len() ; - vtD1 /= dLen1 ; Vector3d vtD2 = ptEn2 - ptSt2 ; double dLen2 = vtD1.Len() ; + if ( dLen1 < EPS_SMALL || dLen2 < EPS_SMALL) { + m_dSqDist = - 1 ; + m_dDist = - 1 ; + return ; + } + vtD1 /= dLen1 ; vtD2 /= dLen2 ; - DistLineLine( ptSt1, vtD1, dLen1, ptSt2, vtD2, dLen2, bIsSegment1, bIsSegment2) ; + Calculate( ptSt1, vtD1, dLen1, ptSt2, vtD2, dLen2, bIsSegment1, bIsSegment2) ; } //---------------------------------------------------------------------------- diff --git a/DistLineLine.h b/DistLineLine.h index 8d14a9d..9e204d2 100644 --- a/DistLineLine.h +++ b/DistLineLine.h @@ -1,9 +1,12 @@ -// File : DistLineLine.h Data : Versione : +//---------------------------------------------------------------------------- +// EgalTech 2020-2020 +//---------------------------------------------------------------------------- +// File : DistLineLine.h Data : 06.11.20 Versione : 2.2k1 // Contenuto : Dichiarazione della classe distanza fra elementi lineari. // // // -// Modifiche : +// Modifiche : 06.11.20 LM Creazione modulo. // // //---------------------------------------------------------------------------- @@ -13,7 +16,7 @@ #include "/EgtDev/Include/EGkVector3d.h" #include "/EgtDev/Include/EGkPoint3d.h" - +//---------------------------------------------------------------------------- class DistLineLine { public : @@ -27,19 +30,12 @@ class DistLineLine public : bool GetSqDist( double& dSqDist) ; bool GetDist( double& dDist) ; - bool IsEpsilon( double dTol) - { + bool IsEpsilon( double dTol) { double dSqDist ; return ( GetSqDist( dSqDist) && ( dSqDist < SQ_EPS_ZERO || dSqDist < dTol * dTol)) ; } - bool IsSmall( void) - { - return IsEpsilon( EPS_SMALL) ; - } - bool IsZero( void) - { - return IsEpsilon( EPS_ZERO) ; - } + bool IsSmall( void) { return IsEpsilon( EPS_SMALL) ; } + bool IsZero( void) { return IsEpsilon( EPS_ZERO) ; } bool GetMinDistPoints( Point3d& ptMinDist1, Point3d& ptMinDist2) ; bool GetParamsAtMinDistPoints( double& dPar1, double& dPar2) ; @@ -54,4 +50,4 @@ class DistLineLine double m_dPar2 ; Point3d m_ptMinDist1 ; Point3d m_ptMinDist2 ; -} ; \ No newline at end of file +} ; diff --git a/EgtGeomKernel.rc b/EgtGeomKernel.rc index 660af81..98d7bfa 100644 Binary files a/EgtGeomKernel.rc and b/EgtGeomKernel.rc differ diff --git a/EgtGeomKernel.vcxproj b/EgtGeomKernel.vcxproj index 1cf2ebb..adcd0c2 100644 --- a/EgtGeomKernel.vcxproj +++ b/EgtGeomKernel.vcxproj @@ -276,10 +276,14 @@ copy $(TargetPath) \EgtProg\Dll64 + + + + @@ -287,8 +291,12 @@ copy $(TargetPath) \EgtProg\Dll64 + + + + diff --git a/EgtGeomKernel.vcxproj.filters b/EgtGeomKernel.vcxproj.filters index aa29634..14923a6 100644 --- a/EgtGeomKernel.vcxproj.filters +++ b/EgtGeomKernel.vcxproj.filters @@ -423,6 +423,18 @@ File di origine\GeoDist + + File di origine\GeoCollision + + + File di origine\GeoCollision + + + File di origine\GeoCollision + + + File di origine\GeoCollision + @@ -1016,6 +1028,18 @@ File di intestazione + + File di intestazione\Include + + + File di intestazione\Include + + + File di intestazione\Include + + + File di intestazione + diff --git a/VolZmap.h b/VolZmap.h index a314f11..e63468b 100644 --- a/VolZmap.h +++ b/VolZmap.h @@ -1,7 +1,7 @@ //---------------------------------------------------------------------------- -// EgalTech 2015-2019 +// EgalTech 2015-2020 //---------------------------------------------------------------------------- -// File : VolZmap.h Data : 12.05.19 Versione : 2.1e3 +// File : VolZmap.h Data : 12.05.19 Versione : 2.2k1 // Contenuto : Dichiarazione della classe Volume Zmap. // // @@ -100,9 +100,9 @@ class VolZmap : public IVolZmap, public IGeoObjRW bool AvoidSphere( const Point3d& ptCenter, double dRad, double dSafeDist, bool bPrecise = false) const override ; bool AvoidCylinder( const Frame3d& frCyl, double dH, double dR, double dSafeDist, bool bPrecise = false) const override ; bool AvoidTruncatedCone( const Point3d& ptBase, const Vector3d vtSymmetryAx, - double dRadTop, double dRadBot, double dHeight, bool bPrecise = false) const override ; + double dRadTop, double dRadBot, double dHeight, double dSafeDist, bool bPrecise = false) const override ; bool AvoidTruncatedPyramid( const Point3d& ptBase, const Vector3d& vtAxZ, const Vector3d& vtAxX, - double dSegTop, double dSegBot, double dHeight, bool bPrecise = false) const override ; + double dSegTop, double dSegBot, double dHeight, double dSafeDist, bool bPrecise = false) const override ; bool Cut( const Plane3d& plPlane) override ; bool Compact( void) override ; int GetPartCount( void) const override ; diff --git a/VolZmapCalculus.cpp b/VolZmapCalculus.cpp index ee1f964..64a9f72 100644 --- a/VolZmapCalculus.cpp +++ b/VolZmapCalculus.cpp @@ -576,7 +576,61 @@ VolZmap::AvoidBox( const Frame3d& frBox, const Vector3d& vtDiag, double dSafeDis // altrimenti verifico con tutte e tre le mappe else { - return false ; + // Ciclo di intersezione dei dexel con il BBox + for ( int nMap = 0 ; nMap < m_nMapNum ; ++ nMap) { + Point3d ptO = ORIG ; + Vector3d vtX = X_AX ; + Vector3d vtY = Y_AX ; + Vector3d vtK = Z_AX ; + // Estremi del box + Point3d ptBoxInf = b3Int.GetMin() ; + Point3d ptBoxSup = b3Int.GetMax() ; + if ( nMap == 1) { + swap( ptBoxInf.x, ptBoxInf.z) ; + swap( ptBoxInf.x, ptBoxInf.y) ; + swap( ptBoxSup.x, ptBoxSup.z) ; + swap( ptBoxSup.x, ptBoxSup.y) ; + vtX = Y_AX ; + vtY = Z_AX ; + vtK = X_AX ; + } + else if ( nMap == 2) { + swap( ptBoxInf.y, ptBoxInf.z) ; + swap( ptBoxInf.x, ptBoxInf.y) ; + swap( ptBoxSup.y, ptBoxSup.z) ; + swap( ptBoxSup.x, ptBoxSup.y) ; + vtX = Z_AX ; + vtY = X_AX ; + vtK = Y_AX ; + } + // Passo da sistema griglia a sistema BBox + ptO.ToLoc( frB) ; + vtX.ToLoc( frB) ; + vtY.ToLoc( frB) ; + vtK.ToLoc( frB) ; + // Limiti su indici + int nStI = Clamp( int( ptBoxInf.x / m_dStep), 0, m_nNx[nMap] - 1) ; + int nEnI = Clamp( int( ptBoxSup.x / m_dStep), 0, m_nNx[nMap] - 1) ; + int nStJ = Clamp( int( ptBoxInf.y / m_dStep), 0, m_nNy[nMap] - 1) ; + int nEnJ = Clamp( int( ptBoxSup.y / m_dStep), 0, m_nNy[nMap] - 1) ; + for ( int i = nStI ; i <= nEnI ; ++ i) { + for ( int j = nStJ ; j <= nEnJ ; ++ j) { + int nPos = j * m_nNx[nMap] + i; + int nSize = int( m_Values[nMap][nPos].size()) ; + if ( nSize == 0) + continue ; + Point3d ptT = ptO + ( i + 0.5) * m_dStep * vtX + ( j + 0.5) * m_dStep * vtY ; + double dMinU, dMaxU ; + if ( IntersLineBox( ptT, vtK, ORIG, ORIG + vtDg, dMinU, dMaxU)) { + for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { + if ( ! ( dMaxU < m_Values[nMap][nPos][nIndex].dMin - EPS_SMALL || + dMinU > m_Values[nMap][nPos][nIndex].dMax + EPS_SMALL)) + return false ; + } + } + } + } + } } return true ; @@ -651,7 +705,68 @@ VolZmap::AvoidSphere( const Point3d& ptCenter, double dRad, double dSafeDist, bo // altrimenti verifico con tutte e tre le mappe else { - return false ; + // Ciclo di intersezione dei dexel con la sfera (nel riferimento intrinseco) + for ( int nMap = 0 ; nMap < m_nMapNum ; ++ nMap) { + Vector3d vtX = X_AX ; + Vector3d vtY = Y_AX ; + Vector3d vtK = Z_AX ; + // Estremi del box + Point3d ptBoxInf = b3Int.GetMin() ; + Point3d ptBoxSup = b3Int.GetMax() ; + if ( nMap == 1) { + swap( ptBoxInf.x, ptBoxInf.z) ; + swap( ptBoxInf.x, ptBoxInf.y) ; + swap( ptBoxSup.x, ptBoxSup.z) ; + swap( ptBoxSup.x, ptBoxSup.y) ; + vtX = Y_AX ; + vtY = Z_AX ; + vtK = X_AX ; + } + else if ( nMap == 2) { + swap( ptBoxInf.y, ptBoxInf.z) ; + swap( ptBoxInf.x, ptBoxInf.y) ; + swap( ptBoxSup.y, ptBoxSup.z) ; + swap( ptBoxSup.x, ptBoxSup.y) ; + vtX = Z_AX ; + vtY = X_AX ; + vtK = Y_AX ; + } + // Limiti su indici + int nStI = Clamp( int( ptBoxInf.x / m_dStep), 0, m_nNx[nMap] - 1) ; + int nEnI = Clamp( int( ptBoxSup.x / m_dStep), 0, m_nNx[nMap] - 1) ; + int nStJ = Clamp( int( ptBoxInf.y / m_dStep), 0, m_nNy[nMap] - 1) ; + int nEnJ = Clamp( int( ptBoxSup.y / m_dStep), 0, m_nNy[nMap] - 1) ; + for ( int i = nStI ; i <= nEnI ; ++ i) { + for ( int j = nStJ ; j <= nEnJ ; ++ j) { + int nPos = j * m_nNx[nMap] + i ; + int nSize = int( m_Values[nMap][nPos].size()) ; + if ( nSize == 0) + continue ; + Point3d ptT = ORIG + ( i + 0.5) * m_dStep * vtX + ( j + 0.5) * m_dStep * vtY ; + Point3d ptI1, ptI2 ; + if ( ::IntersLineSphere( ptT, vtK, ptC, dRad, ptI1, ptI2) != ILST_NO) { + double dMinU, dMaxU ; + if ( nMap == 0) { + dMinU = min( ptI1.z, ptI2.z) ; + dMaxU = max( ptI1.z, ptI2.z) ; + } + else if ( nMap == 1) { + dMinU = min( ptI1.x, ptI2.x) ; + dMaxU = max( ptI1.x, ptI2.x) ; + } + else { + dMinU = min( ptI1.y, ptI2.y) ; + dMaxU = max( ptI1.y, ptI2.y) ; + } + for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { + if ( ! ( dMaxU < m_Values[nMap][nPos][nIndex].dMin - EPS_SMALL || + dMinU > m_Values[nMap][nPos][nIndex].dMax + EPS_SMALL)) + return false ; + } + } + } + } + } } return true ; @@ -749,7 +864,73 @@ VolZmap::AvoidCylinder( const Frame3d& frCyl, double dH, double dR, double dSafe // altrimenti verifico con tutte e tre le mappe else { - return false ; + // Ciclo di intersezione dei dexel con il cilindro (nel riferimento intrinseco) + for ( int nMap = 0 ; nMap < m_nMapNum ; ++ nMap) { + Vector3d vtX = X_AX ; + Vector3d vtY = Y_AX ; + Vector3d vtK = Z_AX ; + // Estremi del box + Point3d ptBoxInf = b3Int.GetMin() ; + Point3d ptBoxSup = b3Int.GetMax() ; + if ( nMap == 1) { + swap( ptBoxInf.x, ptBoxInf.z) ; + swap( ptBoxInf.x, ptBoxInf.y) ; + swap( ptBoxSup.x, ptBoxSup.z) ; + swap( ptBoxSup.x, ptBoxSup.y) ; + vtX = Y_AX ; + vtY = Z_AX ; + vtK = X_AX ; + } + else if ( nMap == 2) { + swap( ptBoxInf.y, ptBoxInf.z) ; + swap( ptBoxInf.x, ptBoxInf.y) ; + swap( ptBoxSup.y, ptBoxSup.z) ; + swap( ptBoxSup.x, ptBoxSup.y) ; + vtX = Z_AX ; + vtY = X_AX ; + vtK = Y_AX ; + } + // Limiti su indici + int nStI = Clamp( int( ptBoxInf.x / m_dStep), 0, m_nNx[nMap] - 1) ; + int nEnI = Clamp( int( ptBoxSup.x / m_dStep), 0, m_nNx[nMap] - 1) ; + int nStJ = Clamp( int( ptBoxInf.y / m_dStep), 0, m_nNy[nMap] - 1) ; + int nEnJ = Clamp( int( ptBoxSup.y / m_dStep), 0, m_nNy[nMap] - 1) ; + for ( int i = nStI ; i <= nEnI ; ++ i) { + for ( int j = nStJ ; j <= nEnJ ; ++ j) { + if ( nMap == 2 && j == 75) + int a = 0 ; + int nPos = j * m_nNx[nMap] + i ; + int nSize = int( m_Values[nMap][nPos].size()) ; + if ( nSize == 0) + continue ; + Point3d ptT = ORIG + ( i + 0.5) * m_dStep * vtX + ( j + 0.5) * m_dStep * vtY ; + Point3d ptI1, ptI2 ; + Vector3d vtN1, vtN2 ; + if ( IntersLineCylinder( ptT, vtK, frC, dH, dR, true, true, ptI1, vtN1, ptI2, vtN2)) { + double dMinU, dMaxU ; + if ( nMap == 0) { + dMinU = min( ptI1.z, ptI2.z) ; + dMaxU = max( ptI1.z, ptI2.z) ; + } + else if ( nMap == 1) { + dMinU = min( ptI1.x, ptI2.x) ; + dMaxU = max( ptI1.x, ptI2.x) ; + } + else { + dMinU = min( ptI1.y, ptI2.y) ; + dMaxU = max( ptI1.y, ptI2.y) ; + } + for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { + double dMin = m_Values[nMap][nPos][nIndex].dMin ; + double dMax = m_Values[nMap][nPos][nIndex].dMax ; + if ( ! ( dMaxU < dMin - EPS_SMALL || + dMinU > dMax + EPS_SMALL)) + return false ; + } + } + } + } + } } return true ; @@ -758,7 +939,7 @@ VolZmap::AvoidCylinder( const Frame3d& frCyl, double dH, double dR, double dSafe //---------------------------------------------------------------------------- bool VolZmap::AvoidTruncatedCone( const Point3d& ptBase, const Vector3d vtSymmetryAx, - double dRadTop, double dRadBot, double dHeight, bool bPrecise) const + double dRadTop, double dRadBot, double dHeight, double dSafeDist, bool bPrecise) const { // Porto il tronco di cono nel sistema intrinseco e normalizzo il vettore. Point3d ptMyBase = ptBase ; @@ -767,11 +948,20 @@ VolZmap::AvoidTruncatedCone( const Point3d& ptBase, const Vector3d vtSymmetryAx, vtMySymmetryAx.ToLoc( m_MapFrame) ; vtMySymmetryAx.Normalize() ; + // Se distanza di sicurezza maggiore di epsilon espando il cono + if ( dSafeDist > EPS_SMALL) { + ptMyBase -= ( dSafeDist * vtMySymmetryAx) ; + dHeight += ( 2 * dSafeDist) ; + dRadTop += dSafeDist ; + dRadBot += dSafeDist ; + } + // Se i BoundingBox non si sovrappongono, ho finito BBox3d b3ConusBox( ptMyBase + 0.5 * dHeight * vtMySymmetryAx) ; b3ConusBox.Expand( dHeight + max( dRadTop, dRadBot)) ; BBox3d b3Zmap( ORIG, Point3d( m_nNx[0] * m_dStep, m_nNy[0] * m_dStep, m_dMaxZ[0])) ; - if ( ! b3Zmap.FindIntersection( b3ConusBox, BBox3d())) + BBox3d b3Int ; + if ( ! b3Zmap.FindIntersection( b3ConusBox, b3Int)) return true ; // Definisco un sistema di riferimento del tronco cono con origine nel vertice virtuale @@ -797,56 +987,123 @@ VolZmap::AvoidTruncatedCone( const Point3d& ptBase, const Vector3d vtSymmetryAx, Frame3d frConusFrame ; frConusFrame.Set( ptVert, vtAx) ; - // Ciclo sulle mappe. - int nMapNum = bPrecise ? min( m_nMapNum, 3) : 1 ; - for ( int nMap = 0 ; nMap < nMapNum ; ++ nMap) { - for ( int nDex = 0 ; nDex < int( m_Values[nMap].size()) ; ++ nDex) { - // Indici del dexel. - int nI = nDex % m_nNx[nMap] ; - int nJ = nDex / m_nNx[nMap] ; - // Posizione del dexel. - double dX = ( nI + 0.5) * m_dStep ; - double dY = ( nJ + 0.5) * m_dStep ; - Point3d ptLineSt( dX, dY, 0.) ; - Vector3d vtLineDir( 0., 0., 1.) ; - // Dal sistema griglia al sistema intrinseco (per la prima griglia coincidono). + // Uso solo la prima mappa + if ( ! bPrecise) { + // Limiti su indici + int nStI = Clamp( int( b3Int.GetMin().x / m_dStep), 0, m_nNx[0] - 1) ; + int nEnI = Clamp( int( b3Int.GetMax().x / m_dStep), 0, m_nNx[0] - 1) ; + int nStJ = Clamp( int( b3Int.GetMin().y / m_dStep), 0, m_nNy[0] - 1) ; + int nEnJ = Clamp( int( b3Int.GetMax().y / m_dStep), 0, m_nNy[0] - 1) ; + // Ciclo di intersezione dei dexel con il cilindro (nel riferimento intrinseco) + for ( int i = nStI ; i <= nEnI ; ++ i) { + for ( int j = nStJ ; j <= nEnJ ; ++ j) { + int nPos = j * m_nNx[0] + i ; + int nSize = int( m_Values[0][nPos].size()) ; + if ( nSize == 0) + continue ; + for ( int k = 0 ; k < 5 ; ++ k) { + Point3d ptLineSt = ORIG + ( i + 0.5) * m_dStep * X_AX + ( j + 0.5) * m_dStep * Y_AX ; + if ( k == 0) + ; + else if ( k == 1) + ptLineSt += -0.4 * m_dStep * X_AX - 0.4 * m_dStep * Y_AX ; + else if ( k == 2) + ptLineSt += +0.4 * m_dStep * X_AX - 0.4 * m_dStep * Y_AX ; + else if ( k == 3) + ptLineSt += +0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ; + else if ( k == 4) + ptLineSt += -0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ; + double dTan = dMaxRad / dTotHeight ; + Point3d ptInt1, ptInt2 ; + if ( IntersLineConus( ptLineSt, Z_AX, frConusFrame, dTan, dDeltaHeight, dTotHeight, true, true, + ptInt1, Vector3d( 0., 0., 0.), ptInt2, Vector3d( 0., 0., 0.))) { + double dZmin = min( ptInt1.z, ptInt2.z) ; + double dZmax = max( ptInt1.z, ptInt2.z) ; + for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { + if ( ! ( dZmax < m_Values[0][nPos][nIndex].dMin - EPS_SMALL || + dZmin > m_Values[0][nPos][nIndex].dMax + EPS_SMALL)) + return false ; + } + } + } + } + } + } + // Uso tutte le mappe + else { + // Ciclo sulle mappe. + for ( int nMap = 0 ; nMap < m_nMapNum ; ++ nMap) { + Point3d ptInfIntBox = b3Int.GetMin() ; + Point3d ptSupIntBox = b3Int.GetMax() ; + // Dal sistema intrinseco al sistema griglia (per la prima griglia coincidono). if ( nMap == 1) { - swap( ptLineSt.x, ptLineSt.y) ; - swap( ptLineSt.x, ptLineSt.z) ; - swap( vtLineDir.x, vtLineDir.y) ; - swap( vtLineDir.x, vtLineDir.z) ; + swap( ptInfIntBox.x, ptInfIntBox.z) ; + swap( ptInfIntBox.x, ptInfIntBox.y) ; + swap( ptSupIntBox.x, ptSupIntBox.z) ; + swap( ptSupIntBox.x, ptSupIntBox.y) ; } else if ( nMap == 2) { - swap( ptLineSt.x, ptLineSt.y) ; - swap( ptLineSt.y, ptLineSt.z) ; - swap( vtLineDir.x, vtLineDir.y) ; - swap( vtLineDir.y, vtLineDir.z) ; + swap( ptInfIntBox.y, ptInfIntBox.z) ; + swap( ptInfIntBox.x, ptInfIntBox.y) ; + swap( ptSupIntBox.y, ptSupIntBox.z) ; + swap( ptSupIntBox.x, ptSupIntBox.y) ; } - double dTan = dMaxRad / dTotHeight ; - Point3d ptInt1, ptInt2 ; - // La retta del dexel interseca il tronco di cono. - if ( IntersLineConus( ptLineSt, vtLineDir, frConusFrame, dTan, dDeltaHeight, dTotHeight, true, true, - ptInt1, Vector3d( 0., 0., 0.), ptInt2, Vector3d( 0., 0., 0.))) { - double dMinU, dMaxU ; - if ( nMap == 0) { - dMinU = min( ptInt1.z, ptInt2.z) ; - dMaxU = max( ptInt1.z, ptInt2.z) ; + // Limiti su indici + int nStI = Clamp( int( ptInfIntBox.x / m_dStep), 0, m_nNx[nMap] - 1) ; + int nEnI = Clamp( int( ptSupIntBox.x / m_dStep), 0, m_nNx[nMap] - 1) ; + int nStJ = Clamp( int( ptInfIntBox.y / m_dStep), 0, m_nNy[nMap] - 1) ; + int nEnJ = Clamp( int( ptSupIntBox.y / m_dStep), 0, m_nNy[nMap] - 1) ; + for ( int nDex = 0 ; nDex < int( m_Values[nMap].size()) ; ++ nDex) { + // Indici del dexel. + int nI = nDex % m_nNx[nMap] ; + int nJ = nDex / m_nNx[nMap] ; + // Se fuori dalla regione ammissibile salto l'iterazione + if ( nI < nStI || nI > nEnI || nJ < nStJ || nJ > nEnJ) + continue ; + // Posizione del dexel. + double dX = ( nI + 0.5) * m_dStep ; + double dY = ( nJ + 0.5) * m_dStep ; + Point3d ptLineSt( dX, dY, 0.) ; + Vector3d vtLineDir( 0., 0., 1.) ; + // Dal sistema griglia al sistema intrinseco (per la prima griglia coincidono). + if ( nMap == 1) { + swap( ptLineSt.x, ptLineSt.y) ; + swap( ptLineSt.x, ptLineSt.z) ; + swap( vtLineDir.x, vtLineDir.y) ; + swap( vtLineDir.x, vtLineDir.z) ; } - else if ( nMap == 1) { - dMinU = min( ptInt1.x, ptInt2.x) ; - dMaxU = max( ptInt1.x, ptInt2.x) ; + else if ( nMap == 2) { + swap( ptLineSt.x, ptLineSt.y) ; + swap( ptLineSt.y, ptLineSt.z) ; + swap( vtLineDir.x, vtLineDir.y) ; + swap( vtLineDir.y, vtLineDir.z) ; } - else { - dMinU = min( ptInt1.y, ptInt2.y) ; - dMaxU = max( ptInt1.y, ptInt2.y) ; - } - // Ciclo sui segmenti del dexel. - for ( int nInt = 0 ; nInt < int( m_Values[nMap][nDex].size()) ; ++ nInt) { - double dMin = m_Values[nMap][nDex][nInt].dMin ; - double dMax = m_Values[nMap][nDex][nInt].dMax ; - // Se c'è intersezione, ho finito. - if ( ! ( dMax < dMinU - EPS_SMALL || dMin > dMaxU + EPS_SMALL)) - return false ; + double dTan = dMaxRad / dTotHeight ; + Point3d ptInt1, ptInt2 ; + // La retta del dexel interseca il tronco di cono. + if ( IntersLineConus( ptLineSt, vtLineDir, frConusFrame, dTan, dDeltaHeight, dTotHeight, true, true, + ptInt1, Vector3d( 0., 0., 0.), ptInt2, Vector3d( 0., 0., 0.))) { + double dMinU, dMaxU ; + if ( nMap == 0) { + dMinU = min( ptInt1.z, ptInt2.z) ; + dMaxU = max( ptInt1.z, ptInt2.z) ; + } + else if ( nMap == 1) { + dMinU = min( ptInt1.x, ptInt2.x) ; + dMaxU = max( ptInt1.x, ptInt2.x) ; + } + else { + dMinU = min( ptInt1.y, ptInt2.y) ; + dMaxU = max( ptInt1.y, ptInt2.y) ; + } + // Ciclo sui segmenti del dexel. + for ( int nInt = 0 ; nInt < int( m_Values[nMap][nDex].size()) ; ++ nInt) { + double dMin = m_Values[nMap][nDex][nInt].dMin ; + double dMax = m_Values[nMap][nDex][nInt].dMax ; + // Se c'è intersezione, ho finito. + if ( ! ( dMax < dMinU - EPS_SMALL || dMin > dMaxU + EPS_SMALL)) + return false ; + } } } } @@ -857,7 +1114,7 @@ VolZmap::AvoidTruncatedCone( const Point3d& ptBase, const Vector3d vtSymmetryAx, //---------------------------------------------------------------------------- bool VolZmap::AvoidTruncatedPyramid( const Point3d& ptBase, const Vector3d& vtAxZ, const Vector3d& vtAxX, - double dSegTop, double dSegBot, double dHeight, bool bPrecise) const + double dSegTop, double dSegBot, double dHeight, double dSafeDist, bool bPrecise) const { // Porto il tronco di piramide nel sistema intrinseco e normalizzo i vettori. Point3d ptMyBase = ptBase ; @@ -869,6 +1126,14 @@ VolZmap::AvoidTruncatedPyramid( const Point3d& ptBase, const Vector3d& vtAxZ, co vtMyAxZ.Normalize() ; vtMyAxX.Normalize() ; + // Se distanza di sicurezza maggiore di epsilon espando il cono + if ( dSafeDist > EPS_SMALL) { + ptMyBase -= ( dSafeDist * vtMyAxZ) ; + dHeight += ( 2 * dSafeDist) ; + dSegTop += ( 2 * dSafeDist) ; + dSegBot += ( 2 * dSafeDist) ; + } + // Se i BoundingBox non si sovrappongono, ho finito Vector3d vtMyAxY = vtMyAxZ ^ vtMyAxX ; vtMyAxY.Normalize() ; @@ -882,7 +1147,8 @@ VolZmap::AvoidTruncatedPyramid( const Point3d& ptBase, const Vector3d& vtAxZ, co b3PyramyBox.Add( ptMyBase + 0.5 * dSegBot * ( vtMyAxX + vtMyAxY) + dHeight * vtMyAxZ) ; b3PyramyBox.Add( ptMyBase - 0.5 * dSegBot * ( vtMyAxX - vtMyAxY) + dHeight * vtMyAxZ) ; BBox3d b3Zmap( ORIG, Point3d( m_nNx[0] * m_dStep, m_nNy[0] * m_dStep, m_dMaxZ[0])) ; - if ( ! b3Zmap.FindIntersection( b3PyramyBox, BBox3d())) + BBox3d b3Int ; + if ( ! b3Zmap.FindIntersection( b3PyramyBox, b3Int)) return true ; // Definisco un sistema di riferimento con origine nel centro della base maggiore del tronco di piramide, @@ -904,56 +1170,120 @@ VolZmap::AvoidTruncatedPyramid( const Point3d& ptBase, const Vector3d& vtAxZ, co } Frame3d frPyramidFrame ; frPyramidFrame.Set( ptMaxBase, vtMyAxX, vtAx ^ vtMyAxX, vtAx) ; - - // Ciclo sulle mappe - int nMapNum = bPrecise ? min( m_nMapNum, 3) : 1 ; - for ( int nMap = 0 ; nMap < nMapNum ; ++ nMap) { - for ( int nDex = 0 ; nDex < int( m_Values[nMap].size()) ; ++ nDex) { - // Indici del dexel - int nI = nDex % m_nNx[nMap] ; - int nJ = nDex / m_nNx[nMap] ; - // Posizione del dexel - double dX = ( nI + 0.5) * m_dStep ; - double dY = ( nJ + 0.5) * m_dStep ; - Point3d ptLineSt( dX, dY, 0.) ; - Vector3d vtLineDir( 0., 0., 1.) ; - // Dal sistema griglia al sistema intrinseco (per la prima griglia coincidono). + + if ( ! bPrecise) { + // Limiti su indici + int nStI = Clamp( int( b3Int.GetMin().x / m_dStep), 0, m_nNx[0] - 1) ; + int nEnI = Clamp( int( b3Int.GetMax().x / m_dStep), 0, m_nNx[0] - 1) ; + int nStJ = Clamp( int( b3Int.GetMin().y / m_dStep), 0, m_nNy[0] - 1) ; + int nEnJ = Clamp( int( b3Int.GetMax().y / m_dStep), 0, m_nNy[0] - 1) ; + // Ciclo di intersezione dei dexel con il cilindro (nel riferimento intrinseco) + for ( int i = nStI ; i <= nEnI ; ++ i) { + for ( int j = nStJ ; j <= nEnJ ; ++ j) { + int nPos = j * m_nNx[0] + i ; + int nSize = int( m_Values[0][nPos].size()) ; + if ( nSize == 0) + continue ; + for ( int k = 0 ; k < 5 ; ++ k) { + Point3d ptLineSt = ORIG + ( i + 0.5) * m_dStep * X_AX + ( j + 0.5) * m_dStep * Y_AX ; + if ( k == 0) + ; + else if ( k == 1) + ptLineSt += -0.4 * m_dStep * X_AX - 0.4 * m_dStep * Y_AX ; + else if ( k == 2) + ptLineSt += +0.4 * m_dStep * X_AX - 0.4 * m_dStep * Y_AX ; + else if ( k == 3) + ptLineSt += +0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ; + else if ( k == 4) + ptLineSt += -0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ; + Point3d ptInt1, ptInt2 ; + if ( IntersLineTruncatedPyramid( ptLineSt, Z_AX, frPyramidFrame, dMinSeg, dMaxSeg, dHeight, + ptInt1, Vector3d( 0., 0., 0.), ptInt2, Vector3d( 0., 0., 0.))) { + double dZmin = min( ptInt1.z, ptInt2.z) ; + double dZmax = max( ptInt1.z, ptInt2.z) ; + for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) { + if ( ! ( dZmax < m_Values[0][nPos][nIndex].dMin - EPS_SMALL || + dZmin > m_Values[0][nPos][nIndex].dMax + EPS_SMALL)) + return false ; + } + } + } + } + } + } + else { + // Ciclo sulle mappe + for ( int nMap = 0 ; nMap < m_nMapNum ; ++ nMap) { + Point3d ptInfIntBox = b3Int.GetMin(); + Point3d ptSupIntBox = b3Int.GetMax(); + // Dal sistema intrinseco al sistema griglia (per la prima griglia coincidono). if ( nMap == 1) { - swap( ptLineSt.x, ptLineSt.y) ; - swap( ptLineSt.x, ptLineSt.z) ; - swap( vtLineDir.x, vtLineDir.y) ; - swap( vtLineDir.x, vtLineDir.z) ; + swap( ptInfIntBox.x, ptInfIntBox.z) ; + swap( ptInfIntBox.x, ptInfIntBox.y) ; + swap( ptSupIntBox.x, ptSupIntBox.z) ; + swap( ptSupIntBox.x, ptSupIntBox.y) ; } else if ( nMap == 2) { - swap( ptLineSt.x, ptLineSt.y) ; - swap( ptLineSt.y, ptLineSt.z) ; - swap( vtLineDir.x, vtLineDir.y) ; - swap( vtLineDir.y, vtLineDir.z) ; + swap( ptInfIntBox.y, ptInfIntBox.z) ; + swap( ptInfIntBox.x, ptInfIntBox.y) ; + swap( ptSupIntBox.y, ptSupIntBox.z) ; + swap( ptSupIntBox.x, ptSupIntBox.y) ; } - Point3d ptInt1, ptInt2 ; - // La retta del dexel interseca il tronco di piramide. - if ( IntersLineTruncatedPyramid( ptLineSt, vtLineDir, frPyramidFrame, dMinSeg, dMaxSeg, dHeight, - ptInt1, Vector3d( 0., 0., 0.), ptInt2, Vector3d( 0., 0., 0.))) { - double dMinU, dMaxU ; - if ( nMap == 0) { - dMinU = min( ptInt1.z, ptInt2.z) ; - dMaxU = max( ptInt1.z, ptInt2.z) ; + // Limiti su indici + int nStI = Clamp( int( ptInfIntBox.x / m_dStep), 0, m_nNx[nMap] - 1) ; + int nEnI = Clamp( int( ptSupIntBox.x / m_dStep), 0, m_nNx[nMap] - 1) ; + int nStJ = Clamp( int( ptInfIntBox.y / m_dStep), 0, m_nNy[nMap] - 1) ; + int nEnJ = Clamp( int( ptSupIntBox.y / m_dStep), 0, m_nNy[nMap] - 1) ; + for ( int nDex = 0 ; nDex < int( m_Values[nMap].size()) ; ++ nDex) { + // Indici del dexel + int nI = nDex % m_nNx[nMap] ; + int nJ = nDex / m_nNx[nMap] ; + // Se fuori dalla regione ammissibile salto l'iterazione + if ( nI < nStI || nI > nEnI || nJ < nStJ || nJ > nEnJ) + continue ; + // Posizione del dexel + double dX = ( nI + 0.5) * m_dStep ; + double dY = ( nJ + 0.5) * m_dStep ; + Point3d ptLineSt( dX, dY, 0.) ; + Vector3d vtLineDir( 0., 0., 1.) ; + // Dal sistema griglia al sistema intrinseco (per la prima griglia coincidono). + if ( nMap == 1) { + swap( ptLineSt.x, ptLineSt.y) ; + swap( ptLineSt.x, ptLineSt.z) ; + swap( vtLineDir.x, vtLineDir.y) ; + swap( vtLineDir.x, vtLineDir.z) ; } - else if ( nMap == 1) { - dMinU = min( ptInt1.x, ptInt2.x) ; - dMaxU = max( ptInt1.x, ptInt2.x) ; + else if ( nMap == 2) { + swap( ptLineSt.x, ptLineSt.y) ; + swap( ptLineSt.y, ptLineSt.z) ; + swap( vtLineDir.x, vtLineDir.y) ; + swap( vtLineDir.y, vtLineDir.z) ; } - else { - dMinU = min( ptInt1.y, ptInt2.y) ; - dMaxU = max( ptInt1.y, ptInt2.y) ; - } - // Ciclo sui segmenti del dexel. - for ( int nInt = 0 ; nInt < int( m_Values[nMap][nDex].size()) ; ++ nInt) { - double dMin = m_Values[nMap][nDex][nInt].dMin ; - double dMax = m_Values[nMap][nDex][nInt].dMax ; - // Se c'è intersezione, ho finito. - if ( ! ( dMax < dMinU - EPS_SMALL || dMin > dMaxU + EPS_SMALL)) - return false ; + Point3d ptInt1, ptInt2 ; + // La retta del dexel interseca il tronco di piramide. + if ( IntersLineTruncatedPyramid( ptLineSt, vtLineDir, frPyramidFrame, dMinSeg, dMaxSeg, dHeight, + ptInt1, Vector3d( 0., 0., 0.), ptInt2, Vector3d( 0., 0., 0.))) { + double dMinU, dMaxU ; + if ( nMap == 0) { + dMinU = min( ptInt1.z, ptInt2.z) ; + dMaxU = max( ptInt1.z, ptInt2.z) ; + } + else if ( nMap == 1) { + dMinU = min( ptInt1.x, ptInt2.x) ; + dMaxU = max( ptInt1.x, ptInt2.x) ; + } + else { + dMinU = min( ptInt1.y, ptInt2.y) ; + dMaxU = max( ptInt1.y, ptInt2.y) ; + } + // Ciclo sui segmenti del dexel. + for ( int nInt = 0 ; nInt < int( m_Values[nMap][nDex].size()) ; ++ nInt) { + double dMin = m_Values[nMap][nDex][nInt].dMin ; + double dMax = m_Values[nMap][nDex][nInt].dMax ; + // Se c'è intersezione, ho finito. + if ( ! ( dMax < dMinU - EPS_SMALL || dMin > dMaxU + EPS_SMALL)) + return false ; + } } } } @@ -1533,18 +1863,21 @@ VolZmap::IntersLineTruncatedPyramid( const Point3d& ptLineSt, const Vector3d& vt if ( abs( vtV.z) < EPS_ZERO && ( ptP.z < EPS_SMALL || ptP.z > dHeight + EPS_SMALL)) return false ; + double dHalfMax = 0.5 * dSegMax ; + double dHalfMin = 0.5 * dSegMin ; + // Cerco le intersezioni con i piani delle facce int nIntNum = 0 ; // Base maggiore Point3d ptIntPlaneMax = ptP + ( - ptP.z / vtV.z) * vtV ; - if ( abs( ptIntPlaneMax.x) <= dSegMax && abs( ptIntPlaneMax.y) <= dSegMax) { + if ( abs( ptIntPlaneMax.x) <= dHalfMax && abs( ptIntPlaneMax.y) <= dHalfMax) { ptInt1 = ptIntPlaneMax ; vtN1 = - Z_AX ; ++ nIntNum ; } // Base minore Point3d ptIntPlaneMin = ptP + ( ( dHeight - ptP.z) / vtV.z) * vtV ; - if ( abs( ptIntPlaneMin.x) <= dSegMin && abs( ptIntPlaneMin.y) <= dSegMin) { + if ( abs( ptIntPlaneMin.x) <= dHalfMin && abs( ptIntPlaneMin.y) <= dHalfMin) { if ( nIntNum == 0) { ptInt1 = ptIntPlaneMin ; vtN1 = Z_AX ; @@ -1571,96 +1904,96 @@ VolZmap::IntersLineTruncatedPyramid( const Point3d& ptLineSt, const Vector3d& vt // Altrimenti se le intersezioni sono fuori dalle basi e dallo stesso lato rispetto a queste ultime non c'è intersezione if ( nIntNum == 0 && - ( ( ptIntPlaneMax.x < - dSegMax - EPS_SMALL && ptIntPlaneMin.x < - dSegMin - EPS_SMALL) || - ( ptIntPlaneMax.x > dSegMax + EPS_SMALL && ptIntPlaneMin.x > dSegMin + EPS_SMALL) || - ( ptIntPlaneMax.y < - dSegMax - EPS_SMALL && ptIntPlaneMin.y < - dSegMin - EPS_SMALL) || - ( ptIntPlaneMax.y > dSegMax + EPS_SMALL && ptIntPlaneMin.y > dSegMin + EPS_SMALL))) + ( ( ptIntPlaneMax.x < - dHalfMax - EPS_SMALL && ptIntPlaneMin.x < - dHalfMin - EPS_SMALL) || + ( ptIntPlaneMax.x > dHalfMax + EPS_SMALL && ptIntPlaneMin.x > dHalfMin + EPS_SMALL) || + ( ptIntPlaneMax.y < - dHalfMax - EPS_SMALL && ptIntPlaneMin.y < - dHalfMin - EPS_SMALL) || + ( ptIntPlaneMax.y > dHalfMax + EPS_SMALL && ptIntPlaneMin.y > dHalfMin + EPS_SMALL))) return false ; // Cerco le eventuali intersezioni con i piani delle facce laterali - Point3d ptPlaneP( - dSegMax, - dSegMax, 0.) ; - Vector3d vtPlaneN( 0., - dHeight, ( dSegMax - dSegMin)) ; + Point3d ptPlaneP( - dHalfMax, - dHalfMax, 0.) ; + Vector3d vtPlaneN( 0., - dHeight, ( dHalfMax - dHalfMin)) ; vtPlaneN.Normalize() ; if ( nIntNum < 2 && abs( vtV * vtPlaneN) > EPS_ZERO) { if ( nIntNum == 0) { - ptInt1 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ; + ptInt1 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtV ; vtN1 = vtPlaneN ; if ( ptInt1.z >= 0 && ptInt1.z <= dHeight && - ( ptInt1.x + dSegMax) * dHeight >= ( dSegMax - dSegMin) * ptInt1.z && - ( ptInt1.x - dSegMax) * dHeight <= ( dSegMin - dSegMax) * ptInt1.z) + ( ptInt1.x + dHalfMax) * dHeight >= ( dHalfMax - dHalfMin) * ptInt1.z && + ( ptInt1.x - dHalfMax) * dHeight <= ( dHalfMin - dHalfMax) * ptInt1.z) ++ nIntNum ; } else { - ptInt2 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ; + ptInt2 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtV ; vtN2 = vtPlaneN ; if ( ( ptInt2 - ptInt2).SqLen() > SQ_EPS_SMALL && ptInt2.z >= 0 && ptInt1.z <= dHeight && - ( ptInt2.x + dSegMax) * dHeight >= ( dSegMax - dSegMin) * ptInt2.z && - ( ptInt2.x - dSegMax) * dHeight <= ( dSegMin - dSegMax) * ptInt2.z) + ( ptInt2.x + dHalfMax) * dHeight >= ( dHalfMax - dHalfMin) * ptInt2.z && + ( ptInt2.x - dHalfMax) * dHeight <= ( dHalfMin - dHalfMax) * ptInt2.z) ++ nIntNum ; } } - vtPlaneN = Vector3d( - dHeight, 0., ( dSegMax - dSegMin)) ; + vtPlaneN = Vector3d( - dHeight, 0., ( dHalfMax - dHalfMin)) ; vtPlaneN.Normalize() ; if ( nIntNum < 2 && abs( vtV * vtPlaneN) > EPS_ZERO) { if ( nIntNum == 0) { - ptInt1 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ; + ptInt1 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtV ; vtN1 = vtPlaneN ; if ( ptInt1.z >= 0 && ptInt1.z <= dHeight && - ( ptInt1.y + dSegMax) * dHeight >= ( dSegMax - dSegMin) * ptInt1.z && - ( ptInt1.y - dSegMax) * dHeight <= ( dSegMin - dSegMax) * ptInt1.z) + ( ptInt1.y + dHalfMax) * dHeight >= ( dHalfMax - dHalfMin) * ptInt1.z && + ( ptInt1.y - dHalfMax) * dHeight <= ( dHalfMin - dHalfMax) * ptInt1.z) ++ nIntNum ; } else { - ptInt2 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ; + ptInt2 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtV ; vtN2 = vtPlaneN ; if ( ( ptInt1 - ptInt2).SqLen() > SQ_EPS_SMALL && ptInt2.z >= 0 && ptInt2.z <= dHeight && - ( ptInt2.y + dSegMax) * dHeight >= ( dSegMax - dSegMin) * ptInt2.z && - ( ptInt2.y - dSegMax) * dHeight <= ( dSegMin - dSegMax) * ptInt2.z) + ( ptInt2.y + dHalfMax) * dHeight >= ( dHalfMax - dHalfMin) * ptInt2.z && + ( ptInt2.y - dHalfMax) * dHeight <= ( dHalfMin - dHalfMax) * ptInt2.z) ++ nIntNum ; } } - ptPlaneP = Point3d( dSegMax, dSegMax, 0.) ; - vtPlaneN = Vector3d( dHeight, 0., ( dSegMax - dSegMin)) ; + ptPlaneP = Point3d( dHalfMax, dHalfMax, 0.) ; + vtPlaneN = Vector3d( dHeight, 0., ( dHalfMax - dHalfMin)) ; vtPlaneN.Normalize() ; if ( nIntNum < 2 && abs( vtV * vtPlaneN) > EPS_ZERO) { if ( nIntNum == 0) { - ptInt1 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ; + ptInt1 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtV ; vtN1 = vtPlaneN ; if ( ptInt1.z >= 0 && ptInt1.z <= dHeight && - ( ptInt1.y + dSegMax) * dHeight >= ( dSegMax - dSegMin) * ptInt1.z && - ( ptInt1.y - dSegMax) * dHeight <= ( dSegMin - dSegMax) * ptInt1.z) + ( ptInt1.y + dHalfMax) * dHeight >= ( dHalfMax - dHalfMin) * ptInt1.z && + ( ptInt1.y - dHalfMax) * dHeight <= ( dHalfMin - dHalfMax) * ptInt1.z) ++ nIntNum ; } else { - ptInt2 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ; + ptInt2 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtV ; vtN2 = vtPlaneN ; if ( ( ptInt1 - ptInt2).SqLen() > SQ_EPS_SMALL && ptInt2.z >= 0 && ptInt2.z <= dHeight && - ( ptInt2.y + dSegMax) * dHeight >= ( dSegMax - dSegMin) * ptInt2.z && - ( ptInt2.y - dSegMax) * dHeight <= ( dSegMin - dSegMax) * ptInt2.z) + ( ptInt2.y + dHalfMax) * dHeight >= ( dHalfMax - dHalfMin) * ptInt2.z && + ( ptInt2.y - dHalfMax) * dHeight <= ( dHalfMin - dHalfMax) * ptInt2.z) ++ nIntNum ; } } - vtPlaneN = Vector3d( 0., dHeight, ( dSegMax - dSegMin)) ; + vtPlaneN = Vector3d( 0., dHeight, ( dHalfMax - dHalfMin)) ; vtPlaneN.Normalize() ; if ( nIntNum < 2 && abs( vtV * vtPlaneN) > EPS_ZERO) { if ( nIntNum == 0) { - ptInt1 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ; + ptInt1 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtV ; vtN1 = vtPlaneN ; if ( ptInt1.z >= 0 && ptInt1.z <= dHeight && - ( ptInt1.x + dSegMax) * dHeight >= (dSegMax - dSegMin) * ptInt1.z && - ( ptInt1.x - dSegMax) * dHeight <= (dSegMin - dSegMax) * ptInt1.z) + ( ptInt1.x + dHalfMax) * dHeight >= ( dHalfMax - dHalfMin) * ptInt1.z && + ( ptInt1.x - dHalfMax) * dHeight <= ( dHalfMin - dHalfMax) * ptInt1.z) ++ nIntNum ; } else { - ptInt2 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ; + ptInt2 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtV ; vtN2 = vtPlaneN ; if ( ( ptInt1 - ptInt2).SqLen() > SQ_EPS_SMALL && ptInt2.z >= 0 && ptInt1.z <= dHeight && - ( ptInt2.x + dSegMax) * dHeight >= ( dSegMax - dSegMin) * ptInt2.z && - ( ptInt2.x - dSegMax) * dHeight <= ( dSegMin - dSegMax) * ptInt2.z) + ( ptInt2.x + dHalfMax) * dHeight >= ( dHalfMax - dHalfMin) * ptInt2.z && + ( ptInt2.x - dHalfMax) * dHeight <= ( dHalfMin - dHalfMax) * ptInt2.z) ++ nIntNum ; } }