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

EgtGeomKernel :

Browse Source 
- migliorie e correzioni a Collision Avoidance
- Intervals con std::vector invece di std::list.
This commit is contained in:
Dario Sassi
2018-07-06 10:10:07 +00:00
parent 73db4fe50c
commit d5dd818493
8 changed files with 209 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CAvToolTriangle.cpp
+74 -44
View File
@@ -30,18 +30,15 @@ GetTopTapFromPrevCurve( const ICurve* pPrevCurve)
return false ;
if ( pPrevCurve->GetType() == CRV_LINE) {
const ICurveLine* pPrevLine = GetCurveLine( pPrevCurve) ;
Point3d ptPrevStart = pPrevLine->GetStart() ;
Point3d ptPrevEnd = pPrevLine->GetEnd() ;
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) ;
Point3d ptPrevStart ; pPrevArc->GetStartPoint( ptPrevStart) ;
Point3d ptPrevEnd ; pPrevArc->GetEndPoint( ptPrevEnd) ;
Point3d ptPrevCen = pPrevArc->GetCenter() ;
double dRadius = pPrevArc->GetRadius() ;
const Point3d& ptPrevCen = pPrevArc->GetCenter() ;
if ( abs( ptPrevCen.x) > EPS_SMALL)
return true ;
}
@@ -56,17 +53,14 @@ GetBotTapFromNextCurve( const ICurve* pNextCurve)
return false ;
if ( pNextCurve->GetType() == CRV_LINE) {
const ICurveLine* pNextLine = GetCurveLine( pNextCurve) ;
Point3d ptNextStart = pNextLine->GetStart() ;
Point3d ptNextEnd = pNextLine->GetEnd() ;
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) ;
Point3d ptNextStart ; pNextArc->GetStartPoint( ptNextStart) ;
Point3d ptNextEnd ; pNextArc->GetEndPoint( ptNextEnd) ;
Point3d ptNextCen = pNextArc->GetCenter() ;
double dRadius = pNextArc->GetRadius() ;
const Point3d& ptNextCen = pNextArc->GetCenter() ;
if ( abs( ptNextCen.x) > EPS_SMALL)
return true ;
}
@@ -83,8 +77,8 @@ CAvToolTriangle( const Tool& tlTool, const Point3d& ptToolOrig, const Vector3d&
const Triangle3d& trTria, const Vector3d& vtMove)
{
// Se avvicinamento non devo fare nulla
if ( vtMove * trTria.GetN() < - EPS_ZERO)
return 0. ;
if ( vtMove * trTria.GetN() < - EPS_ZERO)
return 0. ;
// se utensile cilindrico
if ( tlTool.GetType() == Tool::CYLMILL) {
// parametri geometrici
@@ -180,8 +174,8 @@ CAvToolTriangle( const Tool& tlTool, const Point3d& ptToolOrig, const Vector3d&
if ( pCurve->GetType() == CRV_LINE) {
// Recupero gli estremi
const ICurveLine* pLine = GetCurveLine( pCurve) ;
Point3d ptStart = pLine->GetStart() ;
Point3d ptEnd = pLine->GetEnd() ;
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) {
@@ -219,7 +213,7 @@ CAvToolTriangle( const Tool& tlTool, const Point3d& ptToolOrig, const Vector3d&
const ICurveArc* pArc = GetCurveArc( pCurve) ;
Point3d ptStart ; pArc->GetStartPoint( ptStart) ;
Point3d ptEnd ; pArc->GetEndPoint( ptEnd) ;
Point3d ptCen = pArc->GetCenter() ;
const Point3d& ptCen = pArc->GetCenter() ;
double dRadius = pArc->GetRadius() ;
// Calcolo della distanza di allontanamento del componente corrente
// Sfera
@@ -285,13 +279,23 @@ CAvToolTriangle( const Tool& tlTool, const Point3d& ptToolOrig, const Vector3d&
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 allontanata
Vector3d vtVert0 = ptSpheCen - trTria.GetP( 0) ;
Vector3d vtVert1 = ptSpheCen - trTria.GetP( 1) ;
Vector3d vtVert2 = ptSpheCen - trTria.GetP( 2) ;
if ( vtVert0 * trTria.GetN() > dSpheRad - EPS_SMALL &&
vtVert1 * trTria.GetN() > dSpheRad - EPS_SMALL &&
vtVert2 * trTria.GetN() > dSpheRad - EPS_SMALL)
// Verifica preliminare che 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 < 0.)
return 0. ;
// Se la sfera sta già tutta dalla parte esterna del triangolo, non va ulteriormente allontanata
// Posizione del centro sfera rispetto ai vertici del triangolo
Vector3d vtVe0Cen = ptSpheCen - trTria.GetP( 0) ;
Vector3d vtVe1Cen = ptSpheCen - trTria.GetP( 1) ;
Vector3d vtVe2Cen = ptSpheCen - trTria.GetP( 2) ;
if ( vtVe0Cen * trTria.GetN() > dSpheRad - EPS_SMALL &&
vtVe1Cen * trTria.GetN() > dSpheRad - EPS_SMALL &&
vtVe2Cen * trTria.GetN() > dSpheRad - EPS_SMALL)
return 0. ;
// Valuto Tangenza col piano
@@ -411,9 +415,9 @@ CAvCylinderTriangle( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double d
// Valuto le distanze con segno dei vertici dal piano del disco :
// se sono tutte negative non interferiscono.
double dDistV[3] ;
dDistV[0] = PointPlaneSignedDist( trTria.GetP( 0), ptBase, vtCylAx) ;
dDistV[1] = PointPlaneSignedDist( trTria.GetP( 1), ptBase, vtCylAx) ;
dDistV[2] = PointPlaneSignedDist( trTria.GetP( 2), ptBase, vtCylAx) ;
dDistV[0] = PointPlaneSignedDist( trTria.GetP( 0), ptBase, vtMove) ;
dDistV[1] = PointPlaneSignedDist( trTria.GetP( 1), ptBase, vtMove) ;
dDistV[2] = PointPlaneSignedDist( trTria.GetP( 2), ptBase, vtMove) ;
double dMaxDistV = max( dDistV[0], max( dDistV[1], dDistV[2])) ;
if ( dMaxDistV < 0.)
return 0. ;
@@ -426,6 +430,10 @@ CAvCylinderTriangle( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double d
bInV[2] = ( GetPointLineSqDist( trTria.GetP( 2), ptBase, vtMove) < dSqRad) ;
if ( bInV[0] && bInV[1] && bInV[2])
return max( dMaxDistV, 0.) ;
// Distanza di allontanamento dall'interno del triangolo
double dMaxDistI = DiskTriaInteriorLeakDistLongMot( ptBase, dRad, trTria, vtMove) ;
if ( dMaxDistI > EPS_SMALL)
return dMaxDistI ;
// 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. ;
@@ -447,9 +455,7 @@ CAvCylinderTriangle( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double d
if ( dCurDist > dMaxDistVS)
dMaxDistVS = dCurDist ;
}
// Distanza di allontanamento dall'interno del triangolo
double dMaxDistI = DiskTriaInteriorLeakDistLongMot( ptBase, dRad, trTria, vtMove) ;
return max( dMaxDistVS, dMaxDistI) ;
return dMaxDistVS ;
}
// Movimento perpendicolare all'asse del cilindro
else if ( nMotionType == 3) {
@@ -619,6 +625,21 @@ CAvTrConeTriangle( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double
{
// 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 ptBase = ptMinBase ;
if ( AreOppositeVectorApprox( vtTrConeAx, vtMove))
ptBase += dTrConeH * vtTrConeAx ;
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 dInnLeakDist = TrConeTriangleInteriorLeakDistLongMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH,
trTria, vtMove) ;
if ( dInnLeakDist > EPS_SMALL)
return dInnLeakDist ;
// Distanza di allontanamento dai vertici e lati
double dOutLeakDist = 0 ;
for ( int nV = 0 ; nV < 3 ; ++ nV) {
@@ -633,12 +654,10 @@ CAvTrConeTriangle( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double
double dCurSegDist = TrConeSegmentLeakDistLongMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR,
dTrConeH, trTria.GetP( nV), vtSeg, dSegLen, vtMove) ;
dOutLeakDist = max( dCurSegDist, dOutLeakDist) ;
}
double dInnLeakDist = TrConeTriangleInteriorLeakDistLongMot( ptMinBase, vtTrConeAx, dMinBaseR, dMaxBaseR, dTrConeH,
trTria, vtMove) ;
return max( dOutLeakDist, dInnLeakDist) ;
}
return dOutLeakDist ;
}
// allontanamento in direzione ortogonale all'asse del cono
// Allontanamento in direzione ortogonale all'asse del cono
else if ( AreOrthoApprox( vtTrConeAx, vtMove)) {
double dOutLeakDist = 0. ;
// Distanza di allontanamento da vertici e lati
@@ -659,9 +678,9 @@ CAvTrConeTriangle( const Point3d& ptMinBase, const Vector3d& vtTrConeAx, double
trTria, vtMove) ;
return max( dOutLeakDist, dInnLeakDist) ;
}
// altri casi
// Movimento generico
else
return 0. ;
return -1 ;
}
//----------------------------------------------------------------------------
@@ -906,6 +925,19 @@ CAvTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double d
{
// Allontanamento con direzione coincidente con l'asse del toro
if ( AreSameOrOppositeVectorApprox( vtTorusAx, 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 ptBase = ptTorusCen - dMinRad * vtMove ;
double dMaxDistV = PointPlaneSignedDist( trTria.GetP( 0), ptBase, vtMove) ;
dMaxDistV = max( dMaxDistV, PointPlaneSignedDist( trTria.GetP( 1), ptBase, vtMove)) ;
dMaxDistV = max( dMaxDistV, PointPlaneSignedDist( trTria.GetP( 2), ptBase, vtMove)) ;
if ( dMaxDistV < 0.)
return 0. ;
// Distanza di allontanamento dall'interno
double dInLeakDist = TorusTriangleInteriorLeakDistLongMot( ptTorusCen, dMaxRad, dMinRad,
trTria, vtMove) ;
if ( dInLeakDist > EPS_SMALL)
return dInLeakDist ;
// Distanza di allontanamento dai lati
double dOutLeakDist = 0. ;
for ( int nV = 0 ; nV < 3 ; ++ nV) {
@@ -921,11 +953,9 @@ CAvTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double d
dCurDist > dOutLeakDist)
dOutLeakDist = max( dOutLeakDist, dCurDist) ;
}
double dInLeakDist = TorusTriangleInteriorLeakDistLongMot( ptTorusCen, dMaxRad, dMinRad,
trTria, vtMove) ;
return max( dInLeakDist, dOutLeakDist) ;
return dOutLeakDist ;
}
// Allontanamento in direzione ortogonale all'asse del toro
// Allontanamento in direzione ortogonale all'asse del toro
else if ( AreOrthoApprox( vtTorusAx, vtMove)) {
// Distanza di allontanamento dai lati
double dOutLeakDist = 0. ;
@@ -940,9 +970,9 @@ CAvTorusTriangle( const Point3d& ptTorusCen, const Vector3d& vtTorusAx, double d
double dInLeakDist = TorusTriangleInteriorLeakDistOrtMot( ptTorusCen, vtTorusAx, dMaxRad, dMinRad, trTria, vtMove) ;
return max( dOutLeakDist, dInLeakDist) ;
}
// Casi non gestiti
// Movimento generico
else
return 0. ;
return -1 ;
}
//----------------------------------------------------------------------------
@@ -1029,7 +1059,7 @@ TorusTriangleInteriorLeakDistLongMot( const Point3d& ptTorusCen, double dMaxRad,
// Nessuna interferenza
return 0. ;
}
// Altrimenti individuo le due possibili circonferenzecon cui può avvenire il contatto
// Altrimenti individuo le due possibili circonferenze con cui può avvenire il contatto
Vector3d vtRadial = trTria.GetN() - trTria.GetN() * vtMove * vtMove ;
vtRadial.Normalize() ;
Point3d ptDiskPlus = ptTorusCen + dMaxRad * vtRadial ;