Egalware/EgtGeomKernel
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
866ed0b3d7dec6d030c73ff24938b58d75e23b53
EgtGeomKernel/CAvToolTriangle.cpp
T
Dario Sassi 866ed0b3d7 EgtGeomKernel 1.9h1 : 
- sistemazioni varie in CAvToolTriangle
- utilizzo di std::async in CAvToolSurfTm
- corretto GetAllTriaAroundVertex di SurfTm
- aggiunto ( nothrow) a tutti i new.
2018-08-08 11:02:56 +00:00

2230 lines
106 KiB
C++
Raw Blame History

//----------------------------------------------------------------------------
// EgalTech 2018-2018
//----------------------------------------------------------------------------
// File : CAvToolTriangle.cpp Data : 19.07.18 Versione : 1.9h1
// Contenuto : Implementazione delle funzioni ToolTriangleCollisionAvoid.
//
//
//
// Modifiche : 10.03.18 LM Creazione modulo.
//
//
//----------------------------------------------------------------------------
#include "stdafx.h"
#include "CurveLine.h"
#include "CurveArc.h"
#include "CAvToolTriangle.h"
#include "IntersLineSurfStd.h"
#include "IntersLineTria.h"
#include "/EgtDev/Include/ENkPolynomialRoots.h"
#include "/EgtDev/Include/EgtNumUtils.h"
#include "/EgtDev/Include/EGkIntervals.h"
using namespace std ;
//----------------------------------------------------------------------------
static bool
GetTopTapFromPrevCurve( const ICurve* pPrevCurve)
{
if ( pPrevCurve == nullptr)
return false ;
if ( pPrevCurve->GetType() == CRV_LINE) {
const ICurveLine* pPrevLine = GetCurveLine( pPrevCurve) ;
const Point3d& ptPrevStart = pPrevLine->GetStart() ;
const Point3d& ptPrevEnd = pPrevLine->GetEnd() ;
if ( abs( ptPrevStart.y - ptPrevEnd.y) > EPS_SMALL ||
ptPrevStart.x > ptPrevEnd.x)
return true ;
}
else if ( pPrevCurve->GetType() == CRV_ARC) {
const ICurveArc* pPrevArc = GetCurveArc( pPrevCurve) ;
const Point3d& ptPrevCen = pPrevArc->GetCenter() ;
if ( abs( ptPrevCen.x) > EPS_SMALL)
return true ;
}
return false ;
}
//----------------------------------------------------------------------------
static bool
GetBotTapFromNextCurve( const ICurve* pNextCurve)
{
if ( pNextCurve == nullptr)
return false ;
if ( pNextCurve->GetType() == CRV_LINE) {
const ICurveLine* pNextLine = GetCurveLine( pNextCurve) ;
const Point3d& ptNextStart = pNextLine->GetStart() ;
const Point3d& ptNextEnd = pNextLine->GetEnd() ;
if ( abs( ptNextStart.y - ptNextEnd.y) > EPS_SMALL ||
ptNextStart.x < ptNextEnd.x)
return true ;
}
else if ( pNextCurve->GetType() == CRV_ARC) {
const ICurveArc* pNextArc = GetCurveArc( pNextCurve) ;
const Point3d& ptNextCen = pNextArc->GetCenter() ;
if ( abs( ptNextCen.x) > EPS_SMALL)
return true ;
}
return false ;
}
//----------------------------------------------------------------------------
// La funzione determina la minima distanza di allontanamento lungo una direzione fissata
// per evitare la collisione tra un utensile ed un triangolo.
// Se nella posizione iniziale non c'è gia collisione si restituisce il valore 0.
// In caso di errore si restituisce il valore -1.
double
CAvToolTriangle( const Tool& tlTool, const Point3d& ptToolOrig, const Vector3d& vtToolAx,
const Triangle3d& trTria, const Vector3d& vtMove)
{
// Non ha senso che il movimento sia in direzione "opposta" a quella dell'asse utensile
if ( vtMove * vtToolAx < - EPS_ZERO)
return -1. ;
// Se avvicinamento non devo fare nulla
if ( vtMove * trTria.GetN() < - EPS_ZERO)
return 0. ;
// se utensile cilindrico
if ( tlTool.GetType() == Tool::CYLMILL) {
// parametri geometrici
double dHeigth = tlTool.GetHeigth() ;
double dRadius = tlTool.GetRadius() ;
// prima determino l'allontanamento del disco inferiore
double dDist = CAvDiskTriangle( ptToolOrig - dHeigth * vtToolAx, vtToolAx, dRadius, trTria, vtMove) ;
if ( dDist < - EPS_SMALL)
return dDist ;
// poi verifico quello del cilindro (tenendo conto di quanto è stata allontanato il disco)
double dDist2 = CAvCylinderTriangle( ptToolOrig + dDist * vtMove, vtToolAx, dHeigth, dRadius, trTria, vtMove, false, false) ;
if ( dDist2 < - EPS_SMALL)
return dDist2 ;
return ( dDist + dDist2) ;
}
// se utensile sferico
else if ( tlTool.GetType() == Tool::BALLMILL) {
// parametri geometrici
double dCylHeigth = tlTool.GetHeigth() - tlTool.GetTipHeigth() ;
double dRadius = tlTool.GetRadius() ;
// prima determino l'allontanamento della sfera
Point3d ptSpheOrig = ptToolOrig - dCylHeigth * vtToolAx ;
double dDist = CAvSphereTriangle( ptSpheOrig, dRadius, trTria, vtMove) ;
if ( dDist < - EPS_SMALL)
return dDist ;
// poi verifico quello del cilindro (tenendo conto di quanto è stata allontanata la sfera)
if ( dCylHeigth < EPS_SMALL)
return dDist ;
Point3d ptCylOrig = ptToolOrig + vtMove * dDist ;
double dDist2 = CAvCylinderTriangle( ptCylOrig, vtToolAx, dCylHeigth, dRadius, trTria, vtMove, false, true) ;
if ( dDist2 < - EPS_SMALL)
return dDist2 ;
return ( dDist + dDist2) ;
}
// se utensile a naso di toro
else if ( tlTool.GetType() == Tool::BULLNOSEMILL) {
// parametri geometrici
double dCylHeigth = tlTool.GetHeigth() - tlTool.GetTipHeigth() ;
// prima determino l'allontanamento del disco inferiore
double dDist = CAvDiskTriangle( ptToolOrig - tlTool.GetHeigth() * vtToolAx, vtToolAx, tlTool.GetTipRadius(),
trTria, vtMove) ;
if ( dDist < - EPS_SMALL)
return dDist ;
// poi verifico quello del toro (tenendo conto di quanto è stata allontanato il disco)
Point3d ptTorusCen = ptToolOrig - dCylHeigth * vtToolAx ;
double dDist2 = CAvTorusTriangle( ptTorusCen + dDist * vtMove, vtToolAx,
tlTool.GetRadius() - tlTool.GetCornRadius(), tlTool.GetCornRadius(),
trTria, vtMove, true, false) ;
if ( dDist2 < - EPS_SMALL)
return dDist2 ;
// infine verifico quello del cilindro (tenendo conto dei precedenti allontanamenti)
if ( dCylHeigth < EPS_SMALL)
return ( dDist + dDist2) ;
Point3d ptCylOrig = ptToolOrig + vtMove * ( dDist + dDist2) ;
double dDist3 = CAvCylinderTriangle( ptCylOrig, vtToolAx, dCylHeigth, tlTool.GetRadius(),
trTria, vtMove, false, true) ;
if ( dDist3 < - EPS_SMALL)
return dDist3 ;
return ( dDist + dDist2 + dDist3) ;
}
// se utensile conico
else if ( tlTool.GetType() == Tool::CONEMILL) {
// parametri geometrici
Point3d ptMinBase ;
Vector3d vtConeAx ;
if ( tlTool.GetRadius() > tlTool.GetTipRadius()) {
ptMinBase = ptToolOrig - tlTool.GetHeigth() * vtToolAx ;
vtConeAx = vtToolAx ;
}
else {
ptMinBase = ptToolOrig - ( tlTool.GetHeigth() - tlTool.GetTipHeigth()) * vtToolAx ;
vtConeAx = - vtToolAx ;
}
double dMinR = min( tlTool.GetRadius(), tlTool.GetTipRadius()) ;
double dMaxR = max( tlTool.GetRadius(), tlTool.GetTipRadius()) ;
double dCylHeigth = tlTool.GetHeigth() - tlTool.GetTipHeigth() ;
// prima determino l'allontanamento del disco inferiore
double dDist = CAvDiskTriangle( ptToolOrig - tlTool.GetHeigth() * vtToolAx, vtToolAx, tlTool.GetTipRadius(),
trTria, vtMove) ;
if ( dDist < - EPS_SMALL)
return dDist ;
// poi verifico quello del cono (tenendo conto di quanto è stata allontanato il disco)
double dDist2 = CAvTrConeTriangle( ptMinBase + dDist * vtMove, vtConeAx, dMinR, dMaxR, tlTool.GetTipHeigth(),
trTria, vtMove, true, false) ;
if ( dDist2 < - EPS_SMALL)
return dDist2 ;
// infine verifico quello del cilindro (tenendo conto dei precedenti allontanamenti)
if ( dCylHeigth < EPS_SMALL)
return ( dDist + dDist2) ;
Point3d ptCylOrig = ptToolOrig + vtMove * ( dDist + dDist2) ;
double dDist3 = CAvCylinderTriangle( ptCylOrig, vtToolAx, dCylHeigth, tlTool.GetRadius(),
trTria, vtMove, false, true) ;
if ( dDist3 < - EPS_SMALL)
return dDist3 ;
return ( dDist + dDist2 + dDist3) ;
}
// se utensile generico
else if ( tlTool.GetType() == Tool::GEN) {
// distanza di allontanamento
double dDist = 0 ;
// riferimento per componenti
Point3d ptCompOrig = ptToolOrig - tlTool.GetHeigth() * vtToolAx ;
// analizzo le curve del profilo a partire dall'ultima
const CurveComposite* pToolProfile = tlTool.GetOutline() ;
int nCrv = pToolProfile->GetCurveCount() - 1 ;
const ICurve* pCurve = pToolProfile->GetCurve( nCrv) ;
const ICurve* pNextCurve = nullptr ;
while ( pCurve != nullptr) {
// distanza di allontanamento del tratto corrente
double dDist2 = 0 ;
// curva precedente
const ICurve* pPrevCurve = pToolProfile->GetCurve( -- nCrv) ;
// Se segmento
if ( pCurve->GetType() == CRV_LINE) {
// Recupero gli estremi
const ICurveLine* pLine = GetCurveLine( pCurve) ;
const Point3d& ptStart = pLine->GetStart() ;
const Point3d& ptEnd = pLine->GetEnd() ;
// Ne determino l'altezza
double dHeight = abs( ptStart.y - ptEnd.y) ;
if ( dHeight <= EPS_SMALL) {
// solo se disco verso il basso dell'utensile
if ( ptStart.x > ptEnd.x) {
double dRadius = max( ptStart.x, ptEnd.x) ;
dDist2 = CAvDiskTriangle( ptCompOrig, vtToolAx, dRadius, trTria, vtMove) ;
}
}
else {
// Verifiche curva precedente per eventuale tappo sopra
bool bTop = GetTopTapFromPrevCurve( pPrevCurve) ;
// Verifiche curva successiva per eventuale tappo sotto
bool bBot = GetBotTapFromNextCurve( pNextCurve) ;
// Calcolo distanza di allontanamento del componente corrente
if ( abs( ptStart.x - ptEnd.x) < EPS_SMALL) {
double dRadius = ptStart.x ;
// riferimento del cilindro in alto
ptCompOrig += dHeight * vtToolAx ;
dDist2 = CAvCylinderTriangle( ptCompOrig, vtToolAx, dHeight, dRadius, trTria, vtMove, bTop, bBot) ;
}
else if ( ptStart.x > ptEnd.x) {
double dMaxRad = ptStart.x ;
double dMinRad = ptEnd.x ;
// riferimento del cono in basso (base più piccola)
dDist2 = CAvTrConeTriangle( ptCompOrig, vtToolAx, dMinRad, dMaxRad, dHeight, trTria, vtMove, bTop, bBot) ;
// sposto riferimento in alto
ptCompOrig += dHeight * vtToolAx ;
}
else if ( ptStart.x < ptEnd.x) {
double dMaxRad = ptEnd.x ;
double dMinRad = ptStart.x ;
// riferimento del cono in alto (base più piccola)
ptCompOrig += dHeight * vtToolAx ;
dDist2 = CAvTrConeTriangle( ptCompOrig, - vtToolAx, dMinRad, dMaxRad, dHeight, trTria, vtMove, bTop, bBot) ;
}
}
}
// Se arco
else if ( pCurve->GetType() == CRV_ARC) {
// Recupero estremi, centro e raggio
const ICurveArc* pArc = GetCurveArc( pCurve) ;
Point3d ptStart ; pArc->GetStartPoint( ptStart) ;
Point3d ptEnd ; pArc->GetEndPoint( ptEnd) ;
const Point3d& ptCen = pArc->GetCenter() ;
double dRadius = pArc->GetRadius() ;
double dAngToCen = pArc->GetAngCenter() ;
// Calcolo della distanza di allontanamento del componente corrente
// Se sfera
if ( abs( ptCen.x) < EPS_SMALL) {
// riferimento sul centro sfera
ptCompOrig += ( ptCen.y - ptEnd.y) * vtToolAx ;
dDist2 = CAvSphereTriangle( ptCompOrig, dRadius, trTria, vtMove) ;
// sposto riferimento in alto
ptCompOrig += ( ptStart.y - ptCen.y) * vtToolAx ;
}
// altrimenti toro
else {
// Verifiche curva precedente per eventuale tappo sopra
bool bTop = GetTopTapFromPrevCurve( pPrevCurve) ;
// Verifiche curva successiva per eventuale tappo sotto
bool bBot = GetBotTapFromNextCurve( pNextCurve) ;
// riferimento sul centro toro
ptCompOrig += ( ptCen.y - ptEnd.y) * vtToolAx ;
// verifica presenza semitori
bool bHalfTorusDown = ( ptEnd.y < ptCen.y - EPS_SMALL) ;
bool bHalfTorusUp = ( ptStart.y > ptCen.y + EPS_SMALL) ;
// semi-toro sotto
if ( bHalfTorusDown) {
double dMyDist ;
if ( dAngToCen < 0)
dMyDist = CAvTorusTriangle( ptCompOrig, vtToolAx, ptCen.x, dRadius, trTria, vtMove,
( bHalfTorusUp ? true : bTop), bBot) ;
else
dMyDist = CAvConcaveTorusTriangle( ptCompOrig, - vtToolAx, ptCen.x, dRadius, trTria, vtMove,
( bHalfTorusUp ? true : bTop), bBot) ;
if ( dMyDist < - EPS_SMALL)
return dMyDist ;
dDist2 = max( dDist2, dMyDist) ;
}
// semi-toro sopra
if ( bHalfTorusUp) {
double dMyDist ;
if ( dAngToCen < 0)
dMyDist = CAvTorusTriangle( ptCompOrig, - vtToolAx, ptCen.x, dRadius, trTria, vtMove,
bTop, ( bHalfTorusDown ? true : bBot)) ;
else
dMyDist = CAvConcaveTorusTriangle( ptCompOrig, vtToolAx, ptCen.x, dRadius, trTria, vtMove,
bTop, ( bHalfTorusDown ? true : bBot)) ;
if ( dMyDist < - EPS_SMALL)
return dMyDist ;
dDist2 = max( dDist2, dMyDist) ;
}
// sposto riferimento in alto
ptCompOrig += ( ptStart.y - ptCen.y) * vtToolAx ;
}
}
// Aggiornamento distanza allontanamento
if ( dDist2 < - EPS_SMALL)
return dDist2 ;
dDist += dDist2 ;
ptCompOrig += dDist2 * vtMove ;
// passo alla curva precedente
pNextCurve = pCurve ;
pCurve = pPrevCurve ;
}
return dDist ;
}
// altrimenti utensile di tipo non gestito
else
return - 1. ;
}
//----------------------------------------------------------------------------
// **** SFERA ****
//----------------------------------------------------------------------------
// La funzione determina la minima distanza di allontanamento lungo una direzione fissata
// per evitare la collisione tra una sfera ed un triangolo.
double
CAvSphereTriangle( const Point3d& ptSpheCen, double dSpheRad, const Triangle3d& trTria, const Vector3d& vtMove)
{
// Se la sfera sta già tutta dalla parte esterna del triangolo, non va ulteriormente allontanata
// Posizione del centro sfera rispetto al piano del triangolo
Vector3d vtPCen = ptSpheCen - trTria.GetP( 0) ;
if ( vtPCen * trTria.GetN() > dSpheRad - EPS_SMALL)
return 0. ;
// Se la fetta di spazio delimitata dal piano sotto alla sfera rispetto alla direzione
// di allontanamento non sia già sopra al triangolo (tutti i vertici del triangolo sono sotto).
Point3d ptBase = ptSpheCen - dSpheRad * vtMove ;
double dMaxDistV = PointPlaneSignedDist( trTria.GetP( 0), ptBase, vtMove) ;
dMaxDistV = max( dMaxDistV, PointPlaneSignedDist( trTria.GetP( 1), ptBase, vtMove)) ;
dMaxDistV = max( dMaxDistV, PointPlaneSignedDist( trTria.GetP( 2), ptBase, vtMove)) ;
if ( dMaxDistV < EPS_SMALL)
return 0. ;
// Valuto Tangenza col piano
double dPlaneLeakDist = SpherePlaneLeakDist( ptSpheCen, dSpheRad, trTria.GetP( 0), trTria.GetN(), vtMove) ;
// Se il punto di tangenza esiste, verifico se è interno al triangolo
if ( dPlaneLeakDist >= 0.) {
Point3d ptTan = ptSpheCen + dPlaneLeakDist * vtMove - dSpheRad * trTria.GetN() ;
// Se il punto è interno abbiamo finito
if ( IsPointInsideTriangle( ptTan, trTria))
return dPlaneLeakDist ;
}
// Valuto le distanze di allontanamento dai segmenti (con i loro estremi) e prendo la maggiore
double dLeakDist = 0. ;
for ( int nSeg = 0 ; nSeg < 3 ; ++ nSeg) {
Vector3d vtSeg = trTria.GetP( ( nSeg + 1) % 3) - trTria.GetP( nSeg) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
double dCurLeakDist = SphereSegmentLeakDist( ptSpheCen, dSpheRad, trTria.GetP( nSeg), vtSeg, dSegLen, vtMove) ;
if ( dCurLeakDist > dLeakDist)
dLeakDist = dCurLeakDist ;
}
return dLeakDist ;
}
//----------------------------------------------------------------------------
// Calcola la distanza di allontanamento lungo una direzione fissata di una sfera da un piano.
// Se ritorno -1 non è errore, ma sfera interseca piano e movimento nel piano.
double
SpherePlaneLeakDist( const Point3d& ptSpheCen, double dSpheRad,
const Point3d& ptPlane, const Vector3d& vtPlaneN, const Vector3d& vtMove)
{
double dCosPM = vtPlaneN * vtMove ;
// Se la direzione di allontanamento sta nel piano
if ( abs( dCosPM) < EPS_ZERO) {
if ( abs( ( ptSpheCen - ptPlane) * vtPlaneN) > dSpheRad - EPS_SMALL)
return 0. ;
else
return -1. ;
}
// altrimenti ...
else {
double dLeakDist = ( dSpheRad - ( ptSpheCen - ptPlane) * vtPlaneN) / dCosPM ;
return max( dLeakDist, 0.) ;
}
}
//----------------------------------------------------------------------------
// Calcola la distanza di allontanamento lungo una direzione fissata di una sfera da un segmento.
// Il segmento è descritto da punto iniziale, versore della direzione e lunghezza.
double
SphereSegmentLeakDist( const Point3d& ptSpheCen, double dSpheRad,
const Point3d& ptSeg, const Vector3d& vtSegDir, double dSegLen,
const Vector3d& vtMove)
{
// Controllo con l'interno del segmento
double dU[2] ;
int nTanPointNum = SphereLineTangentPoints( ptSpheCen, dSpheRad - EPS_SMALL, ptSeg, vtSegDir, dSegLen, vtMove, dU[0], dU[1]) ;
double dLeakDistIn = 0. ;
for ( int nSol = 0 ; nSol < nTanPointNum && nTanPointNum != 3 ; ++ nSol) {
if ( dU[nSol] < 0.)
continue ;
Point3d ptC = ptSpheCen + dU[nSol] * vtMove ;
double dSegPar = ( ptC - ptSeg) * vtSegDir ;
if ( dSegPar > 0 && dSegPar < dSegLen)
dLeakDistIn = dU[nSol] ;
}
// Controllo con gli estremi
Point3d ptSegEnd = ptSeg + dSegLen * vtSegDir ;
double dDistStart = SpherePointLeakDist( ptSpheCen, dSpheRad - EPS_SMALL, ptSeg, vtMove) ;
double dDistEnd = SpherePointLeakDist( ptSpheCen, dSpheRad - EPS_SMALL, ptSegEnd, vtMove) ;
// Restituisco il massimo
return max( dLeakDistIn, max( dDistStart, dDistEnd)) ;
}
//----------------------------------------------------------------------------
// Calcola la distanza di allontanamento lungo una direzione fissata di una sfera da un punto.
double
SpherePointLeakDist( const Point3d& ptSpheCen, double dSpheRad, const Point3d ptP, const Vector3d& vtMove)
{
Vector3d vtOR = ptP - ptSpheCen ;
double dDotORMot = vtOR * vtMove ;
Vector3d vtOROrt = vtOR - dDotORMot * vtMove ;
double dSqOROrtLen = vtOROrt.SqLen() ;
double dSqSphRad = dSpheRad * dSpheRad ;
if ( dSqOROrtLen > dSqSphRad + 2 * dSpheRad * EPS_SMALL)
return 0. ;
else
return max( dDotORMot + sqrt( max( dSqSphRad - dSqOROrtLen, 0.)), 0.) ;
}
//----------------------------------------------------------------------------
// **** CILINDRO ****
//----------------------------------------------------------------------------
// La funzione determina la minima distanza di allontanamento lungo una direzione fissata
// per evitare la collisione tra un cilindro ed un triangolo.
double
CAvCylinderTriangle( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad,
const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot)
{
// Movimento lungo la direzione dell'asse del cilindro
if ( AreSameVectorApprox( vtCylAx, vtMove)) {
// la distanza di fuga è determinata dal disco
// o dai componenti sotto il cilindro.
return 0. ;
}
// Movimento perpendicolare all'asse del cilindro
else if ( AreOrthoApprox( vtCylAx, vtMove)) {
double dLeakDist = 0. ;
// INTERNO : se distanza di allontanamento da interno è positiva, abbiamo finito
Point3d ptTopCont, ptBotCont ;
double dTopLeak = DiskPlaneLeakDistOrtMot( ptCylOrig, vtCylAx, dRad, trTria.GetP( 0), trTria.GetN(),
vtMove, ptTopCont) ;
double dBotLeak = DiskPlaneLeakDistOrtMot( ptCylOrig - dHeigth * vtCylAx, vtCylAx, dRad,
trTria.GetP( 0), trTria.GetN(), vtMove, ptBotCont) ;
bool bTopInn = IsPointInsideTriangle( ptTopCont, trTria) ;
bool bBotInn = IsPointInsideTriangle( ptBotCont, trTria) ;
if ( bTopInn) {
if ( bBotInn)
dLeakDist = max( dLeakDist, max( dTopLeak, dBotLeak)) ;
else if ( dTopLeak > dBotLeak)
dLeakDist = max( dLeakDist, dTopLeak) ;
}
else if ( bBotInn) {
if ( dBotLeak > dTopLeak)
dLeakDist = max( dLeakDist, dBotLeak) ;
}
else if ( abs( dTopLeak - dBotLeak) < EPS_SMALL) {
Point3d ptInt1, ptInt2 ;
int nRes = IntersLineTria( ptTopCont, ptBotCont, trTria, ptInt1, ptInt2) ;
if ( nRes != ILTT_NO)
dLeakDist = max( dLeakDist, ( dTopLeak + dBotLeak) / 2) ;
}
// Se distanza da interno positiva abbiamo finito
if ( dLeakDist > EPS_SMALL)
return dLeakDist ;
// PUNTI
for ( int nVrt = 0 ; nVrt < 3 ; ++ nVrt) {
double dCurDist = CylPointLeakDistOrtMotion( ptCylOrig, vtCylAx, dHeigth, dRad, trTria.GetP( nVrt), vtMove, bTop, bBot) ;
if ( dLeakDist < dCurDist)
dLeakDist = dCurDist ;
}
// SEGMENTI
for ( int nVrtS = 0 ; nVrtS < 3 ; ++ nVrtS) {
int nVrtE = ( nVrtS + 1) % 3 ;
Vector3d vtSeg = trTria.GetP( nVrtE) - trTria.GetP( nVrtS) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
double dCurDist = CylSegmentLeakDistOrtMotion( ptCylOrig, vtCylAx, dHeigth, dRad,
trTria.GetP( nVrtS), vtSeg, dSegLen, vtMove, bTop, bBot) ;
if ( dLeakDist < dCurDist)
dLeakDist = dCurDist ;
}
return dLeakDist ;
}
// Movimento generico
else if ( vtMove * vtCylAx > 0)
return 0. ;
// Errore
else
return -1. ;
}
//----------------------------------------------------------------------------
// Restituisce la distanza di fuga di un cilindro da un punto nel caso di moto
// ortogonale all'asse di simmetria. Il cilindro è descritto da raggio e altezza.
// Il suo moto è descritto dalla posizione iniziale, dal versore (NORMA UNITARIA)
// dell'asse di simmetria e dal versore della direzione del moto.
double
CylPointLeakDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dCylHei, double dCylRad,
const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot)
{
double dTopTol = bTop ? EPS_SMALL : - EPS_SMALL ;
double dBotTol = bBot ? - EPS_SMALL : EPS_SMALL ;
double dSqRad = dCylRad * dCylRad ;
Vector3d vtRP = ptP - ptCylOrig ;
// Se il punto sta al di sopra o al di sotto rispetto al cilindro abbiamo finito
double dDotRPCylAx = vtRP * vtCylAx ;
if ( dDotRPCylAx > dTopTol || dDotRPCylAx < - dCylHei + dBotTol)
return 0. ;
Vector3d vtPlaneRP = vtRP - dDotRPCylAx * vtCylAx ;
// Se il punto sta dietro al cilindro abbiamo finito
double dDotPlaneRPRemDir = vtPlaneRP * vtMove ;
if ( dDotPlaneRPRemDir < 0. && vtPlaneRP.SqLen() > dSqRad)
return 0. ;
Vector3d vtPlaneOrtRP = vtPlaneRP - vtPlaneRP * vtMove * vtMove ;
// Se il punto è a destra o sinistra del cilindro abbiamo finito
double dSqOrtLen = vtPlaneOrtRP.SqLen() ;
if ( dSqOrtLen > dSqRad + 2 * dCylRad * EPS_SMALL)
return 0. ;
// Calcolo e restituisco la distanza
return dDotPlaneRPRemDir + sqrt( max( dSqRad - dSqOrtLen, 0.)) ;
}
//----------------------------------------------------------------------------
// Restituisce la distanza di allontanamento di un cilindro da un segmento.
// Il cilindro è descritto da raggio e altezza.
// Il suo moto è descritto dalla posizione iniziale, dal versore (NORMA UNITARIA)
// dell'asse di simmetria e dal versore della direzione del moto.
double
CylSegmentLeakDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dCylHei, double dCylRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove,
bool bTop, bool bBot)
{
double dTopTol = ( bTop ? EPS_SMALL : - EPS_SMALL) ;
double dBotTol = ( bBot ? - EPS_SMALL : EPS_SMALL) ;
// Riferimento con origine nel centro del disco nella posizione iniziale.
// X := vtCylAx, Y:= vtMove, Z:= vtCylAx ^ vtMove.
Vector3d vtPlane = vtCylAx ^ vtMove ;
vtPlane.Normalize() ;
Point3d ptSegEnd = ptSeg + dSegLen * vtSeg ;
Vector3d vtSegStart = ptSeg - ptCylOrig ;
Vector3d vtSegEnd = ptSegEnd - ptCylOrig ;
double dCordStart1 = vtSegStart * vtCylAx ;
double dCordEnd1 = vtSegEnd * vtCylAx ;
Vector3d vtSegStart1 = dCordStart1 * vtCylAx ;
Vector3d vtSegEnd1 = dCordEnd1 * vtCylAx ;
Vector3d vtSegStart23 = vtSegStart - vtSegStart1 ;
Vector3d vtSegEnd23 = vtSegEnd - vtSegEnd1 ;
// Se entrambi gli estremi sono sopra o sotto, non ci può essere interferenza
if ( ( dCordStart1 > dTopTol && dCordEnd1 > dTopTol) ||
( dCordStart1 < - dCylHei + dBotTol && dCordEnd1 < - dCylHei + dBotTol))
return 0. ;
// Se entrambi gli estremi del segmento sono a un lato, non ci può essere interferenza
double dCordStart2 = vtSegStart23 * vtPlane ;
double dCordEnd2 = vtSegEnd23 * vtPlane ;
if ( ( dCordStart2 > dCylRad && dCordEnd2 > dCylRad) ||
( dCordStart2 < - dCylRad && dCordEnd2 < - dCylRad))
return 0. ;
// Verifico la distanza di fuga di superficie laterale e dischi di base e fondo.
double dBaseLeakDist = DiskSegmentLeakDistOrtMot( ptCylOrig, vtCylAx, dCylRad, ptSeg, vtSeg, dSegLen, vtMove) ;
double dBottomLeakDist = DiskSegmentLeakDistOrtMot( ptCylOrig - dCylHei * vtCylAx, vtCylAx, dCylRad,
ptSeg, vtSeg, dSegLen, vtMove) ;
double dSurfLeakDist = 0. ;
// Se il vettore del segmento è parallelo all'asse del cilindro,
// il suo prodotto vettoriale con il versore dell'asse è nullo.
Vector3d vtRad = vtCylAx ^ vtSeg ;
if ( ! vtRad.Normalize()) {
dSurfLeakDist = max( ( ptSeg - ptCylOrig) * vtMove, 0.) ;
}
// Se il versore radiale NON è ortogonale a quello di movimento può esserci tangenza,
// altrimenti il versore del segmento non ha componenti ortogonali al piano
// generato da asse cilindro e moto e non può esserci tangenza.
else if ( abs( vtRad * vtMove) > EPS_SMALL) {
// Nella posizione finale il cilindro e la retta del segmento sono tangenti.
// Vettore che spicca dal punto di tangenza fra cilindro e retta
// associata al segmento e termina sull'asse del cilindro.
vtRad *= dCylRad ;
// Lunghezza della componente del vettore radiale ortogonale alla linea di movimento
double dDotRemRad = abs( vtRad * vtMove) ;
double dOrtLen = sqrt( max( dCylRad * dCylRad - dDotRemRad * dDotRemRad, 0.)) ;
// Cerco lungo la retta un punto che stia nel segmento e abbia distanza dal piano
// abbia distanza dal piano +/- dOrtLen.
Vector3d vtPlane = vtMove ^ vtCylAx ;
vtPlane.Normalize() ;
Vector3d vtD = ptSeg - ptCylOrig ;
double dDotPlaneD = vtD * vtPlane ;
double dDotPlaneSeg = vtSeg * vtPlane ;
double dPlusU = ( dOrtLen - dDotPlaneD) / dDotPlaneSeg ;
double dMinusU = ( - dOrtLen - dDotPlaneD) / dDotPlaneSeg ;
Point3d ptTanPlus = ptSeg + dPlusU * vtSeg ;
Point3d ptTanMinus = ptSeg + dMinusU * vtSeg ;
double dTanCordPlus1 = ( ptTanPlus - ptCylOrig) * vtCylAx ;
double dTanCordMinus1 = ( ptTanMinus - ptCylOrig) * vtCylAx ;
if ( ( dPlusU > - EPS_SMALL && dPlusU < dSegLen + EPS_SMALL) &&
( dTanCordPlus1 < dTopTol && dTanCordPlus1 > - dCylHei + dBotTol)) {
dSurfLeakDist = max( ( ( ptSeg - ptCylOrig) + dPlusU * vtSeg) * vtMove + dDotRemRad, 0.) ;
}
if ( dMinusU > - EPS_SMALL && dMinusU < dSegLen + EPS_SMALL &&
( dTanCordMinus1 < dTopTol && dTanCordMinus1 > - dCylHei + dBotTol)) {
double dNewDist = max( ( ( ptSeg - ptCylOrig) + dMinusU * vtSeg) * vtMove + dDotRemRad, 0.) ;
dSurfLeakDist = max( dNewDist, dSurfLeakDist) ;
}
}
return max( max( dBaseLeakDist, dBottomLeakDist), max( dSurfLeakDist, 0.)) ;
}
//----------------------------------------------------------------------------
// **** CONO ****
//----------------------------------------------------------------------------
double
CAvTrConeTriangle( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot)
{
// Allontanamento con direzione coincidente con l'asse del cono
if ( AreSameOrOppositeVectorApprox( vtTrConeAx, vtMove)) {
// Verifica preliminare che la fetta di spazio delimitata dal piano sotto del cono rispetto alla direzione
// di allontanamento non sia già sopra al triangolo (tutti i vertici del triangolo sono sotto).
Point3d ptInfLim = ptMinBase ;
if ( AreOppositeVectorApprox( vtTrConeAx, vtMove))
ptInfLim += dTrConeH * vtTrConeAx ;
double dMaxDistV = PointPlaneSignedDist( trTria.GetP( 0), ptInfLim, vtMove) ;
dMaxDistV = max( dMaxDistV, PointPlaneSignedDist( trTria.GetP( 1), ptInfLim, vtMove)) ;
dMaxDistV = max( dMaxDistV, PointPlaneSignedDist( trTria.GetP( 2), ptInfLim, vtMove)) ;
if ( dMaxDistV < 0.)
return 0. ;
bool bInside[3] = { GetPointLineSqDist( trTria.GetP( 0), ptMinBase, vtMove) < dMinBaseR * dMinBaseR,
GetPointLineSqDist( trTria.GetP( 1), ptMinBase, vtMove) < dMinBaseR * dMinBaseR,
GetPointLineSqDist( trTria.GetP( 2), ptMinBase, vtMove) < dMinBaseR * dMinBaseR} ;
if ( bInside[0] && bInside[1] && bInside[2])
return 0. ;
// Distanza di allontanamento dall'interno
double dInnLeakDist = TrConeTriangleInteriorLeakDistLongMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH,
trTria, vtMove) ;
if ( dInnLeakDist > EPS_SMALL)
return dInnLeakDist ;
// Distanza di allontanamento dai vertici e lati
double dOutLeakDist = 0 ;
for ( int nV = 0 ; nV < 3 ; ++ nV) {
if ( bInside[nV] && bInside[( nV + 1) % 3])
continue ;
// dal vertice
double dCurVertDist = TrConePointLeakDistLongMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH,
trTria.GetP( nV), vtMove) ;
dOutLeakDist = max( dCurVertDist, dOutLeakDist) ;
// dal lato
Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
double dCurSegDist = TrConeSegmentLeakDistLongMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR,
dTrConeH, trTria.GetP( nV), vtSeg, dSegLen, vtMove) ;
dOutLeakDist = max( dCurSegDist, dOutLeakDist) ;
}
return dOutLeakDist ;
}
// Allontanamento in direzione ortogonale all'asse del cono
else if ( AreOrthoApprox( vtTrConeAx, vtMove)) {
double dLeakDist = 0. ;
// Distanza di allontanamento dall'interno del trianolo: se positiva, abbiamo finito
double dInnLeakDist = TrConeTriangleInteriorLeakDistOrtMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH,
trTria, vtMove) ;
if ( dInnLeakDist > EPS_SMALL)
return dInnLeakDist ;
else
dLeakDist = max( dLeakDist, dInnLeakDist) ;
// Distanza di allontanamento da vertici e lati
for ( int nV = 0; nV < 3; ++ nV) {
// Dai vertici
double dCurVertDist = TrConePointLeakDistOrtMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH,
trTria.GetP( nV), vtMove, bTop, bBot) ;
dLeakDist = max( dLeakDist, dCurVertDist) ;
// Dal lato
Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
double dCurSegDist = TrConeSegmentLeakDistOrtMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH,
trTria.GetP( nV), vtSeg, dSegLen, vtMove, bTop, bBot) ;
dLeakDist = max( dLeakDist, dCurSegDist) ;
}
return dLeakDist ;
}
// Movimento generico
else
return 0. ;
}
//----------------------------------------------------------------------------
double
TrConePointLeakDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Point3d& ptP, const Vector3d& vtMove)
{
double dPointAxisSqDist = GetPointLineSqDist( ptP, ptMinBase, vtTrConeAx) ;
if ( dPointAxisSqDist > dMaxBaseR * dMaxBaseR || dPointAxisSqDist < dMinBaseR * dMinBaseR)
return 0. ;
else {
double dMinBaseLeakDist = PointPlaneSignedDist( ptP, ptMinBase, vtMove) ;
double dMaxBaseLeakDist = PointPlaneSignedDist( ptP, ptMinBase + dTrConeH * vtTrConeAx, vtMove) ;
if ( dMinBaseLeakDist < - EPS_SMALL && dMaxBaseLeakDist < - EPS_SMALL)
return 0. ;
double dDistMinBaseV = dTrConeH * dMinBaseR / ( dMaxBaseR - dMinBaseR) ;
Point3d ptConeV = ptMinBase - dDistMinBaseV * vtTrConeAx ;
Vector3d vtVertP = ptP - ptConeV ;
double dVertPLongCord = vtMove * vtVertP ;
if ( vtMove * vtTrConeAx > 0.) {
double dPointAxisDist = sqrt( GetPointLineSqDist( ptP, ptMinBase, vtMove)) ;
double dLateralSurfLeakDist = dVertPLongCord - dPointAxisDist * dTrConeH / ( dMaxBaseR - dMinBaseR) ;
return max( dLateralSurfLeakDist, 0.) ;
}
else
return max( PointPlaneSignedDist( ptP, ptMinBase + dTrConeH * vtTrConeAx, vtMove), 0.) ;
}
}
//----------------------------------------------------------------------------
double
TrConeSegmentLeakDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen,
const Vector3d& vtMove)
{
// Distanza di allontanamento delle basi
Point3d ptMaxBase = ptMinBase + dTrConeH * vtTrConeAx ;
double dMinBaseLeakDist = DiskSegmentLeakDistLongMot( ptMinBase, dMinBaseR, ptSeg, vtSeg, dSegLen, vtMove) ;
double dMaxBaseLeakDist = DiskSegmentLeakDistLongMot( ptMaxBase, dMaxBaseR, ptSeg, vtSeg, dSegLen, vtMove) ;
double dBasesLeakDist = max( max( dMinBaseLeakDist, dMaxBaseLeakDist), 0.) ;
// Distanza di allontanamento della superficie laterale
double dDistMinBaseV = dTrConeH * dMinBaseR / ( dMaxBaseR - dMinBaseR) ;
Point3d ptConeV = ptMinBase - dDistMinBaseV * vtTrConeAx ;
Frame3d frConusFrame ;
frConusFrame.Set( ptConeV, vtTrConeAx) ;
Point3d ptSegNew = ptSeg ;
Vector3d vtSegNew = vtSeg ;
ptSegNew.ToLoc( frConusFrame) ;
vtSegNew.ToLoc( frConusFrame) ;
double dAngCoef = ( dDistMinBaseV + dTrConeH) / dMaxBaseR ;
double dAlpha = vtSegNew.z * vtSegNew.z - dAngCoef * dAngCoef * ( vtSegNew.x * vtSegNew.x + vtSegNew.y * vtSegNew.y) ;
double dBeta = ptSegNew.z * vtSegNew.z - dAngCoef * dAngCoef * ( ptSegNew.x * vtSegNew.x + ptSegNew.y * vtSegNew.y) ;
double dGamma = ptSegNew.z * ptSegNew.z - dAngCoef * dAngCoef * ( ptSegNew.x * ptSegNew.x + ptSegNew.y * ptSegNew.y) ;
// Se l'equazione è di primo grado, non serve risolvere (si trova altrimenti con i punti estremi del segmento)
if ( abs( dAlpha) < EPS_ZERO)
return 0. ;
// Impongo il determinante nullo, ne deriva una nuova equazione da risolvere
DBLVECTOR vdDeltaCoef( 3) ;
DBLVECTOR vdDeltaRoots ;
vdDeltaCoef[0] = dBeta * dBeta - dAlpha * dGamma ;
vdDeltaCoef[1] = 2 * ( dBeta * vtSegNew.z - dAlpha * ptSegNew.z) ;
vdDeltaCoef[2] = ( vtSegNew.z * vtSegNew.z - dAlpha) ;
int nRoot = PolynomialRoots( 2, vdDeltaCoef, vdDeltaRoots) ;
// Studio le soluzioni
double dLateralSurfLeakDist = 0. ;
for ( int nS = 0 ; nS < nRoot ; ++ nS) {
// Queste soluzioni sono gli spostamenti lungo z del segmento che, lo portano in
// una configurazione di tangenza col cono (delta = 0). Ora risolviamo l'equazione
// dell'intersezione in corrispondenza del parametro di spostamento trovato.
// Essendo il delta nullo la sola soluzione è - b/2a.
double dT = - ( dBeta + vtSegNew.z * vdDeltaRoots[nS]) / dAlpha ;
Point3d ptTan( ptSegNew.x, ptSegNew.y, ptSegNew.z + vdDeltaRoots[nS]) ;
ptTan += dT * vtSegNew ;
if ( ptTan.z > dDistMinBaseV - EPS_SMALL &&
ptTan.z < dDistMinBaseV + dTrConeH + EPS_SMALL &&
dT > - EPS_SMALL && dT < dSegLen + EPS_SMALL) {
if ( ( vtTrConeAx * vtMove > 0 && vdDeltaRoots[nS] < 0.) ||
( vtTrConeAx * vtMove < 0 && vdDeltaRoots[nS] > 0.))
dLateralSurfLeakDist = abs( vdDeltaRoots[nS]) ;
}
}
return max( dBasesLeakDist, dLateralSurfLeakDist) ;
}
//----------------------------------------------------------------------------
double
TrConeTriangleInteriorLeakDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove)
{
Point3d ptMaxBase = ptMinBase + dTrConeH * vtTrConeAx ;
// Movimento nel piano del triangolo: la distanza di allontanamento è determinata dalla frontiera
if ( AreOrthoApprox( vtMove, trTria.GetN()))
return 0. ;
// Movimento ortogolnale al piano
else if ( AreSameOrOppositeVectorApprox( vtMove, trTria.GetN())) {
if ( vtMove * vtTrConeAx > 0) {
Point3d ptTouch ;
double dDist = DiskPlaneLeakDistLongMot( ptMinBase, dMinBaseR, trTria.GetP( 0), trTria.GetN(), vtMove, ptTouch) ;
if ( CoplanarDiscTriangleInterferance( ptTouch, dMinBaseR, trTria))
return max( dDist, 0.) ;
return 0. ;
}
else {
Point3d ptTouch ;
Point3d ptMaxBase = ptMinBase - dTrConeH * vtTrConeAx ;
double dDist = DiskPlaneLeakDistLongMot( ptMaxBase, dMaxBaseR, trTria.GetP( 0), trTria.GetN(), vtMove, ptTouch) ;
if ( CoplanarDiscTriangleInterferance( ptTouch, dMaxBaseR, trTria))
return max( dDist, 0.) ;
return 0. ;
}
}
// Movimento generico rispetto al piano
else {
if ( vtMove * vtTrConeAx > 0) {
Point3d ptTouch ;
double dDist = DiskPlaneLeakDistLongMot( ptMinBase, dMinBaseR, trTria.GetP( 0), trTria.GetN(), vtMove, ptTouch) ;
if ( IsPointInsideTriangle( ptTouch, trTria))
return max( dDist, 0.) ;
return 0. ;
}
else {
Point3d ptTouch ;
Point3d ptMaxBase = ptMinBase - dTrConeH * vtTrConeAx ;
double dDist = DiskPlaneLeakDistLongMot( ptMaxBase, dMaxBaseR, trTria.GetP( 0), trTria.GetN(), vtMove, ptTouch) ;
if ( IsPointInsideTriangle( ptTouch, trTria))
return max( dDist, 0.) ;
return 0. ;
}
}
}
//----------------------------------------------------------------------------
double
TrConePointLeakDistOrtMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
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 ;
Vector3d vtP = ptP - ptMinBase ;
double dPntPosOnAx = vtP * vtTrConeAx ;
if ( dPntPosOnAx < dBotTol || dPntPosOnAx > dTrConeH + dTopTol)
return 0. ;
dPntPosOnAx = Clamp( dPntPosOnAx, 0., dTrConeH) ;
double dR = dMinBaseR + ( dMaxBaseR - dMinBaseR) * dPntPosOnAx / dTrConeH ;
return DiskPointLeakDistOrtMot( ptMinBase + dPntPosOnAx * vtTrConeAx, vtTrConeAx, dR, ptP, vtMove) ;
}
//----------------------------------------------------------------------------
double
TrConeSegmentLeakDistOrtMot( 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 dTopTol = bTop ? EPS_SMALL : - EPS_SMALL ;
double dBotTol = bBot ? - EPS_SMALL : EPS_SMALL ;
double dStH = ( ptSeg - ptMinBase) * vtTrConeAx ;
double dEnH = ( ptSeg + dSegLen * vtSeg - ptMinBase) * vtTrConeAx ;
if ( ( dStH < dBotTol && dEnH < dBotTol) ||
( dStH > dTrConeH + dTopTol && dEnH > dTrConeH + dTopTol))
return 0. ;
// Distanza di allontanamento delle basi
Point3d ptMaxBase = ptMinBase + dTrConeH * vtTrConeAx ;
double dMinBaseLeakDist = DiskSegmentLeakDistOrtMot( ptMinBase, vtTrConeAx, dMinBaseR, ptSeg, vtSeg, dSegLen, vtMove) ;
double dMaxBaseLeakDist = DiskSegmentLeakDistOrtMot( ptMaxBase, vtTrConeAx, dMaxBaseR, ptSeg, vtSeg, dSegLen, vtMove) ;
double dBasesLeakDist = max( max( dMinBaseLeakDist, dMaxBaseLeakDist), 0.) ;
// Distanza di allontanamento della superficie laterale: ci interessa solo la tangenza
double dDistMinBaseV = dTrConeH * dMinBaseR / ( dMaxBaseR - dMinBaseR) ;
Point3d ptConeV = ptMinBase - dDistMinBaseV * vtTrConeAx ;
Frame3d frConusFrame ;
frConusFrame.Set( ptConeV, vtMove, vtTrConeAx ^ vtMove, vtTrConeAx) ;
Point3d ptSegNew = ptSeg ;
Vector3d vtSegNew = vtSeg ;
ptSegNew.ToLoc( frConusFrame) ;
vtSegNew.ToLoc( frConusFrame) ;
double dAngCoef = ( dDistMinBaseV + dTrConeH) / dMaxBaseR ;
double dD0 = ptSegNew.z * ptSegNew.z - dAngCoef * dAngCoef * ( ptSegNew.x * ptSegNew.x + ptSegNew.y * ptSegNew.y) ;
double dD1 = ptSegNew.z * vtSegNew.z - dAngCoef * dAngCoef * ( ptSegNew.x * vtSegNew.x + ptSegNew.y * vtSegNew.y) ;
double dD2 = vtSegNew.z * vtSegNew.z - dAngCoef * dAngCoef * ( vtSegNew.x * vtSegNew.x + vtSegNew.y * vtSegNew.y) ;
// Se l'equazione è di primo grado, non serve risolvere (si trova altrimenti con i punti estremi del segmento)
if ( abs( dD2) < EPS_ZERO)
return 0. ;
// Impongo il determinante nullo, ne deriva una nuova equazione da risolvere
DBLVECTOR vdDeltaCoef( 3) ;
DBLVECTOR vdDeltaRoots ;
vdDeltaCoef[0] = dD1 * dD1 - dD0 * dD2 ;
vdDeltaCoef[1] = 2 * dAngCoef * dAngCoef * ( dD2 * ptSegNew.x - dD1 * vtSegNew.x) ;
vdDeltaCoef[2] = dAngCoef * dAngCoef * ( dAngCoef * dAngCoef * vtSegNew.x * vtSegNew.x + dD2) ;
int nRoot = PolynomialRoots( 2, vdDeltaCoef, vdDeltaRoots) ;
// Studio le soluzioni
double dLateralSurfLeakDist = 0. ;
for ( int nS = 0 ; nS < nRoot ; ++ nS) {
// Queste soluzioni sono gli spostamenti lungo x del segmento che, lo portano in
// una configurazione di tangenza col cono (delta = 0). Ora risolviamo l'equazione
// dell'intersezione in corrispondenza del parametro di spostamento trovato.
// Essendo il delta nullo la sola soluzione è - b/2a.
double dT = ( dAngCoef * dAngCoef * vtSegNew.x * vdDeltaRoots[nS] - dD1) / dD2 ;
Point3d ptTan( ptSegNew.x + vdDeltaRoots[nS], ptSegNew.y, ptSegNew.z) ;
ptTan += dT * vtSegNew ;
if ( ptTan.z > dDistMinBaseV + dBotTol &&
ptTan.z < dDistMinBaseV + dTrConeH + dTopTol &&
dT > - EPS_SMALL && dT < dSegLen + EPS_SMALL) {
if ( - vdDeltaRoots[nS] > dLateralSurfLeakDist)
dLateralSurfLeakDist = - vdDeltaRoots[nS] ;
}
}
return max( dBasesLeakDist, dLateralSurfLeakDist) ;
}
//----------------------------------------------------------------------------
double
TrConeTriangleInteriorLeakDistOrtMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove)
{
// Se l'asse del cono è ortogonale al piano la distanza di
// allontanamento è determinata dai vetici e segmenti.
if ( AreSameOrOppositeVectorApprox( vtTrConeAx, trTria.GetN()))
return 0. ;
// Distanza di allontanamento del disco minore
Point3d ptMinTouch ;
double dMinLeakDist = DiskPlaneLeakDistOrtMot( ptMinBase, vtTrConeAx, dMinBaseR, trTria.GetP( 0), trTria.GetN(),
vtMove, ptMinTouch) ;
// Distanza di allontanamento del disco maggiore
Point3d ptMaxTouch ;
Point3d ptMaxBase = ptMinBase + dTrConeH * vtTrConeAx ;
double dMaxLeakDist = DiskPlaneLeakDistOrtMot( ptMaxBase, vtTrConeAx, dMaxBaseR, trTria.GetP( 0), trTria.GetN(),
vtMove, ptMaxTouch) ;
// Valuto distanza di allontanamento del tronco a partire da quelle dei dischi
bool bMaxInside = IsPointInsideTriangle( ptMaxTouch, trTria) ;
bool bMinInside = IsPointInsideTriangle( ptMinTouch, trTria) ;
double dLeakDist = 0. ;
// Se entrambe interne
if ( bMaxInside && bMinInside)
dLeakDist = max( dMaxLeakDist, dMinLeakDist) ;
// altrimenti se base maggiore interna
else if ( bMaxInside) {
if ( dMaxLeakDist > dMinLeakDist)
dLeakDist = dMaxLeakDist ;
}
// altrimenti se base minore interna
else if ( bMinInside) {
if ( dMinLeakDist > dMaxLeakDist)
dLeakDist = dMinLeakDist ;
}
// altrimenti se tocca la superficie laterale
else if ( abs( dMaxLeakDist - dMinLeakDist) < EPS_SMALL) {
Point3d ptInt1, ptInt2 ;
int nRes = IntersLineTria( ptMaxTouch, ptMinTouch, trTria, ptInt1, ptInt2) ;
if ( nRes != ILTT_NO)
dLeakDist = ( dMaxLeakDist + dMinLeakDist) / 2 ;
}
return dLeakDist ;
}
//----------------------------------------------------------------------------
// **** TORO ****
// In questi algoritmi per toro intendiamo la corona torica esterna
//----------------------------------------------------------------------------
double
CAvTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot)
{
// Allontanamento con direzione coincidente con l'asse del toro
if ( AreSameOrOppositeVectorApprox( vtTorusAx, vtMove)) {
// Le corone toriche non possono effettuare un moto in direzione opposta
// al proprio asse di simmetria: vedi documentazione.
if ( vtTorusAx * vtMove < 0.)
return 0. ;
// Verifica preliminare che la fetta di spazio delimitata dal piano sotto del cono rispetto alla direzione
// di allontanamento non sia già sopra al triangolo (tutti i vertici del triangolo sono sotto).
Point3d ptBase = ptTorusCen - dMinRad * vtMove ;
double dMaxDistV = PointPlaneSignedDist( trTria.GetP( 0), ptBase, vtMove) ;
dMaxDistV = max( dMaxDistV, PointPlaneSignedDist( trTria.GetP( 1), ptBase, vtMove)) ;
dMaxDistV = max( dMaxDistV, PointPlaneSignedDist( trTria.GetP( 2), ptBase, vtMove)) ;
if ( dMaxDistV < 0.)
return 0. ;
// Distanza di allontanamento dall'interno
double dInLeakDist = TorusTriangleInteriorLeakDistLongMot( ptTorusCen, dMaxRad, dMinRad,
trTria, vtMove) ;
if ( dInLeakDist > EPS_SMALL)
return dInLeakDist ;
// Distanza di allontanamento dai lati
double dOutLeakDist = 0. ;
for ( int nV = 0 ; nV < 3 ; ++ nV) {
Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
double dCurDist = TorusSegmentLeakDistLongMot( ptTorusCen, dMaxRad, dMinRad,
trTria.GetP( nV), vtSeg, dSegLen, vtMove) ;
dOutLeakDist = max( dCurDist, dOutLeakDist) ;
// Valuto contatto del disco con i vertici
dCurDist = PointPlaneSignedDist( trTria.GetP( nV), ptTorusCen - dMinRad * vtTorusAx, vtMove) ;
if ( GetPointLineSqDist( trTria.GetP( nV), ptTorusCen, vtTorusAx) < dMaxRad * dMaxRad &&
dCurDist > dOutLeakDist)
dOutLeakDist = max( dOutLeakDist, dCurDist) ;
}
return dOutLeakDist ;
}
// Allontanamento in direzione ortogonale all'asse del toro
else if ( AreOrthoApprox( vtTorusAx, vtMove)) {
double dTopTol = bTop ? EPS_SMALL : - EPS_SMALL ;
double dBotTol = bBot ? - EPS_SMALL : EPS_SMALL ;
double dRadSum = dMaxRad + dMinRad ;
Vector3d vtTrasv = vtTorusAx ^ vtMove ;
Vector3d vtR0 = trTria.GetP( 0) - ptTorusCen ;
Vector3d vtR1 = trTria.GetP( 1) - ptTorusCen ;
Vector3d vtR2 = trTria.GetP( 2) - ptTorusCen ;
if ( ( vtR0 * vtMove < - dRadSum && vtR1 * vtMove < - dRadSum && vtR2 * vtMove < - dRadSum) ||
( vtR0 * vtTrasv < - dRadSum && vtR1 * vtTrasv < - dRadSum && vtR2 * vtTrasv < - dRadSum) ||
( vtR0 * vtTrasv > dRadSum && vtR1 * vtTrasv > dRadSum && vtR2 * vtTrasv > dRadSum) ||
( vtR0 * vtTorusAx > dTopTol && vtR1 * vtTorusAx > dTopTol && vtR2 * vtTorusAx > dTopTol) ||
( 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 dInLeakDist = TorusTriangleInteriorLeakDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, trTria, vtMove) ;
if ( dInLeakDist > EPS_SMALL)
return dInLeakDist ;
// Distanza di allontanamento dai lati
double dOutLeakDist = 0. ;
for ( int nV = 0 ; nV < 3 ; ++ nV) {
Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
double dCurDist = TorusSegmentLeakDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad,
trTria.GetP( nV), vtSeg, dSegLen, vtMove, bTop, bBot) ;
dOutLeakDist = max( dCurDist, dOutLeakDist) ;
}
return max( dOutLeakDist, dInLeakDist) ;
}
// Movimento generico
else
return -1 ;
}
//----------------------------------------------------------------------------
double
EvalLeakDist( const Point3d& ptTorusCen, double dMaxRad, double dMinRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dPar, const Vector3d& vtMove)
{
double dSqMinRad = dMinRad * dMinRad ;
Point3d ptCurPoint = ptSeg + dPar * vtSeg ;
double dPointAxSqLen = GetPointLineSqDist( ptCurPoint, ptTorusCen, vtMove) ;
double dDeltaRad = sqrt( dPointAxSqLen) - dMaxRad ;
double dSQAddLeakDist = dSqMinRad - dDeltaRad * dDeltaRad ;
double dAddLeakDist = ( dSQAddLeakDist > 0. ? sqrt( dSQAddLeakDist) : 0.) ;
double dLeakDist = ( ptCurPoint - ptTorusCen) * vtMove + dAddLeakDist ;
return dLeakDist ;
}
//----------------------------------------------------------------------------
double
TorusSegmentLeakDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad,
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) ;
if ( dLinSegSqDist > ( dMaxRad + dMinRad) * ( dMaxRad + dMinRad))
return 0. ;
// Se estremi del segmento più vicini della corona torica, posso uscire subito
double dStartSqDist = GetPointLineSqDist( ptSeg, ptTorusCen, vtMove) ;
double dEndSqDist = GetPointLineSqDist( ptSeg + vtSeg * dSegLen, ptTorusCen, vtMove) ;
if ( dStartSqDist < dMaxRad * dMaxRad && dEndSqDist < dMaxRad * dMaxRad)
return 0. ;
// Intervallo del segmento non limitato
Intervals SegLimits ;
SegLimits.Set( 0, dSegLen) ;
// Limito il segmento entro il cilindro esterno della corona del toro
double dOutLen1, dOutLen2 ;
int nOutInters = IntersLineInfiniteCylinder( ptSeg, vtSeg, ptTorusCen, vtMove, dMaxRad + dMinRad,
dOutLen1, dOutLen2) ;
if ( nOutInters == CC_TWO_INT || nOutInters == CC_ONE_INT_TAN)
SegLimits.Intersect( dOutLen1, dOutLen2) ;
else if ( nOutInters == CC_NO_INTERS)
SegLimits.Reset() ;
if ( SegLimits.IsEmpty())
return 0. ;
// Tolgo eventuale cilindro interno alla corona del toro
if ( dLinSegSqDist < dMaxRad * dMaxRad) {
double dIntLen1, dIntLen2 ;
int nIntInters = IntersLineInfiniteCylinder( ptSeg, vtSeg, ptTorusCen, vtMove, dMaxRad,
dIntLen1, dIntLen2) ;
if ( nIntInters == CC_TWO_INT)
SegLimits.Subtract( dIntLen1, dIntLen2) ;
}
// Eseguo controllo sugli intervalli
double dMaxLeakDist = 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 INV_GOLDEN_RATIO = ( sqrt( 5.) - 1.) / 2. ;
double dTol = max( 0.05 * dMinRad, EPS_SMALL) ;
double dPSt = 0 ;
double dPEn = dLen ;
double dPIn1 = dPEn - ( dPEn - dPSt) * INV_GOLDEN_RATIO ;
double dPIn2 = dPSt + ( dPEn - dPSt) * INV_GOLDEN_RATIO ;
double dLeakDist1 = EvalLeakDist( ptTorusCen, dMaxRad, dMinRad, ptStart, vtSeg, dPIn1, vtMove) ;
double dLeakDist2 = EvalLeakDist( ptTorusCen, dMaxRad, dMinRad, ptStart, vtSeg, dPIn2, vtMove) ;
while ( dPEn - dPSt > dTol) {
if ( dLeakDist1 > dLeakDist2) {
dPEn = dPIn2 ;
dPIn2 = dPIn1 ;
dLeakDist2 = dLeakDist1 ;
dPIn1 = dPEn - ( dPEn - dPSt) * INV_GOLDEN_RATIO ;
dLeakDist1 = EvalLeakDist( ptTorusCen, dMaxRad, dMinRad, ptStart, vtSeg, dPIn1, vtMove) ;
}
else {
dPSt = dPIn1 ;
dPIn1 = dPIn2 ;
dLeakDist1 = dLeakDist2 ;
dPIn2 = dPSt + ( dPEn - dPSt) * INV_GOLDEN_RATIO ;
dLeakDist2 = EvalLeakDist( ptTorusCen, dMaxRad, dMinRad, ptStart, vtSeg, dPIn2, vtMove) ;
}
}
double dPMax = 0.5 * ( dPSt + dPEn) ;
dMaxLeakDist = max( dMaxLeakDist, EvalLeakDist( ptTorusCen, dMaxRad, dMinRad, ptStart, vtSeg, dPMax, vtMove)) ;
// passo al successivo intervallo
bFound = SegLimits.GetNext( dLen1, dLen2) ;
}
return dMaxLeakDist ;
}
//----------------------------------------------------------------------------
double
TorusTriangleInteriorLeakDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad,
const Triangle3d& trTria, const Vector3d& vtMove)
{
// Se piano del triangolo e asse del toro perpendicolari
if ( AreSameOrOppositeVectorApprox( trTria.GetN(), vtMove)) {
// Se tutti i punti del triangolo distano meno del raggio dMaxRad non c'è interferenza col toro
if ( GetPointLineSqDist( trTria.GetP( 0), ptTorusCen, vtMove) < dMaxRad * dMaxRad &&
GetPointLineSqDist( trTria.GetP( 1), ptTorusCen, vtMove) < dMaxRad * dMaxRad &&
GetPointLineSqDist( trTria.GetP( 2), ptTorusCen, vtMove) < dMaxRad * dMaxRad)
return 0. ;
// Calcolo distanza di allontanamento e verifico se positiva
double dLeakDist = max( dMinRad + ( ( trTria.GetP( 0) - ptTorusCen) * trTria.GetN()) / ( vtMove * trTria.GetN()), 0.) ;
if ( dLeakDist > 0) {
// Se triangolo interferisce con anello esterno del toro (bordo interno)
Point3d ptNewCen = ptTorusCen + ( dLeakDist - dMinRad) * vtMove ;
if ( CoplanarDiscTriangleInterferance( ptNewCen, dMaxRad, trTria))
return dLeakDist ;
}
// Nessuna interferenza
return 0. ;
}
// Altrimenti individuo le due possibili circonferenze con cui può avvenire il contatto
Vector3d vtRadial = trTria.GetN() - trTria.GetN() * vtMove * vtMove ;
vtRadial.Normalize() ;
Point3d ptDiskPlus = ptTorusCen + dMaxRad * vtRadial ;
Point3d ptDiskMinus = ptTorusCen - dMaxRad * vtRadial ;
Vector3d vtDiskAx = vtRadial ^ vtMove;
vtDiskAx.Normalize() ;
Point3d ptTouchPlus, ptTouchMinus ;
double dLeakDist = 0. ;
// Distanza di fuga dalla prima circonferenza
double dCurDist = DiskPlaneLeakDistOrtMot( ptDiskPlus, vtDiskAx, dMinRad, trTria.GetP( 0), trTria.GetN(),
vtMove, ptTouchPlus) ;
// Se distanza distanza di fuga aumento la aggiorno
if ( IsPointInsideTriangle( ptTouchPlus, trTria) && dCurDist > dLeakDist)
dLeakDist = max( dLeakDist, dCurDist) ;
// Distanza di fuga dalla seconda circonferenza
dCurDist = DiskPlaneLeakDistOrtMot( ptDiskMinus, vtDiskAx, dMinRad, trTria.GetP( 0), trTria.GetN(),
vtMove, ptTouchMinus) ;
// Se distanza distanza di fuga aumento la aggiorno
if ( IsPointInsideTriangle( ptTouchMinus, trTria) && dCurDist > dLeakDist)
dLeakDist = max( dLeakDist, dCurDist) ;
return dLeakDist ;
}
//----------------------------------------------------------------------------
double
TorusSegmentLeakDistOrtMot( 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)
{
double dTopTol = ( bTop ? EPS_SMALL : - EPS_SMALL) ;
double dBotTol = ( bBot ? - EPS_SMALL : EPS_SMALL) ;
Vector3d vtTrasv = vtTorusAx ^ vtMove ;
vtTrasv.Normalize() ;
Point3d ptBBInf = ptTorusCen - ( dMaxRad + dMinRad) * ( vtTrasv + vtMove) - ( dMinRad - dBotTol) * vtTorusAx ;
Point3d ptBBSup = ptTorusCen + ( dMaxRad + dMinRad) * ( vtTrasv - vtMove) + dTopTol * vtTorusAx ;
// Clamp piani laterali
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 ;
// Cerco distanza di massimo allontanamento lungo il segmento ridotto
double dMaxLeakDist = 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 > dMaxLeakDist)
dMaxLeakDist = dCurDist ;
}
}
}
return dMaxLeakDist ;
}
//----------------------------------------------------------------------------
double
TorusTriangleInteriorLeakDistOrtMot( 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 ;
// Se piano ortogonale all'asse del toro non ci può essere tangenza
if ( ! vtPlaneOrtToAx.Normalize( EPS_ZERO))
return 0. ;
// Se la componente del versore normale al triangolo nella direzione
// del moto è nulla, non può esserci contatto con l'interno
double dDotMoveN = vtMove * trTria.GetN() ;
if ( abs( dDotMoveN) < EPS_ZERO)
return 0. ;
// Scarto il primo contatto
if ( vtPlaneOrtToAx * vtMove < 0.)
return 0. ;
vtPlaneOrtToAx *= dMaxRad ;
// Trovo il punto che toccherà il piano
Point3d ptTouch = ptTorusCen - vtPlaneOrtToAx - dMinRad * trTria.GetN() ;
double dLeakDist = max( ( ( trTria.GetP( 0) - ptTouch) * trTria.GetN()) / dDotMoveN, 0.) ;
// Se il punto di contatto è interno al triangolo restituisco distanza di fuga non negativa
if ( IsPointInsideTriangle( ptTouch + dLeakDist * vtMove, trTria))
return dLeakDist ;
return 0. ;
}
//----------------------------------------------------------------------------
double
CAvConcaveTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot)
{
// Allontanamento con direzione coincidente con l'asse del toro
if ( AreSameOrOppositeVectorApprox( vtTorusAx, vtMove)) {
// Le corone toriche non possono effettuare un moto in direzione opposta
// al proprio asse di simmetria: vedi documentazione.
if ( vtTorusAx * vtMove < 0.)
return 0. ;
// Distanza di allontanamento dai lati
double dOutLeakDist = 0. ;
for ( int nV = 0 ; nV < 3 ; ++ nV) {
Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
double dCurDist = ConcaveTorusSegmentLeakDistLongMot( ptTorusCen, dMaxRad, dMinRad,
trTria.GetP( nV), vtSeg, dSegLen, vtMove) ;
dOutLeakDist = max( dCurDist, dOutLeakDist) ;
// Valuto contatto del disco con i vertici
dCurDist = PointPlaneSignedDist( trTria.GetP( nV), ptTorusCen, vtMove) ;
if ( GetPointLineSqDist( trTria.GetP( nV), ptTorusCen, vtTorusAx) < ( dMaxRad - dMinRad) * ( dMaxRad - dMinRad) &&
dCurDist > dOutLeakDist)
dOutLeakDist = max( dOutLeakDist, dCurDist) ;
}
double dInLeakDist = ConcaveTorusTriangleInteriorLeakDistLongMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad,
trTria, vtMove) ;
return max( dInLeakDist, dOutLeakDist) ;
}
// Allontanamento con direzione ortogonale a quella dell'asse del toro
else if ( AreOrthoApprox( vtTorusAx, vtMove)) {
double dLeakDist = 0. ;
// Distanza di allontanamento dei segmenti
for ( int nV = 0 ; nV < 3 ; ++ nV) {
Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
double dCurDist = ConcaveTorusSegmentLeakDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad,
trTria.GetP( nV), vtSeg, dSegLen, vtMove, bTop, bBot) ;
dLeakDist = max( dLeakDist, dCurDist) ;
}
double dInnLeakDist = ConcaveTorusTriangleInteriorLeakDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad,
trTria, vtMove) ;
dLeakDist = max( dLeakDist, dInnLeakDist) ;
return dLeakDist ;
}
// Casi non gestiti
else
return 0. ;
}
//----------------------------------------------------------------------------
double
ConcaveEvalLeakDist( const Point3d& ptTorusCen, double dMaxRad, double dMinRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dPar, 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 dSqDeltaLeakDist = dSqMinRad - dDeltaRad * dDeltaRad ;
double dDeltaLeakDist = ( dSqDeltaLeakDist > 0. ? sqrt( dSqDeltaLeakDist) : 0.) ;
double dLeakDist = ( ptCurPoint - ptTorusCen) * vtMove - dDeltaLeakDist ;
return dLeakDist ;
}
//----------------------------------------------------------------------------
double
ConcaveTorusSegmentLeakDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad,
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) ;
if ( dLinSegSqDist > dMaxRad * dMaxRad)
return 0. ;
// Se estremi del segmento più vicini della corona torica, posso uscire subito
double dStartSqDist = GetPointLineSqDist( ptSeg, ptTorusCen, vtMove) ;
double dEndSqDist = GetPointLineSqDist( ptSeg + vtSeg * dSegLen, ptTorusCen, vtMove) ;
if ( dStartSqDist < ( dMaxRad - dMinRad) * ( dMaxRad - dMinRad) &&
dEndSqDist < ( dMaxRad - dMinRad) * ( dMaxRad - dMinRad))
return 0. ;
// Intervallo del segmento non limitato
Intervals SegLimits ;
SegLimits.Set( 0, dSegLen) ;
// Limito il segmento entro il cilindro esterno della corona del toro
double dOutLen1, dOutLen2 ;
int nOutInters = IntersLineInfiniteCylinder( ptSeg, vtSeg, ptTorusCen, vtMove, dMaxRad,
dOutLen1, dOutLen2) ;
if ( nOutInters == CC_TWO_INT || nOutInters == CC_ONE_INT_TAN)
SegLimits.Intersect( dOutLen1, dOutLen2) ;
else if ( nOutInters == CC_NO_INTERS)
SegLimits.Reset() ;
if ( SegLimits.IsEmpty())
return 0. ;
// Tolgo eventuale cilindro interno alla corona del toro
if ( dLinSegSqDist < ( dMaxRad - dMinRad) * ( dMaxRad - dMinRad)) {
double dIntLen1, dIntLen2 ;
int nIntInters = IntersLineInfiniteCylinder( ptSeg, vtSeg, ptTorusCen, vtMove, dMaxRad - dMinRad,
dIntLen1, dIntLen2) ;
if ( nIntInters == CC_TWO_INT)
SegLimits.Subtract( dIntLen1, dIntLen2) ;
}
double dMaxLeakDist = 0.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 dPSt = 0 ;
double dPEn = dLen ;
while ( dPEn - dPSt > dTol) {
double dPIn1 = dPEn - ( dPEn - dPSt) * GOLDEN_RATIO ;
double dPIn2 = dPSt + ( dPEn - dPSt) * GOLDEN_RATIO ;
double dLeakDist1 = ConcaveEvalLeakDist( ptTorusCen, dMaxRad, dMinRad, ptStart, vtSeg, dPIn1, vtMove) ;
double dLeakDist2 = ConcaveEvalLeakDist( ptTorusCen, dMaxRad, dMinRad, ptStart, vtSeg, dPIn2, vtMove) ;
if ( dLeakDist1 > dLeakDist2)
dPEn = dPIn2 ;
else
dPSt = dPIn1 ;
}
double dMaxPar = 0.5 * ( dPSt + dPEn) ;
dMaxLeakDist = max( dMaxLeakDist, ConcaveEvalLeakDist( ptTorusCen, dMaxRad, dMinRad, ptStart, vtSeg, dMaxPar, vtMove)) ;
// passo al successivo intervallo
bFound = SegLimits.GetNext( dLen1, dLen2) ;
}
return dMaxLeakDist ;
}
//----------------------------------------------------------------------------
double
ConcaveTorusTriangleInteriorLeakDistLongMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx,
double dMaxRad, double dMinRad,
const Triangle3d& trTria, const Vector3d& vtMove)
{
// Se piano del triangolo e asse del toro perpendicolari
if ( AreSameOrOppositeVectorApprox( trTria.GetN(), vtMove)) {
// Se tutti i punti del triangolo distano meno del raggio dMaxRad non c'è interferenza col toro
if ( GetPointLineSqDist( trTria.GetP( 0), ptTorusCen, vtMove) < dMinRad * dMinRad &&
GetPointLineSqDist( trTria.GetP( 1), ptTorusCen, vtMove) < dMinRad * dMinRad &&
GetPointLineSqDist( trTria.GetP( 2), ptTorusCen, vtMove) < dMinRad * dMinRad)
return 0. ;
// Calcolo distanza di allontanamento e verifico se positiva
double dLeakDist = max( ( ( trTria.GetP( 0) - ptTorusCen) * trTria.GetN()) / ( vtMove * trTria.GetN()), 0.) ;
if ( dLeakDist > 0) {
// Se triangolo interferisce con anello esterno del toro (bordo interno)
Point3d ptNewCen = ptTorusCen + ( dLeakDist /*- dMinRad*/) * vtMove ;
if ( CoplanarDiscTriangleInterferance( ptNewCen, dMinRad/*dMaxRad*/, trTria))
return dLeakDist ;
}
// Nessuna interferenza
return 0. ;
}
// Altrimenti valuto i dischi
double dLeakDist = 0. ;
dLeakDist = max( DiskTriaInteriorLeakDistLongMot( ptTorusCen, dMinRad, trTria, vtMove), 0.) ;
dLeakDist = max( DiskTriaInteriorLeakDistLongMot( ptTorusCen + dMinRad * vtTorusAx, dMaxRad, trTria, vtMove), dLeakDist) ;
return dLeakDist ;
}
//----------------------------------------------------------------------------
double
ConcaveTorusSegmentLeakDistOrtMot( 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)
{
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
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 ;
double dMaxLeakDist = 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 > dMaxLeakDist)
dMaxLeakDist = dCurDist ;
}
}
}
return dMaxLeakDist ;
}
//----------------------------------------------------------------------------
double
ConcaveTorusTriangleInteriorLeakDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Triangle3d& trTria, const Vector3d& vtMove)
{
double dLeakDist = 0. ;
// Caso movimento nel piano: la distanza di allontanamento è determinata dalla frontiera
if ( AreSameOrOppositeVectorApprox( trTria.GetN(), vtMove))
return dLeakDist ;
// Base minore
Point3d ptContact ;
Point3d ptDiskCen = ptTorusCen ;
double dCurDist = DiskPlaneLeakDistOrtMot( ptDiskCen, vtTorusAx, dMaxRad - dMinRad, trTria.GetP( 0), trTria.GetN(),
vtMove, ptContact) ;
if ( IsPointInsideOpenTriangle( ptContact, trTria))
dLeakDist = max( dLeakDist, dCurDist) ;
// Base maggiore
ptDiskCen += dMinRad * vtTorusAx ;
dCurDist = DiskPlaneLeakDistOrtMot( ptDiskCen, vtTorusAx, dMaxRad, trTria.GetP( 0), trTria.GetN(),
vtMove, ptContact) ;
if ( IsPointInsideOpenTriangle( ptContact, trTria))
dLeakDist = max( dLeakDist, dCurDist) ;
return dLeakDist ;
}
// DISTANZA DI ALLONTANAMENTO PER DISCHI
//----------------------------------------------------------------------------
double
CAvDiskTriangle( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad,
const Triangle3d& trTria, const Vector3d& vtMove)
{
// Direzione di allontanamento diretta come normale al disco
if ( AreSameVectorApprox( vtDiskAx, vtMove)) {
// Valuto le distanze con segno dei vertici dal piano del disco :
// se sono tutte negative non interferiscono.
double dDistV[3] ;
dDistV[0] = PointPlaneSignedDist( trTria.GetP( 0), ptDiskCen, vtMove) ;
dDistV[1] = PointPlaneSignedDist( trTria.GetP( 1), ptDiskCen, vtMove) ;
dDistV[2] = PointPlaneSignedDist( trTria.GetP( 2), ptDiskCen, vtMove) ;
double dMaxDistV = max( dDistV[0], max( dDistV[1], dDistV[2])) ;
if ( dMaxDistV < 0.)
return 0. ;
// Se tutti i punti distano dall'asse di movimento meno del raggio, l'ultimo
// punto di contatto deve essere un vertice del triangolo.
double dSqRad = dDiskRad * dDiskRad ;
bool bInV[3] ;
bInV[0] = ( GetPointLineSqDist( trTria.GetP( 0), ptDiskCen, vtMove) < dSqRad) ;
bInV[1] = ( GetPointLineSqDist( trTria.GetP( 1), ptDiskCen, vtMove) < dSqRad) ;
bInV[2] = ( GetPointLineSqDist( trTria.GetP( 2), ptDiskCen, vtMove) < dSqRad) ;
if ( bInV[0] && bInV[1] && bInV[2])
return max( dMaxDistV, 0.) ;
// Distanza di allontanamento dall'interno del triangolo
double dMaxDistI = DiskTriaInteriorLeakDistLongMot( ptDiskCen, dDiskRad, trTria, vtMove) ;
if ( dMaxDistI > EPS_SMALL)
return dMaxDistI ;
// Se disco è un punto, inutile fare controlli con i lati e i vertici del triangolo
if ( dDiskRad < EPS_SMALL)
return 0. ;
// Ciclo sui segmenti del triangolo e calcolo la loro distanza di allontanamento, calcolo
// anche la distanza di allontanamento dai vertici distanti dall'asse meno del raggio.
double dMaxDistVS = 0. ;
for ( int nVS = 0 ; nVS < 3 ; ++ nVS) {
// Vertici
if ( bInV[nVS] && dDistV[nVS] > dMaxDistVS)
dMaxDistVS = dDistV[nVS] ;
// Se un lato del triangolo ha entrambi gli estremi con distanza negativa dal piano del disco,
// non può interferire con esso.
int nVE = ( nVS + 1) % 3 ;
if ( dDistV[nVS] < 0 && dDistV[nVE] < 0)
continue ;
// Versore e lunghezza del segmento
Vector3d vtSeg = trTria.GetP( nVE) - trTria.GetP( nVS) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
// Distanza dal piano del segmento corrente
double dCurDist = DiskSegmentLeakDistLongMot( ptDiskCen, dDiskRad, trTria.GetP( nVS), vtSeg, dSegLen, vtMove) ;
if ( dCurDist > dMaxDistVS)
dMaxDistVS = dCurDist ;
}
return dMaxDistVS ;
}
// Direzione di allontanamento perpendicolare all'asse del disco
else if ( AreOrthoApprox( vtDiskAx, vtMove)) {
return 0. ;
}
// Direzione di allontanamento generica
else if ( vtDiskAx * vtMove > 0.) {
// Non trattato al momento
return - 1. ;
}
// Errore
else
return -1. ;
}
//----------------------------------------------------------------------------
double
DiskPointLeakDistLongMot( const Point3d& ptDiskCen, double dDiskRad,
const Point3d& ptP, const Vector3d& vtMove)
{
if ( GetPointLineSqDist( ptP, ptDiskCen, vtMove) < dDiskRad * dDiskRad - 2 * dDiskRad * EPS_SMALL)
return PointPlaneSignedDist( ptP, ptDiskCen, vtMove) ;
return 0. ;
}
//----------------------------------------------------------------------------
double
DiskSegmentLeakDistLongMot( const Point3d& ptDiskCen, double dDiskRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen,
const Vector3d& vtMove)
{
// Il disco non può interferire col segmento nel suo moto, se la distanza del
// segmento dall'asse di traslazione è maggiore del raggio.
if ( LineSegmentSqDist( ptDiskCen, vtMove, ptSeg, vtSeg, dSegLen) > dDiskRad * dDiskRad - 2 * dDiskRad * EPS_SMALL)
return 0. ;
// Imposto l'equazione: la distanza quadrata del generico punto della retta associata al
// segmento dall'asse lungo cui si sposta il disco deve valere raggio quadrato del disco.
Vector3d vtLineSegOrt = ( ptSeg - ptDiskCen) - ( ptSeg - ptDiskCen) * vtMove * vtMove ;
Vector3d vtSegOrt = vtSeg - vtSeg * vtMove * vtMove ;
DBLVECTOR vdCoef(3) ;
DBLVECTOR vdRoots ;
vdCoef[0] = vtLineSegOrt.SqLen() - dDiskRad * dDiskRad ;
vdCoef[1] = 2 * vtLineSegOrt * vtSegOrt ;
vdCoef[2] = vtSegOrt.SqLen() ;
// Segmento e asse paralleli
if ( vdCoef[2] < SQ_EPS_ZERO)
return 0. ;
// Soluzione dell'equazione
int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ;
// Ciclo sulle soluzioni per trovare la distanza di allontanamento
double dSegDist = 0. ;
for ( int nSol = 0 ; nSol < nRoot ; ++ nSol) {
// Soluzione interna al segmento
if ( vdRoots[nSol] > 0. && vdRoots[nSol] < dSegLen) {
Point3d ptC = ptSeg + vdRoots[nSol] * vtSeg ;
// Distanza del punto soluzione dal piano del disco nella posizione iniziale
double dCurDist = PointPlaneSignedDist( ptC, ptDiskCen, vtMove) ;
if ( dCurDist > dSegDist)
dSegDist = dCurDist ;
}
}
return dSegDist ;
}
//----------------------------------------------------------------------------
// Determina la distanza di allontanamento di un disco, che trasla lungo il suo asse di simmetria,
// dall'interno di un triangolo. Il disco è descritto dal suo raggio e il moto dal
// centro nella posizione iniziale e dal versore del suo asse di simmetria
// (COINCIDENTE CON IL VERSORE DELLA DIREZIONE DI ALLONTANAMENTO).
//----------------------------------------------------------------------------
double
DiskTriaInteriorLeakDistLongMot( const Point3d& ptDiskCen, double dDiskRad,
const Triangle3d& trTria, const Vector3d& vtMove)
{
// Se disco e triangolo sono complanari
if ( AreSameVectorApprox( vtMove, trTria.GetN())) {
double dDist = max( PointPlaneSignedDist( ptDiskCen, trTria.GetP( 0), trTria.GetN()), 0.) ;
if ( CoplanarDiscTriangleInterferance( ptDiskCen + dDist * vtMove, dDiskRad, trTria))
return dDist ;
}
// Se disco e triangolo sono ortogonali non può esserci contatto con interno
if ( AreOrthoApprox( vtMove, trTria.GetN()))
return 0. ;
// Cerco un punto di contatto nell'interno del triangolo.
double dLeakDist = 0. ;
// Vettore radiale
Vector3d vtRad = trTria.GetN() - ( trTria.GetN() * vtMove) * vtMove ;
vtRad.Normalize() ;
// Punti delle due rette candidate all'intersezione col triangolo
Point3d ptStPlus = ptDiskCen + dDiskRad * vtRad ;
Point3d ptStMinus = ptDiskCen - dDiskRad * vtRad ;
// Intersezioni con le rette
double dDistPlus = ( ( trTria.GetP( 0) - ptStPlus) * trTria.GetN()) / ( vtMove * trTria.GetN()) ;
double dDistMinus = ( ( trTria.GetP( 0) - ptStMinus) * trTria.GetN()) / ( vtMove * trTria.GetN()) ;
if ( dDistPlus > dDistMinus) {
if ( IsPointInsideTriangle( ptStPlus + dDistPlus * vtMove, trTria))
dLeakDist = max( dLeakDist, dDistPlus) ;
}
else if ( IsPointInsideTriangle( ptStMinus + dDistMinus * vtMove, trTria))
dLeakDist = max( dLeakDist, dDistMinus) ;
return dLeakDist ;
}
//----------------------------------------------------------------------------
double
DiskPlaneLeakDistLongMot( const Point3d& ptDiskCen, double dDiskRad,
const Point3d& ptPlane, const Vector3d& vtPlane,
const Vector3d& vtMove, Point3d& ptTouch)
{
// Disco ortogonale al piano: competenza della frontiera
if ( AreOrthoApprox( vtPlane, vtMove))
return -1. ;
// Disco e piano complanari
if ( AreSameOrOppositeVectorApprox( vtPlane, vtMove)) {
double dLeakDist = ( ptPlane - ptDiskCen) * vtMove ;
ptTouch = ptDiskCen + dLeakDist * vtMove ;
return max( 0., dLeakDist) ;
}
// Vettore radiale
Vector3d vtRadLine = vtPlane - ( vtPlane * vtMove) * vtMove ;
vtRadLine.Normalize() ;
Point3d ptStPlus = ptDiskCen + dDiskRad * vtRadLine ;
Point3d ptStMinus = ptDiskCen - dDiskRad * vtRadLine ;
double dLeakPlus = ( ( ptPlane - ptStPlus) * vtPlane) / ( vtMove * vtPlane) ;
double dLeakMinus = ( ( ptPlane - ptStMinus) * vtPlane) / ( vtMove * vtPlane) ;
if ( dLeakPlus > dLeakMinus) {
ptTouch = ptStPlus + dLeakPlus * vtMove ;
return dLeakPlus ;
}
else {
ptTouch = ptStMinus + dLeakMinus * vtMove ;
return dLeakMinus ;
}
}
//----------------------------------------------------------------------------
double
DiskPointLeakDistOrtMot( const Point3d& ptDisc, const Vector3d& vtDiskAx, double dDiskRad,
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 ;
// Se il punto non appartiene al piano del disco, la distanza di fuga è nulla
Vector3d vtCompOutOfPlane = vtP * vtDiskAx * vtDiskAx ;
if ( vtCompOutOfPlane.SqLen() > EPS_SMALL * EPS_SMALL)
return 0. ;
// Depuro da eventuali componenti fuori dal piano del disco
vtP -= vtCompOutOfPlane ;
double dLenLong = vtP * vtMove ;
Vector3d vtPOrt = vtP - dLenLong * vtMove ;
double dSqLenOrt = vtPOrt * vtPOrt ;
if ( dSqLenOrt > dDiskRad * dDiskRad + 2 * dDiskRad * EPS_SMALL)
return 0. ;
return max( dLenLong + sqrt( max( dDiskRad * dDiskRad - dSqLenOrt, 0.)), 0.) ;
}
//----------------------------------------------------------------------------
// Restituisce la distanza di fuga di un disco da un segmento nel caso di
// moto ortogonale all'asse di simmetria.
// Il disco è descritto dal suo raggio. Il movimento è descritto dal versore
// dell'asse del disco, dal centro nella posizione iniziale e dal
// versore della direzione del moto. Il segmento è descritto da punto iniziale,
// versore direzione e lunghezza.
double
DiskSegmentLeakDistOrtMot( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove)
{
// Se il segmento non è nel piano, il più remoto punto di contatto è,
// se esiete, l'intersezione fra piano e segmento. Il caso in cui il
// segmento giace nel piano non ci interessa
if ( ! ( abs( vtSeg * vtDiskAx) < EPS_ZERO)) {
Point3d ptSegEnd = ptSeg + dSegLen * vtSeg ;
double dDotStart = ( ptSeg - ptDiskCen) * vtDiskAx ;
double dDotEnd = ( ptSegEnd - ptDiskCen) * vtDiskAx ;
if ( dDotStart * dDotEnd < 0.) {
double dS = ( ptDiskCen - ptSeg) * vtDiskAx / ( vtSeg * vtDiskAx) ;
Point3d ptInt = ptSeg + dS * vtSeg ;
Vector3d vtOI = ( ptInt - ptDiskCen) ;
double dLongCord = vtOI * vtMove ;
Vector3d vtLong = dLongCord * vtMove ;
Vector3d vtOrt = vtOI - vtLong ;
double dSqOrtLen = vtOrt.SqLen() ;
if ( dSqOrtLen > dDiskRad * dDiskRad + 2 * dDiskRad * EPS_SMALL)
return 0. ;
else
return max( dLongCord + sqrt( max( dDiskRad * dDiskRad - dSqOrtLen, 0.)), 0.) ;
}
else
return 0. ;
}
return 0. ;
}
//----------------------------------------------------------------------------
double
DiskPlaneLeakDistOrtMot( const Point3d& ptDiscCen, const Vector3d& vtDiskAx, double dDiskRad,
const Point3d& ptPlane, const Vector3d& vtPlane,
const Vector3d& vtMove, Point3d& ptContact)
{
Vector3d vtLine = vtPlane ^ vtDiskAx ;
if ( vtLine.Normalize()) {
// Se il raggio è minore di epsilon, lo consideriamo un punto
if ( dDiskRad < EPS_SMALL) {
// Denominatore sempre diverso da zero
double dDesplacement = ( ( ptPlane - ptDiscCen)) * vtPlane / ( vtMove * vtPlane) ;
ptContact = ptDiscCen + dDesplacement * vtMove ;
return dDesplacement ;
}
Vector3d vtRad = dDiskRad * vtLine ^ vtDiskAx ;
if ( vtRad * vtMove > 0)
vtRad *= - 1 ;
double dPar = ( ptPlane - ptDiscCen) * vtPlane / ( vtMove * vtPlane) ;
double dRadCos = abs( vtRad * vtMove) ;
// Se la retta intersezione fra i piani è parallela alla direzione di
// allontanamento la distanza di fuga è determinata da punti e segmenti.
if ( dDiskRad && dRadCos < EPS_SMALL)
return 0. ;
double dDeltaPar = dDiskRad * dDiskRad / dRadCos ;
double dDesplacement = max( dPar + dDeltaPar, 0.) ;
ptContact = ptDiscCen + dDesplacement * vtMove + vtRad ;
return dDesplacement ;
}
return 0. ;
}
// FUNZIONI GEOMETRICHE DI BASE PER IL CALCOLO DELLA DISTANZA DI ALLONTANAMENTO
//----------------------------------------------------------------------------
// Dati un punto e una retta, restituisce il quadrato della loro distanza.
// La retta è descritta da un suo punto e dal suo versore tangente.
double
GetPointLineSqDist( const Point3d& ptP, const Point3d& ptLine, const Vector3d& vtLine)
{
return ( ( ptP - ptLine) - ( ptP - ptLine) * vtLine * vtLine).SqLen() ;
}
//----------------------------------------------------------------------------
// Dati un punto e un piano, ne restituisce la distanza con segno.
// Il piano è descritto da un suo punto e dal suo versore normale.
double
PointPlaneSignedDist( const Point3d& ptP, const Point3d& ptPlane, const Vector3d& vtNorm)
{
return ( ptP - ptPlane) * vtNorm ;
}
//----------------------------------------------------------------------------
// Dati un sistema di tre punti e un piano, ne calcola le tre distanze con segno e ne restituisce la
// maggiore. I tre punti sono passati mediante un triangolo, il piano è descritto da un suo punto e
// dal suo versore normale.
double
ThreePointPlaneSignedDist( const Triangle3d& trTria, const Point3d& ptPlane, const Vector3d& vtNorm)
{
double dMaxSgnDist = - INFINITO ;
for ( int nV = 0 ; nV < 3 ; ++ nV) {
double dCurrSgnDist = PointPlaneSignedDist( trTria.GetP( nV), ptPlane, vtNorm) ;
if ( dCurrSgnDist > dMaxSgnDist)
dMaxSgnDist = dCurrSgnDist ;
}
return dMaxSgnDist ;
}
//----------------------------------------------------------------------------
// Calcola, se esiste, il punto di una retta a distanza con segno data da un piano.
// Il piano è descritto da punto e versore normale.
// La retta è descritta da punto e versore direzione.
// I casi possibili sono:
// - Nessun punto della retta dista d dal piano (retta parallela al piano con distanza diversa da d),
// viene restituito 0 e il valore di dPar non ha senso.
// - Un solo punto della retta dista d dal piano (retta non parallela al piano), viene restituito 1
// e il valore di dPar è il parametro del punto sulla retta.
// - Tutti i punti della retta distano d dal piano (retta parallela al piano con distanza d), viene
// restituito 2 e qualunque valore di dPar ha senso.
int
LinePlaneDDistPar( const Point3d& ptPlane, const Vector3d& vtPlane, const Point3d& ptLine, const Vector3d& vtLine,
double dDist, double& dPar)
{
// Vettore congiungente il punto del piano con il punto iniziale della retta
Vector3d vtPlLn = ptLine - ptPlane ;
// Distanza con segno del punto iniziale della retta dal piano
double dSgnDist0 = vtPlLn * vtPlane ;
// Prodotto scalare fra versore della direzione della retta e versore normale al piano
double dDotDirNorm = vtLine * vtPlane ;
// Retta parallela al piano
if ( abs( dDotDirNorm) < EPS_ZERO) {
if ( abs( dSgnDist0 - dDist) < EPS_SMALL)
return 2 ;
else
return 0 ;
}
// Caso generale
else {
dPar = ( dDist - dSgnDist0) / dDotDirNorm ;
return 1 ;
}
}
//----------------------------------------------------------------------------
// Date due rette nello spazio restituisce il quadrato della loro distanza.
// Le rette sono definite da un punto e dal versore direzione.
double
LineLineSqDist( const Point3d& ptP1, const Vector3d& vtD1,
const Point3d& ptP2, const Vector3d& vtD2)
{
// Se parallele
if ( abs( 1. - abs( vtD1 * vtD2)) < EPS_SMALL)
return ( ( ptP2 - ptP1) - ( ptP2 - ptP1) * vtD1 * vtD1).SqLen() ;
// Caso generale
else {
Vector3d vtOrtD2 = vtD2 - vtD2 * vtD1 * vtD1 ;
Vector3d vtOrtP1P2 = ( ptP2 - ptP1) - ( ptP2 - ptP1) * vtD1 * vtD1 ;
double dD = ( vtOrtD2 * vtOrtP1P2) / vtOrtP1P2.SqLen() ;
return ( vtOrtP1P2.SqLen() + dD * dD * vtOrtD2.SqLen() + 2 * dD * vtOrtD2 * vtOrtP1P2) ;
}
}
//----------------------------------------------------------------------------
// Data una retta e un segmento, ne restituisce il quadrato della distanza.
// La retta è definita da un punto e dal versore direzione.
// Il segmento è definito da un estremo, dal versore direzione verso l'altro estremo e dalla lunghezza.
double
LineSegmentSqDist( const Point3d& ptPLn, const Vector3d& vtDLn,
const Point3d& ptPSg, const Vector3d& vtDSg, double dSgLen)
{
// Se paralleli
if ( AreSameOrOppositeVectorApprox( vtDLn, vtDSg))
return ( ( ptPSg - ptPLn) - ( ptPSg - ptPLn) * vtDLn * vtDLn).SqLen() ;
// Caso generale
else {
double dDLn = ( ptPSg - ptPLn) * vtDLn ;
double dDSg = ( ptPSg - ptPLn) * vtDSg ;
double dDLnSg = vtDLn * vtDSg ;
// posizione parametrica del punto sul segmento
double dSgPar = ( dDLn * dDLnSg - dDSg) / ( 1. - dDLnSg * dDLnSg) ;
// se prima dell'inizio, è distanza dell'inizio dalla linea
if ( dSgPar <= 0.)
return GetPointLineSqDist( ptPSg, ptPLn, vtDLn) ;
// se dopo fine, è distanza fine dalla linea
else if ( dSgPar >= dSgLen)
return GetPointLineSqDist( ptPSg + dSgLen * vtDSg, ptPLn, vtDLn) ;
// altrimenti, determino i due punti a minima distanza
else {
double dLnPar = ( dDLn - dDSg * dDLnSg) / ( 1. - dDLnSg * dDLnSg) ;
Point3d ptMinLn = ptPLn + dLnPar * vtDLn ;
Point3d ptMinSg = ptPSg + dSgPar * vtDSg ;
return SqDist( ptMinLn, ptMinSg) ;
}
}
}
//----------------------------------------------------------------------------
// Stabilisce se un punto appartiene a un triangolo interno o frontiera.
bool
IsPointInsideTriangle( const Point3d& ptP, const Triangle3d& trTria)
{
// Se il punto non è nel box del triangolo, è sicuramente esterno
BBox3d b3Tria( trTria.GetP( 0)) ;
b3Tria.Add( trTria.GetP( 1)) ;
b3Tria.Add( trTria.GetP( 2)) ;
if ( ! b3Tria.Encloses( ptP))
return false ;
// Se il punto non sta sul piano del triangolo, è sicuramente esterno
if ( abs( ( ptP - trTria.GetP( 0)) * trTria.GetN()) > EPS_SMALL)
return false ;
// Verifico se il punto è a destra del primo lato
Vector3d vtV0 = trTria.GetP( 1) - trTria.GetP( 0) ;
vtV0.Normalize() ;
double dProd0 = ( vtV0 ^ ( ptP - trTria.GetP( 0))) * trTria.GetN() ;
if ( dProd0 < - EPS_SMALL)
return false ;
// Verifico se il punto è a destra del secondo lato
Vector3d vtV1 = trTria.GetP( 2) - trTria.GetP( 1) ;
vtV1.Normalize() ;
double dProd1 = ( vtV1 ^ ( ptP - trTria.GetP( 1))) * trTria.GetN() ;
if ( dProd1 < - EPS_SMALL)
return false ;
// Verifico se il punto è a destra del terzo lato
Vector3d vtV2 = trTria.GetP( 0) - trTria.GetP( 2) ;
vtV2.Normalize() ;
double dProd2 = ( vtV2 ^ ( ptP - trTria.GetP( 2))) * trTria.GetN() ;
if ( dProd2 < - EPS_SMALL)
return false ;
// Punto a sinistra di tutti i lati, quindi interno
return true ;
}
//----------------------------------------------------------------------------
// Stabilisce se un punto appartiene all'interno di un triangolo
bool
IsPointInsideOpenTriangle( const Point3d& ptP, const Triangle3d& trTria)
{
// Se il punto non è nel box del triangolo, è sicuramente esterno
BBox3d b3Tria( trTria.GetP( 0)) ;
b3Tria.Add( trTria.GetP( 1)) ;
b3Tria.Add( trTria.GetP( 2)) ;
if ( ! b3Tria.Encloses( ptP))
return false ;
// Se il punto non sta sul piano del triangolo, è sicuramente esterno
if ( abs( ( ptP - trTria.GetP( 0)) * trTria.GetN()) > EPS_SMALL)
return false ;
// Verifico se il punto è a destra del primo lato
Vector3d vtV0 = trTria.GetP( 1) - trTria.GetP( 0) ;
vtV0.Normalize() ;
double dProd0 = ( vtV0 ^ ( ptP - trTria.GetP( 0))) * trTria.GetN() ;
if ( dProd0 < EPS_SMALL)
return false ;
// Verifico se il punto è a destra del secondo lato
Vector3d vtV1 = trTria.GetP( 2) - trTria.GetP( 1) ;
vtV1.Normalize() ;
double dProd1 = ( vtV1 ^ ( ptP - trTria.GetP( 1))) * trTria.GetN() ;
if ( dProd1 < EPS_SMALL)
return false ;
// Verifico se il punto è a destra del terzo lato
Vector3d vtV2 = trTria.GetP( 0) - trTria.GetP( 2) ;
vtV2.Normalize() ;
double dProd2 = ( vtV2 ^ ( ptP - trTria.GetP( 2))) * trTria.GetN() ;
if ( dProd2 < EPS_SMALL)
return false ;
// Punto a sinistra di tutti i lati, quindi interno
return true ;
}
//----------------------------------------------------------------------------
// Valuta l'interferenza di un disco e un triangolo complanari.
bool
CoplanarDiscTriangleInterferance( const Point3d& ptCen, double dRad, const Triangle3d& trTria)
{
// Se il centro del disco non sta sul piano del triangolo, non sono complanari
if ( abs( PointPlaneSignedDist( ptCen, trTria.GetP( 0), trTria.GetN())) > EPS_SMALL)
return false ;
// Se il centro giace nel triangolo
if ( IsPointInsideTriangle( ptCen, trTria))
return true ;
double dSqMinDist = dRad * dRad + 2 * dRad * EPS_SMALL ;
Vector3d vtSeg0 = ( trTria.GetP(1) - trTria.GetP(0)) / Dist( trTria.GetP(1), trTria.GetP(0)) ;
Vector3d vtSeg1 = ( trTria.GetP(2) - trTria.GetP(1)) / Dist( trTria.GetP(2), trTria.GetP(1)) ;
Vector3d vtSeg2 = ( trTria.GetP(0) - trTria.GetP(2)) / Dist( trTria.GetP(0), trTria.GetP(2)) ;
// Se il centro non si avvicina più del raggio dalle rette associate ai segmenti, non c'è interferenza
if ( GetPointLineSqDist( ptCen, trTria.GetP(0), vtSeg0) > dSqMinDist &&
GetPointLineSqDist( ptCen, trTria.GetP(1), vtSeg1) > dSqMinDist &&
GetPointLineSqDist( ptCen, trTria.GetP(2), vtSeg2) > dSqMinDist)
return false ;
// Se il centro non si avvicina più del raggio ai vertici, non c'è interferenza
if ( SqDist( ptCen, trTria.GetP(0)) > dSqMinDist &&
SqDist( ptCen, trTria.GetP(1)) > dSqMinDist &&
SqDist( ptCen, trTria.GetP(2)) > dSqMinDist)
return false ;
// interferenza
return true ;
}
//----------------------------------------------------------------------------
// Date due rette, descritte da un punto e un vettore direzione,
// trova i parametri corrispondenti ai punti di minima distanza.
// Se le rette non sono parallele abbiamo una singola coppia di
// punti e viene restituito true, altrimenti ne abbiamo infinite
// e viene restituito false. Si noti che i parametri dU1, dU2
// non hanno necessariamente il significato di lunghezza;
// perché non è richiesto che i vettori siano normalizzati.
bool
FindLineLineMinDistPar( const Point3d& ptL1, const Vector3d& vtV1,
const Point3d& ptL2, const Vector3d& vtV2,
double& dU1, double& dU2)
{
// Se le rette sono parallele
if ( AreSameOrOppositeVectorExact( vtV1, vtV2))
return false ;
// Vettore congiungente i punti iniziali
Vector3d vtD = ptL2 - ptL1 ;
// Proiezioni di vtD sui versori direzione
double dD1 = vtD * vtV1 ;
double dD2 = vtD * vtV2 ;
// Prodotto scalare fra i versori direzione
double dV = vtV1 * vtV2 ;
// Se le rette sono perpendicolari
if ( abs( vtV1 * vtV2) < EPS_ZERO) {
dU1 = dD1 ;
dU2 = - dD2 ;
}
// Caso generale
else {
dU1 = ( dD1 - dD2 * dV) / ( 1 - dV * dV) ;
dU2 = ( - dD2 + dD1 * dV) / ( 1 - dV * dV) ;
}
return true ;
}
//----------------------------------------------------------------------------
// Calcolo delle posizioni di tangenza di una sfera che si muove dalla posizione
// originale lungo una direzione prefissata rispetto ad una retta.
// Le posizioni sono rappresentate dalla distanza percorsa.
// Il parametro di ritorno può assumere i seguenti valori :
// 0 : la sfera non è mai tangente alla retta; dU1 e dU2 non hanno senso
// 1 : la sfera è tangente alla retta in una posizione; solo dU1 ha senso
// 2 : la sfera è tangente alla retta in due posizioni; dU1 e dU2 valide
// 3 : la sfera è sempre tangente
int
SphereLineTangentPoints( const Point3d& ptSpheCen, double dSpheRad,
const Point3d& ptSeg, const Vector3d& vtSegDir, double dSegLen,
const Vector3d& vtMove, double& dU1, double& dU2)
{
// Definisco l'equazione
Vector3d vtMotOrt = vtMove - vtMove * vtSegDir * vtSegDir ;
Vector3d vtLineSeg = ptSeg - ptSpheCen ;
Vector3d vtLineSegOrt = vtLineSeg - vtLineSeg * vtSegDir * vtSegDir ;
// Setto i coefficienti dell'equazione
DBLVECTOR vdCoeff( 3) ;
vdCoeff[0] = vtLineSegOrt.SqLen() - dSpheRad * dSpheRad ;
vdCoeff[1] = - 2 * vtMotOrt * vtLineSegOrt ;
vdCoeff[2] = vtMotOrt.SqLen() ;
if ( vdCoeff[2] < EPS_ZERO * EPS_ZERO) {
if ( abs( vdCoeff[0]) < EPS_SMALL)
return 3 ;
else
return 0 ;
}
// Risolvo l'equazione
DBLVECTOR vdRoots ;
int nRoots = PolynomialRoots( 2, vdCoeff, vdRoots) ;
// Studio le soluzioni
if ( nRoots == 1)
dU1 = vdRoots[0] ;
else if ( nRoots == 2) {
dU1 = min( vdRoots[0], vdRoots[1]) ;
dU2 = max( vdRoots[0], vdRoots[1]) ;
}
return nRoots ;
}
//----------------------------------------------------------------------------
// Valuta l'allontanamento di un disco, lungo il proprio asse di simmetria, da un piano.
// Il disco è descritto dal centro nella posizione iniziale e dal raggio (la normale è il versore traslazione).
// Il piano è descritto da un suo punto e dal suo versore normale.
// Il moto è descritto dal versore di traslazione.
// Il valore restituito è un intero che descrive la situazione di allontanamento e,
// quando sensato, per referenza restituisce il punto di ultimo contatto.
// 0: Nessun contatto
// 1: Un contatto
// 2: Disco e piano ortogonali, con disco tg al piano, il contatto è una semiretta
// 3: Disco e piano ortogonali, con disco secante il piano, il contatto è una striscia
// 4: Disco e piano paralleli, come ultimo contatto si restituisce il centro.
// Nel caso 0 il parametro ptTouch non ha senso, così come nel caso 2 e nel caso 3
int
DiskPlaneLastContactLongMot( const Point3d& ptDiskCen, double dDiskRad,
const Point3d& ptPlane, const Vector3d& vtPlane,
const Vector3d& vtMove, Point3d& ptTouch)
{
// Se disco e piano sono paralleli
if ( AreSameOrOppositeVectorApprox( vtMove, vtPlane)) {
double dDist = PointPlaneSignedDist( ptPlane, ptDiskCen, vtMove) ;
if ( dDist > - EPS_SMALL) {
ptTouch = ptDiskCen + dDist * vtMove ;
return 4 ;
}
return 0 ;
}
// Vettore radiale
Vector3d vtRadLine = vtMove * ( vtPlane * vtMove) - vtPlane ;
// Disco e piano ortogonali
if ( vtRadLine.IsNormalized()) {
double dDist = abs( PointPlaneSignedDist(ptPlane, ptDiskCen, vtMove)) ;
if ( abs( dDist - dDiskRad) < EPS_SMALL)
return 2 ;
else if ( dDist < dDiskRad)
return 3 ;
return 0 ;
}
// Cerco un punto di contatto nell'interno del triangolo. Se tale punto
// esiste, la retta intersezione fra il piano del triangolo e quello del
// disco è tangente alla circonferenza.
vtRadLine.Normalize() ;
// Punti delle due rette candidate all'intersezione col triangolo
Point3d ptStPlus = ptDiskCen + dDiskRad * vtRadLine ;
Point3d ptStMinus = ptDiskCen - dDiskRad * vtRadLine ;
// Parametri d'intersezione delle rette col piano
double dDistPlus = ( ( ptPlane - ptStPlus) * vtPlane) / ( vtMove * vtPlane) ;
double dDistMinus = ( ( ptPlane - ptStMinus) * vtPlane) / ( vtMove * vtPlane) ;
double dDist ;
if ( dDistPlus > dDistMinus) {
dDist = dDistPlus ;
ptTouch = ptStPlus + dDist * vtMove ;
}
else {
dDist = dDistMinus ;
ptTouch = ptStMinus + dDist * vtMove ;
}
if ( dDist > - EPS_SMALL)
return 1 ;
return 0 ;
}
Reference in New Issue View Git Blame Copy Permalink