Dario Sassi
3a3c591771
- prima versione di Collision Avoiding per Utensili rispetto a Superfici TriMesh.
903 lines
41 KiB
C++
903 lines
41 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) ;
|
|
// poi verifico quello del cilindro (tenendo conto di quanto è stata allontanata la sfera
|
|
Point3d ptCylOrig = ptToolOrig + vtMove * max( dDist, 0.) ;
|
|
double dDist2 = CAvCylinderTriangle( ptCylOrig, vtToolAx, dCylHeigth, dRadius, trTria, vtMove) ;
|
|
return ( max( dDist, 0.) + max( dDist2, 0.)) ;
|
|
}
|
|
// 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 ;
|
|
|
|
// Moto 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 ;
|
|
// La sua normale vale
|
|
// Nel Caso 1 coincide con quella dell'utensile, nel caso 2 è opposta
|
|
// !!!!! DA FARE !!!!!
|
|
// 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, vtCylAx, dRad, trTria.GetP( nVS), vtSeg, dSegLen) ;
|
|
if ( dCurDist > dMaxDistVS)
|
|
dMaxDistVS = dCurDist ;
|
|
}
|
|
// Distanza di allontanamento dall'interno del triangolo
|
|
double dMaxDistI = DiskTriaInteriorLeakDistLongMot( ptBase, vtCylAx, dRad, trTria) ;
|
|
return max( dMaxDistVS, dMaxDistI) ;
|
|
}
|
|
// Moto perpendicolare all'asse del cilindro
|
|
else if ( nMotionType == 3) {
|
|
// PROVE PUNTI E SEGMENTI
|
|
double dLeakDist = - DBL_MAX ;
|
|
for ( int nVrt = 0 ; nVrt < 3 ; ++ nVrt) {
|
|
double dCurDist = CylPointLeakDistOrtMotion( trTria.GetP( nVrt), ptCylOrig, vtCylAx, vtMove, dHeigth, dRad) ;
|
|
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( trTria.GetP( nVrtS), ptCylOrig, vtSeg,
|
|
vtCylAx, vtMove, dSegLen, dHeigth, dRad) ;
|
|
if ( dLeakDist < dCurDist)
|
|
dLeakDist = dCurDist ;
|
|
}
|
|
// FINE PROVA SEGMENTI
|
|
// PROVA INTERNO
|
|
Point3d ptBaseCont, ptBottomCont ;
|
|
double dBaseLeak = DiskPlaneLeakDistOrtMot( trTria, ptCylOrig, vtCylAx, vtMove, dRad, ptBaseCont) ;
|
|
double dBottomLeak = DiskPlaneLeakDistOrtMot( trTria, ptCylOrig - dHeigth * vtCylAx, vtCylAx, vtMove, dRad, 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 ;
|
|
}
|
|
// Moto generico
|
|
else
|
|
return -1. ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Determina la distanza di fuga di un disco, che trasla lungo il suo asse di simmetria, da
|
|
// un segmento. Il disco è descritto dal quadrato del suo raggio, il moto dal centro nella
|
|
// posizione di partenza e il versore di traslazione. Il segmento è
|
|
// descritto da un punto suo estremo, dal versore direzione diretto verso
|
|
// l'altro estremo e dalla sua lunghezza.
|
|
//double
|
|
//DiskSegmentLeakDistLongMotion( const Point3d& ptDisc, const Point3d& ptSeg,
|
|
// const Vector3d& vtDiscLeak, const Vector3d& vtSeg,
|
|
// double dSegLen, double dDiscSqRad)
|
|
//{
|
|
// // Il disco non può interferire col segmento nel suo moto, se la distanza del
|
|
// // segmento dall'asse di traslazione è maggiore del raggio.
|
|
// if ( LineSegmentSqDist( ptDisc, ptSeg, vtDiscLeak, vtSeg, dSegLen) > dDiscSqRad)
|
|
// 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 - ptDisc) - ( ptSeg - ptDisc) * vtDiscLeak * vtDiscLeak ;
|
|
// Vector3d vtSegOrt = vtSeg - vtSeg * vtDiscLeak * vtDiscLeak ;
|
|
// DBLVECTOR vdCoef( 3) ;
|
|
// DBLVECTOR vdRoots ;
|
|
// vdCoef[0] = vtLineSegOrt.SqLen() - dDiscSqRad ;
|
|
// vdCoef[1] = 2 * vtLineSegOrt * vtSegOrt ;
|
|
// vdCoef[2] = vtSegOrt.SqLen() ;
|
|
// // Soluzione dell'equazione
|
|
// int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ;
|
|
// // Ciclo sulle soluzioni per trovare la distanza di allontanamento
|
|
// double dSegDist = - DBL_MAX ;
|
|
// 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, ptDisc, vtDiscLeak) ;
|
|
// if ( dSegDist < dCurDist)
|
|
// dSegDist = dCurDist ;
|
|
// }
|
|
// }
|
|
// return dSegDist ;
|
|
//}
|
|
///// ALTRA VERSIONE SPERIMENTALE DELLA PRECEDNTE
|
|
double
|
|
DiskSegmentLeakDistLongMotion( const Point3d& ptDisc, const Vector3d& vtDiscLeak, double dDiscRad,
|
|
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen)
|
|
{
|
|
// Il disco non può interferire col segmento nel suo moto, se la distanza del
|
|
// segmento dall'asse di traslazione è maggiore del raggio.
|
|
if ( LineSegmentSqDist( ptDisc, vtDiscLeak, 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 - ptDisc) - ( ptSeg - ptDisc) * vtDiscLeak * vtDiscLeak ;
|
|
Vector3d vtSegOrt = vtSeg - vtSeg * vtDiscLeak * vtDiscLeak ;
|
|
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, ptDisc, vtDiscLeak) ;
|
|
double dLenEn = PointPlaneSignedDist( ptSeg + dSegLen * vtSeg, ptDisc, vtDiscLeak) ;
|
|
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, ptDisc, vtDiscLeak) ;
|
|
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& ptDisk, const Vector3d& vtDiskAx, double dDiskRad,
|
|
const Triangle3d& trTria)
|
|
{
|
|
// Se disco e triangolo sono complanari
|
|
if ( AreSameVectorApprox( vtDiskAx, trTria.GetN())) {
|
|
// verifico solo il centro, se centro esterno non più del raggio sicuramente toccherebbe i lati
|
|
double dDist = PointPlaneSignedDist( ptDisk, trTria.GetP( 0), trTria.GetN()) ;
|
|
if ( IsPointInsideTriangle( ptDisk - 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 = vtDiskAx * ( trTria.GetN() * vtDiskAx) - trTria.GetN() ;
|
|
vtRadLine.Normalize() ;
|
|
// Punti delle due rette candidate all'intersezione col triangolo
|
|
Point3d ptStPlus = ptDisk + dDiskRad * vtRadLine ;
|
|
Point3d ptStMinus = ptDisk - dDiskRad * vtRadLine ;
|
|
// Intersezioni con la prima retta
|
|
double dDistPlus = 0. ;
|
|
Point3d ptIntPlus1, ptIntPlus2 ;
|
|
int nIntPlus = IntersLineTria( ptStPlus, vtDiskAx, 10, trTria, ptIntPlus1, ptIntPlus2, false) ;
|
|
if ( nIntPlus != ILTT_NO) {
|
|
double dDist1 = PointPlaneSignedDist( ptIntPlus1, ptDisk, vtDiskAx) ;
|
|
// Se l'intersezione è doppia, ha senso anche dDist2.
|
|
if ( nIntPlus == ILTT_SEGM ||
|
|
nIntPlus == ILTT_SEGM_ON_EDGE) {
|
|
double dDist2 = PointPlaneSignedDist( ptIntPlus2, ptDisk, vtDiskAx) ;
|
|
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, vtDiskAx, 10, trTria, ptIntMinus1, ptIntMinus2, false) ;
|
|
if ( nIntMinus != ILTT_NO) {
|
|
double dDist1 = PointPlaneSignedDist( ptIntMinus1, ptDisk, vtDiskAx) ;
|
|
// Se l'intersezione è doppia, ha senso anche dDist2.
|
|
if ( nIntMinus == ILTT_SEGM ||
|
|
nIntMinus == ILTT_SEGM_ON_EDGE) {
|
|
double dDist2 = PointPlaneSignedDist( ptIntMinus2, ptDisk, vtDiskAx) ;
|
|
dDistMinus = max( dDist1, dDist2) ;
|
|
}
|
|
// Altrimenti ha senso solo dDist1
|
|
else
|
|
dDistMinus = dDist1 ;
|
|
}
|
|
return max( dDistPlus, dDistMinus) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Restituisce la distanza di allontanamento di un cilindro, che trasla lungo il suo asse
|
|
// di simmetria, da un triangolo.
|
|
// Il cilindro è descritto da raggio e altezza. Il suo moto è descritto dalla
|
|
// posizione iniziale, dal versore dell'asse di simmetria
|
|
// e dal versore della direzione del moto. Il segmento è descritto da punto
|
|
// iniziale, versore direzione e lunghezza.
|
|
double
|
|
CylTriaSegmentLeakDistLongMot( const Triangle3d& trTria, const Vector3d& vtMotion,
|
|
const Point3d& ptCylBase, double dCylRad)
|
|
{
|
|
// Cerco i punti di contatto sui segmenti
|
|
double dSegDist = - DBL_MIN ;
|
|
// Ciclo sui segmenti
|
|
for ( int nVS = 0 ; nVS < 3 ; ++ nVS) {
|
|
int nVE = ( nVS + 1) % 3 ;
|
|
// Versore del segmento
|
|
Vector3d vtSeg = trTria.GetP( nVE) - trTria.GetP( nVS) ;
|
|
double dSegLen = vtSeg.Len() ;
|
|
vtSeg /= dSegLen ;
|
|
// Vettore punto retta
|
|
Vector3d vtPointLine = trTria.GetP( nVS) - ptCylBase ;
|
|
// Componenti dei vettori ortogonali all'asse del cilindro
|
|
Vector3d vtSegOrt = vtSeg - vtSeg * vtMotion * vtMotion ;
|
|
Vector3d vtPointLineOrt = vtPointLine - vtPointLine * vtMotion * vtMotion ;
|
|
|
|
DBLVECTOR vdCoef(3) ;
|
|
DBLVECTOR vdRoots ;
|
|
vdCoef[0] = vtPointLineOrt.SqLen() - dCylRad * dCylRad ;
|
|
vdCoef[1] = 2 * vtPointLineOrt * vtSegOrt ;
|
|
vdCoef[2] = vtSegOrt.SqLen() ;
|
|
|
|
// Soluzione dell'equazione
|
|
int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ;
|
|
for ( int nSol = 0 ; nSol < nRoot ; ++ nSol) {
|
|
Point3d ptInt = trTria.GetP( nVS) + vdRoots[nSol] * vtSegOrt ;
|
|
if ( ( ptInt - trTria.GetP( nVS)) * vtSeg > 0 &&
|
|
( ptInt - trTria.GetP( nVS)) * vtSeg < dSegLen) {
|
|
double dCurDist = ( ptInt - ptCylBase) * vtMotion ;
|
|
if ( dSegDist < dCurDist)
|
|
dSegDist = dCurDist ;
|
|
}
|
|
}
|
|
}
|
|
return dSegDist ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// 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& ptP, const Point3d& ptCylOrig,
|
|
const Vector3d& vtCylAx, const Vector3d& vtMove,
|
|
double dCylHei, double dCylRad)
|
|
{
|
|
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& ptSeg, const Point3d& ptDiscOrig,
|
|
const Vector3d& vtSeg, const Vector3d& vtDiscAx, const Vector3d& vtMove,
|
|
double dSegLen, double dDiscRad)
|
|
{
|
|
// 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 - ptDiscOrig) * vtDiscAx ;
|
|
double dDotEnd = ( ptSegEnd - ptDiscOrig) * vtDiscAx ;
|
|
if ( dDotStart * dDotEnd < 0.) {
|
|
double dS = ( ptDiscOrig - ptSeg) * vtDiscAx / ( vtSeg * vtDiscAx) ;
|
|
Point3d ptInt = ptSeg + dS * vtSeg ;
|
|
Vector3d vtOI = ( ptInt - ptDiscOrig) ;
|
|
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& ptSeg, const Point3d& ptCylOrig,
|
|
const Vector3d& vtSeg, const Vector3d& vtCylAx, const Vector3d& vtMove,
|
|
double dSegLen, double dCylHei, double dCylRad)
|
|
{
|
|
// 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( ptSeg, ptCylOrig, vtSeg, vtCylAx,
|
|
vtMove, dSegLen, dCylRad) ;
|
|
double dBottomLeakDist = DiskSegmentLeakDistOrtMot( ptSeg, ptCylOrig - dCylHei * vtCylAx,
|
|
vtSeg, vtCylAx, vtMove, dSegLen, dCylRad) ;
|
|
|
|
double dSurfLeakDist = - DBL_MAX ;
|
|
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 Triangle3d& trTria, const Point3d& ptDiscOrig, const Vector3d& vtDiscAx,
|
|
const Vector3d& vtMotion, double dCylRad, Point3d& ptContact)
|
|
{
|
|
Vector3d vtLine = trTria.GetN() ^ vtDiscAx ;
|
|
if ( vtLine.Normalize() && abs( vtMotion * vtLine) < 1 - EPS_ZERO) {
|
|
// Definisco l'equazione
|
|
DBLVECTOR vdCoef( 3) ;
|
|
DBLVECTOR vdRoots ;
|
|
double dP = ( trTria.GetP( 0) - ptDiscOrig) * trTria.GetN() / ( vtMotion * trTria.GetN()) ;
|
|
Point3d ptLine = ptDiscOrig + dP * vtMotion ;
|
|
Vector3d vtMotOrt = vtMotion - vtMotion * vtLine * vtLine ;
|
|
Vector3d vtDist = ptLine - ptDiscOrig ;
|
|
Vector3d vtDistOrt = vtDist - vtDist * vtLine * vtLine ;
|
|
// Setto i coefficienti dell'equazione
|
|
vdCoef[0] = vtDistOrt.SqLen() - dCylRad * dCylRad ;
|
|
vdCoef[1] = - 2 * vtMotOrt * vtDistOrt ;
|
|
vdCoef[2] = vtMotOrt.SqLen() ;
|
|
// Risolvo l'equazione
|
|
int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ;
|
|
// Studio le soluzioni
|
|
double dLeakDist = - DBL_MAX ;
|
|
for ( int nS = 0 ; nS < nRoot ; ++ nS) {
|
|
if ( dLeakDist < vdRoots[nS]) {
|
|
double dLinePar = vdRoots[nS] * vtMotion * vtLine - vtDist * vtLine ;
|
|
ptContact = ptLine + dLinePar * vtLine ;
|
|
dLeakDist = vdRoots[nS] ;
|
|
}
|
|
}
|
|
return max( dLeakDist, 0.) ;
|
|
}
|
|
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 ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// 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 ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// 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 ;
|
|
}
|
|
|
|
|
|
|
|
|
|
|