diff --git a/CAvToolTriangle.cpp b/CAvToolTriangle.cpp index e7a753a..5992af1 100644 --- a/CAvToolTriangle.cpp +++ b/CAvToolTriangle.cpp @@ -27,15 +27,15 @@ using namespace std ; //---------------------------------------------------------------------------- static bool GetTopTapFromPrevCurve( const ICurve* pPrevCurve) ; static bool GetBotTapFromNextCurve( const ICurve* pNextCurve) ; +// Sfera static double CAvSphereTriangle( const Point3d& ptSpheCen, double dSpheRad, const Triangle3d& trTria, const Vector3d& vtMove) ; -static double SpherePlaneEscapeDist( const Point3d& ptSpheCen, double dSpheRad, - const Point3d& ptPlane, const Vector3d& vtPlaneN, const Vector3d& vtMove) ; -static double SphereTriaInteriorEscapeDist( const Point3d& ptSpheCen, double dSpheRad, - const Triangle3d& trTria, const Vector3d& vtMove) ; +static double SpherePointEscapeDist( const Point3d& ptSpheCen, double dSpheRad, const Point3d ptP, const Vector3d& vtMove) ; static double SphereSegmentEscapeDist( const Point3d& ptSpheCen, double dSpheRad, const Point3d& ptSeg, const Vector3d& vtSegDir, double dSegLen, const Vector3d& vtMove) ; -static double SpherePointEscapeDist( const Point3d& ptSpheCen, double dSpheRad, const Point3d ptP, const Vector3d& vtMove) ; +static double SphereTriaInteriorEscapeDist( const Point3d& ptSpheCen, double dSpheRad, + const Triangle3d& trTria, const Vector3d& vtMove) ; +// Cilindro static double CAvCylinderTriangle( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad, const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot) ; static double CylPointEscapeDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dCylHei, double dCylRad, @@ -52,6 +52,7 @@ static double CylSegmentEscapeDistGenMot( const Point3d& ptCylOrig, const Vector const Vector3d& vtMove, bool bTop, bool bBot) ; static double CylTriaInteriorEscapeDistGenMot( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad, const Triangle3d& trTria, const Vector3d& vtMove) ; +// Tronco di cono static double CAvTrConeTriangle( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot) ; static double TrConePointEscapeDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, @@ -75,32 +76,37 @@ static double TrConeSegmentEscapeDistGenMot( const Point3d& ptMinBase, const Vec const Vector3d& vtMove, bool bTop, bool bBot) ; static double TrConeTriaInteriorEscapeDistGenMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) ; +// Toro convesso static double CAvTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot) ; -static double TorusPointEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, - const Point3d& ptP, const Vector3d& vtMove) ; +static double TorusPointEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad, + const Point3d& ptP, const Vector3d& vtMove) ; static double TorusSegmentEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ; -static double TorusTriangleInteriorEscapeDistLongMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, - const Triangle3d& trTria, const Vector3d& vtMove) ; -static double TorusSegmentEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, - const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove, - bool bTop, bool bBot) ; -static double TorusTriangleInteriorEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, - const Triangle3d& trTria, const Vector3d& vtMove) ; +static double TorusTriaInteriorEscapeDistLongMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, + const Triangle3d& trTria, const Vector3d& vtMove) ; +static double TorusPointEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, + const Point3d& ptP, const Vector3d& vtMove) ; static double TorusSegmentEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove, bool bTop, bool bBot) ; +static double TorusTriaInteriorEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, + const Triangle3d& trTria, const Vector3d& vtMove) ; static double TorusPointEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Point3d& ptP, const Vector3d& vtMove) ; static double TorusSegmentEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ; -static double TorusTriangleInteriorEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, - const Triangle3d& trTria, const Vector3d& vtMove) ; +static double TorusTriaInteriorEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, + const Triangle3d& trTria, const Vector3d& vtMove) ; +// Toro concavo static double CAvConcaveTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot) ; +static double ConcaveTorusPointEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad, + const Point3d& ptP, const Vector3d& vtMove) ; static double ConcaveTorusSegmentEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ; +static double ConcaveTorusPointEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, + const Point3d& ptP, const Vector3d& vtMove) ; static double ConcaveTorusSegmentEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove, bool bTop, bool bBot) ; @@ -108,6 +114,7 @@ static double ConcaveTorusPointEscapeDistGenMot( const Point3d& ptTorusCen, cons const Point3d& ptP, const Vector3d& vtMove) ; static double ConcaveTorusSegmentEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ; +// Disco static double CAvDiskTriangle( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad, const Triangle3d& trTria, const Vector3d& vtMove) ; static double DiskPointEscapeDistLongMot( const Point3d& ptDiskCen, double dDiskRad, @@ -127,6 +134,7 @@ 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) ; @@ -146,8 +154,6 @@ static int SphereLineTangentPoints( const Point3d& ptSpheCen, double dSpheRad static int DiskPlaneLastContactLongMot( const Point3d& ptDiskCen, double dDiskRad, const Point3d& ptPlane, const Vector3d& vtPlane, const Vector3d& vtMove, Point3d& ptTouch) ; -static double EvalSecondDegreePolynomial( double dCoeff0, double dCoeff1, double dCoeff2, double dVariable) - { return (( dCoeff2 * dVariable + dCoeff1) * dVariable + dCoeff0) ; } static bool ClampSegmentOutPlane( const Plane3d& plPlane, Point3d& ptSegP, const Vector3d& vtSegV, double& dSegLen, double dSignTol = 0) ; //---------------------------------------------------------------------------- @@ -532,8 +538,8 @@ SpherePointEscapeDist( const Point3d& ptSpheCen, double dSpheRad, const Point3d // Il segmento è descritto da punto iniziale, versore della direzione e lunghezza. double SphereSegmentEscapeDist( const Point3d& ptSpheCen, double dSpheRad, - const Point3d& ptSeg, const Vector3d& vtSegDir, double dSegLen, - const Vector3d& vtMove) + const Point3d& ptSeg, const Vector3d& vtSegDir, double dSegLen, + const Vector3d& vtMove) { // Controllo con l'interno del segmento double dU[2] ; @@ -661,7 +667,7 @@ CAvCylinderTriangle( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double d // dell'asse di simmetria e dal versore della direzione del moto. double CylPointEscapeDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dCylHei, double dCylRad, - const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot) + const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot) { double dTopTol = bTop ? EPS_SMALL : - EPS_SMALL ; double dBotTol = bBot ? - EPS_SMALL : EPS_SMALL ; @@ -692,8 +698,8 @@ CylPointEscapeDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, // dell'asse di simmetria e dal versore della direzione del moto. double CylSegmentEscapeDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dCylHei, double dCylRad, - const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove, - bool bTop, bool bBot) + const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove, + bool bTop, bool bBot) { double dTopTol = ( bTop ? EPS_SMALL : - EPS_SMALL) ; double dBotTol = ( bBot ? - EPS_SMALL : EPS_SMALL) ; @@ -769,7 +775,7 @@ CylSegmentEscapeDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx //---------------------------------------------------------------------------- double CylTriaInteriorEscapeDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dCylHei, double dCylRad, - const Triangle3d& trTria, const Vector3d& vtMove) + const Triangle3d& trTria, const Vector3d& vtMove) { // La superficie laterale può trovarsi in estremo contatto con l'interno solo in tangenza. // Se non ci può essere tangenza guardo il disco sopra per le giunzioni @@ -796,7 +802,7 @@ CylTriaInteriorEscapeDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vt //---------------------------------------------------------------------------- double CylPointEscapeDistGenMot( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad, - const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot) + const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot) { // Setto le tolleranze sui limiti del cilindro double dTopTol = ( bTop ? EPS_SMALL : - EPS_SMALL) ; @@ -827,8 +833,8 @@ CylPointEscapeDistGenMot( const Point3d& ptCylOrig, const Vector3d& vtCylAx, dou //---------------------------------------------------------------------------- double CylSegmentEscapeDistGenMot( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad, - const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, - const Vector3d& vtMove, bool bTop, bool bBot) + const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, + const Vector3d& vtMove, bool bTop, bool bBot) { // Setto le tolleranze sui limiti del cilindro double dTopTol = ( bTop ? EPS_SMALL : - EPS_SMALL) ; @@ -918,7 +924,7 @@ CylSegmentEscapeDistGenMot( const Point3d& ptCylOrig, const Vector3d& vtCylAx, d //---------------------------------------------------------------------------- double CylTriaInteriorEscapeDistGenMot( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad, - const Triangle3d& trTria, const Vector3d& vtMove) + const Triangle3d& trTria, const Vector3d& vtMove) { double dEscapeDist = 0. ; if ( ! AreOrthoApprox( vtCylAx, trTria.GetN()) || AreOrthoApprox( vtMove, trTria.GetN())) @@ -1059,7 +1065,7 @@ CAvTrConeTriangle( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double //---------------------------------------------------------------------------- double TrConePointEscapeDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, - double dTrConeH, const Point3d& ptP, const Vector3d& vtMove) + double dTrConeH, const Point3d& ptP, const Vector3d& vtMove) { double dPointAxisSqDist = GetPointLineSqDist( ptP, ptMinBase, vtTrConeAx) ; // Non c'è interferenza fra punto e superficie se la distanza del punto dall'asse non è compresa fra i raggi @@ -1079,8 +1085,8 @@ TrConePointEscapeDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrCone //---------------------------------------------------------------------------- double TrConeSegmentEscapeDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, - double dTrConeH, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, - const Vector3d& vtMove) + double dTrConeH, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, + const Vector3d& vtMove) { // Tangente dell'angolo di semi-apertura del cono e vertice del cono double dTanTheta = ( dMaxBaseR - dMinBaseR) / dTrConeH ; @@ -1171,7 +1177,7 @@ TrConeSegmentEscapeDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrCo //---------------------------------------------------------------------------- double TrConeTriaInteriorEscapeDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, - double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) + double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) { double dEscapeDist = 0. ; // Affinché ci possa essere tangenza, l'angolo fra il versore dell'asse A @@ -1207,8 +1213,8 @@ TrConeTriaInteriorEscapeDistLongMot( const Point3d& ptMinBase, const Vector3d& v //---------------------------------------------------------------------------- double TrConePointEscapeDistOrtMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, - double dTrConeH, const Point3d& ptP, const Vector3d& vtMove, - bool bTop, bool bBot) + double dTrConeH, const Point3d& ptP, const Vector3d& vtMove, + bool bTop, bool bBot) { double dTopTol = bTop ? EPS_SMALL : - EPS_SMALL ; double dBotTol = bBot ? - EPS_SMALL : EPS_SMALL ; @@ -1224,8 +1230,8 @@ TrConePointEscapeDistOrtMot( const Point3d& ptMinBase, const Vector3d& vtTrConeA //---------------------------------------------------------------------------- double TrConeSegmentEscapeDistOrtMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, - double dTrConeH, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, - const Vector3d& vtMove, bool bTop, bool bBot) + double dTrConeH, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, + const Vector3d& vtMove, bool bTop, bool bBot) { double dTopTol = bTop ? EPS_SMALL : - EPS_SMALL ; double dBotTol = bBot ? - EPS_SMALL : EPS_SMALL ; @@ -1280,7 +1286,7 @@ TrConeSegmentEscapeDistOrtMot( const Point3d& ptMinBase, const Vector3d& vtTrCon //---------------------------------------------------------------------------- double TrConeTriaInteriorEscapeDistOrtMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, - double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) + double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) { double dEscapeDist = 0. ; // Affinché ci possa essere tangenza, l'angolo fra il versore dell'asse A @@ -1316,7 +1322,7 @@ TrConeTriaInteriorEscapeDistOrtMot( const Point3d& ptMinBase, const Vector3d& vt //---------------------------------------------------------------------------- double TrConePointEscapeDistGenMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, - double dTrConeH, const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot) + double dTrConeH, const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot) { double dTopTol = bTop ? EPS_SMALL : - EPS_SMALL ; double dBotTol = bBot ? - EPS_SMALL : EPS_SMALL ; @@ -1353,8 +1359,8 @@ TrConePointEscapeDistGenMot( const Point3d& ptMinBase, const Vector3d& vtTrConeA //---------------------------------------------------------------------------- double TrConeSegmentEscapeDistGenMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, - double dTrConeH, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, - const Vector3d& vtMove, bool bTop, bool bBot) + double dTrConeH, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, + const Vector3d& vtMove, bool bTop, bool bBot) { // Setto le tolleranze sui limiti del cono double dTopTol = ( bTop ? EPS_SMALL : - EPS_SMALL) ; @@ -1448,7 +1454,7 @@ TrConeSegmentEscapeDistGenMot( const Point3d& ptMinBase, const Vector3d& vtTrCon //---------------------------------------------------------------------------- double TrConeTriaInteriorEscapeDistGenMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR, - double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) + double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) { double dEscapeDist = 0. ; // Affinché ci possa essere tangenza, l'angolo fra il versore dell'asse A @@ -1500,7 +1506,7 @@ CAvTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double d if ( dMaxDistV < 0.) return 0. ; // Distanza di allontanamento dall'interno - double dInEscapeDist = TorusTriangleInteriorEscapeDistLongMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, + double dInEscapeDist = TorusTriaInteriorEscapeDistLongMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, trTria, vtMove) ; if ( dInEscapeDist > EPS_SMALL) return dInEscapeDist ; @@ -1539,7 +1545,7 @@ CAvTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double d ( vtR0 * vtTorusAx < - dMinRad + dBotTol && vtR1 * vtTorusAx < - dMinRad + dBotTol && vtR2 * vtTorusAx < - dMinRad + dBotTol)) return 0. ; // Distanza di allontanamento dall'interno del triangolo: se positivo abbiamo finito - double dInEscapeDist = TorusTriangleInteriorEscapeDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, trTria, vtMove) ; + double dInEscapeDist = TorusTriaInteriorEscapeDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, trTria, vtMove) ; if ( dInEscapeDist > EPS_SMALL) return dInEscapeDist ; // Distanza di allontanamento dai lati @@ -1574,7 +1580,7 @@ CAvTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double d ( dDot0 > dRadSum && dDot1 > dRadSum && dDot2 > dRadSum)) return 0. ; // Distanza di allontanamento dall'interno - double dInnEscapeDist = TorusTriangleInteriorEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, trTria, vtMove) ; + double dInnEscapeDist = TorusTriaInteriorEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, trTria, vtMove) ; // Se positiva abbiamo finito if ( dInnEscapeDist > EPS_SMALL) return dInnEscapeDist ; @@ -1593,9 +1599,9 @@ CAvTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double d } //---------------------------------------------------------------------------- -static double +double TorusPointEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad, - const Point3d& ptP, const Vector3d& vtMove) + const Point3d& ptP, const Vector3d& vtMove) { double dPointAxSqLen = GetPointLineSqDist( ptP, ptTorusCen, vtMove) ; double dDeltaRad = sqrt( dPointAxSqLen) - dMaxRad ; @@ -1610,7 +1616,7 @@ TorusPointEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double d //---------------------------------------------------------------------------- double TorusSegmentEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad, - const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) + const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) { // Se segmento dista dall'asse del toro più del raggio esterno, posso uscire subito double dLinSegSqDist = LineSegmentSqDist( ptTorusCen, vtMove, ptSeg, vtSeg, dSegLen) ; @@ -1686,8 +1692,8 @@ TorusSegmentEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double //---------------------------------------------------------------------------- double -TorusTriangleInteriorEscapeDistLongMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, - const Triangle3d& trTria, const Vector3d& vtMove) +TorusTriaInteriorEscapeDistLongMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, + const Triangle3d& trTria, const Vector3d& vtMove) { // Siamo interessati solo ai casi di tangenza, poiché il punto di contatto // estremo con l'interno di un triangolo non può che essere di tangenza. @@ -1716,7 +1722,7 @@ TorusTriangleInteriorEscapeDistLongMot( const Point3d& ptTorusCen, const Vector3 //---------------------------------------------------------------------------- double TorusPointEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, - const Point3d& ptP, const Vector3d& vtMove) + const Point3d& ptP, const Vector3d& vtMove) { Vector3d vtR = ptP - ptTorusCen ; // Quota del piano di movimento del punto @@ -1742,8 +1748,8 @@ TorusPointEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx //---------------------------------------------------------------------------- double TorusSegmentEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, - const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove, - bool bTop, bool bBot) + const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove, + bool bTop, bool bBot) { // Dati segmento modificabli Point3d ptMySeg = ptSeg ; @@ -1751,12 +1757,12 @@ TorusSegmentEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorus // Versore ortogonale a asse utensile e movimento Vector3d vtTrasv = vtTorusAx ^ vtMove ; vtTrasv.Normalize() ; - // Piano a sinistra + // Piano a destra Plane3d plLeft ; plLeft.Set( ptTorusCen - ( dMaxRad + dMinRad) * vtTrasv, vtTrasv) ; if ( ! ClampSegmentOutPlane( plLeft, ptMySeg, vtSeg, dMySegLen)) return 0. ; - // Piano a destra + // Piano a sinistra Plane3d plRight ; plRight.Set( ptTorusCen + ( dMaxRad + dMinRad) * vtTrasv, -vtTrasv) ; if ( ! ClampSegmentOutPlane( plRight, ptMySeg, vtSeg, dMySegLen)) @@ -1781,7 +1787,7 @@ TorusSegmentEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorus // Cerco distanza di massimo allontanamento lungo il segmento ridotto double dEscapeDist = 0. ; double dStep = max( 0.1 * dMinRad, EPS_SMALL) ; - int nStepNum = int( dMySegLen / dStep) ; + int nStepNum = max( int( dMySegLen / dStep), 1) ; dStep = dMySegLen / nStepNum ; int nMax = -1 ; for ( int i = 0 ; i <= nStepNum ; ++ i) { @@ -1829,8 +1835,8 @@ TorusSegmentEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorus //---------------------------------------------------------------------------- double -TorusTriangleInteriorEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, - const Triangle3d& trTria, const Vector3d& vtMove) +TorusTriaInteriorEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, + const Triangle3d& trTria, const Vector3d& vtMove) { // Componente ortogonale all'asse del toro del vettore del piano del triangolo Vector3d vtPlaneOrtToAx = trTria.GetN() - ( trTria.GetN() * vtTorusAx) * vtTorusAx ; @@ -1858,7 +1864,7 @@ TorusTriangleInteriorEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d //---------------------------------------------------------------------------- double TorusPointEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, - const Point3d& ptP, const Vector3d& vtMove) + const Point3d& ptP, const Vector3d& vtMove) { // Intersezione fra semi-retta e corona torica BOOLVECTOR vbType ; @@ -1879,49 +1885,89 @@ TorusPointEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx //---------------------------------------------------------------------------- double TorusSegmentEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, - const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) + const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) { + Point3d ptMySeg = ptSeg ; + double dMySegLen = dSegLen ; // Restringo il segmento nella striscia di spazio in cui si sposta il toro - Point3d ptMySegP = ptSeg ; - Vector3d vtPlane = vtTorusAx ^ vtMove ; - if ( ! vtPlane.Normalize()) + Vector3d vtTrasv = vtTorusAx ^ vtMove ; + if ( ! vtTrasv.Normalize()) return - 1. ; - Point3d ptPlaneLatPlus = ptTorusCen + ( dMaxRad + dMinRad) * vtPlane ; + // Piano a sinistra Plane3d plPlane ; - plPlane.Set( ( ORIG - ptPlaneLatPlus) * vtPlane, - vtPlane) ; - ClampSegmentOutPlane( plPlane, ptMySegP, vtSeg, dSegLen) ; - Point3d ptPlanLateMinus = ptTorusCen - ( dMaxRad + dMinRad) * vtPlane ; - plPlane.Set( ( ptPlanLateMinus - ORIG) * vtPlane, vtPlane) ; - ClampSegmentOutPlane( plPlane, ptMySegP, vtSeg, dSegLen) ; + plPlane.Set( ptTorusCen + ( dMaxRad + dMinRad) * vtTrasv, - vtTrasv) ; + if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen)) + return 0. ; + // Piano a destra + plPlane.Set( ptTorusCen - ( dMaxRad + dMinRad) * vtTrasv, vtTrasv) ; + if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen)) + return 0. ; double dDotAxMv = vtMove * vtTorusAx ; Vector3d vtOrtMv = vtMove - dDotAxMv * vtTorusAx ; if ( ! vtOrtMv.Normalize()) return - 1. ; - Point3d ptPlaneBehind = ptTorusCen - ( dMaxRad + dMinRad) * vtOrtMv ; - plPlane.Set( ( ptPlaneBehind - ORIG) * vtOrtMv, vtOrtMv) ; - ClampSegmentOutPlane( plPlane, ptMySegP, vtSeg, dSegLen) ; + // Piano dietro + plPlane.Set( ptTorusCen - ( dMaxRad + dMinRad) * vtOrtMv, vtOrtMv) ; + if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen)) + return 0. ; + // Piano orizzontale Point3d ptUpDw = ptTorusCen - ( dDotAxMv > 0. ? dMinRad : 0.) * vtTorusAx ; Vector3d vtUPDw = dDotAxMv > 0. ? vtTorusAx : - vtTorusAx ; - plPlane.Set( ( ptUpDw - ORIG) * vtUPDw, vtUPDw) ; - ClampSegmentOutPlane( plPlane, ptMySegP, vtSeg, dSegLen) ; + plPlane.Set( ptUpDw, vtUPDw) ; + if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen)) + return 0. ; // Cerco il punto del segmento ridotto che ha distanza di allontanamento massima double dEscapeDist = 0. ; - double dStep = max( 0.05 * dMinRad, EPS_SMALL) ; - int nStepNum = int( dSegLen / dStep) ; - dStep = dSegLen / nStepNum ; + double dStep = max( 0.1 * dMinRad, EPS_SMALL) ; + int nStepNum = max( int( dMySegLen / dStep), 1) ; + dStep = dMySegLen / nStepNum ; + int nMax = -1 ; for ( int n = 0 ; n <= nStepNum ; ++ n) { - Point3d ptCurPoint = ptMySegP + ( n * dStep) * vtSeg ; + Point3d ptCurPoint = ptMySeg + ( n * dStep) * vtSeg ; double dCurrDist = TorusPointEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ; - if ( dEscapeDist < dCurrDist) + if ( dEscapeDist < dCurrDist) { dEscapeDist = dCurrDist ; + nMax = n ; + } } + // affino la ricerca (curva parabolica nell'intorno del massimo trovato) + if ( nMax != -1) { + // nuovo passo e punto di partenza + const int SUB_STEPS = 10 ; + double dNewStep = dStep / SUB_STEPS ; + Point3d ptStart = ptMySeg + ( nMax * dStep) * vtSeg ; + // cerco dopo + if ( nMax < nStepNum) { + for ( int j = 1 ; j < SUB_STEPS ; ++ j) { + Point3d ptCurPoint = ptStart + ( j * dNewStep) * vtSeg ; + // Minima distanza per evitare collisione + double dDist = TorusPointEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ; + if ( dDist > dEscapeDist) + dEscapeDist = dDist ; + else + break ; + } + } + // cerco prima + if ( nMax > 0) { + for ( int j = 1 ; j < SUB_STEPS ; ++ j) { + Point3d ptCurPoint = ptStart - ( j * dNewStep) * vtSeg ; + // Minima distanza per evitare collisione + double dDist = TorusPointEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ; + if ( dDist > dEscapeDist) + dEscapeDist = dDist ; + else + break ; + } + } + } return dEscapeDist ; } //---------------------------------------------------------------------------- double -TorusTriangleInteriorEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, - const Triangle3d& trTria, const Vector3d& vtMove) +TorusTriaInteriorEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, + const Triangle3d& trTria, const Vector3d& vtMove) { // Siamo interessati solo ai casi di tangenza, poiché il punto di contatto // estremo con l'interno di un triangolo non può che essere di tangenza. @@ -2016,25 +2062,20 @@ CAvConcaveTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, d //---------------------------------------------------------------------------- double -ConcaveEvalEscapeDist( const Point3d& ptTorusCen, double dMaxRad, double dMinRad, - const Point3d& ptSeg, const Vector3d& vtSeg, double dPar, const Vector3d& vtMove) +ConcaveTorusPointEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad, + const Point3d& ptP, const Vector3d& vtMove) { - double dSqMinRad = dMinRad * dMinRad ; - double dSqMaxRad = dMaxRad * dMaxRad ; - double dSqInnRad = ( dMaxRad - dMinRad) * ( dMaxRad - dMinRad) ; - Point3d ptCurPoint = ptSeg + dPar * vtSeg ; - double dPointAxSqLen = Clamp( GetPointLineSqDist( ptCurPoint, ptTorusCen, vtMove), dSqInnRad, dSqMaxRad) ; - double dDeltaRad = dMaxRad - sqrt( dPointAxSqLen) ; - double dSqDeltaEscapeDist = dSqMinRad - dDeltaRad * dDeltaRad ; - double dDeltaEscapeDist = ( dSqDeltaEscapeDist > 0. ? sqrt( dSqDeltaEscapeDist) : 0.) ; - double dEscapeDist = ( ptCurPoint - ptTorusCen) * vtMove - dDeltaEscapeDist ; - return dEscapeDist ; + double dPointAxSqLen = GetPointLineSqDist( ptP, ptTorusCen, vtMove) ; + double dDeltaRad = sqrt( dPointAxSqLen) - dMaxRad ; + if ( dDeltaRad > 0 || dDeltaRad < - dMinRad) + return 0. ; + return ( ptP - ptTorusCen) * vtMove - sqrt( dMinRad * dMinRad - dDeltaRad * dDeltaRad) ; } //---------------------------------------------------------------------------- double ConcaveTorusSegmentEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad, - const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) + const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) { // Se segmento dista dall'asse del toro più del raggio massimo, posso uscire subito double dLinSegSqDist = LineSegmentSqDist( ptTorusCen, vtMove, ptSeg, vtSeg, dSegLen) ; @@ -2069,128 +2110,160 @@ ConcaveTorusSegmentEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, SegLimits.Subtract( dIntLen1, dIntLen2) ; } - double dMaxEscapeDist = 0.0 ; + // Eseguo controllo sugli intervalli + double dMaxEscapeDist = 0. ; double dLen1, dLen2 ; bool bFound = SegLimits.GetFirst( dLen1, dLen2) ; while ( bFound) { // verifico l'intervallo Point3d ptStart = ptSeg + vtSeg * dLen1 ; - double dLen = dLen2 - dLen1 ; - const double GOLDEN_RATIO = ( sqrt( 5) - 1) * 0.5 ; - double dTol = max( /*0.1*/0.05 * dMinRad, EPS_SMALL) ; + double dLen = dLen2 - dLen1 ; + const double INV_GOLDEN_RATIO = ( sqrt( 5.) - 1.) / 2. ; + double dTol = max( 0.01 * dMinRad, EPS_SMALL) ; double dPSt = 0 ; - double dPEn = dLen ; + double dPEn = dLen ; + double dPIn1 = dPEn - ( dPEn - dPSt) * INV_GOLDEN_RATIO ; + double dPIn2 = dPSt + ( dPEn - dPSt) * INV_GOLDEN_RATIO ; + double dEscapeDist1 = ConcaveTorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad, + ptStart + dPIn1 * vtSeg, vtMove) ; + double dEscapeDist2 = ConcaveTorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad, + ptStart + dPIn2 * vtSeg, vtMove) ; while ( dPEn - dPSt > dTol) { - double dPIn1 = dPEn - ( dPEn - dPSt) * GOLDEN_RATIO ; - double dPIn2 = dPSt + ( dPEn - dPSt) * GOLDEN_RATIO ; - double dEscapeDist1 = ConcaveEvalEscapeDist( ptTorusCen, dMaxRad, dMinRad, ptStart, vtSeg, dPIn1, vtMove) ; - double dEscapeDist2 = ConcaveEvalEscapeDist( ptTorusCen, dMaxRad, dMinRad, ptStart, vtSeg, dPIn2, vtMove) ; - if ( dEscapeDist1 > dEscapeDist2) - dPEn = dPIn2 ; - else + if ( dEscapeDist1 > dEscapeDist2) { + dPEn = dPIn2 ; + dPIn2 = dPIn1 ; + dEscapeDist2 = dEscapeDist1 ; + dPIn1 = dPEn - ( dPEn - dPSt) * INV_GOLDEN_RATIO ; + dEscapeDist1 = ConcaveTorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad, + ptStart + dPIn1 * vtSeg, vtMove) ; + } + else { dPSt = dPIn1 ; + dPIn1 = dPIn2 ; + dEscapeDist1 = dEscapeDist2 ; + dPIn2 = dPSt + ( dPEn - dPSt) * INV_GOLDEN_RATIO ; + dEscapeDist2 = ConcaveTorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad, + ptStart + dPIn2 * vtSeg, vtMove) ; + } } - double dMaxPar = 0.5 * ( dPSt + dPEn) ; - dMaxEscapeDist = max( dMaxEscapeDist, ConcaveEvalEscapeDist( ptTorusCen, dMaxRad, dMinRad, ptStart, vtSeg, dMaxPar, vtMove)) ; + double dPMax = 0.5 * ( dPSt + dPEn) ; + dMaxEscapeDist = max( dMaxEscapeDist, ConcaveTorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad, + ptStart + dPMax * vtSeg, vtMove)) ; // passo al successivo intervallo bFound = SegLimits.GetNext( dLen1, dLen2) ; } return dMaxEscapeDist ; } +//---------------------------------------------------------------------------- +double +ConcaveTorusPointEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, + const Point3d& ptP, const Vector3d& vtMove) +{ + double dSqMinRad = dMinRad * dMinRad ; + Vector3d vtR = ptP - ptTorusCen ; + double dH = vtR * vtTorusAx ; + Vector3d vtPlaneR = vtR - dH * vtTorusAx ; + double dL = vtPlaneR * vtMove ; + Vector3d vtPlaneOrtR = vtPlaneR - dL * vtMove ; + double dDeltaRad = dSqMinRad - dH * dH > 0. ? sqrt( dSqMinRad - dH * dH) : 0. ; + // Raggio della circonferenza alla quota dH + double dHRad = dMaxRad - dDeltaRad ; + double dSqDeltaL = dHRad * dHRad - vtPlaneOrtR.SqLen() ; + double dEscapeDist = dSqDeltaL > 0. ? dL + sqrt( dSqDeltaL) : 0. ; + return dEscapeDist ; +} + //---------------------------------------------------------------------------- double ConcaveTorusSegmentEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, - const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove, - bool bTop, bool bBot) + const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove, + bool bTop, bool bBot) { - double dTopTol = bTop ? EPS_SMALL : - EPS_SMALL ; - double dBotTol = bBot ? - EPS_SMALL : EPS_SMALL ; - - Vector3d vtTrasv = vtTorusAx ^ vtMove ; - vtTrasv.Normalize() ; - Point3d ptBBInf = ptTorusCen - dMaxRad * ( vtTrasv + vtMove) + dBotTol * vtTorusAx ; - Point3d ptBBSup = ptTorusCen + dMaxRad * ( vtTrasv - vtMove) + ( dMinRad + dTopTol) * vtTorusAx ; - // Clamp piani laterali + // Dati segmento modificabli Point3d ptMySeg = ptSeg ; double dMySegLen = dSegLen ; - double dDotSegTrasv = vtSeg * vtTrasv ; - bool bInsideLateral = false ; - if ( abs( dDotSegTrasv) > EPS_ZERO) { - double dMinU = min( ( ( ptBBInf - ptMySeg) * vtTrasv) / dDotSegTrasv, - ( ( ptBBSup - ptMySeg) * vtTrasv) / dDotSegTrasv) ; - double dMaxU = max( ( ( ptBBInf - ptMySeg) * vtTrasv) / dDotSegTrasv, - ( ( ptBBSup - ptMySeg) * vtTrasv) / dDotSegTrasv) ; - if ( dMinU <= dMySegLen && dMaxU >= 0.) { - if ( dMinU > 0.) { - ptMySeg += dMinU * vtSeg ; - dMySegLen -= dMinU ; - dMaxU -= dMinU ; - } - if ( dMaxU < dMySegLen) - dMySegLen -= ( dMySegLen - dMaxU) ; - bInsideLateral = true ; - } - } - else if ( ( ( ptMySeg - ptBBInf) * vtTrasv >= 0. && ( ptMySeg - ptBBSup) * vtTrasv <= 0.) || - ( ( ptMySeg + dMySegLen * vtSeg - ptBBInf) * vtTrasv >= 0. && - ( ptMySeg + dMySegLen * vtSeg - ptBBSup) * vtTrasv <= 0.)) - bInsideLateral = true ; - // Clamp piani orizzontali - double dDotSegAx = vtSeg * vtTorusAx ; - bool bInsideOrizontal = false ; - if ( abs( dDotSegAx) > EPS_ZERO) { - double dMinV = min( ( ( ptBBInf - ptMySeg) * vtTorusAx) / dDotSegAx, - ( ( ptBBSup - ptMySeg) * vtTorusAx) / dDotSegAx) ; - double dMaxV = max( ( ( ptBBInf - ptMySeg) * vtTorusAx) / dDotSegAx, - ( ( ptBBSup - ptMySeg) * vtTorusAx) / dDotSegAx) ; - if ( dMinV <= dMySegLen && dMaxV >= 0.) { - if ( dMinV > 0.) { - ptMySeg += dMinV * vtSeg ; - dMySegLen -= dMinV ; - dMaxV -= dMinV ; - } - if ( dMaxV < dMySegLen) - dMySegLen -= ( dMySegLen - dMaxV) ; - bInsideOrizontal = true ; - } - } - else if ( ( ( ptMySeg - ptBBInf) * vtTorusAx >= 0. && ( ptMySeg - ptBBSup) * vtTorusAx <= 0.) || - ( ( ptMySeg + dMySegLen * vtSeg - ptBBInf) * vtTorusAx >= 0. && - ( ptMySeg + dMySegLen * vtSeg - ptBBSup) * vtTorusAx <= 0.)) - bInsideOrizontal = true ; + // Versore ortogonale a asse utensile e movimento + Vector3d vtTrasv = vtTorusAx ^ vtMove ; + vtTrasv.Normalize() ; + // Piano a destra + Plane3d plLeft ; + plLeft.Set( ptTorusCen - dMaxRad * vtTrasv, vtTrasv) ; + if ( ! ClampSegmentOutPlane( plLeft, ptMySeg, vtSeg, dMySegLen)) + return 0. ; + // Piano a sinistra + Plane3d plRight ; + plRight.Set( ptTorusCen + dMaxRad * vtTrasv, -vtTrasv) ; + if ( ! ClampSegmentOutPlane( plRight, ptMySeg, vtSeg, dMySegLen)) + return 0. ; + // Piano dietro + Plane3d plBack ; + plBack.Set( ptTorusCen - dMaxRad * vtMove, vtMove) ; + if ( ! ClampSegmentOutPlane( plBack, ptMySeg, vtSeg, dMySegLen)) + return 0. ; + // Piano sotto + double dBotTol = ( bBot ? - EPS_SMALL : EPS_SMALL) ; + Plane3d plBot ; + plBot.Set( ptTorusCen, vtTorusAx) ; + if ( ! ClampSegmentOutPlane( plBot, ptMySeg, vtSeg, dMySegLen, dBotTol)) + return 0. ; + // Piano sopra + double dTopTol = ( bTop ? -EPS_SMALL : EPS_SMALL) ; + Plane3d plTop ; + plTop.Set( ptTorusCen + dMinRad * vtTorusAx, -vtTorusAx) ; + if ( ! ClampSegmentOutPlane( plTop, ptMySeg, vtSeg, dMySegLen, dTopTol)) + return 0. ; - - double dMaxEscapeDist = 0. ; - if ( bInsideLateral && bInsideOrizontal) { - double dSqMinRad = dMinRad * dMinRad ; - double dStep = max( /*0.1*/0.05 * dMinRad, EPS_SMALL) ; - int nStepNum = int( dMySegLen / dStep) ; - for ( int n = 0 ; n <= nStepNum ; ++ n) { - Point3d ptCurPoint = ptMySeg + ( n * dStep) * vtSeg ; - Vector3d vtR = ptCurPoint - ptTorusCen ; - double dH = vtR * vtTorusAx ; - Vector3d vtPlaneR = vtR - dH * vtTorusAx ; - double dL = vtPlaneR * vtMove ; - Vector3d vtPlaneOrtR = vtPlaneR - dL * vtMove ; - double dDeltaRad = dSqMinRad - dH * dH > 0. ? sqrt( dSqMinRad - dH * dH) : 0. ; - // Raggio della circonferenza alla quota dH - double dHRad = dMaxRad - dDeltaRad ; - double dSqDeltaL = dHRad * dHRad - vtPlaneOrtR.SqLen() ; - if ( dSqDeltaL > 0.) { - double dCurDist = dL + sqrt( dSqDeltaL) ; - if ( dCurDist > dMaxEscapeDist) - dMaxEscapeDist = dCurDist ; + // Cerco distanza di massimo allontanamento lungo il segmento ridotto + double dEscapeDist = 0. ; + double dStep = max( 0.1 * dMinRad, EPS_SMALL) ; + int nStepNum = max( int( dMySegLen / dStep), 1) ; + dStep = dMySegLen / nStepNum ; + int nMax = -1 ; + for ( int n = 0 ; n <= nStepNum ; ++ n) { + Point3d ptCurPoint = ptMySeg + ( n * dStep) * vtSeg ; + double dCurDist = ConcaveTorusPointEscapeDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ; + if ( dCurDist > dEscapeDist) { + dEscapeDist = dCurDist ; + nMax = n ; + } + } + // affino la ricerca (curva parabolica nell'intorno del massimo trovato) + if ( nMax != -1) { + // nuovo passo e punto di partenza + const int SUB_STEPS = 10 ; + double dNewStep = dStep / SUB_STEPS ; + Point3d ptStart = ptMySeg + ( nMax * dStep) * vtSeg ; + // cerco dopo + if ( nMax < nStepNum) { + for ( int j = 1 ; j < SUB_STEPS ; ++ j) { + Point3d ptCurPoint = ptStart + ( j * dNewStep) * vtSeg ; + double dCurDist = ConcaveTorusPointEscapeDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ; + if ( dCurDist > dEscapeDist) + dEscapeDist = dCurDist ; + else + break ; } } - } - return dMaxEscapeDist ; + // cerco prima + if ( nMax > 0) { + for ( int j = 1 ; j < SUB_STEPS ; ++ j) { + Point3d ptCurPoint = ptStart - ( j * dNewStep) * vtSeg ; + double dCurDist = ConcaveTorusPointEscapeDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ; + if ( dCurDist > dEscapeDist) + dEscapeDist = dCurDist ; + else + break ; + } + } + } + return dEscapeDist ; } //---------------------------------------------------------------------------- double ConcaveTorusPointEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, - const Point3d& ptP, const Vector3d& vtMove) + const Point3d& ptP, const Vector3d& vtMove) { BOOLVECTOR vbType ; DBLVECTOR vdPar ; @@ -2211,41 +2284,80 @@ ConcaveTorusPointEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vt //---------------------------------------------------------------------------- double ConcaveTorusSegmentEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad, - const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) + const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) { // Restringo il segmento nella striscia di spazio in cui si sposta il toro - Point3d ptMySegP = ptSeg ; - Vector3d vtPlane = vtTorusAx ^ vtMove ; - if ( ! vtPlane.Normalize()) + Point3d ptMySeg = ptSeg ; + double dMySegLen = dSegLen ; + Vector3d vtTrasv = vtTorusAx ^ vtMove ; + if ( ! vtTrasv.Normalize()) return - 1. ; - Point3d ptPlaneLatPlus = ptTorusCen + dMaxRad * vtPlane ; + // Piano a sinistra Plane3d plPlane ; - plPlane.Set( ( ORIG - ptPlaneLatPlus) * vtPlane, - vtPlane) ; - ClampSegmentOutPlane( plPlane, ptMySegP, vtSeg, dSegLen) ; - Point3d ptPlanLateMinus = ptTorusCen - dMaxRad * vtPlane ; - plPlane.Set( ( ptPlanLateMinus - ORIG) * vtPlane, vtPlane) ; - ClampSegmentOutPlane( plPlane, ptMySegP, vtSeg, dSegLen) ; + plPlane.Set( ptTorusCen + dMaxRad * vtTrasv, - vtTrasv) ; + if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen)) + return 0. ; + // Piano a destra + plPlane.Set( ptTorusCen - dMaxRad * vtTrasv, vtTrasv) ; + if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen)) + return 0. ; double dDotAxMv = vtMove * vtTorusAx ; Vector3d vtOrtMv = vtMove - dDotAxMv * vtTorusAx ; if ( ! vtOrtMv.Normalize()) return - 1. ; - Point3d ptPlaneBehind = ptTorusCen - dMaxRad * vtOrtMv ; - plPlane.Set( ( ptPlaneBehind - ORIG) * vtOrtMv, vtOrtMv) ; - ClampSegmentOutPlane( plPlane, ptMySegP, vtSeg, dSegLen) ; + // Piano dietro + plPlane.Set( ptTorusCen - dMaxRad * vtOrtMv, vtOrtMv) ; + if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen)) + return 0. ; + // Piano orizzontale Point3d ptUpDw = ptTorusCen + ( dDotAxMv > 0. ? 0. : dMinRad) * vtTorusAx ; Vector3d vtUPDw = dDotAxMv > 0. ? vtTorusAx : - vtTorusAx ; - plPlane.Set( ( ptUpDw - ORIG) * vtUPDw, vtUPDw) ; - ClampSegmentOutPlane( plPlane, ptMySegP, vtSeg, dSegLen) ; + plPlane.Set( ptUpDw, vtUPDw) ; + if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen)) + return 0. ; // Cerco il punto del segmento ridotto che ha distanza di allontanamento massima double dEscapeDist = 0. ; - double dStep = max( 0.05 * dMinRad, EPS_SMALL) ; - int nStepNum = int( dSegLen / dStep) ; + double dStep = max( 0.1 * dMinRad, EPS_SMALL) ; + int nStepNum = max( int( dMySegLen / dStep), 1) ; + dStep = dMySegLen / nStepNum ; + int nMax = -1 ; for ( int n = 0 ; n <= nStepNum ; ++ n) { - Point3d ptCurPoint = ptMySegP + ( n * dStep) * vtSeg ; + Point3d ptCurPoint = ptMySeg + ( n * dStep) * vtSeg ; double dCurEscapeDist = ConcaveTorusPointEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ; - if ( dEscapeDist < dCurEscapeDist) + if ( dEscapeDist < dCurEscapeDist) { dEscapeDist = dCurEscapeDist ; - } + nMax = n ; + } + } + // affino la ricerca (curva parabolica nell'intorno del massimo trovato) + if ( nMax != -1) { + // nuovo passo e punto di partenza + const int SUB_STEPS = 10 ; + double dNewStep = dStep / SUB_STEPS ; + Point3d ptStart = ptMySeg + ( nMax * dStep) * vtSeg ; + // cerco dopo + if ( nMax < nStepNum) { + for ( int j = 1 ; j < SUB_STEPS ; ++ j) { + Point3d ptCurPoint = ptStart + ( j * dNewStep) * vtSeg ; + double dCurEscapeDist = ConcaveTorusPointEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ; + if ( dEscapeDist < dCurEscapeDist) + dEscapeDist = dCurEscapeDist ; + else + break ; + } + } + // cerco prima + if ( nMax > 0) { + for ( int j = 1 ; j < SUB_STEPS ; ++ j) { + Point3d ptCurPoint = ptStart - ( j * dNewStep) * vtSeg ; + double dCurEscapeDist = ConcaveTorusPointEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ; + if ( dEscapeDist < dCurEscapeDist) + dEscapeDist = dCurEscapeDist ; + else + break ; + } + } + } return dEscapeDist ; } @@ -2379,7 +2491,7 @@ CAvDiskTriangle( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDis //---------------------------------------------------------------------------- double DiskPointEscapeDistLongMot( const Point3d& ptDiskCen, double dDiskRad, - const Point3d& ptP, const Vector3d& vtMove) + const Point3d& ptP, const Vector3d& vtMove) { if ( GetPointLineSqDist( ptP, ptDiskCen, vtMove) < dDiskRad * dDiskRad - 2 * dDiskRad * EPS_SMALL) return PointPlaneSignedDist( ptP, ptDiskCen, vtMove) ; @@ -2389,8 +2501,8 @@ DiskPointEscapeDistLongMot( const Point3d& ptDiskCen, double dDiskRad, //---------------------------------------------------------------------------- double DiskSegmentEscapeDistLongMot( const Point3d& ptDiskCen, double dDiskRad, - const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, - const Vector3d& vtMove) + const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, + const Vector3d& vtMove) { // Il disco non può interferire col segmento nel suo moto, se la distanza del // segmento dall'asse di traslazione è maggiore del raggio. @@ -2435,7 +2547,7 @@ DiskSegmentEscapeDistLongMot( const Point3d& ptDiskCen, double dDiskRad, //---------------------------------------------------------------------------- double DiskTriaInteriorEscapeDistLongMot( const Point3d& ptDiskCen, double dDiskRad, - const Triangle3d& trTria, const Vector3d& vtMove) + const Triangle3d& trTria, const Vector3d& vtMove) { // Se disco e triangolo sono complanari if ( AreSameVectorApprox( vtMove, trTria.GetN())) { @@ -2464,7 +2576,7 @@ DiskTriaInteriorEscapeDistLongMot( const Point3d& ptDiskCen, double dDiskRad, //---------------------------------------------------------------------------- double DiskPointEscapeDistOrtMot( const Point3d& ptDisc, const Vector3d& vtDiskAx, double dDiskRad, - const Point3d& ptP, const Vector3d& vtMove) + const Point3d& ptP, const Vector3d& vtMove) { // Vettore congiungente il punto da cui si allontana il disco e il centro del medesimo Vector3d vtP = ptP - ptDisc ; @@ -2491,7 +2603,7 @@ DiskPointEscapeDistOrtMot( const Point3d& ptDisc, const Vector3d& vtDiskAx, doub // versore direzione e lunghezza. double DiskSegmentEscapeDistOrtMot( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad, - const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) + const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) { // Se il segmento non è nel piano, il più remoto punto di contatto è, // se esiete, l'intersezione fra piano e segmento. Il caso in cui il @@ -2523,7 +2635,7 @@ DiskSegmentEscapeDistOrtMot( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, //---------------------------------------------------------------------------- double DiskTriaInteriorEscapeDistOrtMot( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad, - const Triangle3d& trTria, const Vector3d& vtMove) + const Triangle3d& trTria, const Vector3d& vtMove) { double dDist = 0. ; // Vettore direzione della retta intersezione fra il piano del triangolo e quello del disco @@ -2561,7 +2673,7 @@ DiskTriaInteriorEscapeDistOrtMot( const Point3d& ptDiskCen, const Vector3d& vtDi // proiezioni del movimento su z e x del sistema di riferimento CircFrame. double DiskSegmentEscapeDistGenMot( double dDiskRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, - const Vector3d& vtMove) + const Vector3d& vtMove) { // Il caso in cui il segmento è parallelo al versore del moto è gestito dalla // routine per il calcolo della distanza di allontanamento dei punti. @@ -2602,7 +2714,7 @@ DiskSegmentEscapeDistGenMot( double dDiskRad, const Point3d& ptSeg, const Vector //---------------------------------------------------------------------------- double DiskTriaInteriorEscapeDistGenMot( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad, - const Triangle3d& trTria, const Vector3d& vtMove) + const Triangle3d& trTria, const Vector3d& vtMove) { // Se disco e triangolo sono complanari if ( AreSameVectorApprox( vtDiskAx, trTria.GetN())) {