Dario Sassi
5fb64bdb3f
- correzioni a CAvToolTriangle per utensili conici - migliorato calcolo box utensile in CAvToolSurfTm - cambiata costante tipo utensile per conico.
1069 lines
50 KiB
C++
1069 lines
50 KiB
C++
//----------------------------------------------------------------------------
|
|
// EgalTech 2018-2018
|
|
//----------------------------------------------------------------------------
|
|
// File : CAvToolTriangle.cpp Data : 27.04.18 Versione : 1.9e1
|
|
// Contenuto : Implementazione delle funzioni ToolTriangleCollisionAvoid.
|
|
//
|
|
//
|
|
//
|
|
// Modifiche : 10.03.18 LM Creazione modulo.
|
|
//
|
|
//
|
|
//----------------------------------------------------------------------------
|
|
|
|
#include "stdafx.h"
|
|
#include "CAvToolTriangle.h"
|
|
#include "IntersLineSurfStd.h"
|
|
#include "IntersLineTria.h"
|
|
#include "/EgtDev/Include/ENkPolynomialRoots.h"
|
|
#include "/EgtDev/Include/EgtNumUtils.h"
|
|
|
|
using namespace std ;
|
|
|
|
//----------------------------------------------------------------------------
|
|
// La funzione determina la minima distanza di allontanamento lungo una direzione fissata
|
|
// per evitare la collisione tra un utensile ed un triangolo.
|
|
// Se nella posizione iniziale non c'è gia collisione si restituisce il valore 0.
|
|
// In caso di errore si restituisce il valore -1.
|
|
double
|
|
CAvToolTriangle( const Tool& tlTool, const Point3d& ptToolOrig, const Vector3d& vtToolAx,
|
|
const Triangle3d& trTria, const Vector3d& vtMove)
|
|
{
|
|
// se utensile cilindrico
|
|
if ( tlTool.GetType() == Tool::CYLMILL) {
|
|
// parametri geometrici
|
|
double dHeigth = tlTool.GetHeigth() ;
|
|
double dRadius = tlTool.GetRadius() ;
|
|
// distanza di allontanamento del cilindro
|
|
double dDist = CAvCylinderTriangle( ptToolOrig, vtToolAx, dHeigth, dRadius, trTria, vtMove) ;
|
|
return dDist ;
|
|
}
|
|
// se utensile sferico
|
|
else if ( tlTool.GetType() == Tool::BALLMILL) {
|
|
// parametri geometrici
|
|
double dCylHeigth = tlTool.GetHeigth() - tlTool.GetTipHeigth() ;
|
|
double dRadius = tlTool.GetRadius() ;
|
|
// prima determino l'allontanamento della sfera
|
|
Point3d ptSpheOrig = ptToolOrig - dCylHeigth * vtToolAx ;
|
|
double dDist = CAvSphereTriangle( ptSpheOrig, dRadius, trTria, vtMove) ;
|
|
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 * max( dDist, 0.) ;
|
|
double dDist2 = CAvCylinderTriangle( ptCylOrig, vtToolAx, dCylHeigth, dRadius, trTria, vtMove) ;
|
|
if ( dDist2 < - EPS_SMALL)
|
|
return dDist2 ;
|
|
return ( dDist + dDist2) ;
|
|
}
|
|
// 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 cono
|
|
double dDist = CAvTrConeTriangle( ptMinBase, vtConeAx, dMinR, dMaxR, tlTool.GetTipHeigth(), trTria, vtMove) ;
|
|
if ( dDist < - EPS_SMALL)
|
|
return dDist ;
|
|
// poi verifico quello del cilindro (tenendo conto di quanto è stata allontanato il cono)
|
|
if ( dCylHeigth < EPS_SMALL)
|
|
return dDist ;
|
|
Point3d ptCylOrig = ptToolOrig + vtMove * max( dDist, 0.) ;
|
|
double dDist2 = CAvCylinderTriangle( ptCylOrig, vtToolAx, dCylHeigth, tlTool.GetRadius(), trTria, vtMove) ;
|
|
if ( dDist2 < - EPS_SMALL)
|
|
return dDist2 ;
|
|
return ( dDist + dDist2) ;
|
|
}
|
|
// altrimenti utensile di tipo non gestito
|
|
else
|
|
return - 1. ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// **** SFERA ****
|
|
//----------------------------------------------------------------------------
|
|
// La funzione determina la minima distanza di allontanamento lungo una direzione fissata
|
|
// per evitare la collisione tra una sfera ed un triangolo.
|
|
double
|
|
CAvSphereTriangle( const Point3d& ptSpheCen, double dSpheRad, const Triangle3d& trTria, const Vector3d& vtMove)
|
|
{
|
|
// Se la sfera sta già tutta dalla parte del movimento rispetto al piano del triangolo non va allontanata
|
|
Vector3d vtVert0 = ptSpheCen - trTria.GetP( 0) ;
|
|
Vector3d vtVert1 = ptSpheCen - trTria.GetP( 1) ;
|
|
Vector3d vtVert2 = ptSpheCen - trTria.GetP( 2) ;
|
|
if ( vtVert0 * vtMove > dSpheRad - EPS_SMALL &&
|
|
vtVert1 * vtMove > dSpheRad - EPS_SMALL &&
|
|
vtVert2 * vtMove > dSpheRad - EPS_SMALL)
|
|
return 0. ;
|
|
|
|
// Valuto Tangenza col piano
|
|
double dPlaneLeakDist = SpherePlaneLeakDist( ptSpheCen, dSpheRad, trTria.GetP( 0), trTria.GetN(), vtMove) ;
|
|
// Se il punto di tangenza esiste, verifico se è interno al triangolo
|
|
if ( dPlaneLeakDist >= 0) {
|
|
Point3d ptTan = ptSpheCen + dPlaneLeakDist * vtMove - dSpheRad * trTria.GetN() ;
|
|
// Se il punto è interno abbiamo finito
|
|
if ( IsPointInsideTriangle( ptTan, trTria))
|
|
return dPlaneLeakDist ;
|
|
}
|
|
|
|
// Valuto le distanze di allontanamento dai segmenti (con i loro estremi) e prendo la maggiore
|
|
double dLeakDist = 0. ;
|
|
for ( int nSeg = 0 ; nSeg < 3 ; ++ nSeg) {
|
|
Vector3d vtSeg = trTria.GetP( ( nSeg + 1) % 3) - trTria.GetP( nSeg) ;
|
|
double dSegLen = vtSeg.Len() ;
|
|
vtSeg /= dSegLen ;
|
|
double dCurLeakDist = SphereSegmentLeakDist( ptSpheCen, dSpheRad, trTria.GetP( nSeg), vtSeg, dSegLen, vtMove) ;
|
|
if ( dCurLeakDist > dLeakDist)
|
|
dLeakDist = dCurLeakDist ;
|
|
}
|
|
return dLeakDist ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Calcola la distanza di allontanamento lungo una direzione fissata di una sfera da un piano.
|
|
double
|
|
SpherePlaneLeakDist( const Point3d& ptSpheCen, double dSpheRad,
|
|
const Point3d& ptPlane, const Vector3d& vtPlaneN, const Vector3d& vtMove)
|
|
{
|
|
// Se la direzione di allontanamento sta nel piano
|
|
if ( abs( vtPlaneN * vtMove) < EPS_SMALL) {
|
|
if ( abs( ( ptSpheCen - ptPlane) * vtPlaneN) > dSpheRad)
|
|
return 0. ;
|
|
else
|
|
return -1. ;
|
|
}
|
|
// altrimenti ...
|
|
else {
|
|
double dLeakDist = ( dSpheRad - ( ptSpheCen - ptPlane) * vtPlaneN) / ( vtMove * vtPlaneN) ;
|
|
return max( dLeakDist, 0.) ;
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Calcola la distanza di allontanamento lungo una direzione fissata di una sfera da un segmento.
|
|
// Il segmento è descritto da punto iniziale, versore della direzione e lunghezza.
|
|
double
|
|
SphereSegmentLeakDist( const Point3d& ptSpheCen, double dSpheRad,
|
|
const Point3d& ptSeg, const Vector3d& vtSegDir, double dSegLen,
|
|
const Vector3d& vtMove)
|
|
{
|
|
// Controllo con l'interno del segmento
|
|
double dU[2] ;
|
|
int nTanPointNum = SphereLineTangentPoints( ptSpheCen, dSpheRad, ptSeg, vtSegDir, dSegLen, vtMove, dU[0], dU[1]) ;
|
|
double dLeakDistIn = 0. ;
|
|
for ( int nSol = 0 ; nSol < nTanPointNum && nTanPointNum != 3 ; ++ nSol) {
|
|
if ( dU[nSol] < 0.)
|
|
continue ;
|
|
Point3d ptC = ptSpheCen + dU[nSol] * vtMove ;
|
|
double dSegPar = ( ptC - ptSeg) * vtSegDir ;
|
|
if ( dSegPar > 0 && dSegPar < dSegLen)
|
|
dLeakDistIn = dU[nSol] ;
|
|
}
|
|
// Controllo con gli estremi
|
|
Point3d ptSegEnd = ptSeg + dSegLen * vtSegDir ;
|
|
double dDistStart = SpherePointLeakDist( ptSpheCen, dSpheRad, ptSeg, vtMove) ;
|
|
double dDistEnd = SpherePointLeakDist( ptSpheCen, dSpheRad, ptSegEnd, vtMove) ;
|
|
// Restituisco il massimo
|
|
return max( dLeakDistIn, max( dDistStart, dDistEnd)) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Calcola la distanza di allontanamento lungo una direzione fissata di una sfera da un punto.
|
|
double
|
|
SpherePointLeakDist( const Point3d& ptSpheCen, double dSpheRad, const Point3d ptP, const Vector3d& vtMove)
|
|
{
|
|
Vector3d vtOR = ptP - ptSpheCen ;
|
|
double dDotORMot = vtOR * vtMove ;
|
|
Vector3d vtOROrt = vtOR - dDotORMot * vtMove ;
|
|
double dSqOROrtLen = vtOROrt.SqLen() ;
|
|
double dSqSphRad = dSpheRad * dSpheRad ;
|
|
if ( dSqOROrtLen > dSqSphRad + 2 * dSpheRad * EPS_SMALL)
|
|
return 0. ;
|
|
else
|
|
return max( dDotORMot + sqrt( max( dSpheRad * dSpheRad - dSqOROrtLen, 0.)), 0.) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// **** CILINDRO ****
|
|
//----------------------------------------------------------------------------
|
|
// La funzione determina la minima distanza di allontanamento lungo una direzione fissata
|
|
// per evitare la collisione tra un cilindro ed un triangolo.
|
|
double
|
|
CAvCylinderTriangle( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad,
|
|
const Triangle3d& trTria, const Vector3d& vtMove)
|
|
{
|
|
// Classificazione del moto
|
|
int nMotionType = 0 ;
|
|
if ( AreSameVectorApprox( vtCylAx, vtMove))
|
|
nMotionType = 1 ;
|
|
else if ( AreOppositeVectorApprox( vtCylAx, vtMove))
|
|
nMotionType = 2 ;
|
|
else if ( AreOrthoApprox( vtCylAx, vtMove))
|
|
nMotionType = 3 ;
|
|
|
|
// Movimento lungo la direzione dell'asse del cilindro
|
|
if ( nMotionType == 1 || nMotionType == 2) {
|
|
// In questo caso il problema si riduce alla determinazione della distanza di allontanamento
|
|
// del disco all'estremità opposta rispetto alla direzione di moto.
|
|
// Il punto base di questo disco viene così calcolato
|
|
Point3d ptBase = ptCylOrig ;
|
|
if ( nMotionType == 1)
|
|
ptBase -= dHeigth * vtCylAx ;
|
|
// Valuto le distanze con segno dei vertici dal piano del disco :
|
|
// se sono tutte negative non interferiscono.
|
|
double dDistV[3] ;
|
|
dDistV[0] = PointPlaneSignedDist( trTria.GetP( 0), ptBase, vtCylAx) ;
|
|
dDistV[1] = PointPlaneSignedDist( trTria.GetP( 1), ptBase, vtCylAx) ;
|
|
dDistV[2] = PointPlaneSignedDist( trTria.GetP( 2), ptBase, vtCylAx) ;
|
|
double dMaxDistV = max( dDistV[0], max( dDistV[1], dDistV[2])) ;
|
|
if ( dMaxDistV < 0.)
|
|
return 0. ;
|
|
// Se tutti i punti distano dall'asse di movimento meno del raggio, l'ultimo
|
|
// punto di contatto deve essere un vertice del triangolo.
|
|
double dSqRad = dRad * dRad ;
|
|
bool bInV[3] ;
|
|
bInV[0] = ( GetPointLineSqDist( trTria.GetP( 0), ptBase, vtMove) < dSqRad) ;
|
|
bInV[1] = ( GetPointLineSqDist( trTria.GetP( 1), ptBase, vtMove) < dSqRad) ;
|
|
bInV[2] = ( GetPointLineSqDist( trTria.GetP( 2), ptBase, vtMove) < dSqRad) ;
|
|
if ( bInV[0] && bInV[1] && bInV[2])
|
|
return max( dMaxDistV, 0.) ;
|
|
// Ciclo sui segmenti del triangolo e calcolo la loro distanza di allontanamento, calcolo
|
|
// anche la distanza di allontanamento dai vertici distanti dall'asse meno del raggio.
|
|
double dMaxDistVS = 0. ;
|
|
for ( int nVS = 0 ; nVS < 3 ; ++ nVS) {
|
|
// Vertici
|
|
if ( bInV[nVS] && dDistV[nVS] > dMaxDistVS)
|
|
dMaxDistVS = dDistV[nVS] ;
|
|
// Se un lato del triangolo ha entrambi gli estremi con distanza negativa dal piano del disco,
|
|
// non può interferire con esso.
|
|
int nVE = ( nVS + 1) % 3 ;
|
|
if ( dDistV[nVS] < 0 && dDistV[nVE] < 0)
|
|
continue ;
|
|
// Versore e lunghezza del segmento
|
|
Vector3d vtSeg = trTria.GetP( nVE) - trTria.GetP( nVS) ;
|
|
double dSegLen = vtSeg.Len() ;
|
|
vtSeg /= dSegLen ;
|
|
// Distanza dal piano del segmento corrente
|
|
double dCurDist = DiskSegmentLeakDistLongMotion( ptBase, dRad, trTria.GetP( nVS), vtSeg, dSegLen, vtMove) ;
|
|
if ( dCurDist > dMaxDistVS)
|
|
dMaxDistVS = dCurDist ;
|
|
}
|
|
// Distanza di allontanamento dall'interno del triangolo
|
|
double dMaxDistI = DiskTriaInteriorLeakDistLongMot( ptBase, dRad, trTria, vtMove) ;
|
|
return max( dMaxDistVS, dMaxDistI) ;
|
|
}
|
|
// Movimento perpendicolare all'asse del cilindro
|
|
else if ( nMotionType == 3) {
|
|
// PROVE PUNTI E SEGMENTI
|
|
double dLeakDist = 0. ;
|
|
for ( int nVrt = 0 ; nVrt < 3 ; ++ nVrt) {
|
|
double dCurDist = CylPointLeakDistOrtMotion( ptCylOrig, vtCylAx, dHeigth, dRad, trTria.GetP( nVrt), vtMove) ;
|
|
if ( dLeakDist < dCurDist)
|
|
dLeakDist = dCurDist ;
|
|
}
|
|
// FINE PROVE PUNTI
|
|
// INIZIO PROVA SEGMENTI
|
|
for ( int nVrtS = 0 ; nVrtS < 3 ; ++ nVrtS) {
|
|
int nVrtE = ( nVrtS + 1) % 3 ;
|
|
Vector3d vtSeg = trTria.GetP( nVrtE) - trTria.GetP( nVrtS) ;
|
|
double dSegLen = vtSeg.Len() ;
|
|
vtSeg /= dSegLen ;
|
|
double dCurDist = CylSegmentLeakDistOrtMotion( ptCylOrig, vtCylAx, dHeigth, dRad,
|
|
trTria.GetP( nVrtS), vtSeg, dSegLen, vtMove) ;
|
|
if ( dLeakDist < dCurDist)
|
|
dLeakDist = dCurDist ;
|
|
}
|
|
// FINE PROVA SEGMENTI
|
|
// PROVA INTERNO
|
|
Point3d ptBaseCont, ptBottomCont ;
|
|
double dBaseLeak = DiskPlaneLeakDistOrtMot( ptCylOrig, vtCylAx, dRad, trTria.GetP( 0), trTria.GetN(),
|
|
vtMove, ptBaseCont) ;
|
|
double dBottomLeak = DiskPlaneLeakDistOrtMot( ptCylOrig - dHeigth * vtCylAx, vtCylAx, dRad,
|
|
trTria.GetP( 0), trTria.GetN(), vtMove, ptBottomCont) ;
|
|
if ( ! IsPointInsideTriangle( ptBaseCont, trTria))
|
|
dBaseLeak = 0. ;
|
|
if ( ! IsPointInsideTriangle( ptBottomCont, trTria))
|
|
dBottomLeak = 0. ;
|
|
double dInnerLeak = max( dBaseLeak, dBottomLeak) ;
|
|
if ( dLeakDist < dInnerLeak)
|
|
dLeakDist = dInnerLeak ;
|
|
// FINE PROVA INTERNO
|
|
return dLeakDist ;
|
|
}
|
|
// Movimento generico
|
|
else
|
|
return -1. ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
double
|
|
DiskSegmentLeakDistLongMotion( const Point3d& ptDiskCen, double dDiscRad,
|
|
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) > dDiscRad * dDiscRad)
|
|
return 0. ;
|
|
|
|
// Imposto l'equazione: la distanza quadrata del generico punto della retta associata al
|
|
// segmento dall'asse lungo cui si sposta il disco deve valere raggio quadrato del disco.
|
|
Vector3d vtLineSegOrt = ( ptSeg - ptDiskCen) - ( ptSeg - ptDiskCen) * vtMove * vtMove ;
|
|
Vector3d vtSegOrt = vtSeg - vtSeg * vtMove * vtMove ;
|
|
DBLVECTOR vdCoef(3) ;
|
|
DBLVECTOR vdRoots ;
|
|
vdCoef[0] = vtLineSegOrt.SqLen() - dDiscRad * dDiscRad ;
|
|
vdCoef[1] = 2 * vtLineSegOrt * vtSegOrt ;
|
|
vdCoef[2] = vtSegOrt.SqLen() ;
|
|
// Segmento e asse paralleli
|
|
if ( vdCoef[2] < SQ_EPS_ZERO) {
|
|
if ( abs( vdCoef[0]) < 2 * dDiscRad * EPS_SMALL) {
|
|
double dLenSt = PointPlaneSignedDist( ptSeg, ptDiskCen, vtMove) ;
|
|
double dLenEn = PointPlaneSignedDist( ptSeg + dSegLen * vtSeg, ptDiskCen, vtMove) ;
|
|
return max( max( dLenSt, dLenEn), 0.) ;
|
|
}
|
|
else
|
|
return 0. ;
|
|
}
|
|
// Soluzione dell'equazione
|
|
else {
|
|
int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ;
|
|
// Ciclo sulle soluzioni per trovare la distanza di allontanamento
|
|
double dSegDist = 0. ;
|
|
for ( int nSol = 0 ; nSol < nRoot ; ++ nSol) {
|
|
// Soluzione interna al segmento
|
|
if ( vdRoots[nSol] > 0. && vdRoots[nSol] < dSegLen) {
|
|
Point3d ptC = ptSeg + vdRoots[nSol] * vtSeg ;
|
|
// Distanza del punto soluzione dal piano del disco nella posizione iniziale
|
|
double dCurDist = PointPlaneSignedDist( ptC, 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())) {
|
|
// verifico solo il centro, se centro esterno non più del raggio sicuramente toccherebbe i lati
|
|
double dDist = PointPlaneSignedDist( ptDiskCen, trTria.GetP( 0), trTria.GetN()) ;
|
|
if ( IsPointInsideTriangle( ptDiskCen - dDist * trTria.GetN(), trTria))
|
|
return max( -dDist, 0.) ;
|
|
}
|
|
// Cerco un punto di contatto nell'interno del triangolo. Se tale punto
|
|
// esiste, la retta intersezione fra il piano del triangolo e quello del
|
|
// disco è tangente alla circonferenza.
|
|
// Vettore tangente alla circonferenza e vettore radiale
|
|
Vector3d vtRadLine = vtMove * ( trTria.GetN() * vtMove) - trTria.GetN() ;
|
|
vtRadLine.Normalize() ;
|
|
// Punti delle due rette candidate all'intersezione col triangolo
|
|
Point3d ptStPlus = ptDiskCen + dDiskRad * vtRadLine ;
|
|
Point3d ptStMinus = ptDiskCen - dDiskRad * vtRadLine ;
|
|
// Intersezioni con la prima retta
|
|
double dDistPlus = 0. ;
|
|
Point3d ptIntPlus1, ptIntPlus2 ;
|
|
int nIntPlus = IntersLineTria( ptStPlus, vtMove, 10, trTria, ptIntPlus1, ptIntPlus2, false) ;
|
|
if ( nIntPlus != ILTT_NO) {
|
|
double dDist1 = PointPlaneSignedDist( ptIntPlus1, ptDiskCen, vtMove) ;
|
|
// Se l'intersezione è doppia, ha senso anche dDist2.
|
|
if ( nIntPlus == ILTT_SEGM ||
|
|
nIntPlus == ILTT_SEGM_ON_EDGE) {
|
|
double dDist2 = PointPlaneSignedDist( ptIntPlus2, ptDiskCen, vtMove) ;
|
|
dDistPlus = max( dDist1, dDist2) ;
|
|
}
|
|
// Altrimenti ha senso solo dDist1
|
|
else
|
|
dDistPlus = dDist1 ;
|
|
}
|
|
// Intersezioni con la seconda retta
|
|
double dDistMinus = 0. ;
|
|
Point3d ptIntMinus1, ptIntMinus2 ;
|
|
int nIntMinus = IntersLineTria( ptStMinus, vtMove, 10, trTria, ptIntMinus1, ptIntMinus2, false) ;
|
|
if ( nIntMinus != ILTT_NO) {
|
|
double dDist1 = PointPlaneSignedDist( ptIntMinus1, ptDiskCen, vtMove) ;
|
|
// Se l'intersezione è doppia, ha senso anche dDist2.
|
|
if ( nIntMinus == ILTT_SEGM ||
|
|
nIntMinus == ILTT_SEGM_ON_EDGE) {
|
|
double dDist2 = PointPlaneSignedDist( ptIntMinus2, ptDiskCen, vtMove) ;
|
|
dDistMinus = max( dDist1, dDist2) ;
|
|
}
|
|
// Altrimenti ha senso solo dDist1
|
|
else
|
|
dDistMinus = dDist1 ;
|
|
}
|
|
return max( dDistPlus, dDistMinus) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Valuta l'allontanamento di un disco, che trasla lungo il proprio asse di simmetria,
|
|
// da un piano. Il disco è descritto dal suo centro nella posizione iniziale e dal raggio.
|
|
// Il piano è descritto da un suo punto e dal suo versore normale. Il moto è descritto
|
|
// dal versore di traslazione.
|
|
// La funzione restituisce un intero che descrive la situazione di allontanamento e,
|
|
// nel caso che abbia senso, per referenza restituisce il punto in cui avviene
|
|
// l'estremo contatto.
|
|
// 0: Nessun contatto
|
|
// 1: Un contatto
|
|
// 2: Disco e piano ortogonali, il contatto è una semiretta
|
|
// 3: Disco e piano paralleli, la superficie di contatto è il disco intero
|
|
// Nel caso 0 il parametro ptTouch non ha senso, così come nel caso 2.
|
|
// Nel caso 3 ptTouch è il centro del disco nella posizione finale.
|
|
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 3 ;
|
|
}
|
|
return 0 ;
|
|
}
|
|
// Vettore radiale
|
|
Vector3d vtRadLine = vtMove * ( vtPlane * vtMove) - vtPlane ;
|
|
// Disco e piano ortogonali
|
|
if ( vtRadLine.IsNormalized()) {
|
|
if ( abs( PointPlaneSignedDist( ptPlane, ptDiskCen, vtMove) - dDiskRad) < EPS_SMALL)
|
|
return 2 ;
|
|
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 ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// 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)
|
|
{
|
|
double dSqRad = dCylRad * dCylRad ;
|
|
Vector3d vtRP = ptP - ptCylOrig ;
|
|
// Se il punto sta al di sopra o al di sotto rispetto al cilindro abbiamo finito
|
|
double dDotRPCylAx = vtRP * vtCylAx ;
|
|
if ( dDotRPCylAx > EPS_SMALL || dDotRPCylAx < - dCylHei - EPS_SMALL)
|
|
return 0. ;
|
|
Vector3d vtPlaneRP = vtRP - dDotRPCylAx * vtCylAx ;
|
|
// Se il punto sta dietro al cilindro abbiamo finito
|
|
double dDotPlaneRPRemDir = vtPlaneRP * vtMove ;
|
|
if ( dDotPlaneRPRemDir < 0. && vtPlaneRP.SqLen() > dSqRad)
|
|
return 0. ;
|
|
Vector3d vtPlaneOrtRP = vtPlaneRP - vtPlaneRP * vtMove * vtMove ;
|
|
// Se il punto è a destra o sinistra del cilindro abbiamo finito
|
|
double dSqOrtLen = vtPlaneOrtRP.SqLen() ;
|
|
if ( dSqOrtLen > dSqRad + 2 * dCylRad * EPS_SMALL)
|
|
return 0. ;
|
|
// Calcolo e restituisco la distanza
|
|
return dDotPlaneRPRemDir + sqrt( max( dSqRad - dSqOrtLen, 0.)) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Restituisce la distanza di fuga di un disco da un segmento nel caso di
|
|
// moto ortogonale all'asse di simmetria.
|
|
// Il disco è descritto dal suo raggio. Il movimento è descritto dal versore
|
|
// dell'asse del disco, dal centro nella posizione iniziale e dal
|
|
// versore della direzione del moto. Il segmento è descritto da punto iniziale,
|
|
// versore direzione e lunghezza.
|
|
double
|
|
DiskSegmentLeakDistOrtMot( const Point3d& ptDiscCen, const Vector3d& vtDiscAx, double dDiscRad,
|
|
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 * vtDiscAx) < EPS_ZERO)) {
|
|
Point3d ptSegEnd = ptSeg + dSegLen * vtSeg ;
|
|
double dDotStart = ( ptSeg - ptDiscCen) * vtDiscAx ;
|
|
double dDotEnd = ( ptSegEnd - ptDiscCen) * vtDiscAx ;
|
|
if ( dDotStart * dDotEnd < 0.) {
|
|
double dS = ( ptDiscCen - ptSeg) * vtDiscAx / ( vtSeg * vtDiscAx) ;
|
|
Point3d ptInt = ptSeg + dS * vtSeg ;
|
|
Vector3d vtOI = ( ptInt - ptDiscCen) ;
|
|
double dLongCord = vtOI * vtMove ;
|
|
Vector3d vtLong = dLongCord * vtMove ;
|
|
Vector3d vtOrt = vtOI - vtLong ;
|
|
double dSqOrtLen = vtOrt.SqLen() ;
|
|
if ( dSqOrtLen > dDiscRad * dDiscRad + 2 * dDiscRad * EPS_SMALL)
|
|
return 0. ;
|
|
else
|
|
return max( dLongCord + sqrt( max( dDiscRad * dDiscRad - dSqOrtLen, 0.)), 0.) ;
|
|
}
|
|
else
|
|
return 0. ;
|
|
}
|
|
return 0. ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Restituisce la distanza di allontanamento di un cilindro da un segmento.
|
|
// Il cilindro è descritto da raggio e altezza.
|
|
// Il suo moto è descritto dalla posizione iniziale, dal versore (NORMA UNITARIA)
|
|
// dell'asse di simmetria e dal versore della direzione del moto.
|
|
double
|
|
CylSegmentLeakDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dCylHei, double dCylRad,
|
|
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove)
|
|
{
|
|
// Le variabili fanno rifermiento a un sistema di riferimento
|
|
// con origine nel centro del disco nella posizione iniziale.
|
|
// X := vtCylAx, Y:= vtMove, Z:= vtCylAx ^ vtMove.
|
|
Vector3d vtPlane = vtCylAx ^ vtMove ;
|
|
vtPlane.Normalize() ;
|
|
Point3d ptSegEnd = ptSeg + dSegLen * vtSeg ;
|
|
Vector3d vtSegStart = ptSeg - ptCylOrig ;
|
|
Vector3d vtSegEnd = ptSegEnd - ptCylOrig ;
|
|
double dCordStart1 = vtSegStart * vtCylAx ;
|
|
double dCordEnd1 = vtSegEnd * vtCylAx ;
|
|
Vector3d vtSegStart1 = dCordStart1 * vtCylAx ;
|
|
Vector3d vtSegEnd1 = dCordEnd1 * vtCylAx ;
|
|
Vector3d vtSegStart23 = vtSegStart - vtSegStart1 ;
|
|
Vector3d vtSegEnd23 = vtSegEnd - vtSegEnd1 ;
|
|
// Se entrambi gli estremi del segmento sono a un lato
|
|
// del cilindro non vi può essere interferenza.
|
|
double dCordStart2 = vtSegStart23 * vtPlane ;
|
|
double dCordEnd2 = vtSegEnd23 * vtPlane ;
|
|
if ( ( dCordStart2 > dCylRad && dCordEnd2 > dCylRad) ||
|
|
( dCordStart2 < - dCylRad && dCordEnd2 < - dCylRad))
|
|
return 0. ;
|
|
// Verifico la distanza di fuga di superficie laterale e dischi di base e fondo.
|
|
double dBaseLeakDist = DiskSegmentLeakDistOrtMot( ptCylOrig, vtCylAx, dCylRad, ptSeg, vtSeg, dSegLen, vtMove) ;
|
|
double dBottomLeakDist = DiskSegmentLeakDistOrtMot( ptCylOrig - dCylHei * vtCylAx, vtCylAx, dCylRad,
|
|
ptSeg, vtSeg, dSegLen, vtMove) ;
|
|
|
|
double dSurfLeakDist = 0. ;
|
|
if ( ( dCordStart1 < 0. && dCordStart1 > - dCylHei) ||
|
|
( dCordEnd1 < 0. && dCordEnd1 > - dCylHei)) {
|
|
// Se il vettore del segmento è parallelo all'asse del cilindro,
|
|
// il suo prodotto vettoriale con il versore dell'asse è nullo.
|
|
Vector3d vtRad = vtCylAx ^ vtSeg ;
|
|
if ( ! vtRad.Normalize()) {
|
|
dSurfLeakDist = max( ( ptSeg - ptCylOrig) * vtMove, 0.) ;
|
|
}
|
|
// Se il versore radiale NON è ortogonale a quello di movimento può esserci tangenza,
|
|
// altrimenti il versore del segmento non ha componenti ortogonali al piano
|
|
// generato da asse cilindro e moto e non può esserci tangenza.
|
|
else if ( ! ( abs( vtRad * vtMove) < EPS_SMALL)) {
|
|
// Nella posizione finale il cilindro e la retta del segmento sono tangenti.
|
|
// Vettore che spicca dal punto di tangenza fra cilindro e retta
|
|
// associata al segmento e termina sull'asse del cilindro.
|
|
vtRad *= dCylRad ;
|
|
// Lunghezza della componente del vettore radiale ortogonale alla linea di movimento
|
|
double dDotRemRad = abs( vtRad * vtMove) ;
|
|
double dOrtLen = sqrt( max( dCylRad * dCylRad - dDotRemRad * dDotRemRad, 0.)) ;
|
|
// Cerco lungo la retta un punto che stia nel segmento e abbia distanza dal piano
|
|
// abbia distanza dal piano +/- dOrtLen.
|
|
Vector3d vtPlane = vtMove ^ vtCylAx ;
|
|
vtPlane.Normalize() ;
|
|
Vector3d vtD = ptSeg - ptCylOrig ;
|
|
double dDotPlaneD = vtD * vtPlane ;
|
|
double dDotPlaneSeg = vtSeg * vtPlane ;
|
|
double dPlusU = ( dOrtLen - dDotPlaneD) / dDotPlaneSeg ;
|
|
double dMinusU = ( - dOrtLen - dDotPlaneD) / dDotPlaneSeg ;
|
|
Point3d ptTanPlus = ptSeg + dPlusU * vtSeg ;
|
|
Point3d ptTanMinus = ptSeg + dMinusU * vtSeg ;
|
|
double dTanCordPlus1 = ( ptTanPlus - ptCylOrig) * vtCylAx ;
|
|
double dTanCordMinus1 = ( ptTanMinus - ptCylOrig) * vtCylAx ;
|
|
if ( ( dPlusU > - EPS_SMALL && dPlusU < dSegLen + EPS_SMALL) &&
|
|
( dTanCordPlus1 < 0 && dTanCordPlus1 > - dCylHei)) {
|
|
dSurfLeakDist = max( ( ( ptSeg - ptCylOrig) + dPlusU * vtSeg) * vtMove + dDotRemRad, 0.) ;
|
|
}
|
|
if ( dMinusU > - EPS_SMALL && dMinusU < dSegLen + EPS_SMALL &&
|
|
( dTanCordMinus1 < 0 && dTanCordMinus1 > - dCylHei)) {
|
|
double dNewDist = max( ( ( ptSeg - ptCylOrig) + dMinusU * vtSeg) * vtMove + dDotRemRad, 0.) ;
|
|
dSurfLeakDist = max( dNewDist, dSurfLeakDist) ;
|
|
}
|
|
}
|
|
}
|
|
return max( max( dBaseLeakDist, dBottomLeakDist), max( dSurfLeakDist, 0.)) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Restituisce la distanza di allontanamento di un disco, che trasla ortogonalmente al suo asse
|
|
// di simmetria, da un piano.
|
|
// Il disco è descritto dal suo raggio. Il moto è descritto dal centro nella posizione
|
|
// iniziale, dal versore (NORMA UNITARIA) dell'asse di simmetria del disco e dal versore
|
|
// della direzione del moto.
|
|
double
|
|
DiskPlaneLeakDistOrtMot( const Point3d& ptDiscCen, const Vector3d& vtDiscAx, double dDiscRad,
|
|
const Point3d& ptPlane, const Vector3d& vtPlane,
|
|
const Vector3d& vtMove, Point3d& ptContact)
|
|
{
|
|
Vector3d vtLine = vtPlane ^ vtDiscAx ;
|
|
if ( vtLine.Normalize() && ! AreSameOrOppositeVectorApprox( vtMove, vtLine)) {
|
|
Vector3d vtRad = dDiscRad * vtLine ^ vtDiscAx ;
|
|
if ( vtRad * vtMove > 0)
|
|
vtRad *= - 1 ;
|
|
double dPar = ( ptPlane - ptDiscCen) * vtPlane / ( vtMove * vtPlane) ;
|
|
double dDeltaPar = dDiscRad * dDiscRad / abs( vtRad * vtMove) ;
|
|
double dDesplacement = max( dPar + dDeltaPar, 0.) ;
|
|
ptContact = ptDiscCen + dDesplacement * vtMove + vtRad ;
|
|
return dDesplacement ;
|
|
}
|
|
return 0. ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// **** CONO ****
|
|
//----------------------------------------------------------------------------
|
|
double
|
|
CAvTrConeTriangle( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
|
|
double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove)
|
|
{
|
|
// Allontanamento con direzione coincidente con l'asse del cono
|
|
if ( AreSameOrOppositeVectorApprox( vtTrConeAx, vtMove)) {
|
|
// Distanza di allontanamento dai vertici e lati
|
|
double dOutLeakDist = 0 ;
|
|
for ( int nV = 0 ; nV < 3 ; ++ nV) {
|
|
// 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) ;
|
|
}
|
|
double dInnLeakDist = TrConeTriangleInteriorLeakDistLongMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH,
|
|
trTria, vtMove) ;
|
|
return max( dOutLeakDist, dInnLeakDist) ;
|
|
}
|
|
// altri casi
|
|
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 + 2 * dMaxBaseR * EPS_SMALL)
|
|
return 0. ;
|
|
else if ( dPointAxisSqDist > dMinBaseR * dMinBaseR + 2 * dMinBaseR * EPS_SMALL) {
|
|
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.) ;
|
|
}
|
|
else {
|
|
double dMinBaseLeakDist = PointPlaneSignedDist( ptP, ptMinBase, vtMove) ;
|
|
double dMaxBaseLeakDist = PointPlaneSignedDist( ptP, ptMinBase + dTrConeH * vtTrConeAx, vtMove) ;
|
|
return max( max( dMinBaseLeakDist, dMaxBaseLeakDist), 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 = DiskSegmentLeakDistLongMotion( ptMinBase, dMinBaseR, ptSeg, vtSeg, dSegLen, vtMove) ;
|
|
double dMaxBaseLeakDist = DiskSegmentLeakDistLongMotion( 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)
|
|
{
|
|
// Distanza di allontanamento del disco minore
|
|
Point3d ptMinTouch ;
|
|
int nMinLeakType = DiskPlaneLastContactLongMot( ptMinBase, dMinBaseR, trTria.GetP( 0), trTria.GetN(),
|
|
vtMove, ptMinTouch) ;
|
|
double dMinLeakDist = ( ptMinTouch - ptMinBase) * vtMove ;
|
|
// Distanza di allontanamento del disco maggiore
|
|
Point3d ptMaxTouch ;
|
|
Point3d ptMaxBase = ptMinBase + dTrConeH * vtTrConeAx ;
|
|
int nMaxLeakType = DiskPlaneLastContactLongMot( ptMaxBase, dMaxBaseR, trTria.GetP( 0), trTria.GetN(),
|
|
vtMove, ptMaxTouch) ;
|
|
double dMaxLeakDist = ( ptMaxTouch - ptMaxBase) * vtMove ;
|
|
// se le due distanze sono uguali e tipo di contatto 1, allora superficie laterale cono tangente a piano
|
|
if ( abs( dMaxLeakDist - dMinLeakDist) < EPS_SMALL) {
|
|
Point3d ptInt1, ptInt2 ;
|
|
int nRes = IntersLineTria( ptMinTouch, ptMaxTouch, trTria, ptInt1, ptInt2) ;
|
|
if ( nRes != ILTT_NO)
|
|
return max( ( dMaxLeakDist + dMinLeakDist) / 2, 0.) ;
|
|
}
|
|
// altrimenti sono i dischi a determinare l'allontanamento
|
|
double dMove = 0 ;
|
|
if ( ( nMinLeakType == 1 && IsPointInsideTriangle( ptMinTouch, trTria)) ||
|
|
( nMinLeakType == 3 && CoplanarDiscTriangleInterferance( ptMinTouch, dMinBaseR, trTria)))
|
|
dMove = max( dMinLeakDist, 0.) ;
|
|
if ( ( nMaxLeakType == 1 && IsPointInsideTriangle( ptMaxTouch, trTria)) ||
|
|
( nMaxLeakType == 3 && CoplanarDiscTriangleInterferance( ptMaxTouch, dMaxBaseR, trTria)))
|
|
dMove = max( dMove, dMaxLeakDist) ;
|
|
return dMove ;
|
|
}
|
|
|
|
|
|
// FUNZIONI GEOMETRICHE DI BASE PER IL CALCOLO DELLA DISTANZA DI ALLONTANAMENTO
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Dati un punto e una retta, restituisce il quadrato della loro distanza.
|
|
// La retta è descritta da un suo punto e dal suo versore tangente.
|
|
double
|
|
GetPointLineSqDist( const Point3d& ptP, const Point3d& ptLine, const Vector3d& vtLine)
|
|
{
|
|
return ( ( ptP - ptLine) - ( ptP - ptLine) * vtLine * vtLine).SqLen() ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Dati un punto e un piano, ne restituisce la distanza con segno.
|
|
// Il piano è descritto da un suo punto e dal suo versore normale.
|
|
double
|
|
PointPlaneSignedDist( const Point3d& ptP, const Point3d& ptPlane, const Vector3d& vtNorm)
|
|
{
|
|
return ( ptP - ptPlane) * vtNorm ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Dati un sistema di tre punti e un piano, ne calcola le tre distanze con segno e ne restituisce la
|
|
// maggiore. I tre punti sono passati mediante un triangolo, il piano è descritto da un suo punto e
|
|
// dal suo versore normale.
|
|
double
|
|
ThreePointPlaneSignedDist( const Triangle3d& trTria, const Point3d& ptPlane, const Vector3d& vtNorm)
|
|
{
|
|
double dMaxSgnDist = - INFINITO ;
|
|
for ( int nV = 0 ; nV < 3 ; ++ nV) {
|
|
double dCurrSgnDist = PointPlaneSignedDist( trTria.GetP( nV), ptPlane, vtNorm) ;
|
|
if ( dCurrSgnDist > dMaxSgnDist)
|
|
dMaxSgnDist = dCurrSgnDist ;
|
|
}
|
|
return dMaxSgnDist ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Dati un piano, descritto da un suo punto e dal versore normale,
|
|
// e una retta, descritta da un suo punto e dal versore direzione,
|
|
// e un numero reale d, determina se esiste un punto sulla retta che ha distanza con
|
|
// segno d dal piano. I casi possibili sono:
|
|
// Nessun punto dista d dal piano (retta parallela al piano con distanza diversa da d),
|
|
// viene restituito 0 e il valore di dPar non ha senso.
|
|
// Un solo punto dista d dal piano (retta non parallela al piano), viene restituito 1
|
|
// e il valore di dPar è il parametro del punto sulla retta.
|
|
// Tutti i punti distano d dal piano (retta parallela al piano con distanza d), viene
|
|
// restituito 2 e qualunque valore di dPar ha senso.
|
|
int
|
|
LinePlaneDDistPar( const Point3d& ptPlane, const Vector3d& vtPlane, const Point3d& ptLine, const Vector3d& vtLine,
|
|
double dDist, double& dPar)
|
|
{
|
|
// Vettore congiungente il punto del piano con il punto iniziale della retta
|
|
Vector3d vtPlLn = ptLine - ptPlane ;
|
|
// Distanza con segno del punto iniziale della retta dal piano
|
|
double dSgnDist0 = vtPlLn * vtPlane ;
|
|
// Prodotto scalare fra versore della direzione della retta e versore normale al piano
|
|
double dDotDirNorm = vtLine * vtPlane ;
|
|
// Retta parallela al piano
|
|
if ( abs( dDotDirNorm) < EPS_ZERO) {
|
|
if ( abs( dSgnDist0 - dDist) < EPS_SMALL)
|
|
return 2 ;
|
|
else
|
|
return 0 ;
|
|
}
|
|
// Caso generale
|
|
else {
|
|
dPar = ( dDist - dSgnDist0) / dDotDirNorm ;
|
|
return 1 ;
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Date due rette nello spazio restituisce il quadrato della loro distanza.
|
|
// Le rette vengono descritte da punto di passaggio e versore direzione.
|
|
double
|
|
LineLineSqDist( const Point3d& ptP1, const Vector3d& vtD1, const Point3d& ptP2, const Vector3d& vtD2)
|
|
{
|
|
// Caso di rette parallele
|
|
if ( abs( 1. - abs( vtD1 * vtD2)) < EPS_SMALL)
|
|
return ( ( ptP2 - ptP1) - ( ptP2 - ptP1) * vtD1 * vtD1).SqLen() ;
|
|
// Caso generale
|
|
else {
|
|
Vector3d vtOrtD2 = vtD2 - vtD2 * vtD1 * vtD1 ;
|
|
Vector3d vtOrtP1P2 = ( ptP2 - ptP1) - ( ptP2 - ptP1) * vtD1 * vtD1 ;
|
|
double dD = ( vtOrtD2 * vtOrtP1P2) / vtOrtP1P2.SqLen() ;
|
|
return vtOrtP1P2.SqLen() + dD * dD * vtOrtD2.SqLen() + 2 * dD * vtOrtD2 * vtOrtP1P2 ;
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Data una retta e un segmento, ne restituisce il quadrato della distanza.
|
|
// La retta è descritta da un qualsiasi punto ad essa appartenente e un versore
|
|
// che ne identifica la direzione. Il segmento è descritto da un suo estremo,
|
|
// dal versore direzione diretto verso l'altro estremo e dalla sua lunghezza.
|
|
double
|
|
LineSegmentSqDist( const Point3d& ptPLn, const Vector3d& vtDLn,
|
|
const Point3d& ptPSg, const Vector3d& vtDSg, double dSgLen)
|
|
{
|
|
// Caso di parallelismo
|
|
if ( AreSameOrOppositeVectorApprox( vtDLn, vtDSg))
|
|
return ( ( ptPSg - ptPLn) - ( ptPSg - ptPLn) * vtDLn * vtDLn).SqLen() ;
|
|
// Caso generale
|
|
else {
|
|
double dDLn = ( ptPSg - ptPLn) * vtDLn ;
|
|
double dDSg = ( ptPSg - ptPLn) * vtDSg ;
|
|
double dDLnSg = vtDLn * vtDSg ;
|
|
// posizione parametrica del punto sul segmento
|
|
double dSgPar = ( dDLn * dDLnSg - dDSg) / ( 1. - dDLnSg * dDLnSg) ;
|
|
// se prima dell'inizio, è distanza dell'inizio dalla linea
|
|
if ( dSgPar <= 0.)
|
|
return GetPointLineSqDist( ptPSg, ptPLn, vtDLn) ;
|
|
// se dopo fine, è distanza fine dalla linea
|
|
else if ( dSgPar >= dSgLen)
|
|
return GetPointLineSqDist( ptPSg + dSgLen * vtDSg, ptPLn, vtDLn) ;
|
|
// altrimenti, determino i due punti a minima distanza
|
|
else {
|
|
double dLnPar = ( dDLn - dDSg * dDLnSg) / ( 1. - dDLnSg * dDLnSg) ;
|
|
Point3d ptMinLn = ptPLn + dLnPar * vtDLn ;
|
|
Point3d ptMinSg = ptPSg + dSgPar * vtDSg ;
|
|
return SqDist( ptMinLn, ptMinSg) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Stabilisce se un punto appartiene a un triangolo interno o frontiera.
|
|
bool
|
|
IsPointInsideTriangle( const Point3d& ptP, const Triangle3d& trTria)
|
|
{
|
|
// Se il punto non sta sul piano del triangolo ho finito
|
|
if ( abs( ( ptP - trTria.GetP( 0)) * trTria.GetN()) > EPS_SMALL)
|
|
return false ;
|
|
|
|
// Vettori dei lati normalizzati
|
|
Vector3d vtV0 = trTria.GetP( 1) - trTria.GetP( 0) ;
|
|
vtV0.Normalize() ;
|
|
Vector3d vtV1 = trTria.GetP( 2) - trTria.GetP( 1) ;
|
|
vtV1.Normalize() ;
|
|
Vector3d vtV2 = trTria.GetP( 0) - trTria.GetP( 2) ;
|
|
vtV2.Normalize() ;
|
|
|
|
// E' sufficiente che il segno di un prodotto misto sia negativo per essere fuori
|
|
double dProd0 = ( vtV0 ^ ( ptP - trTria.GetP( 0))) * trTria.GetN() ;
|
|
if ( dProd0 < - EPS_SMALL)
|
|
return false ;
|
|
double dProd1 = ( vtV1 ^ ( ptP - trTria.GetP( 1))) * trTria.GetN() ;
|
|
if ( dProd1 < - EPS_SMALL)
|
|
return false ;
|
|
double dProd2 = ( vtV2 ^ ( ptP - trTria.GetP( 2))) * trTria.GetN() ;
|
|
if ( dProd2 < - EPS_SMALL)
|
|
return false ;
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// 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
|
|
FindMinDistPar( const Point3d& ptL1, const Point3d& ptL2,
|
|
const Vector3d& vtV1, const Vector3d& vtV2,
|
|
double& dU1, double& dU2)
|
|
{
|
|
// Se le rette sono parallele
|
|
if ( abs( abs( vtV1 * vtV2) - 1) < EPS_ZERO)
|
|
return false ;
|
|
|
|
// Vettore congiungente i punti iniziali
|
|
Vector3d vtD = ptL2 - ptL1 ;
|
|
// Proiezioni di vtD sui versori direzione
|
|
double dD1 = vtD * vtV1 ;
|
|
double dD2 = vtD * vtV2 ;
|
|
// Prodotto scalare fra i versori direzione
|
|
double dV = vtV1 * vtV2 ;
|
|
|
|
// Se le rette sono perpendicolari
|
|
if ( abs( vtV1 * vtV2) < EPS_ZERO) {
|
|
dU1 = dD1 ;
|
|
dU2 = - dD2 ;
|
|
}
|
|
// Caso generale
|
|
else {
|
|
dU1 = ( dD1 - dD2 * dV) / ( 1 - dV * dV) ;
|
|
dU2 = ( - dD2 + dD1 * dV) / ( 1 - dV * dV) ;
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Calcolo delle posizioni di tangenza di una sfera che si muove dalla posizione
|
|
// originale lungo una direzione prefissata rispetto ad una retta.
|
|
// Le posizioni sono rappresentate dalla distanza percorsa.
|
|
// Il parametro di ritorno può assumere i seguenti valori :
|
|
// 0 : la sfera non è mai tangente alla retta; dU1 e dU2 non hanno senso
|
|
// 1 : la sfera è tangente alla retta in una posizione; solo dU1 ha senso
|
|
// 2 : la sfera è tangente alla retta in due posizioni; dU1 e dU2 valide
|
|
// 3 : la sfera è sempre tangente
|
|
int
|
|
SphereLineTangentPoints( const Point3d& ptSpheCen, double dSpheRad,
|
|
const Point3d& ptSeg, const Vector3d& vtSegDir, double dSegLen,
|
|
const Vector3d& vtMove, double& dU1, double& dU2)
|
|
{
|
|
// Definisco l'equazione
|
|
Vector3d vtMotOrt = vtMove - vtMove * vtSegDir * vtSegDir ;
|
|
Vector3d vtLineSeg = ptSeg - ptSpheCen ;
|
|
Vector3d vtLineSegOrt = vtLineSeg - vtLineSeg * vtSegDir * vtSegDir ;
|
|
// Setto i coefficienti dell'equazione
|
|
DBLVECTOR vdCoeff( 3) ;
|
|
vdCoeff[0] = vtLineSegOrt.SqLen() - dSpheRad * dSpheRad ;
|
|
vdCoeff[1] = - 2 * vtMotOrt * vtLineSegOrt ;
|
|
vdCoeff[2] = vtMotOrt.SqLen() ;
|
|
if ( vdCoeff[2] < EPS_ZERO * EPS_ZERO) {
|
|
if ( abs( vdCoeff[0]) < EPS_SMALL)
|
|
return 3 ;
|
|
else
|
|
return 0 ;
|
|
}
|
|
// Risolvo l'equazione
|
|
DBLVECTOR vdRoots ;
|
|
int nRoots = PolynomialRoots( 2, vdCoeff, vdRoots) ;
|
|
// Studio le soluzioni
|
|
if ( nRoots == 1)
|
|
dU1 = vdRoots[0] ;
|
|
else if ( nRoots == 2) {
|
|
dU1 = min( vdRoots[0], vdRoots[1]) ;
|
|
dU2 = max( vdRoots[0], vdRoots[1]) ;
|
|
}
|
|
return nRoots ;
|
|
}
|