Egalware/EgtGeomKernel
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
master
EgtGeomKernel/CAvToolTriangle.cpp
T
Riccardo Elitropi 6c4bf3f05a EgtGeomKernel : 
- in CalcPocketing migliorati i controlli per i casi a Trapezio.
- in CAvToolTriangle migliorata la creazione del frame Locale in CAvDiskTriangle per direzioni generiche di vDiskAx e vtMove.
2026-04-14 19:07:16 +02:00

2716 lines
140 KiB
C++
Raw Permalink Blame History

//----------------------------------------------------------------------------
// EgalTech 2018-2020
//----------------------------------------------------------------------------
// File : CAvToolTriangle.cpp Data : 03.12.18 Versione : 2.2k1
// Contenuto : Implementazione delle funzioni ToolTriangleCollisionAvoid.
//
//
//
// Modifiche : 10.03.18 LM Creazione modulo.
// 28.10.20 LM Funzioni ausiliarie spostate nel module CDeUtility.cpp.
//
//----------------------------------------------------------------------------
#include "stdafx.h"
#include "CurveLine.h"
#include "CurveArc.h"
#include "CAvToolTriangle.h"
#include "IntersLineSurfStd.h"
#include "IntersLineTria.h"
#include "CDeUtility.h"
#include "/EgtDev/Include/EGkDistPointLine.h"
#include "/EgtDev/Include/EGkIntervals.h"
#include "/EgtDev/Include/ENkPolynomialRoots.h"
#include "/EgtDev/Include/EgtNumUtils.h"
using namespace std ;
//----------------------------------------------------------------------------
static bool GetTopTapFromPrevCurve( const ICurve* pPrevCurve) ;
static bool GetBotTapFromNextCurve( const ICurve* pNextCurve) ;
// Sfera
static double CAvSphereTriangle( const Point3d& ptSpheCen, double dSpheRad, const Triangle3d& trTria, const Vector3d& vtMove) ;
static double SpherePointEscapeDist( const Point3d& ptSpheCen, double dSpheRad, const Point3d ptP, const Vector3d& vtMove) ;
static double SphereSegmentEscapeDist( const Point3d& ptSpheCen, double dSpheRad,
const Point3d& ptSeg, const Vector3d& vtSegDir, double dSegLen,
const Vector3d& vtMove) ;
static double SphereTriaInteriorEscapeDist( const Point3d& ptSpheCen, double dSpheRad,
const Triangle3d& trTria, const Vector3d& vtMove) ;
// Cilindro
static double CAvCylinderTriangle( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad,
const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot) ;
static double CylPointEscapeDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dCylHei, double dCylRad,
const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot) ;
static double CylSegmentEscapeDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dCylHei, double dCylRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove,
bool bTop, bool bBot) ;
static double CylTriaInteriorEscapeDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dCylHei, double dCylRad,
const Triangle3d& trTria, const Vector3d& vtMove) ;
static double CylPointEscapeDistGenMot( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad,
const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot) ;
static double CylSegmentEscapeDistGenMot( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen,
const Vector3d& vtMove, bool bTop, bool bBot) ;
static double CylTriaInteriorEscapeDistGenMot( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad,
const Triangle3d& trTria, const Vector3d& vtMove) ;
// Tronco di cono
static double CAvTrConeTriangle( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot) ;
static double TrConePointEscapeDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Point3d& ptP, const Vector3d& vtMove) ;
static double TrConeSegmentEscapeDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen,
const Vector3d& vtMove) ;
static double TrConeTriaInteriorEscapeDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) ;
static double TrConePointEscapeDistOrtMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot) ;
static double TrConeSegmentEscapeDistOrtMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen,
const Vector3d& vtMove, bool bTop, bool bBot) ;
static double TrConeTriaInteriorEscapeDistOrtMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) ;
static double TrConePointEscapeDistGenMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot) ;
static double TrConeSegmentEscapeDistGenMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen,
const Vector3d& vtMove, bool bTop, bool bBot) ;
static double TrConeTriaInteriorEscapeDistGenMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove) ;
// Toro convesso
static double CAvTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot) ;
static double TorusPointEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad,
const Point3d& ptP, const Vector3d& vtMove) ;
static double TorusSegmentEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ;
static double TorusTriaInteriorEscapeDistLongMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Triangle3d& trTria, const Vector3d& vtMove) ;
static double TorusPointEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Point3d& ptP, const Vector3d& vtMove) ;
static double TorusSegmentEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove,
bool bTop, bool bBot) ;
static double TorusTriaInteriorEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Triangle3d& trTria, const Vector3d& vtMove) ;
static double TorusPointEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Point3d& ptP, const Vector3d& vtMove) ;
static double TorusSegmentEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ;
static double TorusTriaInteriorEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Triangle3d& trTria, const Vector3d& vtMove) ;
// Toro concavo
static double CAvConcaveTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot) ;
static double ConcaveTorusPointEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad,
const Point3d& ptP, const Vector3d& vtMove) ;
static double ConcaveTorusSegmentEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ;
static double ConcaveTorusPointEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Point3d& ptP, const Vector3d& vtMove) ;
static double ConcaveTorusSegmentEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove,
bool bTop, bool bBot) ;
static double ConcaveTorusPointEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Point3d& ptP, const Vector3d& vtMove) ;
static double ConcaveTorusSegmentEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ;
// Disco
static double CAvDiskTriangle( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad,
const Triangle3d& trTria, const Vector3d& vtMove) ;
static double DiskPointEscapeDistLongMot( const Point3d& ptDiskCen, double dDiskRad,
const Point3d& ptP, const Vector3d& vtMove) ;
static double DiskSegmentEscapeDistLongMot( const Point3d& ptDiskCen, double dDiskRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen,
const Vector3d& vtMove) ;
static double DiskTriaInteriorEscapeDistLongMot( const Point3d& ptDiskCen, double dDiskRad,
const Triangle3d& trTria, const Vector3d& vtMove) ;
static double DiskPointEscapeDistOrtMot( const Point3d& ptDisc, const Vector3d& vtDiskAx, double dDiscRad,
const Point3d& ptP, const Vector3d& vtMove) ;
static double DiskSegmentEscapeDistOrtMot( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove) ;
static double DiskTriaInteriorEscapeDistOrtMot( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad,
const Triangle3d& trTria, const Vector3d& vtMove) ;
static double DiskSegmentEscapeDistGenMot( double dDiskRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen,
const Vector3d& vtMove) ;
static double DiskTriaInteriorEscapeDistGenMot( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad,
const Triangle3d& trTria, const Vector3d& vtMove) ;
//----------------------------------------------------------------------------
// La funzione determina la minima distanza di allontanamento lungo una direzione fissata
// per evitare la collisione tra un utensile ed un triangolo.
// Se nella posizione iniziale non c'è gia collisione si restituisce il valore 0.
// In caso di errore si restituisce il valore -1.
double
CAvToolTriangle( const Tool& tlTool, const Point3d& ptToolOrig, const Vector3d& vtToolAx,
const Triangle3d& trTria, const Vector3d& vtMove)
{
// Non ha senso che il movimento sia in direzione "opposta" a quella dell'asse utensile
if ( vtMove * vtToolAx < - 10 * EPS_ZERO)
return -1. ;
// Se avvicinamento non devo fare nulla
if ( vtMove * trTria.GetN() < - EPS_ZERO)
return 0. ;
// Tolleranza su dischi (parti piatte perpendicolari all'asse utensile)
double dEpsilon = ( vtMove * vtToolAx > 0.5 ? 0 : EPS_SMALL) ;
// se utensile cilindrico
if ( tlTool.GetType() == Tool::CYLMILL) {
// parametri geometrici
double dHeigth = tlTool.GetHeigth() ;
double dRadius = tlTool.GetRadius() ;
// prima determino l'allontanamento del disco inferiore
double dDist = CAvDiskTriangle( ptToolOrig - ( dHeigth - dEpsilon) * vtToolAx, vtToolAx, dRadius, trTria, vtMove) ;
if ( dDist < - EPS_SMALL)
return dDist ;
// poi verifico quello del cilindro (tenendo conto di quanto è stata allontanato il disco)
double dDist2 = CAvCylinderTriangle( ptToolOrig + dDist * vtMove, vtToolAx, dHeigth, dRadius, trTria, vtMove, false, false) ;
if ( dDist2 < - EPS_SMALL)
return dDist2 ;
// verifico quello del disco sopra
double dDist3 = CAvDiskTriangle( ptToolOrig - dEpsilon * vtToolAx + ( dDist + dDist2) * vtMove, vtToolAx, dRadius, trTria, vtMove) ;
if ( dDist3 < - EPS_SMALL)
return dDist3 ;
return ( dDist + dDist2 + dDist3) ;
}
// se utensile sferico
else if ( tlTool.GetType() == Tool::BALLMILL) {
// parametri geometrici
double dCylHeigth = tlTool.GetHeigth() - tlTool.GetTipHeigth() ;
double dRadius = tlTool.GetRadius() ;
// prima determino l'allontanamento della sfera
Point3d ptSpheOrig = ptToolOrig - dCylHeigth * vtToolAx ;
double dDist = CAvSphereTriangle( ptSpheOrig, dRadius, trTria, vtMove) ;
if ( dDist < - EPS_SMALL)
return dDist ;
// poi verifico quello del cilindro (tenendo conto di quanto è stata allontanata la sfera)
if ( dCylHeigth < EPS_SMALL)
return dDist ;
Point3d ptCylOrig = ptToolOrig + vtMove * dDist ;
double dDist2 = CAvCylinderTriangle( ptCylOrig, vtToolAx, dCylHeigth, dRadius, trTria, vtMove, false, true) ;
if ( dDist2 < - EPS_SMALL)
return dDist2 ;
// verifico quello del disco sopra
Point3d ptDiskUpOrig = ptCylOrig + dDist2 * vtMove ;
double dDist3 = CAvDiskTriangle( ptDiskUpOrig - dEpsilon * vtToolAx, vtToolAx, dRadius, trTria, vtMove) ;
if ( dDist3 < - EPS_SMALL)
return dDist3 ;
return ( dDist + dDist2 + dDist3) ;
}
// se utensile a naso di toro
else if ( tlTool.GetType() == Tool::BULLNOSEMILL) {
// parametri geometrici
double dCylHeigth = tlTool.GetHeigth() - tlTool.GetTipHeigth() ;
// prima determino l'allontanamento del disco inferiore
double dDist = CAvDiskTriangle( ptToolOrig - ( tlTool.GetHeigth() - dEpsilon) * vtToolAx, vtToolAx, tlTool.GetTipRadius(),
trTria, vtMove) ;
if ( dDist < - EPS_SMALL)
return dDist ;
// poi verifico quello del toro (tenendo conto di quanto è stata allontanato il disco)
Point3d ptTorusCen = ptToolOrig - dCylHeigth * vtToolAx ;
double dDist2 = CAvTorusTriangle( ptTorusCen + dDist * vtMove, vtToolAx,
tlTool.GetRadius() - tlTool.GetCornRadius(), tlTool.GetCornRadius(),
trTria, vtMove, true, false) ;
if ( dDist2 < - EPS_SMALL)
return dDist2 ;
// infine verifico quello del cilindro (tenendo conto dei precedenti allontanamenti)
if ( dCylHeigth < EPS_SMALL)
return ( dDist + dDist2) ;
Point3d ptCylOrig = ptToolOrig + vtMove * ( dDist + dDist2) ;
double dDist3 = CAvCylinderTriangle( ptCylOrig, vtToolAx, dCylHeigth, tlTool.GetRadius(),
trTria, vtMove, false, true) ;
if ( dDist3 < - EPS_SMALL)
return dDist3 ;
// verifico quello del disco sopra
Point3d ptDiskUpOrig = ptCylOrig + vtMove * dDist3 ;
double dDist4 = CAvDiskTriangle( ptDiskUpOrig - dEpsilon * vtToolAx, vtToolAx, tlTool.GetRadius(), trTria, vtMove) ;
if ( dDist4 < - EPS_SMALL)
return dDist4 ;
return ( dDist + dDist2 + dDist3 + dDist4) ;
}
// se utensile conico
else if ( tlTool.GetType() == Tool::CONEMILL) {
// parametri geometrici
Point3d ptMinBase ;
Vector3d vtConeAx ;
bool bTop, bBot ;
if ( tlTool.GetRadius() > tlTool.GetTipRadius()) {
ptMinBase = ptToolOrig - tlTool.GetHeigth() * vtToolAx ;
vtConeAx = vtToolAx ;
bTop = true ;
bBot = false ;
}
else {
ptMinBase = ptToolOrig - ( tlTool.GetHeigth() - tlTool.GetTipHeigth()) * vtToolAx ;
vtConeAx = - vtToolAx ;
bTop = false ;
bBot = true ;
}
double dMinR = min( tlTool.GetRadius(), tlTool.GetTipRadius()) ;
double dMaxR = max( tlTool.GetRadius(), tlTool.GetTipRadius()) ;
double dCylHeigth = tlTool.GetHeigth() - tlTool.GetTipHeigth() ;
// prima determino l'allontanamento del disco inferiore
double dDist = CAvDiskTriangle( ptToolOrig - ( tlTool.GetHeigth() - dEpsilon) * vtToolAx, vtToolAx, tlTool.GetTipRadius(),
trTria, vtMove) ;
if ( dDist < - EPS_SMALL)
return dDist ;
// poi verifico quello del cono (tenendo conto di quanto è stata allontanato il disco)
double dDist2 = CAvTrConeTriangle( ptMinBase + dDist * vtMove, vtConeAx, dMinR, dMaxR, tlTool.GetTipHeigth(),
trTria, vtMove, bTop, bBot) ;
if ( dDist2 < - EPS_SMALL)
return dDist2 ;
// infine verifico quello del cilindro (tenendo conto dei precedenti allontanamenti)
if ( dCylHeigth < EPS_SMALL)
return ( dDist + dDist2) ;
Point3d ptCylOrig = ptToolOrig + vtMove * ( dDist + dDist2) ;
double dDist3 = CAvCylinderTriangle( ptCylOrig, vtToolAx, dCylHeigth, tlTool.GetRadius(),
trTria, vtMove, false, true) ;
if ( dDist3 < - EPS_SMALL)
return dDist3 ;
// verifico quello del disco sopra
Point3d ptDiskUpOrig = ptCylOrig + vtMove * dDist3 ;
double dDist4 = CAvDiskTriangle( ptDiskUpOrig - dEpsilon * vtToolAx, vtToolAx, tlTool.GetRadius(), trTria, vtMove) ;
if ( dDist4 < - EPS_SMALL)
return dDist4 ;
return ( dDist + dDist2 + dDist3 + dDist4) ;
}
// se utensile generico
else if ( tlTool.GetType() == Tool::GEN) {
// distanza di allontanamento
double dDist = 0 ;
// riferimento per componenti
Point3d ptCompOrig = ptToolOrig - tlTool.GetHeigth() * vtToolAx ;
// analizzo le curve del profilo a partire dall'ultima
const CurveComposite& ToolProfile = tlTool.GetApproxOutline() ;
int nCrv = ToolProfile.GetCurveCount() - 1 ;
const ICurve* pCurve = ToolProfile.GetCurve( nCrv) ;
const ICurve* pNextCurve = nullptr ;
while ( pCurve != nullptr) {
// distanza di allontanamento del tratto corrente
double dDist2 = 0 ;
// curva precedente
const ICurve* pPrevCurve = ToolProfile.GetCurve( -- nCrv) ;
// Se segmento
if ( pCurve->GetType() == CRV_LINE) {
// Recupero gli estremi
const ICurveLine* pLine = GetCurveLine( pCurve) ;
const Point3d& ptStart = pLine->GetStart() ;
const Point3d& ptEnd = pLine->GetEnd() ;
// Ne determino l'altezza
double dHeight = abs( ptStart.y - ptEnd.y) ;
if ( dHeight <= EPS_SMALL) {
double dRadius = max( ptStart.x, ptEnd.x) ;
// Se disco verso il basso dell'utensile
if ( ptStart.x > ptEnd.x)
dDist2 = CAvDiskTriangle( ptCompOrig + dEpsilon * vtToolAx, vtToolAx, dRadius, trTria, vtMove) ;
// Se disco verso l'alto
else
dDist2 = CAvDiskTriangle( ptCompOrig - dEpsilon * vtToolAx, vtToolAx, dRadius, trTria, vtMove) ;
}
else {
// Verifiche curva precedente per eventuale tappo sopra
bool bTop = GetTopTapFromPrevCurve( pPrevCurve) ;
// Verifiche curva successiva per eventuale tappo sotto
bool bBot = GetBotTapFromNextCurve( pNextCurve) ;
// Calcolo distanza di allontanamento del componente corrente
if ( abs( ptStart.x - ptEnd.x) < EPS_SMALL) {
double dRadius = ptStart.x ;
// riferimento del cilindro in alto
ptCompOrig += dHeight * vtToolAx ;
dDist2 = CAvCylinderTriangle( ptCompOrig, vtToolAx, dHeight, dRadius, trTria, vtMove, bTop, bBot) ;
}
else if ( ptStart.x > ptEnd.x) {
double dMaxRad = ptStart.x ;
double dMinRad = ptEnd.x ;
// riferimento del cono in basso (base più piccola)
dDist2 = CAvTrConeTriangle( ptCompOrig, vtToolAx, dMinRad, dMaxRad, dHeight, trTria, vtMove, bTop, bBot) ;
// sposto riferimento in alto
ptCompOrig += dHeight * vtToolAx ;
}
else if ( ptStart.x < ptEnd.x) {
double dMaxRad = ptEnd.x ;
double dMinRad = ptStart.x ;
// riferimento del cono in alto (base più piccola)
ptCompOrig += dHeight * vtToolAx ;
dDist2 = CAvTrConeTriangle( ptCompOrig, - vtToolAx, dMinRad, dMaxRad, dHeight, trTria, vtMove, bTop, bBot) ;
}
}
}
// Se arco
else if ( pCurve->GetType() == CRV_ARC) {
// Recupero estremi, centro e raggio
const ICurveArc* pArc = GetCurveArc( pCurve) ;
Point3d ptStart ; pArc->GetStartPoint( ptStart) ;
Point3d ptEnd ; pArc->GetEndPoint( ptEnd) ;
const Point3d& ptCen = pArc->GetCenter() ;
double dRadius = pArc->GetRadius() ;
double dAngToCen = pArc->GetAngCenter() ;
// Calcolo della distanza di allontanamento del componente corrente
// Se sfera
if ( abs( ptCen.x) < EPS_SMALL) {
// riferimento sul centro sfera
ptCompOrig += ( ptCen.y - ptEnd.y) * vtToolAx ;
dDist2 = CAvSphereTriangle( ptCompOrig, dRadius, trTria, vtMove) ;
// sposto riferimento in alto
ptCompOrig += ( ptStart.y - ptCen.y) * vtToolAx ;
}
// altrimenti toro
else {
// Verifiche curva precedente per eventuale tappo sopra
bool bTop = GetTopTapFromPrevCurve( pPrevCurve) ;
// Verifiche curva successiva per eventuale tappo sotto
bool bBot = GetBotTapFromNextCurve( pNextCurve) ;
// riferimento sul centro toro
ptCompOrig += ( ptCen.y - ptEnd.y) * vtToolAx ;
// verifica presenza semitori
bool bHalfTorusDown = ( ptEnd.y < ptCen.y - EPS_SMALL) ;
bool bHalfTorusUp = ( ptStart.y > ptCen.y + EPS_SMALL) ;
// semi-toro sotto
if ( bHalfTorusDown) {
double dMyDist ;
if ( dAngToCen < 0)
dMyDist = CAvTorusTriangle( ptCompOrig, vtToolAx, ptCen.x, dRadius, trTria, vtMove,
( bHalfTorusUp ? true : bTop), bBot) ;
else
dMyDist = CAvConcaveTorusTriangle( ptCompOrig, - vtToolAx, ptCen.x, dRadius, trTria, vtMove,
( bHalfTorusUp ? true : bTop), bBot) ;
if ( dMyDist < - EPS_SMALL)
return dMyDist ;
dDist2 = max( dDist2, dMyDist) ;
}
// semi-toro sopra
if ( bHalfTorusUp) {
double dMyDist ;
if ( dAngToCen < 0)
dMyDist = CAvTorusTriangle( ptCompOrig, - vtToolAx, ptCen.x, dRadius, trTria, vtMove,
bTop, ( bHalfTorusDown ? true : bBot)) ;
else
dMyDist = CAvConcaveTorusTriangle( ptCompOrig, vtToolAx, ptCen.x, dRadius, trTria, vtMove,
bTop, ( bHalfTorusDown ? true : bBot)) ;
if ( dMyDist < - EPS_SMALL)
return dMyDist ;
dDist2 = max( dDist2, dMyDist) ;
}
// sposto riferimento in alto
ptCompOrig += ( ptStart.y - ptCen.y) * vtToolAx ;
}
}
// Aggiornamento distanza allontanamento
if ( dDist2 < - EPS_SMALL)
return dDist2 ;
dDist += dDist2 ;
ptCompOrig += dDist2 * vtMove ;
// passo alla curva precedente
pNextCurve = pCurve ;
pCurve = pPrevCurve ;
}
return dDist ;
}
// altrimenti utensile di tipo non gestito
else
return - 1. ;
}
//----------------------------------------------------------------------------
bool
GetTopTapFromPrevCurve( const ICurve* pPrevCurve)
{
if ( pPrevCurve == nullptr)
return false ;
if ( pPrevCurve->GetType() == CRV_LINE) {
const ICurveLine* pPrevLine = GetCurveLine( pPrevCurve) ;
const Point3d& ptPrevStart = pPrevLine->GetStart() ;
const Point3d& ptPrevEnd = pPrevLine->GetEnd() ;
if ( abs( ptPrevStart.y - ptPrevEnd.y) > EPS_SMALL ||
ptPrevStart.x > ptPrevEnd.x)
return true ;
}
else if ( pPrevCurve->GetType() == CRV_ARC) {
const ICurveArc* pPrevArc = GetCurveArc( pPrevCurve) ;
const Point3d& ptPrevCen = pPrevArc->GetCenter() ;
if ( abs( ptPrevCen.x) > EPS_SMALL)
return true ;
}
return false ;
}
//----------------------------------------------------------------------------
bool
GetBotTapFromNextCurve( const ICurve* pNextCurve)
{
if ( pNextCurve == nullptr)
return false ;
if ( pNextCurve->GetType() == CRV_LINE) {
const ICurveLine* pNextLine = GetCurveLine( pNextCurve) ;
const Point3d& ptNextStart = pNextLine->GetStart() ;
const Point3d& ptNextEnd = pNextLine->GetEnd() ;
if ( abs( ptNextStart.y - ptNextEnd.y) > EPS_SMALL ||
ptNextStart.x < ptNextEnd.x)
return true ;
}
else if ( pNextCurve->GetType() == CRV_ARC) {
const ICurveArc* pNextArc = GetCurveArc( pNextCurve) ;
const Point3d& ptNextCen = pNextArc->GetCenter() ;
if ( abs( ptNextCen.x) > EPS_SMALL)
return true ;
}
return false ;
}
//----------------------------------------------------------------------------
// **** SFERA ****
//----------------------------------------------------------------------------
// La funzione determina la minima distanza di allontanamento lungo una direzione fissata
// per evitare la collisione tra una sfera ed un triangolo.
double
CAvSphereTriangle( const Point3d& ptSpheCen, double dSpheRad, const Triangle3d& trTria, const Vector3d& vtMove)
{
// Se la sfera sta già tutta dalla parte esterna del triangolo, non va ulteriormente allontanata
// Posizione del centro sfera rispetto al piano del triangolo
Vector3d vtPCen = ptSpheCen - trTria.GetP( 0) ;
if ( vtPCen * trTria.GetN() > dSpheRad - EPS_SMALL)
return 0. ;
// Se la fetta di spazio delimitata dal piano sotto alla sfera rispetto alla direzione
// di allontanamento non sia già sopra al triangolo (tutti i vertici del triangolo sono sotto).
Point3d ptBase = ptSpheCen - dSpheRad * vtMove ;
double dMaxDistV = PointPlaneSignedDist( trTria.GetP( 0), ptBase, vtMove) ;
dMaxDistV = max( dMaxDistV, PointPlaneSignedDist( trTria.GetP( 1), ptBase, vtMove)) ;
dMaxDistV = max( dMaxDistV, PointPlaneSignedDist( trTria.GetP( 2), ptBase, vtMove)) ;
if ( dMaxDistV < EPS_SMALL)
return 0. ;
// Valuto allontanamento da interno triangolo
double dEscapeDist = SphereTriaInteriorEscapeDist( ptSpheCen, dSpheRad, trTria, vtMove) ;
if ( dEscapeDist > 0.)
return dEscapeDist ;
// Valuto le distanze di allontanamento dai segmenti (con i loro estremi) e prendo la maggiore
for ( int nSeg = 0 ; nSeg < 3 ; ++ nSeg) {
Vector3d vtSeg = trTria.GetP( ( nSeg + 1) % 3) - trTria.GetP( nSeg) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
double dCurEscapeDist = max( SpherePointEscapeDist( ptSpheCen, dSpheRad, trTria.GetP( nSeg), vtMove),
SphereSegmentEscapeDist( ptSpheCen, dSpheRad, trTria.GetP( nSeg), vtSeg, dSegLen, vtMove)) ;
if ( dCurEscapeDist > dEscapeDist)
dEscapeDist = dCurEscapeDist ;
}
return dEscapeDist ;
}
//----------------------------------------------------------------------------
// Calcola la distanza di allontanamento lungo una direzione fissata di una sfera da un punto.
double
SpherePointEscapeDist( const Point3d& ptSpheCen, double dSpheRad, const Point3d ptP, const Vector3d& vtMove)
{
Vector3d vtOR = ptP - ptSpheCen ;
double dDotORMot = vtOR * vtMove ;
Vector3d vtOROrt = vtOR - dDotORMot * vtMove ;
double dSqOROrtLen = vtOROrt.SqLen() ;
double dSqSphRad = dSpheRad * dSpheRad ;
if ( dSqOROrtLen > dSqSphRad - 2 * dSpheRad * EPS_SMALL)
return 0. ;
else
return max( dDotORMot + sqrt( max( dSqSphRad - dSqOROrtLen, 0.)), 0.) ;
}
//----------------------------------------------------------------------------
// 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
SphereSegmentEscapeDist( const Point3d& ptSpheCen, double dSpheRad,
const Point3d& ptSeg, const Vector3d& vtSegDir, double dSegLen,
const Vector3d& vtMove)
{
// Controllo con l'interno del segmento
double dU[2] ;
int nTanPointNum = SphereLineTangentPoints( ptSpheCen, dSpheRad - EPS_SMALL, ptSeg, vtSegDir, dSegLen, vtMove, dU[0], dU[1]) ;
double dEscapeDist = 0. ;
for ( int nSol = 0 ; nSol < nTanPointNum && nTanPointNum != 3 ; ++ nSol) {
if ( dU[nSol] < dEscapeDist)
continue ;
Point3d ptC = ptSpheCen + dU[nSol] * vtMove ;
double dSegPar = ( ptC - ptSeg) * vtSegDir ;
if ( dSegPar > 0 && dSegPar < dSegLen)
dEscapeDist = dU[nSol] ;
}
return dEscapeDist ;
}
//----------------------------------------------------------------------------
// Calcola la distanza di allontanamento lungo una direzione fissata di una sfera da un piano.
double
SphereTriaInteriorEscapeDist( const Point3d& ptSpheCen, double dSpheRad,
const Triangle3d& trTria, const Vector3d& vtMove)
{
double dCosNM = trTria.GetN() * vtMove ;
// Se la direzione di allontanamento sta nel piano
if ( abs( dCosNM) < EPS_ZERO)
return 0. ;
// altrimenti ...
double dEscapeDist = ( dSpheRad - ( ptSpheCen - trTria.GetP( 0)) * trTria.GetN()) / dCosNM ;
if ( dEscapeDist < 0.)
return 0. ;
Point3d ptTan = ptSpheCen + dEscapeDist * vtMove - dSpheRad * trTria.GetN() ;
if ( IsPointInsideTriangle( ptTan, trTria, TriangleType::EXACT))
return dEscapeDist ;
return 0. ;
}
//----------------------------------------------------------------------------
// **** CILINDRO ****
//----------------------------------------------------------------------------
// La funzione determina la minima distanza di allontanamento lungo una direzione fissata
// per evitare la collisione tra un cilindro ed un triangolo.
double
CAvCylinderTriangle( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad,
const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot)
{
double dDiskEscapeDist = 0 ;
if ( bTop)
dDiskEscapeDist = CAvDiskTriangle( ptCylOrig, vtCylAx, dRad, trTria, vtMove) ;
// Movimento lungo la direzione dell'asse del cilindro
if ( AreSameVectorApprox( vtCylAx, vtMove)) {
// la distanza di fuga è determinata dal disco
// o dai componenti sotto il cilindro.
return dDiskEscapeDist ;
}
// Movimento perpendicolare all'asse del cilindro
else if ( AreOrthoApprox( vtCylAx, vtMove)) {
double dEscapeDist = CylTriaInteriorEscapeDistOrtMotion( ptCylOrig, vtCylAx, dHeigth, dRad, trTria, vtMove) ;
// Se distanza da interno positiva abbiamo finito
if ( dEscapeDist > EPS_SMALL)
return dEscapeDist ;
// PUNTI
for ( int nVrt = 0 ; nVrt < 3 ; ++ nVrt) {
double dCurDist = CylPointEscapeDistOrtMotion( ptCylOrig, vtCylAx, dHeigth, dRad, trTria.GetP( nVrt), vtMove, bTop, bBot) ;
if ( dEscapeDist < dCurDist)
dEscapeDist = dCurDist ;
}
// SEGMENTI
for ( int nVrtS = 0 ; nVrtS < 3 ; ++ nVrtS) {
int nVrtE = ( nVrtS + 1) % 3 ;
Vector3d vtSeg = trTria.GetP( nVrtE) - trTria.GetP( nVrtS) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
double dCurDist = CylSegmentEscapeDistOrtMotion( ptCylOrig, vtCylAx, dHeigth, dRad,
trTria.GetP( nVrtS), vtSeg, dSegLen, vtMove, bTop, bBot) ;
if ( dEscapeDist < dCurDist)
dEscapeDist = dCurDist ;
}
return max( dEscapeDist, dDiskEscapeDist) ;
}
// Movimento generico
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 dEscapeDist = 0. ;
// distanza di allontanamento dall'interno
dEscapeDist = max( dEscapeDist, CylTriaInteriorEscapeDistGenMot( ptCylOrig, vtCylAx, dHeigth, dRad, trTria, vtMove)) ;
if ( dEscapeDist > EPS_SMALL)
return dEscapeDist ;
// 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 ;
dEscapeDist = max( dEscapeDist, CylPointEscapeDistGenMot( ptCylOrig, vtCylAx, dHeigth, dRad,
trTria.GetP( nV), vtMove, bTop, bBot)) ;
dEscapeDist = max( dEscapeDist, CylSegmentEscapeDistGenMot( ptCylOrig, vtCylAx, dHeigth, dRad,
trTria.GetP( nV), vtSeg, dSegLen, vtMove, bTop, bBot)) ;
}
return max( dEscapeDist, dDiskEscapeDist) ; ;
}
// Errore
else
return -1. ;
}
//----------------------------------------------------------------------------
// Restituisce la distanza di fuga di un cilindro da un punto nel caso di moto
// ortogonale all'asse di simmetria. Il cilindro è descritto da raggio e altezza.
// Il suo moto è descritto dalla posizione iniziale, dal versore (NORMA UNITARIA)
// dell'asse di simmetria e dal versore della direzione del moto.
double
CylPointEscapeDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dCylHei, double dCylRad,
const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot)
{
double dTopTol = bTop ? EPS_SMALL : - EPS_SMALL ;
double dBotTol = bBot ? - EPS_SMALL : EPS_SMALL ;
double dSqRad = dCylRad * dCylRad ;
Vector3d vtRP = ptP - ptCylOrig ;
// Se il punto sta al di sopra o al di sotto rispetto al cilindro abbiamo finito
double dDotRPCylAx = vtRP * vtCylAx ;
if ( dDotRPCylAx > dTopTol || dDotRPCylAx < - dCylHei + dBotTol)
return 0. ;
Vector3d vtPlaneRP = vtRP - dDotRPCylAx * vtCylAx ;
// Se il punto sta dietro al cilindro abbiamo finito
double dDotPlaneRPRemDir = vtPlaneRP * vtMove ;
if ( dDotPlaneRPRemDir < 0. && vtPlaneRP.SqLen() > dSqRad)
return 0. ;
Vector3d vtPlaneOrtRP = vtPlaneRP - vtPlaneRP * vtMove * vtMove ;
// Se il punto è a destra o sinistra del cilindro abbiamo finito
double dSqOrtLen = vtPlaneOrtRP.SqLen() ;
if ( dSqOrtLen > dSqRad - 2 * dCylRad * EPS_SMALL)
return 0. ;
// Calcolo e restituisco la distanza
return dDotPlaneRPRemDir + sqrt( max( dSqRad - dSqOrtLen, 0.)) ;
}
//----------------------------------------------------------------------------
// Restituisce la distanza di allontanamento di un cilindro da un segmento.
// Il cilindro è descritto da raggio e altezza.
// Il suo moto è descritto dalla posizione iniziale, dal versore (NORMA UNITARIA)
// dell'asse di simmetria e dal versore della direzione del moto.
double
CylSegmentEscapeDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dCylHei, double dCylRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove,
bool bTop, bool bBot)
{
double dTopTol = ( bTop ? EPS_SMALL : - EPS_SMALL) ;
double dBotTol = ( bBot ? - EPS_SMALL : EPS_SMALL) ;
// Riferimento con origine nel centro del disco nella posizione iniziale.
// X := vtCylAx, Y:= vtMove, Z:= vtCylAx ^ vtMove.
Vector3d vtPlane = vtCylAx ^ vtMove ;
vtPlane.Normalize() ;
Point3d ptSegEnd = ptSeg + dSegLen * vtSeg ;
Vector3d vtSegStart = ptSeg - ptCylOrig ;
Vector3d vtSegEnd = ptSegEnd - ptCylOrig ;
double dCordStart1 = vtSegStart * vtCylAx ;
double dCordEnd1 = vtSegEnd * vtCylAx ;
// Se entrambi gli estremi sono sopra o sotto, non ci può essere interferenza
if ( ( dCordStart1 > dTopTol && dCordEnd1 > dTopTol) ||
( dCordStart1 < - dCylHei + dBotTol && dCordEnd1 < - dCylHei + dBotTol))
return 0. ;
Vector3d vtSegStart1 = dCordStart1 * vtCylAx ;
Vector3d vtSegEnd1 = dCordEnd1 * vtCylAx ;
Vector3d vtSegStart23 = vtSegStart - vtSegStart1 ;
Vector3d vtSegEnd23 = vtSegEnd - vtSegEnd1 ;
double dCordStart3 = vtSegStart23 * vtPlane ;
double dCordEnd3 = vtSegEnd23 * vtPlane ;
// Se entrambi gli estremi del segmento sono a un lato, non ci può essere interferenza
if ( ( dCordStart3 > dCylRad && dCordEnd3 > dCylRad) ||
( dCordStart3 < - dCylRad && dCordEnd3 < - dCylRad))
return 0. ;
double dEscapeDist = 0. ;
// Se il vettore del segmento è parallelo all'asse del cilindro,
// il suo prodotto vettoriale con il versore dell'asse è nullo.
Vector3d vtRad = vtCylAx ^ vtSeg ;
if ( ! vtRad.Normalize()) {
if ( dCordStart3 > - dCylRad + EPS_SMALL && dCordStart3 < dCylRad - EPS_SMALL)
dEscapeDist = max( dEscapeDist, ( ptSeg - ptCylOrig) * vtMove) ;
}
// 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 +/- dOrtLen
Vector3d vtD = ptSeg - ptCylOrig ;
double dDotPlaneD = - vtD * vtPlane ;
double dDotPlaneSeg = - vtSeg * vtPlane ;
double dPlusU = ( dOrtLen - dDotPlaneD) / dDotPlaneSeg ;
double dMinusU = ( - dOrtLen - dDotPlaneD) / dDotPlaneSeg ;
Point3d ptTanPlus = ptSeg + dPlusU * vtSeg ;
Point3d ptTanMinus = ptSeg + dMinusU * vtSeg ;
double dTanCordPlus1 = ( ptTanPlus - ptCylOrig) * vtCylAx ;
double dTanCordMinus1 = ( ptTanMinus - ptCylOrig) * vtCylAx ;
if ( ( dPlusU > - EPS_SMALL && dPlusU < dSegLen + EPS_SMALL) &&
( dTanCordPlus1 < dTopTol && dTanCordPlus1 > - dCylHei + dBotTol)) {
dEscapeDist = max( ( ( ptSeg - ptCylOrig) + dPlusU * vtSeg) * vtMove + dDotRemRad, 0.) ;
}
if ( dMinusU > - EPS_SMALL && dMinusU < dSegLen + EPS_SMALL &&
( dTanCordMinus1 < dTopTol && dTanCordMinus1 > - dCylHei + dBotTol)) {
double dNewDist = max( ( ( ptSeg - ptCylOrig) + dMinusU * vtSeg) * vtMove + dDotRemRad, 0.) ;
dEscapeDist = max( dNewDist, dEscapeDist) ;
}
}
return max( dEscapeDist, 0.) ;
}
//----------------------------------------------------------------------------
double
CylTriaInteriorEscapeDistOrtMotion( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dCylHei, double dCylRad,
const Triangle3d& trTria, const Vector3d& vtMove)
{
// La superficie laterale può trovarsi in estremo contatto con l'interno solo in tangenza.
// Se non ci può essere tangenza guardo il disco sopra per le giunzioni
if ( ! AreOrthoExact( vtCylAx, trTria.GetN()))
return 0. ;
// Ci può essere tangenza cerco segmento candidato a toccare il triangolo
double dDist = 0. ;
Point3d ptTouch0 = ptCylOrig - dCylRad * trTria.GetN() ;
double dU = ( ( trTria.GetP( 0) - ptTouch0) * trTria.GetN()) / ( vtMove * trTria.GetN()) ;
// Se avvicinamento, distanza nulla
if ( dU < dDist)
dDist = 0. ;
ptTouch0 += ( dU * vtMove) ;
Point3d ptTouch1 = ptTouch0 - dCylHei * vtCylAx ;
Point3d ptInt0, ptInt1 ;
int nIntType = IntersLineTria( ptTouch0, ptTouch1, trTria, ptInt0, ptInt1) ;
// Il segmento interseca triangolo aggiorno la distanza di allontanamento
if ( ! ( nIntType == ILTT_NO || nIntType == ILTT_VERT || nIntType == ILTT_IN || nIntType == ILTT_EDGE))
dDist = dU ;
return dDist ;
}
//----------------------------------------------------------------------------
double
CylPointEscapeDistGenMot( 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
double dMyRad = dRad - EPS_SMALL ;
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 - dMyRad * dMyRad ;
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 dEscapeDist = 0. ;
for ( int n = 0 ; n < nRoot ; ++ n) {
double dDotIntAx = ( vtR0 - vdRoots[n] * vtMove) * vtCylAx ;
if ( vdRoots[n] > dEscapeDist && dDotIntAx < dTopTol && dDotIntAx > - dHeigth + dBotTol)
dEscapeDist = vdRoots[n] ;
}
return dEscapeDist ;
}
//----------------------------------------------------------------------------
double
CylSegmentEscapeDistGenMot( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen,
const Vector3d& vtMove, bool bTop, bool bBot)
{
// 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
double dMyRad = dRad - EPS_SMALL ;
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 - dMyRad * dMyRad ;
vdCoef[1] = - 2 * vtR0Ort * vtMoveOrt ;
vdCoef[2] = vtMoveOrt * vtMoveOrt ;
// Risoluzione dell'equazione e studio delle soluzioni
int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ;
double dEscapeDist = 0. ;
for ( int n = 0 ; n < nRoot ; ++ n) {
// Se abbiamo trovato una potenziale nuova distanza di allontanamento
if ( vdRoots[n] > dEscapeDist) {
// 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))
dEscapeDist = vdRoots[n] ;
}
}
return dEscapeDist ;
}
// 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 - dMyRad * dMyRad ;
// 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 dEscapeDist = 0. ;
for ( int n = 0 ; n < nDeltaRoot ; ++ n) {
if ( vdDeltaRoots[n] > dEscapeDist) {
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)
dEscapeDist = vdDeltaRoots[n] ;
}
}
}
return dEscapeDist ;
}
//----------------------------------------------------------------------------
double
CylTriaInteriorEscapeDistGenMot( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double dHeigth, double dRad,
const Triangle3d& trTria, const Vector3d& vtMove)
{
double dEscapeDist = 0. ;
if ( ! AreOrthoApprox( vtCylAx, trTria.GetN()) || AreOrthoApprox( vtMove, trTria.GetN()))
return dEscapeDist ;
// 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 dEscapeDist ;
// 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))
dEscapeDist = dU ;
return dEscapeDist ;
}
//----------------------------------------------------------------------------
// **** CONO ****
//----------------------------------------------------------------------------
double
CAvTrConeTriangle( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot)
{
double dDiskDist ;
if ( vtTrConeAx * vtMove < - EPS_ZERO)
dDiskDist = CAvDiskTriangle( ptMinBase, - vtTrConeAx, dMinBaseR, trTria, vtMove) ;
else
dDiskDist = CAvDiskTriangle( ptMinBase + dTrConeH * vtTrConeAx, vtTrConeAx, dMaxBaseR, trTria, vtMove) ;
// Allontanamento con direzione coincidente con l'asse del cono
if ( AreSameOrOppositeVectorApprox( vtTrConeAx, vtMove)) {
// Verifica preliminare che la fetta di spazio delimitata dal piano sotto del cono rispetto alla direzione
// di allontanamento non sia già sopra al triangolo (tutti i vertici del triangolo sono sotto).
Point3d ptInfLim = ptMinBase ;
if ( AreOppositeVectorApprox( vtTrConeAx, vtMove))
ptInfLim += dTrConeH * vtTrConeAx ;
double dMaxDistV = PointPlaneSignedDist( trTria.GetP( 0), ptInfLim, vtMove) ;
dMaxDistV = max( dMaxDistV, PointPlaneSignedDist( trTria.GetP( 1), ptInfLim, vtMove)) ;
dMaxDistV = max( dMaxDistV, PointPlaneSignedDist( trTria.GetP( 2), ptInfLim, vtMove)) ;
if ( dMaxDistV < 0.)
return 0. ;
bool bInside[3] = { GetPointLineSqDist( trTria.GetP( 0), ptMinBase, vtMove) < dMinBaseR * dMinBaseR,
GetPointLineSqDist( trTria.GetP( 1), ptMinBase, vtMove) < dMinBaseR * dMinBaseR,
GetPointLineSqDist( trTria.GetP( 2), ptMinBase, vtMove) < dMinBaseR * dMinBaseR} ;
if ( bInside[0] && bInside[1] && bInside[2])
return 0. ;
// Distanza di allontanamento dall'interno
double dInnEscapeDist = TrConeTriaInteriorEscapeDistLongMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH,
trTria, vtMove) ;
if ( dInnEscapeDist > EPS_SMALL)
return dInnEscapeDist ;
// Distanza di allontanamento dai vertici e lati
double dOutEscapeDist = 0 ;
for ( int nV = 0 ; nV < 3 ; ++ nV) {
if ( bInside[nV] && bInside[( nV + 1) % 3])
continue ;
// dal vertice
double dCurVertDist = TrConePointEscapeDistLongMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH,
trTria.GetP( nV), vtMove) ;
dOutEscapeDist = max( dCurVertDist, dOutEscapeDist) ;
// dal lato
Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
double dCurSegDist = TrConeSegmentEscapeDistLongMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR,
dTrConeH, trTria.GetP( nV), vtSeg, dSegLen, vtMove) ;
dOutEscapeDist = max( dCurSegDist, dOutEscapeDist) ;
}
return max( dOutEscapeDist, dDiskDist) ;
}
// Allontanamento in direzione ortogonale all'asse del cono
else if ( AreOrthoApprox( vtTrConeAx, vtMove)) {
double dEscapeDist = 0. ;
// Distanza di allontanamento dall'interno del trianolo: se positiva, abbiamo finito
double dInnEscapeDist = TrConeTriaInteriorEscapeDistOrtMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH,
trTria, vtMove) ;
if ( dInnEscapeDist > EPS_SMALL)
return dInnEscapeDist ;
else
dEscapeDist = max( dEscapeDist, dInnEscapeDist) ;
// Distanza di allontanamento da vertici e lati
for ( int nV = 0; nV < 3; ++ nV) {
// Dai vertici
double dCurVertDist = TrConePointEscapeDistOrtMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH,
trTria.GetP( nV), vtMove, bTop, bBot) ;
dEscapeDist = max( dEscapeDist, dCurVertDist) ;
// Dal lato
Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
double dCurSegDist = TrConeSegmentEscapeDistOrtMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH,
trTria.GetP( nV), vtSeg, dSegLen, vtMove, bTop, bBot) ;
dEscapeDist = max( dEscapeDist, dCurSegDist) ;
}
return max( dEscapeDist, dDiskDist) ;
}
// Movimento generico
else {
// Controlli preliminari sull'interferenza fra tronco 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 dEscapeDist = 0. ;
// Allontanamento di allontanamento dall'interno del triangolo
dEscapeDist = max( dEscapeDist, TrConeTriaInteriorEscapeDistGenMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR,
dTrConeH, trTria, vtMove)) ;
// Se positiva abbiamo terminato
if ( dEscapeDist > EPS_SMALL)
return dEscapeDist ;
// 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 ;
dEscapeDist = max( dEscapeDist, TrConePointEscapeDistGenMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH,
trTria.GetP( n), vtMove, bTop, bBot)) ;
dEscapeDist = max( dEscapeDist, TrConeSegmentEscapeDistGenMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH,
trTria.GetP( n), vtSeg, dSegLen, vtMove, bTop, bBot)) ;
}
return max( dEscapeDist, dDiskDist) ;
}
}
//----------------------------------------------------------------------------
double
TrConePointEscapeDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Point3d& ptP, const Vector3d& vtMove)
{
double dPointAxisSqDist = GetPointLineSqDist( ptP, ptMinBase, vtTrConeAx) ;
// Non c'è interferenza fra punto e superficie se la distanza del punto dall'asse non è compresa fra i raggi
if ( dPointAxisSqDist > dMaxBaseR * dMaxBaseR - 2 * dMaxBaseR * EPS_SMALL ||
dPointAxisSqDist < dMinBaseR * dMinBaseR)
return 0. ;
double dMinBaseEscapeDist = PointPlaneSignedDist( ptP, ptMinBase, vtMove) ;
double dMaxBaseEscapeDist = PointPlaneSignedDist( ptP, ptMinBase + dTrConeH * vtTrConeAx, vtMove) ;
// Non vi è interferenza se il cono è già oltre il punto
if ( dMinBaseEscapeDist < - EPS_SMALL && dMaxBaseEscapeDist < - EPS_SMALL)
return 0. ;
double dAddDist = ( sqrt( dPointAxisSqDist) - dMinBaseR) * dTrConeH / ( dMaxBaseR - dMinBaseR) ;
return dMinBaseEscapeDist + ( vtMove * vtTrConeAx > 0. ? - dAddDist : dAddDist) ;
}
//----------------------------------------------------------------------------
double
TrConeSegmentEscapeDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen,
const Vector3d& vtMove)
{
// 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 dEscapeDist = 0. ;
for ( int n = 0 ; n < nRoot ; ++ n) {
if ( vdRoots[n] > dEscapeDist) {
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 > 0. && dLongComp0 < dTrConeH) ||
( dLongComp1 > 0. && dLongComp1 < dTrConeH) ||
( dLongCompM > 0. && dLongCompM < dTrConeH)))
dEscapeDist = vdRoots[n] ;
}
}
return dEscapeDist ;
}
// 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 dEscapeDist = 0. ;
for ( int n = 0 ; n < nDeltaRoot ; ++ n) {
if ( vdDeltaRoots[n] > dEscapeDist) {
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 > - EPS_SMALL && dDotTanAx < dTrConeH - EPS_SMALL)
dEscapeDist = vdDeltaRoots[n] ;
}
}
}
return dEscapeDist ;
}
//----------------------------------------------------------------------------
double
TrConeTriaInteriorEscapeDistLongMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove)
{
double dEscapeDist = 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 = ( dMaxBaseR - dMinBaseR) / dTrConeH ;
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 dEscapeDist ;
// 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 dEscapeDist ;
// 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))
dEscapeDist = dU ;
return dEscapeDist ;
}
//----------------------------------------------------------------------------
double
TrConePointEscapeDistOrtMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Point3d& ptP, const Vector3d& vtMove,
bool bTop, bool bBot)
{
double dTopTol = bTop ? EPS_SMALL : - EPS_SMALL ;
double dBotTol = bBot ? - EPS_SMALL : EPS_SMALL ;
Vector3d vtP = ptP - ptMinBase ;
double dPntPosOnAx = vtP * vtTrConeAx ;
if ( dPntPosOnAx < dBotTol || dPntPosOnAx > dTrConeH + dTopTol)
return 0. ;
dPntPosOnAx = Clamp( dPntPosOnAx, 0., dTrConeH) ;
double dR = dMinBaseR + ( dMaxBaseR - dMinBaseR) * dPntPosOnAx / dTrConeH ;
return DiskPointEscapeDistOrtMot( ptMinBase + dPntPosOnAx * vtTrConeAx, vtTrConeAx, dR, ptP, vtMove) ;
}
//----------------------------------------------------------------------------
double
TrConeSegmentEscapeDistOrtMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen,
const Vector3d& vtMove, bool bTop, bool bBot)
{
double dTopTol = bTop ? EPS_SMALL : - EPS_SMALL ;
double dBotTol = bBot ? - EPS_SMALL : EPS_SMALL ;
double dStH = ( ptSeg - ptMinBase) * vtTrConeAx ;
double dEnH = ( ptSeg + dSegLen * vtSeg - ptMinBase) * vtTrConeAx ;
if ( ( dStH < dBotTol && dEnH < dBotTol) ||
( dStH > dTrConeH + dTopTol && dEnH > dTrConeH + dTopTol))
return 0. ;
// Distanza di allontanamento della superficie laterale: ci interessa solo la tangenza
double dDistMinBaseV = dTrConeH * dMinBaseR / ( dMaxBaseR - dMinBaseR) ;
Point3d ptConeV = ptMinBase - dDistMinBaseV * vtTrConeAx ;
Frame3d frConusFrame ;
frConusFrame.Set( ptConeV, vtMove, vtTrConeAx ^ vtMove, vtTrConeAx) ;
Point3d ptSegNew = ptSeg ;
Vector3d vtSegNew = vtSeg ;
ptSegNew.ToLoc( frConusFrame) ;
vtSegNew.ToLoc( frConusFrame) ;
double dAngCoef = ( dDistMinBaseV + dTrConeH) / dMaxBaseR ;
double dD0 = ptSegNew.z * ptSegNew.z - dAngCoef * dAngCoef * ( ptSegNew.x * ptSegNew.x + ptSegNew.y * ptSegNew.y) ;
double dD1 = ptSegNew.z * vtSegNew.z - dAngCoef * dAngCoef * ( ptSegNew.x * vtSegNew.x + ptSegNew.y * vtSegNew.y) ;
double dD2 = vtSegNew.z * vtSegNew.z - dAngCoef * dAngCoef * ( vtSegNew.x * vtSegNew.x + vtSegNew.y * vtSegNew.y) ;
// Se l'equazione è di primo grado, non serve risolvere (si trova altrimenti con i punti estremi del segmento)
if ( abs( dD2) < EPS_ZERO)
return 0. ;
// Impongo il determinante nullo, ne deriva una nuova equazione da risolvere
DBLVECTOR vdDeltaCoef( 3) ;
DBLVECTOR vdDeltaRoots ;
vdDeltaCoef[0] = dD1 * dD1 - dD0 * dD2 ;
vdDeltaCoef[1] = 2 * dAngCoef * dAngCoef * ( dD2 * ptSegNew.x - dD1 * vtSegNew.x) ;
vdDeltaCoef[2] = dAngCoef * dAngCoef * ( dAngCoef * dAngCoef * vtSegNew.x * vtSegNew.x + dD2) ;
int nRoot = PolynomialRoots( 2, vdDeltaCoef, vdDeltaRoots) ;
// Studio le soluzioni
double dEscapeDist = 0. ;
for ( int nS = 0 ; nS < nRoot ; ++ nS) {
// Queste soluzioni sono gli spostamenti lungo x del segmento che, lo portano in
// una configurazione di tangenza col cono (delta = 0). Ora risolviamo l'equazione
// dell'intersezione in corrispondenza del parametro di spostamento trovato.
// Essendo il delta nullo la sola soluzione è - b/2a.
double dT = ( dAngCoef * dAngCoef * vtSegNew.x * vdDeltaRoots[nS] - dD1) / dD2 ;
Point3d ptTan( ptSegNew.x + vdDeltaRoots[nS], ptSegNew.y, ptSegNew.z) ;
ptTan += dT * vtSegNew ;
if ( ptTan.z > dDistMinBaseV + dBotTol &&
ptTan.z < dDistMinBaseV + dTrConeH + dTopTol &&
dT > - EPS_SMALL && dT < dSegLen + EPS_SMALL) {
if ( - vdDeltaRoots[nS] > dEscapeDist)
dEscapeDist = - vdDeltaRoots[nS] ;
}
}
return dEscapeDist ;
}
//----------------------------------------------------------------------------
double
TrConeTriaInteriorEscapeDistOrtMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove)
{
double dEscapeDist = 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 = ( dMaxBaseR - dMinBaseR) / dTrConeH ;
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 dEscapeDist ;
// 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 dEscapeDist ;
// 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))
dEscapeDist = dU ;
return dEscapeDist ;
}
//----------------------------------------------------------------------------
double
TrConePointEscapeDistGenMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Point3d& ptP, const Vector3d& vtMove, bool bTop, bool bBot)
{
double 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 dEscapeDist = 0. ;
for ( int n = 0 ; n < nRoot ; ++ n) {
if ( vdRoots[n] > dEscapeDist) {
Point3d ptTouch = ptP - vdRoots[n] * vtMove ;
double dLongComp = ( ptTouch - ptMinBase) * vtTrConeAx ;
if ( dLongComp > dBotTol && dLongComp < dTrConeH + dTopTol)
dEscapeDist = vdRoots[n] ;
}
}
return dEscapeDist ;
}
//----------------------------------------------------------------------------
double
TrConeSegmentEscapeDistGenMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen,
const Vector3d& vtMove, bool bTop, bool bBot)
{
// 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 dEscapeDist = 0. ;
for ( int n = 0 ; n < nRoot ; ++ n) {
if ( vdRoots[n] > dEscapeDist) {
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)))
dEscapeDist = vdRoots[n] ;
}
}
return dEscapeDist ;
}
// 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 dEscapeDist = 0. ;
for ( int n = 0 ; n < nDeltaRoot ; ++ n) {
if ( vdDeltaRoots[n] > dEscapeDist) {
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)
dEscapeDist = vdDeltaRoots[n] ;
}
}
}
return dEscapeDist ;
}
//----------------------------------------------------------------------------
double
TrConeTriaInteriorEscapeDistGenMot( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double dMinBaseR, double dMaxBaseR,
double dTrConeH, const Triangle3d& trTria, const Vector3d& vtMove)
{
double dEscapeDist = 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 dEscapeDist ;
// 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 dEscapeDist ;
// 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))
dEscapeDist = dU ;
return dEscapeDist ;
}
//----------------------------------------------------------------------------
// **** TORO ****
// In questi algoritmi per toro intendiamo la corona torica esterna
//----------------------------------------------------------------------------
double
CAvTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot)
{
// Allontanamento in direzione coincidente con l'asse del toro
if ( AreSameVectorApprox( vtTorusAx, vtMove)) {
// Verifica preliminare che la fetta di spazio delimitata dal piano sotto del toro rispetto alla direzione
// di allontanamento non sia già sopra al triangolo (tutti i vertici del triangolo sono sotto).
Point3d ptBase = ptTorusCen - dMinRad * vtMove ;
double dMaxDistV = PointPlaneSignedDist( trTria.GetP( 0), ptBase, vtMove) ;
dMaxDistV = max( dMaxDistV, PointPlaneSignedDist( trTria.GetP( 1), ptBase, vtMove)) ;
dMaxDistV = max( dMaxDistV, PointPlaneSignedDist( trTria.GetP( 2), ptBase, vtMove)) ;
if ( dMaxDistV < 0.)
return 0. ;
// Distanza di allontanamento dall'interno
double dInEscapeDist = TorusTriaInteriorEscapeDistLongMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad,
trTria, vtMove) ;
if ( dInEscapeDist > EPS_SMALL)
return dInEscapeDist ;
// Distanza di allontanamento dai lati
double dOutEscapeDist = 0. ;
for ( int nV = 0 ; nV < 3 ; ++ nV) {
Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
double dCurDist = TorusSegmentEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad,
trTria.GetP( nV), vtSeg, dSegLen, vtMove) ;
dOutEscapeDist = max( dCurDist, dOutEscapeDist) ;
// Valuto contatto del disco con i vertici
dCurDist = PointPlaneSignedDist( trTria.GetP( nV), ptTorusCen - dMinRad * vtTorusAx, vtMove) ;
if ( GetPointLineSqDist( trTria.GetP( nV), ptTorusCen, vtTorusAx) < dMaxRad * dMaxRad &&
dCurDist > dOutEscapeDist)
dOutEscapeDist = max( dOutEscapeDist, dCurDist) ;
}
return dOutEscapeDist ;
}
// Allontanamento in direzione ortogonale all'asse del toro
else if ( AreOrthoApprox( vtTorusAx, vtMove)) {
// Verifico che il triangolo intersechi il volume spazzato dal oro nel suo possibile movimento
double dTopTol = bTop ? EPS_SMALL : - EPS_SMALL ;
double dBotTol = bBot ? - EPS_SMALL : EPS_SMALL ;
double dRadSum = dMaxRad + dMinRad ;
Vector3d vtTrasv = vtTorusAx ^ vtMove ;
Vector3d vtR0 = trTria.GetP( 0) - ptTorusCen ;
Vector3d vtR1 = trTria.GetP( 1) - ptTorusCen ;
Vector3d vtR2 = trTria.GetP( 2) - ptTorusCen ;
if ( ( vtR0 * vtMove < - dRadSum && vtR1 * vtMove < - dRadSum && vtR2 * vtMove < - dRadSum) ||
( vtR0 * vtTrasv < - dRadSum && vtR1 * vtTrasv < - dRadSum && vtR2 * vtTrasv < - dRadSum) ||
( vtR0 * vtTrasv > dRadSum && vtR1 * vtTrasv > dRadSum && vtR2 * vtTrasv > dRadSum) ||
( vtR0 * vtTorusAx > dTopTol && vtR1 * vtTorusAx > dTopTol && vtR2 * vtTorusAx > dTopTol) ||
( vtR0 * vtTorusAx < - dMinRad + dBotTol && vtR1 * vtTorusAx < - dMinRad + dBotTol && vtR2 * vtTorusAx < - dMinRad + dBotTol))
return 0. ;
// Distanza di allontanamento dall'interno del triangolo: se positivo abbiamo finito
double dInEscapeDist = TorusTriaInteriorEscapeDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, trTria, vtMove) ;
if ( dInEscapeDist > EPS_SMALL)
return dInEscapeDist ;
// Distanza di allontanamento dai lati
double dOutEscapeDist = 0. ;
for ( int nV = 0 ; nV < 3 ; ++ nV) {
Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
double dCurDist = TorusSegmentEscapeDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad,
trTria.GetP( nV), vtSeg, dSegLen, vtMove, bTop, bBot) ;
dOutEscapeDist = max( dCurDist, dOutEscapeDist) ;
}
return max( dOutEscapeDist, dInEscapeDist) ;
}
// Allontanamento in direzione generica
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 dInnEscapeDist = TorusTriaInteriorEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, trTria, vtMove) ;
// Se positiva abbiamo finito
if ( dInnEscapeDist > EPS_SMALL)
return dInnEscapeDist ;
// Distanza di allontanamento da frontiera
double dBordEscapeDist = 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 = TorusSegmentEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad,
trTria.GetP( n), vtSeg, dSegLen, vtMove) ;
dBordEscapeDist = max( dBordEscapeDist, dCurDist) ;
}
return dBordEscapeDist ;
}
}
//----------------------------------------------------------------------------
double
TorusPointEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad,
const Point3d& ptP, const Vector3d& vtMove)
{
double dPointAxSqLen = GetPointLineSqDist( ptP, ptTorusCen, vtMove) ;
double dDeltaRad = sqrt( dPointAxSqLen) - dMaxRad ;
double dSQAddEscapeDist = dMinRad * dMinRad - dDeltaRad * dDeltaRad ;
if ( dSQAddEscapeDist <= 0.)
return 0. ;
double dAddEscapeDist = sqrt( dSQAddEscapeDist) ;
double dEscapeDist = ( ptP - ptTorusCen) * vtMove + dAddEscapeDist ;
return dEscapeDist ;
}
//----------------------------------------------------------------------------
double
TorusSegmentEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove)
{
// Se segmento dista dall'asse del toro più del raggio esterno, posso uscire subito
double dLinSegSqDist = LineSegmentSqDist( ptTorusCen, vtMove, ptSeg, vtSeg, dSegLen) ;
if ( dLinSegSqDist > ( dMaxRad + dMinRad) * ( dMaxRad + dMinRad))
return 0. ;
// Se estremi del segmento più vicini della corona torica, posso uscire subito
double dStartSqDist = GetPointLineSqDist( ptSeg, ptTorusCen, vtMove) ;
double dEndSqDist = GetPointLineSqDist( ptSeg + vtSeg * dSegLen, ptTorusCen, vtMove) ;
if ( dStartSqDist < dMaxRad * dMaxRad && dEndSqDist < dMaxRad * dMaxRad)
return 0. ;
// Intervallo del segmento non limitato
Intervals SegLimits ;
SegLimits.Set( 0, dSegLen) ;
// Limito il segmento entro il cilindro esterno della corona del toro
double dOutLen1, dOutLen2 ;
int nOutInters = IntersLineInfiniteCylinder( ptSeg, vtSeg, ptTorusCen, vtMove, dMaxRad + dMinRad,
dOutLen1, dOutLen2) ;
if ( nOutInters == CC_TWO_INT)
SegLimits.Intersect( dOutLen1, dOutLen2) ;
else if ( nOutInters == CC_NO_INTERS || nOutInters == CC_ONE_INT_TAN)
SegLimits.Reset() ;
if ( SegLimits.IsEmpty())
return 0. ;
// Tolgo eventuale cilindro interno alla corona del toro
if ( dLinSegSqDist < dMaxRad * dMaxRad) {
double dIntLen1, dIntLen2 ;
int nIntInters = IntersLineInfiniteCylinder( ptSeg, vtSeg, ptTorusCen, vtMove, dMaxRad,
dIntLen1, dIntLen2) ;
if ( nIntInters == CC_TWO_INT)
SegLimits.Subtract( dIntLen1, dIntLen2) ;
}
// Eseguo controllo sugli intervalli
double dMaxEscapeDist = 0. ;
double dLen1, dLen2 ;
bool bFound = SegLimits.GetFirst( dLen1, dLen2) ;
while ( bFound) {
// verifico l'intervallo
Point3d ptStart = ptSeg + vtSeg * dLen1 ;
double dLen = dLen2 - dLen1 ;
const double INV_GOLDEN_RATIO = ( sqrt( 5.) - 1.) / 2. ;
double dTol = max( 0.01 * dMinRad, EPS_SMALL) ;
double dPSt = 0 ;
double dPEn = dLen ;
double dPIn1 = dPEn - ( dPEn - dPSt) * INV_GOLDEN_RATIO ;
double dPIn2 = dPSt + ( dPEn - dPSt) * INV_GOLDEN_RATIO ;
double dEscapeDist1 = TorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad, ptStart + dPIn1 * vtSeg, vtMove) ;
double dEscapeDist2 = TorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad, ptStart + dPIn2 * vtSeg, vtMove) ;
while ( dPEn - dPSt > dTol) {
if ( dEscapeDist1 > dEscapeDist2) {
dPEn = dPIn2 ;
dPIn2 = dPIn1 ;
dEscapeDist2 = dEscapeDist1 ;
dPIn1 = dPEn - ( dPEn - dPSt) * INV_GOLDEN_RATIO ;
dEscapeDist1 = TorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad, ptStart + dPIn1 * vtSeg, vtMove) ;
}
else {
dPSt = dPIn1 ;
dPIn1 = dPIn2 ;
dEscapeDist1 = dEscapeDist2 ;
dPIn2 = dPSt + ( dPEn - dPSt) * INV_GOLDEN_RATIO ;
dEscapeDist2 = TorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad, ptStart + dPIn2 * vtSeg, vtMove) ;
}
}
double dPMax = 0.5 * ( dPSt + dPEn) ;
dMaxEscapeDist = max( dMaxEscapeDist, TorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad, ptStart + dPMax * vtSeg, vtMove)) ;
// passo al successivo intervallo
bFound = SegLimits.GetNext( dLen1, dLen2) ;
}
return dMaxEscapeDist ;
}
//----------------------------------------------------------------------------
double
TorusTriaInteriorEscapeDistLongMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Triangle3d& trTria, const Vector3d& vtMove)
{
// Siamo interessati solo ai casi di tangenza, poiché il punto di contatto
// estremo con l'interno di un triangolo non può che essere di tangenza.
double dCompLong = trTria.GetN() * vtTorusAx ;
Vector3d vtNormOrt = trTria.GetN() - dCompLong * vtTorusAx ;
// Se il triangolo è orientato in modo che non 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 ( ! IsPointInsideTriangle( ptTouch, trTria, TriangleType::OPEN))
dDist = 0. ;
return dDist ;
}
//----------------------------------------------------------------------------
double
TorusPointEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Point3d& ptP, const Vector3d& vtMove)
{
Vector3d vtR = ptP - ptTorusCen ;
// Quota del piano di movimento del punto
double dH = vtR * vtTorusAx ;
if ( dH > EPS_SMALL)
return 0. ;
// Raggio della circonferenza alla quota dH
double dHRad = dMaxRad + sqrt( max( dMinRad * dMinRad - dH * dH, 0.)) ;
// Vettore movimento nel piano e sua lunghezza
Vector3d vtPlaneR = vtR - dH * vtTorusAx ;
double dL = vtPlaneR * vtMove ;
if ( dL <= - dHRad)
return 0. ;
// Distanza della linea di movimento dall'asse del toro
Vector3d vtPlaneOrtR = vtPlaneR - dL * vtMove ;
double dSqDist = vtPlaneOrtR.SqLen() ;
if ( dSqDist >= dHRad * dHRad)
return 0. ;
// Distanza di movimento
return ( dL + sqrt( dHRad * dHRad - dSqDist)) ;
}
//----------------------------------------------------------------------------
double
TorusSegmentEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove,
bool bTop, bool bBot)
{
// Dati segmento modificabli
Point3d ptMySeg = ptSeg ;
double dMySegLen = dSegLen ;
// Versore ortogonale a asse utensile e movimento
Vector3d vtTrasv = vtTorusAx ^ vtMove ;
vtTrasv.Normalize() ;
// Piano a destra
Plane3d plLeft ;
plLeft.Set( ptTorusCen - ( dMaxRad + dMinRad) * vtTrasv, vtTrasv) ;
if ( ! ClampSegmentOutPlane( plLeft, ptMySeg, vtSeg, dMySegLen))
return 0. ;
// Piano a sinistra
Plane3d plRight ;
plRight.Set( ptTorusCen + ( dMaxRad + dMinRad) * vtTrasv, -vtTrasv) ;
if ( ! ClampSegmentOutPlane( plRight, ptMySeg, vtSeg, dMySegLen))
return 0. ;
// Piano dietro
Plane3d plBack ;
plBack.Set( ptTorusCen - ( dMaxRad + dMinRad) * vtMove, vtMove) ;
if ( ! ClampSegmentOutPlane( plBack, ptMySeg, vtSeg, dMySegLen))
return 0. ;
// Piano sotto
double dBotTol = ( bBot ? - EPS_SMALL : EPS_SMALL) ;
Plane3d plBot ;
plBot.Set( ptTorusCen - dMinRad * vtTorusAx, vtTorusAx) ;
if ( ! ClampSegmentOutPlane( plBot, ptMySeg, vtSeg, dMySegLen, dBotTol))
return 0. ;
// Piano sopra
double dTopTol = ( bTop ? -EPS_SMALL : EPS_SMALL) ;
Plane3d plTop ;
plTop.Set( ptTorusCen, -vtTorusAx) ;
if ( ! ClampSegmentOutPlane( plTop, ptMySeg, vtSeg, dMySegLen, dTopTol))
return 0. ;
// Cerco distanza di massimo allontanamento lungo il segmento ridotto
double dEscapeDist = 0. ;
double dStep = max( 0.1 * dMinRad, EPS_SMALL) ;
int nStepNum = max( int( dMySegLen / dStep), 1) ;
dStep = dMySegLen / nStepNum ;
int nMax = -1 ;
for ( int i = 0 ; i <= nStepNum ; ++ i) {
Point3d ptP = ptMySeg + ( i * dStep) * vtSeg ;
// Minima distanza per evitare collisione
double dDist = TorusPointEscapeDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptP, vtMove) ;
if ( dDist > dEscapeDist) {
dEscapeDist = dDist ;
nMax = i ;
}
}
// affino la ricerca (curva parabolica nell'intorno del massimo trovato)
if ( nMax != -1) {
// nuovo passo e punto di partenza
const int SUB_STEPS = 10 ;
double dNewStep = dStep / SUB_STEPS ;
Point3d ptStart = ptMySeg + ( nMax * dStep) * vtSeg ;
// cerco dopo
if ( nMax < nStepNum) {
for ( int j = 1 ; j < SUB_STEPS ; ++ j) {
Point3d ptP = ptStart + ( j * dNewStep) * vtSeg ;
// Minima distanza per evitare collisione
double dDist = TorusPointEscapeDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptP, vtMove) ;
if ( dDist > dEscapeDist)
dEscapeDist = dDist ;
else
break ;
}
}
// cerco prima
if ( nMax > 0) {
for ( int j = 1 ; j < SUB_STEPS ; ++ j) {
Point3d ptP = ptStart - ( j * dNewStep) * vtSeg ;
// Minima distanza per evitare collisione
double dDist = TorusPointEscapeDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptP, vtMove) ;
if ( dDist > dEscapeDist)
dEscapeDist = dDist ;
else
break ;
}
}
}
return dEscapeDist ;
}
//----------------------------------------------------------------------------
double
TorusTriaInteriorEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Triangle3d& trTria, const Vector3d& vtMove)
{
// Componente ortogonale all'asse del toro del vettore del piano del triangolo
Vector3d vtPlaneOrtToAx = trTria.GetN() - ( trTria.GetN() * vtTorusAx) * vtTorusAx ;
// Se piano ortogonale all'asse del toro non ci può essere tangenza
if ( ! vtPlaneOrtToAx.Normalize( EPS_ZERO))
return 0. ;
// Se la componente del versore normale al triangolo nella direzione
// del moto è nulla, non può esserci contatto con l'interno
double dDotMoveN = vtMove * trTria.GetN() ;
if ( abs( dDotMoveN) < EPS_ZERO)
return 0. ;
// Scarto il primo contatto
if ( vtPlaneOrtToAx * vtMove < 0.)
return 0. ;
vtPlaneOrtToAx *= dMaxRad ;
// Trovo il punto che toccherà il piano
Point3d ptTouch = ptTorusCen - vtPlaneOrtToAx - dMinRad * trTria.GetN() ;
double dEscapeDist = max( ( ( trTria.GetP( 0) - ptTouch) * trTria.GetN()) / dDotMoveN, 0.) ;
// Se il punto di contatto è interno al triangolo restituisco distanza di fuga non negativa
if ( IsPointInsideTriangle( ptTouch + dEscapeDist * vtMove, trTria, TriangleType::OPEN))
return dEscapeDist ;
return 0. ;
}
//----------------------------------------------------------------------------
double
TorusPointEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Point3d& ptP, const Vector3d& vtMove)
{
// Intersezione fra semi-retta e corona torica
BOOLVECTOR vbType ;
DBLVECTOR vdPar ;
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
TorusSegmentEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove)
{
Point3d ptMySeg = ptSeg ;
double dMySegLen = dSegLen ;
// Restringo il segmento nella striscia di spazio in cui si sposta il toro
Vector3d vtTrasv = vtTorusAx ^ vtMove ;
if ( ! vtTrasv.Normalize())
return - 1. ;
// Piano a sinistra
Plane3d plPlane ;
plPlane.Set( ptTorusCen + ( dMaxRad + dMinRad) * vtTrasv, - vtTrasv) ;
if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen))
return 0. ;
// Piano a destra
plPlane.Set( ptTorusCen - ( dMaxRad + dMinRad) * vtTrasv, vtTrasv) ;
if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen))
return 0. ;
double dDotAxMv = vtMove * vtTorusAx ;
Vector3d vtOrtMv = vtMove - dDotAxMv * vtTorusAx ;
if ( ! vtOrtMv.Normalize())
return - 1. ;
// Piano dietro
plPlane.Set( ptTorusCen - ( dMaxRad + dMinRad) * vtOrtMv, vtOrtMv) ;
if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen))
return 0. ;
// Piano orizzontale
Point3d ptUpDw = ptTorusCen - ( dDotAxMv > 0. ? dMinRad : 0.) * vtTorusAx ;
Vector3d vtUPDw = dDotAxMv > 0. ? vtTorusAx : - vtTorusAx ;
plPlane.Set( ptUpDw, vtUPDw) ;
if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen))
return 0. ;
// Cerco il punto del segmento ridotto che ha distanza di allontanamento massima
double dEscapeDist = 0. ;
double dStep = max( 0.1 * dMinRad, EPS_SMALL) ;
int nStepNum = max( int( dMySegLen / dStep), 1) ;
dStep = dMySegLen / nStepNum ;
int nMax = -1 ;
for ( int n = 0 ; n <= nStepNum ; ++ n) {
Point3d ptCurPoint = ptMySeg + ( n * dStep) * vtSeg ;
double dCurrDist = TorusPointEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ;
if ( dEscapeDist < dCurrDist) {
dEscapeDist = dCurrDist ;
nMax = n ;
}
}
// affino la ricerca (curva parabolica nell'intorno del massimo trovato)
if ( nMax != -1) {
// nuovo passo e punto di partenza
const int SUB_STEPS = 10 ;
double dNewStep = dStep / SUB_STEPS ;
Point3d ptStart = ptMySeg + ( nMax * dStep) * vtSeg ;
// cerco dopo
if ( nMax < nStepNum) {
for ( int j = 1 ; j < SUB_STEPS ; ++ j) {
Point3d ptCurPoint = ptStart + ( j * dNewStep) * vtSeg ;
// Minima distanza per evitare collisione
double dDist = TorusPointEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ;
if ( dDist > dEscapeDist)
dEscapeDist = dDist ;
else
break ;
}
}
// cerco prima
if ( nMax > 0) {
for ( int j = 1 ; j < SUB_STEPS ; ++ j) {
Point3d ptCurPoint = ptStart - ( j * dNewStep) * vtSeg ;
// Minima distanza per evitare collisione
double dDist = TorusPointEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ;
if ( dDist > dEscapeDist)
dEscapeDist = dDist ;
else
break ;
}
}
}
return dEscapeDist ;
}
//----------------------------------------------------------------------------
double
TorusTriaInteriorEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Triangle3d& trTria, const Vector3d& vtMove)
{
// Siamo interessati solo ai casi di tangenza, poiché il punto di contatto
// estremo con l'interno di un triangolo non può che essere di tangenza.
double dCompLong = trTria.GetN() * vtTorusAx ;
Vector3d vtNormOrt = trTria.GetN() - dCompLong * vtTorusAx ;
// Se il triangolo è orientato in modo che non 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 ( ! IsPointInsideTriangle( ptTouch, trTria, TriangleType::OPEN))
dDist = 0. ;
return dDist ;
}
//----------------------------------------------------------------------------
double
CAvConcaveTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Triangle3d& trTria, const Vector3d& vtMove, bool bTop, bool bBot)
{
double dDiskDist ;
if ( vtTorusAx * vtMove < - EPS_ZERO)
dDiskDist = CAvDiskTriangle( ptTorusCen, - vtTorusAx, dMaxRad - dMinRad, trTria, vtMove) ;
else
dDiskDist = CAvDiskTriangle( ptTorusCen + dMinRad * vtTorusAx, vtTorusAx, dMaxRad, trTria, vtMove) ;
// Allontanamento con direzione coincidente con l'asse del toro
if ( AreSameOrOppositeVectorApprox( vtTorusAx, vtMove)) {
// Le corone toriche non possono effettuare un moto in direzione opposta
// al proprio asse di simmetria: vedi documentazione.
if ( vtTorusAx * vtMove < 0.)
return 0. ;
// Distanza di allontanamento dai lati
double dOutEscapeDist = 0. ;
for ( int nV = 0 ; nV < 3 ; ++ nV) {
Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
double dCurDist = ConcaveTorusSegmentEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad,
trTria.GetP( nV), vtSeg, dSegLen, vtMove) ;
dOutEscapeDist = max( dCurDist, dOutEscapeDist) ;
// Valuto contatto del disco con i vertici
dCurDist = PointPlaneSignedDist( trTria.GetP( nV), ptTorusCen, vtMove) ;
if ( GetPointLineSqDist( trTria.GetP( nV), ptTorusCen, vtTorusAx) < ( dMaxRad - dMinRad) * ( dMaxRad - dMinRad) &&
dCurDist > dOutEscapeDist)
dOutEscapeDist = max( dOutEscapeDist, dCurDist) ;
}
return max( dDiskDist, dOutEscapeDist) ;
}
// Allontanamento con direzione ortogonale a quella dell'asse del toro
else if ( AreOrthoApprox( vtTorusAx, vtMove)) {
double dEscapeDist = 0. ;
// Distanza di allontanamento dei segmenti
for ( int nV = 0 ; nV < 3 ; ++ nV) {
Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
double dCurDist = ConcaveTorusSegmentEscapeDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad,
trTria.GetP( nV), vtSeg, dSegLen, vtMove, bTop, bBot) ;
dEscapeDist = max( dEscapeDist, dCurDist) ;
}
dEscapeDist = max( dEscapeDist, dDiskDist) ;
return dEscapeDist ;
}
// Casi non gestiti
else {
double dEscapeDist = 0. ;
// Distanza di allontanamento dei segmenti
for ( int nV = 0 ; nV < 3 ; ++ nV) {
Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
double dCurDist = ConcaveTorusSegmentEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad,
trTria.GetP( nV), vtSeg, dSegLen, vtMove) ;
dEscapeDist = max( dEscapeDist, dCurDist) ;
}
dEscapeDist = max( dEscapeDist, dDiskDist) ;
return dEscapeDist ;
}
}
//----------------------------------------------------------------------------
double
ConcaveTorusPointEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad,
const Point3d& ptP, const Vector3d& vtMove)
{
double dPointAxSqLen = GetPointLineSqDist( ptP, ptTorusCen, vtMove) ;
double dDeltaRad = sqrt( dPointAxSqLen) - dMaxRad ;
if ( dDeltaRad > 0 || dDeltaRad < - dMinRad)
return 0. ;
return ( ptP - ptTorusCen) * vtMove - sqrt( dMinRad * dMinRad - dDeltaRad * dDeltaRad) ;
}
//----------------------------------------------------------------------------
double
ConcaveTorusSegmentEscapeDistLongMot( const Point3d& ptTorusCen, double dMaxRad, double dMinRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove)
{
// Se segmento dista dall'asse del toro più del raggio massimo, posso uscire subito
double dLinSegSqDist = LineSegmentSqDist( ptTorusCen, vtMove, ptSeg, vtSeg, dSegLen) ;
if ( dLinSegSqDist > dMaxRad * dMaxRad)
return 0. ;
// Se estremi del segmento più vicini della corona torica, posso uscire subito
double dStartSqDist = GetPointLineSqDist( ptSeg, ptTorusCen, vtMove) ;
double dEndSqDist = GetPointLineSqDist( ptSeg + vtSeg * dSegLen, ptTorusCen, vtMove) ;
if ( dStartSqDist < ( dMaxRad - dMinRad) * ( dMaxRad - dMinRad) &&
dEndSqDist < ( dMaxRad - dMinRad) * ( dMaxRad - dMinRad))
return 0. ;
// Intervallo del segmento non limitato
Intervals SegLimits ;
SegLimits.Set( 0, dSegLen) ;
// Limito il segmento entro il cilindro esterno della corona del toro
double dOutLen1, dOutLen2 ;
int nOutInters = IntersLineInfiniteCylinder( ptSeg, vtSeg, ptTorusCen, vtMove, dMaxRad,
dOutLen1, dOutLen2) ;
if ( nOutInters == CC_TWO_INT || nOutInters == CC_ONE_INT_TAN)
SegLimits.Intersect( dOutLen1, dOutLen2) ;
else if ( nOutInters == CC_NO_INTERS)
SegLimits.Reset() ;
if ( SegLimits.IsEmpty())
return 0. ;
// Tolgo eventuale cilindro interno alla corona del toro
if ( dLinSegSqDist < ( dMaxRad - dMinRad) * ( dMaxRad - dMinRad)) {
double dIntLen1, dIntLen2 ;
int nIntInters = IntersLineInfiniteCylinder( ptSeg, vtSeg, ptTorusCen, vtMove, dMaxRad - dMinRad,
dIntLen1, dIntLen2) ;
if ( nIntInters == CC_TWO_INT)
SegLimits.Subtract( dIntLen1, dIntLen2) ;
}
// Eseguo controllo sugli intervalli
double dMaxEscapeDist = 0. ;
double dLen1, dLen2 ;
bool bFound = SegLimits.GetFirst( dLen1, dLen2) ;
while ( bFound) {
// verifico l'intervallo
Point3d ptStart = ptSeg + vtSeg * dLen1 ;
double dLen = dLen2 - dLen1 ;
const double INV_GOLDEN_RATIO = ( sqrt( 5.) - 1.) / 2. ;
double dTol = max( 0.01 * dMinRad, EPS_SMALL) ;
double dPSt = 0 ;
double dPEn = dLen ;
double dPIn1 = dPEn - ( dPEn - dPSt) * INV_GOLDEN_RATIO ;
double dPIn2 = dPSt + ( dPEn - dPSt) * INV_GOLDEN_RATIO ;
double dEscapeDist1 = ConcaveTorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad,
ptStart + dPIn1 * vtSeg, vtMove) ;
double dEscapeDist2 = ConcaveTorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad,
ptStart + dPIn2 * vtSeg, vtMove) ;
while ( dPEn - dPSt > dTol) {
if ( dEscapeDist1 > dEscapeDist2) {
dPEn = dPIn2 ;
dPIn2 = dPIn1 ;
dEscapeDist2 = dEscapeDist1 ;
dPIn1 = dPEn - ( dPEn - dPSt) * INV_GOLDEN_RATIO ;
dEscapeDist1 = ConcaveTorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad,
ptStart + dPIn1 * vtSeg, vtMove) ;
}
else {
dPSt = dPIn1 ;
dPIn1 = dPIn2 ;
dEscapeDist1 = dEscapeDist2 ;
dPIn2 = dPSt + ( dPEn - dPSt) * INV_GOLDEN_RATIO ;
dEscapeDist2 = ConcaveTorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad,
ptStart + dPIn2 * vtSeg, vtMove) ;
}
}
double dPMax = 0.5 * ( dPSt + dPEn) ;
dMaxEscapeDist = max( dMaxEscapeDist, ConcaveTorusPointEscapeDistLongMot( ptTorusCen, dMaxRad, dMinRad,
ptStart + dPMax * vtSeg, vtMove)) ;
// passo al successivo intervallo
bFound = SegLimits.GetNext( dLen1, dLen2) ;
}
return dMaxEscapeDist ;
}
//----------------------------------------------------------------------------
double
ConcaveTorusPointEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Point3d& ptP, const Vector3d& vtMove)
{
double dSqMinRad = dMinRad * dMinRad ;
Vector3d vtR = ptP - ptTorusCen ;
double dH = vtR * vtTorusAx ;
Vector3d vtPlaneR = vtR - dH * vtTorusAx ;
double dL = vtPlaneR * vtMove ;
Vector3d vtPlaneOrtR = vtPlaneR - dL * vtMove ;
double dDeltaRad = dSqMinRad - dH * dH > 0. ? sqrt( dSqMinRad - dH * dH) : 0. ;
// Raggio della circonferenza alla quota dH
double dHRad = dMaxRad - dDeltaRad ;
double dSqDeltaL = dHRad * dHRad - vtPlaneOrtR.SqLen() ;
double dEscapeDist = dSqDeltaL > 0. ? dL + sqrt( dSqDeltaL) : 0. ;
return dEscapeDist ;
}
//----------------------------------------------------------------------------
double
ConcaveTorusSegmentEscapeDistOrtMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove,
bool bTop, bool bBot)
{
// Dati segmento modificabli
Point3d ptMySeg = ptSeg ;
double dMySegLen = dSegLen ;
// Versore ortogonale a asse utensile e movimento
Vector3d vtTrasv = vtTorusAx ^ vtMove ;
vtTrasv.Normalize() ;
// Piano a destra
Plane3d plLeft ;
plLeft.Set( ptTorusCen - dMaxRad * vtTrasv, vtTrasv) ;
if ( ! ClampSegmentOutPlane( plLeft, ptMySeg, vtSeg, dMySegLen))
return 0. ;
// Piano a sinistra
Plane3d plRight ;
plRight.Set( ptTorusCen + dMaxRad * vtTrasv, -vtTrasv) ;
if ( ! ClampSegmentOutPlane( plRight, ptMySeg, vtSeg, dMySegLen))
return 0. ;
// Piano dietro
Plane3d plBack ;
plBack.Set( ptTorusCen - dMaxRad * vtMove, vtMove) ;
if ( ! ClampSegmentOutPlane( plBack, ptMySeg, vtSeg, dMySegLen))
return 0. ;
// Piano sotto
double dBotTol = ( bBot ? - EPS_SMALL : EPS_SMALL) ;
Plane3d plBot ;
plBot.Set( ptTorusCen, vtTorusAx) ;
if ( ! ClampSegmentOutPlane( plBot, ptMySeg, vtSeg, dMySegLen, dBotTol))
return 0. ;
// Piano sopra
double dTopTol = ( bTop ? -EPS_SMALL : EPS_SMALL) ;
Plane3d plTop ;
plTop.Set( ptTorusCen + dMinRad * vtTorusAx, -vtTorusAx) ;
if ( ! ClampSegmentOutPlane( plTop, ptMySeg, vtSeg, dMySegLen, dTopTol))
return 0. ;
// Cerco distanza di massimo allontanamento lungo il segmento ridotto
double dEscapeDist = 0. ;
double dStep = max( 0.1 * dMinRad, EPS_SMALL) ;
int nStepNum = max( int( dMySegLen / dStep), 1) ;
dStep = dMySegLen / nStepNum ;
int nMax = -1 ;
for ( int n = 0 ; n <= nStepNum ; ++ n) {
Point3d ptCurPoint = ptMySeg + ( n * dStep) * vtSeg ;
double dCurDist = ConcaveTorusPointEscapeDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ;
if ( dCurDist > dEscapeDist) {
dEscapeDist = dCurDist ;
nMax = n ;
}
}
// affino la ricerca (curva parabolica nell'intorno del massimo trovato)
if ( nMax != -1) {
// nuovo passo e punto di partenza
const int SUB_STEPS = 10 ;
double dNewStep = dStep / SUB_STEPS ;
Point3d ptStart = ptMySeg + ( nMax * dStep) * vtSeg ;
// cerco dopo
if ( nMax < nStepNum) {
for ( int j = 1 ; j < SUB_STEPS ; ++ j) {
Point3d ptCurPoint = ptStart + ( j * dNewStep) * vtSeg ;
double dCurDist = ConcaveTorusPointEscapeDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ;
if ( dCurDist > dEscapeDist)
dEscapeDist = dCurDist ;
else
break ;
}
}
// cerco prima
if ( nMax > 0) {
for ( int j = 1 ; j < SUB_STEPS ; ++ j) {
Point3d ptCurPoint = ptStart - ( j * dNewStep) * vtSeg ;
double dCurDist = ConcaveTorusPointEscapeDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ;
if ( dCurDist > dEscapeDist)
dEscapeDist = dCurDist ;
else
break ;
}
}
}
return dEscapeDist ;
}
//----------------------------------------------------------------------------
double
ConcaveTorusPointEscapeDistGenMot( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double dMaxRad, double dMinRad,
const Point3d& ptP, const Vector3d& vtMove)
{
BOOLVECTOR vbType ;
DBLVECTOR vdPar ;
// Intersezione fra semi-retta e corona torica
int nIntType = LinCompTorusInnUpInt( 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
ConcaveTorusSegmentEscapeDistGenMot( 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 ptMySeg = ptSeg ;
double dMySegLen = dSegLen ;
Vector3d vtTrasv = vtTorusAx ^ vtMove ;
if ( ! vtTrasv.Normalize())
return - 1. ;
// Piano a sinistra
Plane3d plPlane ;
plPlane.Set( ptTorusCen + dMaxRad * vtTrasv, - vtTrasv) ;
if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen))
return 0. ;
// Piano a destra
plPlane.Set( ptTorusCen - dMaxRad * vtTrasv, vtTrasv) ;
if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen))
return 0. ;
double dDotAxMv = vtMove * vtTorusAx ;
Vector3d vtOrtMv = vtMove - dDotAxMv * vtTorusAx ;
if ( ! vtOrtMv.Normalize())
return - 1. ;
// Piano dietro
plPlane.Set( ptTorusCen - dMaxRad * vtOrtMv, vtOrtMv) ;
if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen))
return 0. ;
// Piano orizzontale
Point3d ptUpDw = ptTorusCen + ( dDotAxMv > 0. ? 0. : dMinRad) * vtTorusAx ;
Vector3d vtUPDw = dDotAxMv > 0. ? vtTorusAx : - vtTorusAx ;
plPlane.Set( ptUpDw, vtUPDw) ;
if ( ! ClampSegmentOutPlane( plPlane, ptMySeg, vtSeg, dMySegLen))
return 0. ;
// Cerco il punto del segmento ridotto che ha distanza di allontanamento massima
double dEscapeDist = 0. ;
double dStep = max( 0.1 * dMinRad, EPS_SMALL) ;
int nStepNum = max( int( dMySegLen / dStep), 1) ;
dStep = dMySegLen / nStepNum ;
int nMax = -1 ;
for ( int n = 0 ; n <= nStepNum ; ++ n) {
Point3d ptCurPoint = ptMySeg + ( n * dStep) * vtSeg ;
double dCurEscapeDist = ConcaveTorusPointEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ;
if ( dEscapeDist < dCurEscapeDist) {
dEscapeDist = dCurEscapeDist ;
nMax = n ;
}
}
// affino la ricerca (curva parabolica nell'intorno del massimo trovato)
if ( nMax != -1) {
// nuovo passo e punto di partenza
const int SUB_STEPS = 10 ;
double dNewStep = dStep / SUB_STEPS ;
Point3d ptStart = ptMySeg + ( nMax * dStep) * vtSeg ;
// cerco dopo
if ( nMax < nStepNum) {
for ( int j = 1 ; j < SUB_STEPS ; ++ j) {
Point3d ptCurPoint = ptStart + ( j * dNewStep) * vtSeg ;
double dCurEscapeDist = ConcaveTorusPointEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ;
if ( dEscapeDist < dCurEscapeDist)
dEscapeDist = dCurEscapeDist ;
else
break ;
}
}
// cerco prima
if ( nMax > 0) {
for ( int j = 1 ; j < SUB_STEPS ; ++ j) {
Point3d ptCurPoint = ptStart - ( j * dNewStep) * vtSeg ;
double dCurEscapeDist = ConcaveTorusPointEscapeDistGenMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, ptCurPoint, vtMove) ;
if ( dEscapeDist < dCurEscapeDist)
dEscapeDist = dCurEscapeDist ;
else
break ;
}
}
}
return dEscapeDist ;
}
// DISTANZA DI ALLONTANAMENTO PER DISCHI
//----------------------------------------------------------------------------
double
CAvDiskTriangle( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad,
const Triangle3d& trTria, const Vector3d& vtMove)
{
// Direzione di allontanamento diretta come normale al disco
if ( AreSameVectorApprox( vtDiskAx, vtMove)) {
// Valuto le distanze con segno dei vertici dal piano del disco :
// se sono tutte negative non interferiscono.
double dDistV[3] ;
dDistV[0] = PointPlaneSignedDist( trTria.GetP( 0), ptDiskCen, vtMove) ;
dDistV[1] = PointPlaneSignedDist( trTria.GetP( 1), ptDiskCen, vtMove) ;
dDistV[2] = PointPlaneSignedDist( trTria.GetP( 2), ptDiskCen, vtMove) ;
double dMaxDistV = max( dDistV[0], max( dDistV[1], dDistV[2])) ;
if ( dMaxDistV < 0.)
return 0. ;
// Se tutti i punti distano dall'asse di movimento meno del raggio, l'ultimo
// punto di contatto deve essere un vertice del triangolo.
double dSqRad = dDiskRad * dDiskRad ;
bool bInV[3] ;
bInV[0] = ( GetPointLineSqDist( trTria.GetP( 0), ptDiskCen, vtMove) < dSqRad) ;
bInV[1] = ( GetPointLineSqDist( trTria.GetP( 1), ptDiskCen, vtMove) < dSqRad) ;
bInV[2] = ( GetPointLineSqDist( trTria.GetP( 2), ptDiskCen, vtMove) < dSqRad) ;
if ( bInV[0] && bInV[1] && bInV[2])
return max( dMaxDistV, 0.) ;
// Distanza di allontanamento dall'interno del triangolo
double dMaxDistI = DiskTriaInteriorEscapeDistLongMot( ptDiskCen, dDiskRad, trTria, vtMove) ;
if ( dMaxDistI > EPS_SMALL)
return dMaxDistI ;
// Se disco è un punto, inutile fare controlli con i lati e i vertici del triangolo
if ( dDiskRad < EPS_SMALL)
return 0. ;
// Ciclo sui segmenti del triangolo e calcolo la loro distanza di allontanamento, calcolo
// anche la distanza di allontanamento dai vertici distanti dall'asse meno del raggio.
double dMaxDistVS = 0. ;
for ( int nVS = 0 ; nVS < 3 ; ++ nVS) {
// Vertici
if ( bInV[nVS] && dDistV[nVS] > dMaxDistVS)
dMaxDistVS = dDistV[nVS] ;
// Se un lato del triangolo ha entrambi gli estremi con distanza negativa dal piano del disco,
// non può interferire con esso.
int nVE = ( nVS + 1) % 3 ;
if ( dDistV[nVS] < 0 && dDistV[nVE] < 0)
continue ;
// Versore e lunghezza del segmento
Vector3d vtSeg = trTria.GetP( nVE) - trTria.GetP( nVS) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
// Distanza dal piano del segmento corrente
double dCurDist = DiskSegmentEscapeDistLongMot( ptDiskCen, dDiskRad, trTria.GetP( nVS), vtSeg, dSegLen, vtMove) ;
if ( dCurDist > dMaxDistVS)
dMaxDistVS = dCurDist ;
}
return dMaxDistVS ;
}
// Direzione di allontanamento perpendicolare all'asse del disco
else if ( AreOrthoApprox( vtDiskAx, vtMove)) {
// Allontanamento dall'interno del triangolo
double dDist = max( DiskTriaInteriorEscapeDistOrtMot( ptDiskCen, vtDiskAx, dDiskRad, trTria, vtMove), 0.) ;
if ( dDist > EPS_SMALL)
return dDist ;
// Distanza di allontanamento dalla frontiera: non considero i punti
for ( int nV = 0 ; nV < 3 ; ++ nV) {
Vector3d vtSeg = trTria.GetP( ( nV + 1) % 3) - trTria.GetP( nV) ;
double dSegLen = vtSeg.Len() ;
vtSeg /= dSegLen ;
dDist = max( dDist, DiskSegmentEscapeDistOrtMot( ptDiskCen, vtDiskAx, dDiskRad, trTria.GetP( nV), vtSeg, dSegLen, vtMove)) ;
}
return dDist ;
}
// Direzione di allontanamento generica
else if ( vtDiskAx * vtMove > 0.) {
Point3d ptV0 = trTria.GetP( 0) ;
Point3d ptV1 = trTria.GetP( 1) ;
Point3d ptV2 = trTria.GetP( 2) ;
// Distanze di allontanamento dei vertici
double dVertEscapeDist[3] ;
dVertEscapeDist[0] = - ( ( ptDiskCen - ptV0) * vtDiskAx) / ( vtMove * vtDiskAx) ;
dVertEscapeDist[1] = - ( ( ptDiskCen - ptV1) * vtDiskAx) / ( vtMove * vtDiskAx) ;
dVertEscapeDist[2] = - ( ( ptDiskCen - ptV2) * vtDiskAx) / ( vtMove * vtDiskAx) ;
// Triangolo proiettato sul piano del disco
Triangle3d trTriaOnPlane ;
trTriaOnPlane.Set( ptV0 - dVertEscapeDist[0] * vtMove,
ptV1 - dVertEscapeDist[1] * vtMove,
ptV2 - dVertEscapeDist[2] * vtMove) ;
// Se non c'è interferenza fra triangolo proiettato e disco la distanza di fuga è nulla
if ( ! CoplanarDiscTriangleInterference( ptDiskCen, dDiskRad, trTriaOnPlane, TriangleType::CLOSED)) // OPEN EXACT O CLOSED ?
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( dVertEscapeDist[0], dVertEscapeDist[1]), max( dVertEscapeDist[2], 0.)) ;
// Caso generale
// Allontanamento dall'interno
double dEscapeDist = max( DiskTriaInteriorEscapeDistGenMot( ptDiskCen, vtDiskAx, dDiskRad, trTria, vtMove), 0.) ;
// Allontanamento dalla frontiera
Vector3d vtMoveOrt = OrthoCompo( vtMove, vtDiskAx) ;
vtMoveOrt.Normalize() ;
Frame3d DiskFrame ;
DiskFrame.Set( ptDiskCen, vtDiskAx, vtMoveOrt) ;
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 ;
dEscapeDist = max( max( DiskSegmentEscapeDistGenMot( dDiskRad, trTriaLoc.GetP( n), vtSeg, dSegLen, vtMoveLoc),
bInside[n] ? max( dVertEscapeDist[n], 0.) : 0.), dEscapeDist) ;
}
return dEscapeDist ;
}
// Errore
else
return -1. ;
}
//----------------------------------------------------------------------------
double
DiskPointEscapeDistLongMot( const Point3d& ptDiskCen, double dDiskRad,
const Point3d& ptP, const Vector3d& vtMove)
{
if ( GetPointLineSqDist( ptP, ptDiskCen, vtMove) < dDiskRad * dDiskRad - 2 * dDiskRad * EPS_SMALL)
return PointPlaneSignedDist( ptP, ptDiskCen, vtMove) ;
return 0. ;
}
//----------------------------------------------------------------------------
double
DiskSegmentEscapeDistLongMot( const Point3d& ptDiskCen, double dDiskRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen,
const Vector3d& vtMove)
{
// Il disco non può interferire col segmento nel suo moto, se la distanza del
// segmento dall'asse di traslazione è maggiore del raggio.
if ( LineSegmentSqDist( ptDiskCen, vtMove, ptSeg, vtSeg, dSegLen) > dDiskRad * dDiskRad - 2 * dDiskRad * EPS_SMALL)
return 0. ;
// Imposto l'equazione: la distanza quadrata del generico punto della retta associata al
// segmento dall'asse lungo cui si sposta il disco deve valere raggio quadrato del disco.
Vector3d vtLineSegOrt = ( ptSeg - ptDiskCen) - ( ptSeg - ptDiskCen) * vtMove * vtMove ;
Vector3d vtSegOrt = vtSeg - vtSeg * vtMove * vtMove ;
DBLVECTOR vdCoef(3) ;
DBLVECTOR vdRoots ;
vdCoef[0] = vtLineSegOrt.SqLen() - dDiskRad * dDiskRad ;
vdCoef[1] = 2 * vtLineSegOrt * vtSegOrt ;
vdCoef[2] = vtSegOrt.SqLen() ;
// Segmento e asse paralleli
if ( vdCoef[2] < SQ_EPS_ZERO)
return 0. ;
// Soluzione dell'equazione
int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ;
// Ciclo sulle soluzioni per trovare la distanza di allontanamento
double dSegDist = 0. ;
for ( int nSol = 0 ; nSol < nRoot ; ++ nSol) {
// Soluzione interna al segmento
if ( vdRoots[nSol] > 0. && vdRoots[nSol] < dSegLen + EPS_ZERO) {
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
DiskTriaInteriorEscapeDistLongMot( const Point3d& ptDiskCen, double dDiskRad,
const Triangle3d& trTria, const Vector3d& vtMove)
{
// Se disco e triangolo sono complanari
if ( AreSameVectorApprox( vtMove, trTria.GetN())) {
double dDist = max( - PointPlaneSignedDist( ptDiskCen, trTria.GetP( 0), trTria.GetN()), 0.) ;
if ( CoplanarDiscTriangleInterference( ptDiskCen + dDist * vtMove, dDiskRad - EPS_SMALL, trTria, TriangleType::EXACT))
return dDist ;
return 0. ;
}
// Se disco e triangolo sono ortogonali non può esserci contatto con interno
if ( AreOrthoApprox( vtMove, trTria.GetN()))
return 0. ;
// Cerco un punto di contatto nell'interno del triangolo.
double dEscapeDist = 0. ;
// Vettore radiale
Vector3d vtRad = trTria.GetN() - ( trTria.GetN() * vtMove) * vtMove ;
vtRad.Normalize() ;
// Punto candidato all'essere di estremo contatto
Point3d ptStLine = ptDiskCen - ( dDiskRad - EPS_SMALL) * vtRad ;
// Intersezioni con retta
double dDist = ( ( trTria.GetP( 0) - ptStLine) * trTria.GetN()) / ( vtMove * trTria.GetN()) ;
if ( IsPointInsideTriangle( ptStLine + dDist * vtMove, trTria, TriangleType::EXACT))
dEscapeDist = max( dEscapeDist, dDist) ;
return dEscapeDist ;
}
//----------------------------------------------------------------------------
double
DiskPointEscapeDistOrtMot( const Point3d& ptDisc, const Vector3d& vtDiskAx, double dDiskRad,
const Point3d& ptP, const Vector3d& vtMove)
{
// Vettore congiungente il punto da cui si allontana il disco e il centro del medesimo
Vector3d vtP = ptP - ptDisc ;
// Se il punto non appartiene al piano del disco, la distanza di fuga è nulla
Vector3d vtCompOutOfPlane = vtP * vtDiskAx * vtDiskAx ;
if ( vtCompOutOfPlane.SqLen() > EPS_SMALL * EPS_SMALL)
return 0. ;
// Depuro da eventuali componenti fuori dal piano del disco
vtP -= vtCompOutOfPlane ;
double dLenLong = vtP * vtMove ;
Vector3d vtPOrt = vtP - dLenLong * vtMove ;
double dSqLenOrt = vtPOrt * vtPOrt ;
if ( dSqLenOrt > dDiskRad * dDiskRad - 2 * dDiskRad * EPS_SMALL)
return 0. ;
return max( dLenLong + sqrt( max( dDiskRad * dDiskRad - dSqLenOrt, 0.)), 0.) ;
}
//----------------------------------------------------------------------------
// Restituisce la distanza di fuga di un disco da un segmento nel caso di
// moto ortogonale all'asse di simmetria.
// Il disco è descritto dal suo raggio. Il movimento è descritto dal versore
// dell'asse del disco, dal centro nella posizione iniziale e dal
// versore della direzione del moto. Il segmento è descritto da punto iniziale,
// versore direzione e lunghezza.
double
DiskSegmentEscapeDistOrtMot( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad,
const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen, const Vector3d& vtMove)
{
// Se il segmento non è nel piano, il più remoto punto di contatto è,
// se esiete, l'intersezione fra piano e segmento. Il caso in cui il
// segmento giace nel piano non ci interessa
Point3d ptMyDisk = ptDiskCen/* + EPS_SMALL * vtDiskAx*/ ;
if ( ! ( abs( vtSeg * vtDiskAx) < EPS_ZERO)) {
Point3d ptSegEnd = ptSeg + dSegLen * vtSeg ;
double dDotStart = ( ptSeg - ptMyDisk) * vtDiskAx ;
double dDotEnd = ( ptSegEnd - ptMyDisk) * vtDiskAx ;
if ( dDotStart * dDotEnd < 0.) {
double dS = ( ptMyDisk - ptSeg) * vtDiskAx / ( vtSeg * vtDiskAx) ;
Point3d ptInt = ptSeg + dS * vtSeg ;
Vector3d vtOI = ( ptInt - ptMyDisk) ;
double dLongCord = vtOI * vtMove ;
Vector3d vtLong = dLongCord * vtMove ;
Vector3d vtOrt = vtOI - vtLong ;
double dSqOrtLen = vtOrt.SqLen() ;
if ( dSqOrtLen > dDiskRad * dDiskRad - 2 * dDiskRad * EPS_SMALL)
return 0. ;
else
return max( dLongCord + sqrt( max( dDiskRad * dDiskRad - dSqOrtLen, 0.)), 0.) ;
}
else
return 0. ;
}
return 0. ;
}
//----------------------------------------------------------------------------
double
DiskTriaInteriorEscapeDistOrtMot( const Point3d& ptDiskCen, const Vector3d& vtDiskAx, double dDiskRad,
const Triangle3d& trTria, const Vector3d& vtMove)
{
double dDist = 0. ;
// Vettore direzione della retta intersezione fra il piano del triangolo e quello del disco
Vector3d vtLine = vtDiskAx ^ trTria.GetN() ;
// Se i piani sono paralleli, non ci può essere punto di estremo contatto nell'interno del triangolo
if ( ! vtLine.Normalize())
return dDist ;
// Nella configurazione di estremo contatto il disco deve essere tangente a questa retta;
// Il vettore radiale congiungente il centro del disco con il potenziale punto di contatto
Vector3d vtRad = vtLine ^ vtDiskAx ;
// Se il vettore radiale è ortogonale al versore di movimento non può esserci configurazione di estremo contatto.
if ( abs( vtRad * vtMove) < EPS_ZERO)
return dDist ;
// Il versore radiale deve sempre avere puntare contro la direzione di allontanamento
if ( vtRad * vtMove > 0)
vtRad *= - 1 ;
// Punto candidato a essere di estremo contatto
Point3d ptTouch = ptDiskCen + dDiskRad * vtRad ;
// Parametro intersezione del punto candidato ad essere di estremo contatto col piano del triangolo
double dU = ( ( trTria.GetP( 0) - ptTouch) * trTria.GetN()) / ( vtMove * trTria.GetN()) ;
if ( dU < 0.)
return dDist ;
// Contatto all'interno del triangolo
if ( IsPointInsideTriangle( ptTouch + dU * vtMove, trTria, TriangleType::EXACT))
dDist = max( dDist, dU) ;
return dDist ;
}
//----------------------------------------------------------------------------
// 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
DiskSegmentEscapeDistGenMot( double dDiskRad, const Point3d& ptSeg, const Vector3d& vtSeg, double dSegLen,
const Vector3d& vtMove)
{
// Il caso in cui il segmento è parallelo al versore del moto è gestito dalla
// routine per il calcolo della distanza di allontanamento dei punti.
if ( AreSameOrOppositeVectorApprox( vtMove, vtSeg))
return 0. ;
// 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 ;
// Quadrato del raggio
double dSqRad = ( dDiskRad - EPS_SMALL) * ( dDiskRad - EPS_SMALL) ;
// Setto i coeficienti dell'equazione
DBLVECTOR vdCoef(3) ;
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
DBLVECTOR vdRoots ;
int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ;
double dLenCoef = sqrt( 1 + dSqCoef) ;
double dEscapeDist = 0. ;
for ( int n = 0 ; n < nRoot ; ++ n) {
if ( vdRoots[n] > 0 && vdRoots[n] < dSegLen) {
Point3d ptTouch = ptSeg + vdRoots[n] * vtSeg ;
dEscapeDist = max( dEscapeDist, dLenCoef * ptTouch.z) ;
}
}
return dEscapeDist ;
}
//----------------------------------------------------------------------------
double
DiskTriaInteriorEscapeDistGenMot( 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 ( CoplanarDiscTriangleInterference( ptDiskCen + dDist * vtMove, dDiskRad, trTria, TriangleType::EXACT))
return dDist ;
return 0. ;
}
// Se il movimento è nel piano del triangolo non può esserci contatto con interno
if ( AreOrthoApprox( vtMove, trTria.GetN()))
return 0. ;
// Cerco un punto di contatto nell'interno del triangolo.
double dEscapeDist = 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, TriangleType::EXACT))
dEscapeDist = max( dEscapeDist, dDist) ;
return dEscapeDist ;
}
Reference in New Issue View Git Blame Copy Permalink