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

EgtGeomKernel 1.9j5 :

Browse Source 
- in CAvToolTriangle aggiunta gestione movimenti generici.
This commit is contained in:
Dario Sassi
2018-10-25 18:32:37 +00:00
parent fc00d6273a
commit cee2ae0c77
4 changed files with 706 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CAvToolTriangle.cpp
+675 -37
View File
@@ -1,7 +1,7 @@
//----------------------------------------------------------------------------
// EgalTech 2018-2018
//----------------------------------------------------------------------------
// File : CAvToolTriangle.cpp Data : 19.07.18 Versione : 1.9h1
// File : CAvToolTriangle.cpp Data : 25.10.18 Versione : 1.9j4
// Contenuto : Implementazione delle funzioni ToolTriangleCollisionAvoid.
//
//
@@ -17,6 +17,7 @@
#include "CAvToolTriangle.h"
#include "IntersLineSurfStd.h"
#include "IntersLineTria.h"
#include "DistPointLine.h"
#include "/EgtDev/Include/ENkPolynomialRoots.h"
#include "/EgtDev/Include/EgtNumUtils.h"
#include "/EgtDev/Include/EGkIntervals.h"
@@ -90,7 +91,7 @@ CAvToolTriangle( const Tool& tlTool, const Point3d& ptToolOrig, const Vector3d&
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) ;
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)
@@ -131,7 +132,7 @@ CAvToolTriangle( const Tool& tlTool, const Point3d& ptToolOrig, const Vector3d&
Point3d ptTorusCen = ptToolOrig - dCylHeigth * vtToolAx ;
double dDist2 = CAvTorusTriangle( ptTorusCen + dDist * vtMove, vtToolAx,
tlTool.GetRadius() - tlTool.GetCornRadius(), tlTool.GetCornRadius(),
trTria, vtMove, true, false) ;
trTria, vtMove, true, false) ;
if ( dDist2 < - EPS_SMALL)
return dDist2 ;
// infine verifico quello del cilindro (tenendo conto dei precedenti allontanamenti)
@@ -492,8 +493,38 @@ CAvCylinderTriangle( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double d
return dLeakDist ;
}
// Movimento generico
else if ( vtMove * vtCylAx > 0)
return 0. ;
else if ( vtMove * vtCylAx > 0.) {
// Controlli preliminari sull'interferenza fra cilindro e triangolo
Vector3d vtOrtMv = vtMove - ( vtMove * vtCylAx) * vtCylAx ;
if ( ! vtOrtMv.Normalize())
return - 1. ;
Vector3d vtR0 = trTria.GetP( 0) - ptCylOrig ;
Vector3d vtR1 = trTria.GetP( 1) - ptCylOrig ;
Vector3d vtR2 = trTria.GetP( 2) - ptCylOrig ;
if ( vtR0 * vtOrtMv < - dRad && vtR1 * vtOrtMv < - dRad && vtR2 * vtOrtMv < - dRad)
return 0. ;
Vector3d vtLat = vtCylAx ^ vtOrtMv ;
if ( ( vtR0 * vtLat < - dRad && vtR1 * vtLat < - dRad && vtR2 * vtLat < - dRad) ||
( vtR0 * vtLat > dRad && vtR1 * vtLat > dRad && vtR2 * vtLat > dRad))
return 0. ;
// Calcolo la distanza di allontanamento
double dLeakDist = 0. ;
// distanza di allontanamento dall'interno
dLeakDist = max( dLeakDist, CylTriaInteriorLeakDistGenMot( ptCylOrig, vtCylAx, dHeigth, dRad, trTria, vtMove)) ;
if ( dLeakDist > EPS_SMALL)
return dLeakDist ;
// Distanza di allontanamento dalla frontiera
for ( int nV = 0 ; nV < 3 ; ++ nV) {
Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
dLeakDist = max( dLeakDist, CylPointLeakDistGenMot( ptCylOrig, vtCylAx, dHeigth, dRad,
trTria.GetP( nV), vtMove, bTop, bBot)) ;
dLeakDist = max( dLeakDist, CylSegmentLeakDistGenMot( ptCylOrig, vtCylAx, dHeigth, dRad,
trTria.GetP( nV), vtSeg, dSegLen, vtMove, bTop, bBot)) ;
}
return dLeakDist ;
}
// Errore
else
return -1. ;
@@ -614,6 +645,150 @@ CylSegmentLeakDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx,
return max( max( dBaseLeakDist, dBottomLeakDist), max( dSurfLeakDist, 0.)) ;
}
//----------------------------------------------------------------------------
double
CylPointLeakDistGenMot( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad,
const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot)
{
// Setto le tolleranze sui limiti del cilindro
double dTopTol = ( bTop ? EPS_SMALL : - EPS_SMALL) ;
double dBotTol = ( bBot ? - EPS_SMALL : EPS_SMALL) ;
// Grandezze da cui dipendono i parametri
Vector3d vtR0 = ptP - ptCylOrig ;
double dDotAxR0 = vtR0 * vtCylAx ;
double dDotMoveAx = vtMove * vtCylAx ;
// Definisco i parametri dell'equazione
DBLVECTOR vdCoef( 3) ;
DBLVECTOR vdRoots ;
vdCoef[0] = vtR0 * vtR0 - dDotAxR0 * dDotAxR0 - dRad * dRad ;
vdCoef[1] = 2 * ( dDotMoveAx * dDotAxR0 - vtR0 * vtMove) ;
vdCoef[2] = 1 - dDotMoveAx * dDotMoveAx ;
// Risolvo l'equazione
int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ;
// Ciclo sulle soluzioni per cercare la distanza di allontanamento
double dLeakDist = 0. ;
for ( int n = 0 ; n < nRoot ; ++ n) {
double dDotIntAx = ( vtR0 - vdRoots[n] * vtMove) * vtCylAx ;
if ( vdRoots[n] > dLeakDist && dDotIntAx < dTopTol && dDotIntAx > - dHeigth + dBotTol)
dLeakDist = vdRoots[n] ;
}
return dLeakDist ;
}
//----------------------------------------------------------------------------
double
CylSegmentLeakDistGenMot( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen,
const Vector3d& vtMove, bool bTop, bool bBot)
{
// Setto le tolleranze sui limiti del cilindro
double dTopTol = ( bTop ? EPS_SMALL : - EPS_SMALL) ;
double dBotTol = ( bBot ? - EPS_SMALL : EPS_SMALL) ;
// Grandezze da cui dipendono i parametri
Vector3d vtR0 = ptSeg - ptCylOrig ;
double dDotR0Ax = vtR0 * vtCylAx ;
double dDotMoveAx = vtMove * vtCylAx ;
double dDotSegAx = vtSeg * vtCylAx ;
// Se l'asse del cilindro e il segmento hanno la medesima direzione
if ( AreSameOrOppositeVectorApprox( vtCylAx, vtSeg)) {
// Valuto distanze di allontanamento degli estremi del segmento; ovviamente esse,
// a causa del parallelismo fra segmento e asse del cilindro, sono uguali tra loro.
// Calcolo quindi solo quella del punto di partenza.
Vector3d vtMoveOrt = vtMove - dDotMoveAx * vtCylAx ;
Vector3d vtR0Ort = vtR0 - dDotR0Ax * vtCylAx ;
DBLVECTOR vdCoef( 3) ;
DBLVECTOR vdRoots ;
vdCoef[0] = vtR0Ort * vtR0Ort - dRad * dRad ;
vdCoef[1] = - 2 * vtR0Ort * vtMoveOrt ;
vdCoef[2] = vtMoveOrt * vtMoveOrt ;
// Risoluzione dell'equazione e studio delle soluzioni
int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ;
double dLeakDist = 0. ;
for ( int n = 0 ; n < nRoot ; ++ n) {
// Se abbiamo trovato una potenziale nuova distanza di allontanamento
if ( vdRoots[n] > dLeakDist) {
// Posizioni di contatto degli estremi e del punto medio del segmento
Point3d ptTouch0 = ptSeg - vdRoots[n] * vtMove ;
Point3d ptTouch1 = ptTouch0 + dSegLen * vtSeg ;
Point3d ptTouchM = ( ptTouch0 + ptTouch1) / 2 ;
double dDot0 = ( ptTouch0 - ptCylOrig) * vtCylAx ;
double dDot1 = ( ptTouch1 - ptCylOrig) * vtCylAx ;
double dDotM = ( ptTouchM - ptCylOrig) * vtCylAx ;
// Se almeno uno dei tre punti di contatto è sul cilindro, la potenziale distanza è valida
if ( ( dDot0 < dTopTol && dDot0 > - dHeigth + dBotTol) ||
( dDot1 < dTopTol && dDot1 > - dHeigth + dBotTol) ||
( dDotM < dTopTol && dDotM > - dHeigth + dBotTol))
dLeakDist = vdRoots[n] ;
}
}
return dLeakDist ;
}
// L'equazione nel parametro del segmento è di secondo grado
double dAlpha = 1 - dDotSegAx * dDotSegAx ;
double dBeta1 = dDotSegAx * dDotMoveAx - vtSeg * vtMove ;
double dBeta2 = vtR0 * vtSeg - dDotR0Ax * dDotSegAx ;
double dGamma = 1 - dDotMoveAx * dDotMoveAx ;
double dLambda = dDotR0Ax * dDotMoveAx - vtR0 * vtMove ;
double dEta = vtR0 * vtR0 - dDotR0Ax * dDotR0Ax - dRad * dRad ;
// Abbiamo un'equazione di secondo grado in due incognite.
// Un'incognita rappresenta il parametro del segmento, l'altra
// lo spostamento nella direzione di traslazione.
// Se consideriamo come variabile il parametro del segmento e lo
// spostamento come parametro dell'equazione, dobbiamo imporre
// Delta = 0 per ottenere una nuova equazione nella sola variabile
// spostamento. Le soluzioni di quest'ultima equazione sono candidate
// a essere distanze di fuga.
// Equazione Delta = 0
DBLVECTOR vdDeltaCoef( 3) ;
DBLVECTOR vdDeltaRoots ;
vdDeltaCoef[0] = 4 * ( dBeta2 * dBeta2 - dAlpha * dEta) ;
vdDeltaCoef[1] = 8 * ( dBeta1 * dBeta2 - dAlpha * dLambda) ;
vdDeltaCoef[2] = 4 * ( dBeta1 * dBeta1 - dAlpha * dGamma) ;
// Risolvo l'equazione
int nDeltaRoot = PolynomialRoots( 2, vdDeltaCoef, vdDeltaRoots) ;
// Ciclo sulle soluzioni e per ogni soluzione valida valuto se la tangenza è
// situata sul cilindro.
double dLeakDist = 0. ;
for ( int n = 0 ; n < nDeltaRoot ; ++ n) {
if ( vdDeltaRoots[n] > dLeakDist) {
double dS = - ( dBeta1 * vdDeltaRoots[n] + dBeta2) / dAlpha ;
if ( dS > - EPS_SMALL && dS < dSegLen + EPS_SMALL) {
Point3d ptTan = ptSeg + dS * vtSeg - vdDeltaRoots[n] * vtMove ;
Vector3d vtTan = ptTan - ptCylOrig ;
double dDotTanAx = vtTan * vtCylAx ;
if ( dDotTanAx < dTopTol && dDotTanAx > - dHeigth + dBotTol)
dLeakDist = vdDeltaRoots[n] ;
}
}
}
return dLeakDist ;
}
//----------------------------------------------------------------------------
double
CylTriaInteriorLeakDistGenMot( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad,
const Triangle3d& trTria, const Vector3d& vtMove)
{
double dLeakDist = 0. ;
if ( ! ( AreOrthoApprox( vtCylAx, trTria.GetN())))
return dLeakDist ;
// Un punto candidato a essere di estremo contatto è ptP = ptCylOrig - dRad * trTria.GetN(),
// dU è il parametro a cui la retta ptP + dU * vtMove interseca il piano del triangolo.
double dU = ( dRad - ( ptCylOrig - trTria.GetP(0)) * trTria.GetN()) / ( vtMove * trTria.GetN()) ;
if ( dU < EPS_SMALL)
return dLeakDist ;
// Estremi del segmento, facente parte del cilindro, candidato ad
// essere di estremo contatto, traslato sul piano del triangolo.
Point3d ptTouch0 = ptCylOrig - dRad * trTria.GetN() + dU * vtMove ;
Point3d ptTouch1 = ptTouch0 - dHeigth * vtCylAx ;
// Studio l'intersezione del segmento col triangolo
Point3d ptInt0, ptInt1 ;
int nIntType = IntersLineTria( ptTouch0, ptTouch1, trTria, ptInt0, ptInt1) ;
if ( ! ( nIntType == ILTT_NO || nIntType == ILTT_VERT || nIntType == ILTT_IN))
dLeakDist = dU ;
return dLeakDist ;
}
//----------------------------------------------------------------------------
// **** CONO ****
@@ -690,8 +865,40 @@ CAvTrConeTriangle( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double
return dLeakDist ;
}
// Movimento generico
else
return 0. ;
else {
// Controlli preliminari sull'interferenza fra ctronco di cono e triangolo
Vector3d vtOrtMv = vtMove - ( vtMove * vtTrConeAx) * vtTrConeAx ;
if ( ! vtOrtMv.Normalize())
return - 1. ;
Vector3d vtR0 = trTria.GetP( 0) - ptMinBase ;
Vector3d vtR1 = trTria.GetP( 1) - ptMinBase ;
Vector3d vtR2 = trTria.GetP( 2) - ptMinBase ;
if ( vtR0 * vtOrtMv < - dMaxBaseR && vtR1 * vtOrtMv < - dMaxBaseR && vtR2 * vtOrtMv < - dMaxBaseR)
return 0. ;
Vector3d vtLat = vtTrConeAx ^ vtOrtMv ;
if ( ( vtR0 * vtLat < - dMaxBaseR && vtR1 * vtLat < - dMaxBaseR && vtR2 * vtLat < - dMaxBaseR) ||
( vtR0 * vtLat > dMaxBaseR && vtR1 * vtLat > dMaxBaseR && vtR2 * vtLat > dMaxBaseR))
return 0. ;
// Calcolo la distanza di allontanamento
double dLeakDist = 0. ;
// Allontanamento di allontanamento dall'interno del triangolo
dLeakDist = max( dLeakDist, TrConeTriaInteriorLeakDistGenMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR,
dTrConeH, trTria, vtMove)) ;
// Se positiva abbiamo terminato
if ( dLeakDist > EPS_SMALL)
return dLeakDist ;
// Allontanamento dalla frontiera
for ( int n = 0 ; n < 3 ; ++ n) {
Vector3d vtSeg = trTria.GetP( ( n + 1) % 3) - trTria.GetP( n) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
dLeakDist = max( dLeakDist, TrConePointLeakDistGenMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH,
trTria.GetP( n), vtMove, bTop, bBot)) ;
dLeakDist = max( dLeakDist, TrConeSegmentLeakDistGenMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH,
trTria.GetP( n), vtSeg, dSegLen, vtMove, bTop, bBot)) ;
}
return dLeakDist ;
}
}
//----------------------------------------------------------------------------
@@ -949,6 +1156,174 @@ TrConeTriangleInteriorLeakDistOrtMot( const Point3d& ptMinBase, const Vector3d&
return dLeakDist ;
}
//----------------------------------------------------------------------------
double
TrConePointLeakDistGenMot( 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 ;
// Tangente dell'angolo di semi-apertura del cono e vertice del cono
double dTanAlpha = ( dMaxBaseR - dMinBaseR) / dTrConeH ;
double dDistMinBaseV = dMinBaseR / dTanAlpha ;
Point3d ptConeV = ptMinBase - dDistMinBaseV * vtTrConeAx ;
// Grandezze da cui dipendono i parametri dell'equazione
Vector3d vtR0 = ptP - ptConeV ;
double dDotMoveAx = vtMove * vtTrConeAx ;
double dDotR0Ax = vtR0 * vtTrConeAx ;
double dB = 1 + dTanAlpha * dTanAlpha ;
// Definizione dell'equazione
DBLVECTOR vdCoef( 3) ;
DBLVECTOR vdRoots ;
vdCoef[0] = vtR0 * vtR0 - dB * dDotR0Ax * dDotR0Ax ;
vdCoef[1] = 2 * ( dB * dDotR0Ax * dDotMoveAx - vtR0 * vtMove) ;
vdCoef[2] = 1 - dB * dDotMoveAx * dDotMoveAx ;
// Soluzione dell'equazione
int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ;
// Analisi delle soluzioni e calcolo della distanza di allontanamento
double dLeakDist = 0. ;
for ( int n = 0 ; n < nRoot ; ++ n) {
if ( vdRoots[n] > dLeakDist) {
Point3d ptTouch = ptP - vdRoots[n] * vtMove ;
double dLongComp = ( ptTouch - ptMinBase) * vtTrConeAx ;
if ( dLongComp > dBotTol && dLongComp < dTrConeH + dTopTol)
dLeakDist = vdRoots[n] ;
}
}
return dLeakDist ;
}
//----------------------------------------------------------------------------
double
TrConeSegmentLeakDistGenMot( 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)
{
// Setto le tolleranze sui limiti del cono
double dTopTol = ( bTop ? EPS_SMALL : - EPS_SMALL) ;
double dBotTol = ( bBot ? - EPS_SMALL : EPS_SMALL) ;
// Tangente dell'angolo di semi-apertura del cono e vertice del cono
double dTanTheta = ( dMaxBaseR - dMinBaseR) / dTrConeH ;
double dDistMinBaseV = dMinBaseR / dTanTheta ;
Point3d ptConeV = ptMinBase - dDistMinBaseV * vtTrConeAx ;
// Grandezze da cui dipendono i parametri
Vector3d vtR0 = ptSeg - ptConeV ;
double dDotR0Ax = vtR0 * vtTrConeAx ;
double dDotMoveAx = vtMove * vtTrConeAx ;
double dDotSegAx = vtSeg * vtTrConeAx ;
double dB = 1 + dTanTheta * dTanTheta ;
// Coseno e seno dell'angolo di semi-apertura del cono
double dCosAlpha = 1 / sqrt( 1 + dTanTheta * dTanTheta) ;
double dSinAlpha = ( 1 - dCosAlpha * dCosAlpha > 0 ? sqrt( 1 - dCosAlpha * dCosAlpha) : 0.) ;
// Caso in cui il segmento è parallelo a una generatrice
if ( abs( dDotSegAx - dCosAlpha) < EPS_ANG_ZERO * dSinAlpha) {
// Definizione dell'equazione
DBLVECTOR vdCoef( 3) ;
DBLVECTOR vdRoots ;
vdCoef[0] = vtR0 * vtR0 - dB * dDotR0Ax * dDotR0Ax ;
vdCoef[1] = 2 * ( dB * dDotR0Ax * dDotMoveAx - vtR0 * vtMove) ;
vdCoef[2] = 1 - dB * dDotMoveAx * dDotMoveAx ;
// Soluzione dell'equazione
int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ;
// Analisi delle soluzioni e calcolo della distanza di allontanamento
double dLeakDist = 0. ;
for ( int n = 0 ; n < nRoot ; ++ n) {
if ( vdRoots[n] > dLeakDist) {
Point3d ptTouch0 = ptSeg - vdRoots[n] * vtMove ;
Point3d ptTouch1 = ptTouch0 + dSegLen * vtSeg ;
Point3d ptTouchM = ( ptTouch0 + ptTouch1) / 2 ;
double dLongComp0 = ( ptTouch0 - ptMinBase) * vtTrConeAx ;
double dLongComp1 = ( ptTouch1 - ptMinBase) * vtTrConeAx ;
double dLongCompM = ( ptTouchM - ptMinBase) * vtTrConeAx ;
// Se almeno uno dei tre punti di contatto è sul cilindro, la potenziale distanza è valida
if ( ( ptTouch0 - ptConeV) * vtTrConeAx > - EPS_SMALL &&
( ( dLongComp0 > dBotTol && dLongComp0 < dTrConeH + dTopTol) ||
( dLongComp1 > dBotTol && dLongComp1 < dTrConeH + dTopTol) ||
( dLongCompM > dBotTol && dLongCompM < dTrConeH + dTopTol)))
dLeakDist = vdRoots[n] ;
}
}
return dLeakDist ;
}
// Caso generale
double dAlpha = 1 - dB * dDotSegAx * dDotSegAx ;
double dBeta1 = dB * dDotMoveAx * dDotSegAx - vtMove * vtSeg ;
double dBeta2 = vtR0 * vtSeg - dB * dDotR0Ax * dDotSegAx ;
double dGamma = 1 - dB * dDotMoveAx * dDotMoveAx ;
double dLambda = dB * dDotR0Ax * dDotMoveAx - vtR0 * vtMove ;
double dEta = vtR0 * vtR0 - dB * dDotR0Ax * dDotR0Ax ;
// Abbiamo un'equazione di secondo grado in due incognite.
// Un'incognita rappresenta il parametro del segmento, l'altra
// lo spostamento nella direzione di traslazione.
// Se consideriamo come variabile il parametro del segmento e lo
// spostamento come parametro dell'equazione, dobbiamo imporre
// Delta = 0 per ottenere una nuova equazione nella sola variabile
// spostamento. Le soluzioni di quest'ultima equazione sono candidate
// a essere distanze di fuga.
// Equazione Delta = 0
DBLVECTOR vdDeltaCoef( 3) ;
DBLVECTOR vdDeltaRoots ;
vdDeltaCoef[0] = 4 * ( dBeta2 * dBeta2 - dAlpha * dEta) ;
vdDeltaCoef[1] = 8 * ( dBeta1 * dBeta2 - dAlpha * dLambda) ;
vdDeltaCoef[2] = 4 * ( dBeta1 * dBeta1 - dAlpha * dGamma) ;
// Risolvo l'equazione
int nDeltaRoot = PolynomialRoots( 2, vdDeltaCoef, vdDeltaRoots) ;
// Ciclo sulle soluzioni e per ogni soluzione valida valuto se la tangenza è
// situata sul cilindro.
double dLeakDist = 0. ;
for ( int n = 0 ; n < nDeltaRoot ; ++ n) {
if ( vdDeltaRoots[n] > dLeakDist) {
double dS = - ( dBeta1 * vdDeltaRoots[n] + dBeta2) / dAlpha ;
if ( dS > - EPS_SMALL && dS < dSegLen + EPS_SMALL) {
Point3d ptTan = ptSeg + dS * vtSeg - vdDeltaRoots[n] * vtMove ;
Vector3d vtTan = ptTan - ptMinBase ;
double dDotTanAx = vtTan * vtTrConeAx ;
if ( dDotTanAx > dBotTol && dDotTanAx < dTrConeH + dTopTol)
dLeakDist = vdDeltaRoots[n] ;
}
}
}
return dLeakDist ;
}
//----------------------------------------------------------------------------
double
TrConeTriaInteriorLeakDistGenMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove)
{
double dLeakDist = 0. ;
// Affinché ci possa essere tangenza, l'angolo fra il versore dell'asse A
// e il versore normale del triangolo N deve essere uguale a beta = Pi / 2 - alpha
// dove alpha è l'angolo di semi-apertura del cono. Poiché Tan beta = 1 / Tan alpha e
// cos beta = 1 / sqrt( 1 + ( tan beta)^2), abbiamo che cos beta = tan alpha / sqrt( 1 + ( tan alpha)^2).
// Quindi affinché possa esserci tangenza si deve avere N * A - tan alpha / sqrt( 1 + ( tan alpha)^2) = 0.
double dTanAlpha = dTrConeH / ( dMaxBaseR - dMinBaseR) ;
double dCosBeta = dTanAlpha / sqrt( 1 + dTanAlpha * dTanAlpha) ;
double dSinBeta = ( 1 - dCosBeta * dCosBeta > 0 ? sqrt( 1 - dCosBeta * dCosBeta) : 0.) ;
if ( abs( trTria.GetN() * vtTrConeAx - dCosBeta) > dSinBeta * EPS_ANG_ZERO)
return dLeakDist ;
// Calcolo componente ortogonale di N ad A
Vector3d vtOrtN = trTria.GetN() - ( trTria.GetN() * vtTrConeAx) * vtTrConeAx ;
vtOrtN.Normalize() ;
// Un punto candidato a essere di estremo contatto è ptP = ptMinBase - dMinRad * vtOrtN,
// dU è il parametro a cui la retta ptP + dU * vtMove interseca il piano del triangolo.
double dU = ( dMinBaseR * trTria.GetN() * vtOrtN - ( ptMinBase - trTria.GetP(0)) * trTria.GetN()) / ( vtMove * trTria.GetN()) ;
if ( dU < EPS_SMALL)
return dLeakDist ;
// Estremi del segmento, facente parte del cilindro, candidato ad
// essere di estremo contatto, traslato sul piano del triangolo.
Point3d ptTouch0 = ptMinBase - dMinBaseR * vtOrtN + dU * vtMove ;
Point3d ptTouch1 = ptTouch0 - ( dMaxBaseR - dMinBaseR) * vtOrtN + dTrConeH * vtTrConeAx ;
// Studio l'intersezione del segmento col triangolo
Point3d ptInt0, ptInt1 ;
int nIntType = IntersLineTria( ptTouch0, ptTouch1, trTria, ptInt0, ptInt1) ;
if ( ! ( nIntType == ILTT_NO || nIntType == ILTT_VERT || nIntType == ILTT_IN))
dLeakDist = dU ;
return dLeakDist ;
}
//----------------------------------------------------------------------------
// **** TORO ****
// In questi algoritmi per toro intendiamo la corona torica esterna
@@ -1028,8 +1403,40 @@ CAvTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double d
}
// Movimento generico
else
return -1 ;
else {
// Controlli preliminari sull'interferenza del triangolo con il toro
Vector3d vtOrtMv = vtMove - ( vtMove * vtTorusAx) * vtTorusAx ;
vtOrtMv.Normalize( /*EPS_ZERO*/) ;
Point3d ptBehind = ptTorusCen - dMaxRad * vtOrtMv ;
double dRadSum = dMaxRad + dMinRad ;
if ( ( trTria.GetP( 0) - ptBehind) * vtOrtMv < - dRadSum &&
( trTria.GetP( 1) - ptBehind) * vtOrtMv < - dRadSum &&
( trTria.GetP( 2) - ptBehind) * vtOrtMv < - dRadSum)
return 0. ;
Vector3d vtLat = vtTorusAx ^ vtOrtMv ;
double dDot0 = ( trTria.GetP( 0) - ptTorusCen) * vtLat ;
double dDot1 = ( trTria.GetP( 1) - ptTorusCen) * vtLat ;
double dDot2 = ( trTria.GetP( 2) - ptTorusCen) * vtLat ;
if ( ( dDot0 < - dRadSum && dDot1 < - dRadSum && dDot2 < - dRadSum) ||
( dDot0 > dRadSum && dDot1 > dRadSum && dDot2 > dRadSum))
return 0. ;
// Distanza di allontanamento dall'interno
double dInnLeakDist = TorusTriangleInteriorLeakDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, trTria, vtMove) ;
// Se positiva abbiamo finito
if ( dInnLeakDist > EPS_SMALL)
return dInnLeakDist ;
// Distanza di allontanamento da frontiera
double dBordLeakDist = 0. ;
for ( int n = 0 ; n < 3 ; ++ n) {
Vector3d vtSeg = trTria.GetP( ( n + 1) % 3) - trTria.GetP( n) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
double dCurDist = TorusSegmentLeakDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad,
trTria.GetP( n), vtSeg, dSegLen, vtMove) ;
dBordLeakDist = max( dBordLeakDist, dCurDist) ;
}
return dBordLeakDist ;
}
}
//----------------------------------------------------------------------------
@@ -1288,6 +1695,97 @@ TorusTriangleInteriorLeakDistOrtMot( const Point3d& ptTorusCen, const Vector3d&
return 0. ;
}
//----------------------------------------------------------------------------
double
TorusPointLeakDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Point3d& ptP, const Vector3d& vtMove)
{
std::vector<bool> vbType ;
std::vector<double> vdPar ;
// Intersezione fra semi-retta e corona torica
int nIntType = LinCompTorusExtInt( ptP, - vtMove, 1, Ray, ptTorusCen, vtTorusAx, dMinRad, dMaxRad, vbType, vdPar) ;
if ( nIntType == T_ERROR)
return nIntType ;
// Cerco il maggior parametro corrispondente a un contatto secante
double dDist = 0. ;
for ( int n = 0 ; n < int( vdPar.size()) ; ++ n) {
// Se secante e maggiore della distanza corrente aggiorno la distanza
if ( vbType[n] && dDist < vdPar[n])
dDist = vdPar[n] ;
}
return dDist ;
}
//----------------------------------------------------------------------------
double
TorusSegmentLeakDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove)
{
// Restringo il segmento nella striscia di spazio in cui si sposta il toro
Point3d ptMySegP = ptSeg ;
Vector3d vtPlane = vtTorusAx ^ vtMove ;
if ( ! vtPlane.Normalize())
return - 1. ;
Point3d ptPlaneLatPlus = ptTorusCen + ( dMaxRad + dMinRad) * vtPlane ;
Plane3d plPlane ;
plPlane.Set( ( ORIG - ptPlaneLatPlus) * vtPlane, - vtPlane) ;
ClampSegmentOutPlane( plPlane, ptMySegP, vtSeg, dSegLen) ;
Point3d ptPlanLateMinus = ptTorusCen - ( dMaxRad + dMinRad) * vtPlane ;
plPlane.Set( ( ptPlanLateMinus - ORIG) * vtPlane, vtPlane) ;
ClampSegmentOutPlane( plPlane, ptMySegP, vtSeg, dSegLen) ;
double dDotAxMv = vtMove * vtTorusAx ;
Vector3d vtOrtMv = vtMove - dDotAxMv * vtTorusAx ;
if ( ! vtOrtMv.Normalize())
return - 1. ;
Point3d ptPlaneBehind = ptTorusCen - ( dMaxRad + dMinRad) * vtOrtMv ;
plPlane.Set( ( ptPlaneBehind - ORIG) * vtOrtMv, vtOrtMv) ;
ClampSegmentOutPlane( plPlane, ptMySegP, vtSeg, dSegLen) ;
Point3d ptUpDw = ptTorusCen - ( dDotAxMv > 0. ? dMinRad : 0.) * vtTorusAx ;
Vector3d vtUPDw = dDotAxMv > 0. ? vtTorusAx : - vtTorusAx ;
plPlane.Set( ( ptUpDw - ORIG) * vtUPDw, vtUPDw) ;
ClampSegmentOutPlane( plPlane, ptMySegP, vtSeg, dSegLen) ;
// Cerco il punto del segmento ridotto che ha distanza di allontanamento massima
double dLeakDist = 0. ;
double dStep = max( 0.05 * dMinRad, EPS_SMALL) ;
int nStepNum = int( dSegLen / dStep) ;
for ( int n = 0 ; n <= nStepNum ; ++ n) {
Point3d ptCurPoint = ptMySegP + ( n * dStep) * vtSeg ;
double dCurLeakDist = TorusPointLeakDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ;
if ( dLeakDist < dCurLeakDist)
dLeakDist = dCurLeakDist ;
}
return dLeakDist ;
}
//----------------------------------------------------------------------------
double
TorusTriangleInteriorLeakDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Triangle3d& trTria, const Vector3d& vtMove)
{
// Siamo interessati solo ai casi di tangenza, poiché il punto di contatto
// estremo con l'interno di un triangolo non può che essere di tangenza.
double dCompLong = trTria.GetN() * vtTorusAx ;
Vector3d vtNormOrt = trTria.GetN() - dCompLong * vtTorusAx ;
// Se il triangolo è orientato in modo che possa esserci un punto di tangenza oppure la
// direzione di allontanamentoè nel piano del triangolo abbiamo finito
if ( ( dCompLong < 0. || dCompLong >= 1.) || ( ! vtNormOrt.Normalize()) ||
abs( vtMove * trTria.GetN()) < EPS_ZERO)
return 0. ;
// Punto del toro candidato a toccare l'interno del triangolo
Point3d ptTouch = ptTorusCen - dMaxRad * vtNormOrt - dMinRad * trTria.GetN() ;
// Distanza percorsa dal punto per posizionarsi sul piano del triangolo
double dDist = ( ( trTria.GetP( 0) - ptTouch) * trTria.GetN()) / ( vtMove * trTria.GetN()) ;
// Se distanza negativa abbiamo finito
if ( dDist < 0.)
return 0. ;
// Sposto il punto sul piano del triangolo
ptTouch += ( dDist * vtMove) ;
// Se ora il punto non giace dentro il triangolo distanza nulla
if ( ! IsPointInsideOpenTriangle( ptTouch, trTria))
dDist = 0. ;
return dDist ;
}
//----------------------------------------------------------------------------
double
CAvConcaveTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
@@ -1634,8 +2132,52 @@ CAvDiskTriangle( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDis
}
// Direzione di allontanamento generica
else if ( vtDiskAx * vtMove > 0.) {
// Non trattato al momento
return - 1. ;
Point3d ptV0 = trTria.GetP( 0) ;
Point3d ptV1 = trTria.GetP( 1) ;
Point3d ptV2 = trTria.GetP( 2) ;
// Distanze di allontanamento dei vertici
double dVertLeakDist[3] ;
dVertLeakDist[0] = - ( ( ptDiskCen - ptV0) * vtDiskAx) / ( vtMove * vtDiskAx) ;
dVertLeakDist[1] = - ( ( ptDiskCen - ptV1) * vtDiskAx) / ( vtMove * vtDiskAx) ;
dVertLeakDist[2] = - ( ( ptDiskCen - ptV2) * vtDiskAx) / ( vtMove * vtDiskAx) ;
// Triangolo proiettato sul piano del disco
Triangle3d trTriaOnPlane ;
trTriaOnPlane.Set( ptV0 - dVertLeakDist[0] * vtMove,
ptV1 - dVertLeakDist[1] * vtMove,
ptV2 - dVertLeakDist[2] * vtMove) ;
// Se non c'è interferenza fra triangolo proiettato e disco la distanza di fuga è nulla
if ( ! CoplanarDiscTriangleInterferance( ptDiskCen, dDiskRad, trTriaOnPlane))
return 0. ;
double dSqSafeRad = dDiskRad * dDiskRad - 2 * dDiskRad * EPS_SMALL ;
bool bInside[3] ;
bInside[0] = SqDist( ptDiskCen, trTriaOnPlane.GetP( 0)) < dSqSafeRad ;
bInside[1] = SqDist( ptDiskCen, trTriaOnPlane.GetP( 1)) < dSqSafeRad ;
bInside[2] = SqDist( ptDiskCen, trTriaOnPlane.GetP( 2)) < dSqSafeRad ;
// Tutte le immagini dei vertici cadono nel disco: basta valutare i vertici
if ( bInside[0] && bInside[1] && bInside[2])
return max( max( dVertLeakDist[0], dVertLeakDist[1]), max( dVertLeakDist[2], 0.)) ;
// Caso generale
// Allontanamento dall'interno
double dLeakDist = max( DiskTriaInteriorLeakDistGenMot( ptDiskCen, vtDiskAx, dDiskRad, trTria, vtMove), 0.) ;
// Allontanamento dalla frontiera
Vector3d vtMoveOrt = vtMove - vtMove * vtDiskAx * vtDiskAx ;
vtMoveOrt.Normalize() ;
Frame3d DiskFrame ;
Vector3d vtJ = vtDiskAx ^ vtMoveOrt ;
vtJ.Normalize() ;
DiskFrame.Set( ptDiskCen, vtMoveOrt, vtJ, vtDiskAx) ;
Triangle3d trTriaLoc = trTria ;
Vector3d vtMoveLoc = vtMove ;
trTriaLoc.ToLoc( DiskFrame) ;
vtMoveLoc.ToLoc( DiskFrame) ;
for ( int n = 0 ; n < 3 ; ++ n) {
Vector3d vtSeg = trTriaLoc.GetP( ( n + 1) % 3) - trTriaLoc.GetP( n) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
dLeakDist = max( max( DiskSegmentLeakDistGenMot( dDiskRad, trTriaLoc.GetP( n), vtSeg, dSegLen, vtMoveLoc),
bInside[n] ? max( dVertLeakDist[n], 0.) : 0.), dLeakDist) ;
}
return dLeakDist ;
}
// Errore
else
@@ -1708,6 +2250,7 @@ DiskTriaInteriorLeakDistLongMot( const Point3d& ptDiskCen, double dDiskRad,
double dDist = max( PointPlaneSignedDist( ptDiskCen, trTria.GetP( 0), trTria.GetN()), 0.) ;
if ( CoplanarDiscTriangleInterferance( ptDiskCen + dDist * vtMove, dDiskRad, trTria))
return dDist ;
return 0. ;
}
// Se disco e triangolo sono ortogonali non può esserci contatto con interno
if ( AreOrthoApprox( vtMove, trTria.GetN()))
@@ -1717,20 +2260,12 @@ DiskTriaInteriorLeakDistLongMot( const Point3d& ptDiskCen, double dDiskRad,
// 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) ;
// Punto candidato all'essere di estremo contatto
Point3d ptStLine = ptDiskCen - dDiskRad * vtRad ;
// Intersezioni con retta
double dDist = ( ( trTria.GetP( 0) - ptStLine) * trTria.GetN()) / ( vtMove * trTria.GetN()) ;
if ( IsPointInsideTriangle( ptStLine + dDist * vtMove, trTria))
dLeakDist = max( dLeakDist, dDist) ;
return dLeakDist ;
}
@@ -1856,6 +2391,83 @@ DiskPlaneLeakDistOrtMot( const Point3d& ptDiscCen, const Vector3d& vtDiskAx, dou
return 0. ;
}
//----------------------------------------------------------------------------
// La funzione restituisce la distanza di allontanamento di un disco da un segmento.
// NB: L'origine del sistema di riferimento deve essere nel centro della circonferenza
// di base, la cui traslazione obliqua genera il cilindro ellittico, e l'asse z deve
// essere l'asse di simmetria di tale circonferenza.
// NB: I parametri dLongMvLen e dOrtMvLen sono rispettivamente le lunghezze delle
// proiezioni del movimento su z e x del sistema di riferimento CircFrame.
double
DiskSegmentLeakDistGenMot( double dDiskRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen,
const Vector3d& vtMove)
{
// Quadrato del raggio
double dSqRad = dDiskRad * dDiskRad ;
// Il caso in cui il segmento è parallelo al versore del moto è gestito dalla
// routine per il calcolo della distanza di allontanamento dei punti.
if ( AreSameOrOppositeVectorApprox( vtMove, vtSeg))
return 0. ;
vector <double> vdCoef(3) ;
vector <double> vdRoots ;
// Siamo nel caso in cui il movimento è generico rispetto all'asse del disco, quindi
// sicuramente vtMove.z > 0, quindi esiste il rapporto dObCoef
double dObCoef = vtMove.x / vtMove.z ;
double dSqCoef = dObCoef * dObCoef ;
// Setto i coeficienti dell'equazione
vdCoef[0] = dSqCoef * ptSeg.z * ptSeg.z + ptSeg.x * ptSeg.x + ptSeg.y * ptSeg.y
- 2 * dObCoef * ptSeg.z * ptSeg.x - dSqRad ;
vdCoef[1] = 2 * ( dSqCoef * vtSeg.z * ptSeg.z + vtSeg.x * ptSeg.x + vtSeg.y * ptSeg.y
- dObCoef * ( vtSeg.z * ptSeg.x + vtSeg.x * ptSeg.z)) ;
vdCoef[2] = dSqCoef * vtSeg.z * vtSeg.z + vtSeg.x * vtSeg.x + vtSeg.y * vtSeg.y
- 2 * dObCoef * vtSeg.z * vtSeg.x ;
// Numero di soluzioni: l'equazione ammette o due soluzioni (eventualmente
// coincidenti) oppure nessuna o infinite se la la retta
// appartiene alla superficie
double dLenCoef = sqrt( 1 + dSqCoef) ;
int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ;
double dLeakDist = 0. ;
for ( int n = 0 ; n < nRoot ; ++ n) {
if ( vdRoots[n] > 0 && vdRoots[n] < dSegLen) {
Point3d ptTouch = ptSeg + vdRoots[n] * vtSeg ;
dLeakDist = max( dLeakDist, dLenCoef * ptTouch.z) ;
}
}
return dLeakDist ;
}
//----------------------------------------------------------------------------
double
DiskTriaInteriorLeakDistGenMot( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad,
const Triangle3d& trTria, const Vector3d& vtMove)
{
// Se disco e triangolo sono complanari
if ( AreSameVectorApprox( vtDiskAx, trTria.GetN())) {
double dDist = max( ( ( trTria.GetP( 0) - ptDiskCen) * trTria.GetN()) / ( vtMove * trTria.GetN()), 0.) ;
if ( CoplanarDiscTriangleInterferance( ptDiskCen + dDist * vtMove, dDiskRad, trTria))
return dDist ;
return 0. ;
}
// Se disco e triangolo sono ortogonali non può esserci contatto con interno
if ( AreOrthoApprox( vtDiskAx, trTria.GetN()))
return 0. ;
// Cerco un punto di contatto nell'interno del triangolo.
double dLeakDist = 0. ;
// Vettore radiale
Vector3d vtRad = trTria.GetN() - ( trTria.GetN() * vtDiskAx) * vtDiskAx ;
vtRad.Normalize() ;
// Punti delle due rette candidate all'intersezione col triangolo
Point3d ptStLine = ptDiskCen - dDiskRad * vtRad ;
// Intersezioni con le rette
double dDist = ( ( trTria.GetP( 0) - ptStLine) * trTria.GetN()) / ( vtMove * trTria.GetN()) ;
if ( IsPointInsideTriangle( ptStLine + dDist * vtMove, trTria))
dLeakDist = max( dLeakDist, dDist) ;
return dLeakDist ;
}
// FUNZIONI GEOMETRICHE DI BASE PER IL CALCOLO DELLA DISTANZA DI ALLONTANAMENTO
//----------------------------------------------------------------------------
@@ -2059,6 +2671,10 @@ IsPointInsideOpenTriangle( const Point3d& ptP, const Triangle3d& trTria)
//----------------------------------------------------------------------------
// Valuta l'interferenza di un disco e un triangolo complanari.
// Si assume che disco e piano siano paralleli, ovvero che valga a n = 1 con
// a versore dell'asse del disco e n versore normale al triangolo.
// L'informazione su n è contenuta nell'oggetto triangolo; quella su a non è passata
// in quanto inutile sotto le nostre assunzioni.
bool
CoplanarDiscTriangleInterferance( const Point3d& ptCen, double dRad, const Triangle3d& trTria)
{
@@ -2068,19 +2684,13 @@ CoplanarDiscTriangleInterferance( const Point3d& ptCen, double dRad, const Trian
// 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)) ;
double dSqMinDist = dRad * dRad + 2 * dRad * EPS_SMALL ;
// 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)
double dSegPointDist0, dSegPointDist1, dSegPointDist2 ;
( DistPointLine( ptCen, trTria.GetP( 0), trTria.GetP( 1))).GetSqDist( dSegPointDist0) ;
( DistPointLine( ptCen, trTria.GetP( 1), trTria.GetP( 2))).GetSqDist( dSegPointDist1) ;
( DistPointLine( ptCen, trTria.GetP( 2), trTria.GetP( 0))).GetSqDist( dSegPointDist2) ;
if ( dSegPointDist0 > dSqMinDist && dSegPointDist1 > dSqMinDist && dSegPointDist2 > dSqMinDist)
return false ;
// interferenza
return true ;
@@ -2227,3 +2837,31 @@ DiskPlaneLastContactLongMot( const Point3d& ptDiskCen, double dDiskRad,
return 1 ;
return 0 ;
}
//----------------------------------------------------------------------------
// Si limita il segmento alla parte di spazio con distanza positiva dal piano.
// Se si modifica il segmento si restituisce true, altrimenti false.
bool
ClampSegmentOutPlane( const Plane3d& plPlane, Point3d& ptSegP, const Vector3d& vtSegV, double& dSegLen)
{
// Distanze degli estremi del segmento dal piano
double dDistStart = DistPointPlane( ptSegP, plPlane) ;
double dDistEnd = DistPointPlane( ptSegP + dSegLen * vtSegV, plPlane) ;
// Se il segmento sta nel piano o è tutto esterno, va bene
if ( dDistStart > -EPS_ZERO && dDistEnd > -EPS_ZERO)
return false ;
// Se il segmento è tutto interno, va eliminato
if ( dDistStart < 0 && dDistEnd < 0) {
dSegLen = 0 ;
return true ;
}
// Altrimenti il segmento attraversa il piano e va limitato
double dIntersLen = dSegLen * abs( dDistStart) / ( abs( dDistStart) + abs( dDistEnd)) ;
if ( dDistStart > 0)
dSegLen = dIntersLen ;
else {
ptSegP += dIntersLen * vtSegV ;
dSegLen -= dIntersLen ;
}
return true ;
}