Egalware/EgtGeomKernel
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

EgtGeomKernel :

Browse Source 
- migliorie a CAvToolTriangle derivate da modifiche a toro convesso.
This commit is contained in:
Dario Sassi
2019-01-09 11:09:00 +00:00
parent a9ea0c3bf0
commit 3feb2e1bf2
1 changed files with 327 additions and 215 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CAvToolTriangle.cpp
+327 -215
View File
@@ -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())) {