From 280d414e3011e0d36051a4989a788db20c7bbf42 Mon Sep 17 00:00:00 2001 From: Dario Sassi Date: Mon, 23 Jul 2018 06:06:16 +0000 Subject: [PATCH] EgtGeomKernel : - migliorie varie a Collision Avoidance di Tools. --- CAvToolSurfTm.cpp | 15 ++++++++--- CAvToolSurfTm.h | 1 + CAvToolTriangle.cpp | 64 ++++++++++++++++++++++----------------------- HashGrids2d.cpp | 11 ++++---- Tool.cpp | 20 ++++++++++++-- Tool.h | 3 +++ 6 files changed, 70 insertions(+), 44 deletions(-) diff --git a/CAvToolSurfTm.cpp b/CAvToolSurfTm.cpp index c4ad8b1..5872799 100644 --- a/CAvToolSurfTm.cpp +++ b/CAvToolSurfTm.cpp @@ -16,6 +16,7 @@ #include "CAvToolSurfTm.h" #include "DistPointLine.h" #include "DllMain.h" +#include "/EgtDev/Include/EGnStringUtils.h" using namespace std ; @@ -31,7 +32,7 @@ CreateCAvToolSurfTm( void) // CAvToolSurfTm //---------------------------------------------------------------------------- CAvToolSurfTm::CAvToolSurfTm( void) - : m_pSTm( nullptr), m_Tool( false) + : m_pSTm( nullptr), m_Tool( false), m_nTriaCAv( 0) { } @@ -100,6 +101,7 @@ CAvToolSurfTm::TestPath( PNTULIST& lPntM, const Vector3d& vtDir, double dLinTol) // Ciclo sui punti int nCount = int( lPntM.size()) ; int nCurr = 0 ; + m_nTriaCAv = 0 ; Point3d ptPrev, ptCurr ; auto itPntMPrev = lPntM.end() ; auto itPntMCurr = lPntM.begin() ; @@ -122,7 +124,7 @@ CAvToolSurfTm::TestPath( PNTULIST& lPntM, const Vector3d& vtDir, double dLinTol) ++ itPntMCurr ; // gestione eventi (ogni 200 punti) ++ nCurr ; - if ( ( nCurr % 200) == 0) { + if ( ( nCurr % 50) == 0) { int nRes = ProcessEvents( int( nCurr * 100. / nCount), 0) ; if ( nRes == 0) { // pulisco HashGrid 2d @@ -135,6 +137,9 @@ CAvToolSurfTm::TestPath( PNTULIST& lPntM, const Vector3d& vtDir, double dLinTol) ProcessEvents( 100, 0) ; // pulisco HashGrid 2d m_HGrids.Clear() ; + // Per debug + string sLog = "TriaCav=" + ToString( m_nTriaCAv) ; + LOG_INFO( GetEGkLogger(), sLog.c_str()) ; return true ; } @@ -154,9 +159,11 @@ CAvToolSurfTm::MyTestMidPointHG( PNTULIST& lPntM, const PNTULIST::iterator& itPn double dMidMove = MyTestPositionHG( ptNewMid, vtDir) ; if ( dMidMove < - EPS_SMALL) return false ; + // massima distanza ammissibile + double dMaxSqDist = max( m_Tool.GetRefRadius() * m_Tool.GetRefRadius(), 0.5 * 0.5) ; // se punto medio oltre tolleranza dalla linea tra i punti includenti o linea troppo lunga, devo aggiungerlo if ( abs(( Media( itPntMPrev->first, itPntMCurr->first, 0.5) - ptNewMid) * m_frMove.VersZ()) > 0.5 * dLinTol || - SqDist( itPntMPrev->first, itPntMCurr->first) > m_Tool.GetRadius() * m_Tool.GetRadius()) { + SqDist( itPntMPrev->first, itPntMCurr->first) > dMaxSqDist) { // aggiungo lPntM.emplace( itPntMCurr, ptNewMid, - dMidMove) ; auto itPntMMid = itPntMCurr ; @@ -178,6 +185,7 @@ CAvToolSurfTm::MyTestPosition( Point3d& ptT, const Vector3d& vtDir) for ( int nTria = m_pSTm->GetFirstTriangle( Tria) ; nTria != SVT_NULL ; nTria = m_pSTm->GetNextTriangle( nTria, Tria)) { + ++ m_nTriaCAv ; double dDist = CAvToolTriangle( m_Tool, ptT, vtDir, Tria, m_frMove.VersZ()) ; if ( dDist < - EPS_SMALL) return -1 ; @@ -232,6 +240,7 @@ CAvToolSurfTm::MyTestPositionHG( Point3d& ptT, const Vector3d& vtDir) Triangle3d Tria ; if ( ! m_pSTm->GetTriangle( nT, Tria)) return - 1 ; + ++ m_nTriaCAv ; double dDist = CAvToolTriangle( m_Tool, ptT, vtDir, Tria, m_frMove.VersZ()) ; if ( dDist < - EPS_SMALL) return -1 ; diff --git a/CAvToolSurfTm.h b/CAvToolSurfTm.h index 2b05aa5..fd42100 100644 --- a/CAvToolSurfTm.h +++ b/CAvToolSurfTm.h @@ -53,4 +53,5 @@ class CAvToolSurfTm : public ICAvToolSurfTm const SurfTriMesh* m_pSTm ; HashGrids2d m_HGrids ; Tool m_Tool ; + int m_nTriaCAv ; } ; diff --git a/CAvToolTriangle.cpp b/CAvToolTriangle.cpp index 6ea1ffd..635ed37 100644 --- a/CAvToolTriangle.cpp +++ b/CAvToolTriangle.cpp @@ -291,29 +291,25 @@ CAvToolTriangle( const Tool& tlTool, const Point3d& ptToolOrig, const Vector3d& double CAvSphereTriangle( const Point3d& ptSpheCen, double dSpheRad, const Triangle3d& trTria, const Vector3d& vtMove) { - // Verifica preliminare che la fetta di spazio delimitata dal piano sotto alla sfera rispetto alla direzione + // 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 < 0.) + if ( dMaxDistV < EPS_SMALL) 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 double dPlaneLeakDist = SpherePlaneLeakDist( ptSpheCen, dSpheRad, trTria.GetP( 0), trTria.GetN(), vtMove) ; // Se il punto di tangenza esiste, verifico se è interno al triangolo - if ( dPlaneLeakDist >= 0) { + if ( dPlaneLeakDist >= 0.) { Point3d ptTan = ptSpheCen + dPlaneLeakDist * vtMove - dSpheRad * trTria.GetN() ; // Se il punto è interno abbiamo finito if ( IsPointInsideTriangle( ptTan, trTria)) @@ -340,8 +336,9 @@ double SpherePlaneLeakDist( const Point3d& ptSpheCen, double dSpheRad, const Point3d& ptPlane, const Vector3d& vtPlaneN, const Vector3d& vtMove) { + double dCosPM = vtPlaneN * vtMove ; // Se la direzione di allontanamento sta nel piano - if ( abs( vtPlaneN * vtMove) < EPS_ZERO) { + if ( abs( dCosPM) < EPS_ZERO) { if ( abs( ( ptSpheCen - ptPlane) * vtPlaneN) > dSpheRad - EPS_SMALL) return 0. ; else @@ -349,7 +346,7 @@ SpherePlaneLeakDist( const Point3d& ptSpheCen, double dSpheRad, } // altrimenti ... else { - double dLeakDist = ( dSpheRad - ( ptSpheCen - ptPlane) * vtPlaneN) / ( vtMove * vtPlaneN) ; + double dLeakDist = ( dSpheRad - ( ptSpheCen - ptPlane) * vtPlaneN) / dCosPM ; return max( dLeakDist, 0.) ; } } @@ -395,7 +392,7 @@ SpherePointLeakDist( const Point3d& ptSpheCen, double dSpheRad, const Point3d pt if ( dSqOROrtLen > dSqSphRad + 2 * dSpheRad * EPS_SMALL) return 0. ; else - return max( dDotORMot + sqrt( max( dSpheRad * dSpheRad - dSqOROrtLen, 0.)), 0.) ; + return max( dDotORMot + sqrt( max( dSqSphRad - dSqOROrtLen, 0.)), 0.) ; } //---------------------------------------------------------------------------- @@ -469,9 +466,9 @@ CAvCylinderTriangle( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double d } return dMaxDistVS ; } + // Movimento perpendicolare all'asse del cilindro else if ( nMotionType == 3) { - double dLeakDist = 0. ; // INTERNO : se distanza di allontanamento da interno è positiva, abbiamo finito Point3d ptTopCont, ptBotCont ; @@ -519,6 +516,7 @@ CAvCylinderTriangle( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double d } return dLeakDist ; } + // Movimento generico else return -1. ; @@ -1931,25 +1929,25 @@ IsPointInsideTriangle( const Point3d& ptP, const Triangle3d& trTria) if ( abs( ( ptP - trTria.GetP( 0)) * trTria.GetN()) > EPS_SMALL) return false ; - // Vettori dei lati normalizzati + // Verifico se il punto è a destra del primo lato Vector3d vtV0 = trTria.GetP( 1) - trTria.GetP( 0) ; vtV0.Normalize() ; - Vector3d vtV1 = trTria.GetP( 2) - trTria.GetP( 1) ; - vtV1.Normalize() ; - Vector3d vtV2 = trTria.GetP( 0) - trTria.GetP( 2) ; - vtV2.Normalize() ; - - // E' sufficiente che il segno di un prodotto misto sia negativo per essere fuori double dProd0 = ( vtV0 ^ ( ptP - trTria.GetP( 0))) * trTria.GetN() ; if ( dProd0 < - EPS_SMALL) return false ; + // Verifico se il punto è a destra del secondo lato + Vector3d vtV1 = trTria.GetP( 2) - trTria.GetP( 1) ; + vtV1.Normalize() ; double dProd1 = ( vtV1 ^ ( ptP - trTria.GetP( 1))) * trTria.GetN() ; if ( dProd1 < - EPS_SMALL) return false ; + // Verifico se il punto è a destra del terzo lato + Vector3d vtV2 = trTria.GetP( 0) - trTria.GetP( 2) ; + vtV2.Normalize() ; double dProd2 = ( vtV2 ^ ( ptP - trTria.GetP( 2))) * trTria.GetN() ; if ( dProd2 < - EPS_SMALL) return false ; - + // Punto a sinistra di tutti i lati, quindi interno return true ; } @@ -1969,25 +1967,25 @@ IsPointInsideOpenTriangle( const Point3d& ptP, const Triangle3d& trTria) if ( abs( ( ptP - trTria.GetP( 0)) * trTria.GetN()) > EPS_SMALL) return false ; - // Vettori dei lati normalizzati + // Verifico se il punto è a destra del primo lato Vector3d vtV0 = trTria.GetP( 1) - trTria.GetP( 0) ; vtV0.Normalize() ; - Vector3d vtV1 = trTria.GetP( 2) - trTria.GetP( 1) ; - vtV1.Normalize() ; - Vector3d vtV2 = trTria.GetP( 0) - trTria.GetP( 2) ; - vtV2.Normalize() ; - - // E' sufficiente che il segno di un prodotto misto sia negativo per essere fuori double dProd0 = ( vtV0 ^ ( ptP - trTria.GetP( 0))) * trTria.GetN() ; if ( dProd0 < EPS_SMALL) return false ; + // Verifico se il punto è a destra del secondo lato + Vector3d vtV1 = trTria.GetP( 2) - trTria.GetP( 1) ; + vtV1.Normalize() ; double dProd1 = ( vtV1 ^ ( ptP - trTria.GetP( 1))) * trTria.GetN() ; if ( dProd1 < EPS_SMALL) return false ; + // Verifico se il punto è a destra del terzo lato + Vector3d vtV2 = trTria.GetP( 0) - trTria.GetP( 2) ; + vtV2.Normalize() ; double dProd2 = ( vtV2 ^ ( ptP - trTria.GetP( 2))) * trTria.GetN() ; if ( dProd2 < EPS_SMALL) return false ; - + // Punto a sinistra di tutti i lati, quindi interno return true ; } diff --git a/HashGrids2d.cpp b/HashGrids2d.cpp index cbbbb50..12a9607 100644 --- a/HashGrids2d.cpp +++ b/HashGrids2d.cpp @@ -27,7 +27,7 @@ const size_t occupiedCellsVectorSize = 256 ; const size_t minimalGridDensity = 8 ; const size_t gridActivationThreshold = 64 ; const double hierarchyFactor = 2 ; -const double MIN_CELL_SIZE = 20.0 ; +const double MIN_CELL_SIZE = 5.0 ; //---------------------------------------------------------------------------- // HashGrid2d @@ -447,7 +447,7 @@ HashGrid2d::Enlarge( void) InitNeighborOffsets() ; // ... all previously removed objs are reinserted. - for ( auto p = PObjVecTemp.begin() ; p < PObjVecTemp.end() ; ++ p) { + for ( auto p = PObjVecTemp.begin() ; p < PObjVecTemp.end() ; ++ p) { Add( **p) ; } } @@ -657,7 +657,7 @@ HashGrids2d::Find( const BBox3d& b3Test, INTVECTOR& vnIds) sort( vnIds.begin(), vnIds.end()) ; vnIds.erase( unique( vnIds.begin(), vnIds.end()), vnIds.end()) ; - return ( vnIds.size() > 0) ; + return ( vnIds.size() > 0) ; } @@ -685,7 +685,7 @@ HashGrids2d::addGrid( ObjData& obj) obj.box.GetDiameter( size) ; // If the body is finite in size, it must be assigned to a grid with suitably sized cells. - if ( size > 0) { + if ( size > - EPS_ZERO) { size = max( size, MIN_CELL_SIZE) ; @@ -777,6 +777,5 @@ HashGrids2d::addList( ObjData& obj) obj.pHGrid = nullptr ; obj.nHash = m_nonGridObjs.size() ; obj.nCellId = 1 ; - m_nonGridObjs.push_back( &obj) ; + m_nonGridObjs.push_back( &obj) ; } - diff --git a/Tool.cpp b/Tool.cpp index 38f0325..9106eef 100644 --- a/Tool.cpp +++ b/Tool.cpp @@ -27,7 +27,7 @@ using namespace std ; //---------------------------------------------------------------------------- Tool::Tool( bool bApproxWithLines) : m_dLinTol( LIN_TOL_STD), m_dAngTolDeg( ANG_TOL_APPROX_DEG), m_nType( UNDEF), m_nCurrentNum( 0), m_dHeight( 0), - m_dTipHeight( 0), m_dRadius( 0), m_dRCorner( 0), m_dTipRadius( 0), m_dMrtChsWidth( 0), m_dMrtChsThickness( 0), + m_dTipHeight( 0), m_dRadius( 0), m_dRCorner( 0), m_dTipRadius( 0), m_dRefRadius( 0), m_dMrtChsWidth( 0), m_dMrtChsThickness( 0), m_bApproxWithLines( bApproxWithLines) { } @@ -69,6 +69,7 @@ Tool::SetStdTool( const string& sToolName, double dH, double dR, double dCornR, m_dTipHeight = 0 ; m_dTipRadius = m_dRadius ; m_dRCorner = 0 ; + m_dRefRadius = m_dRadius ; // profilo Point3d pt0( 0, 0, 0) ; Point3d pt1( m_dRadius, 0, 0) ; @@ -88,6 +89,7 @@ Tool::SetStdTool( const string& sToolName, double dH, double dR, double dCornR, m_dTipHeight = dCornR ; m_dTipRadius = dR - dCornR ; m_dRCorner = dCornR ; + m_dRefRadius = m_dRadius ; // profilo Point3d pt0( 0, 0, 0) ; Point3d pt1( m_dRadius, 0, 0) ; @@ -112,6 +114,7 @@ Tool::SetStdTool( const string& sToolName, double dH, double dR, double dCornR, m_dTipHeight = m_dRadius ; m_dTipRadius = 0 ; m_dRCorner = m_dRadius ; + m_dRefRadius = m_dRadius ; // profilo Point3d pt0( 0, 0, 0) ; Point3d pt1( m_dRadius, 0, 0) ; @@ -166,6 +169,8 @@ Tool::SetAdvTool( const string& sToolName, double dH, double dR, if ( m_dRCorner < EPS_SMALL) { m_nType = CONEMILL ; m_dRCorner = 0 ; + // Assegno il raggio di riferimento + m_dRefRadius = m_dRadius * min( 1., m_dRadius / m_dTipHeight) ; // profilo m_Outline.AddPoint( pt0) ; m_Outline.AddLine( pt1) ; @@ -181,6 +186,8 @@ Tool::SetAdvTool( const string& sToolName, double dH, double dR, if ( m_dTipRadius < m_dRadius) { // se punta a sfera if ( m_dTipRadius < EPS_SMALL) { + // Assegno il raggio di riferimento + m_dRefRadius = m_dRCorner ; // circonferenza del corner Point3d ptC( 0, - m_dHeight + m_dRCorner, 0) ; CurveArc cvCirc ; @@ -211,6 +218,8 @@ Tool::SetAdvTool( const string& sToolName, double dH, double dR, } // altrimenti punta a naso di toro else { + // Assegno il raggio di riferimento + m_dRefRadius = m_dTipRadius ; // punto da smussare con corner Point3d ptInt( m_dTipRadius, - m_dHeight, 0) ; // linea sopra il corner @@ -243,6 +252,8 @@ Tool::SetAdvTool( const string& sToolName, double dH, double dR, // il raggio della punta non può essere inferiore al raggio corner if ( m_dTipRadius < m_dRCorner) return false ; + // Assegno il raggio di riferimento + m_dRefRadius = m_dTipRadius ; // circonferenza del corner Point3d ptC( m_dTipRadius - m_dRCorner, - m_dHeight + m_dRCorner, 0) ; CurveArc cvCirc ; @@ -345,6 +356,8 @@ Tool::SetSawTool( const string& sToolName, double dH, double dR, m_Outline.AddLine( pt5) ; } } + // Assegno il raggio di riferimento + m_dRefRadius = dThick ; return SetGenTool( sToolName, &m_Outline, nToolNum) ; } @@ -433,7 +446,10 @@ Tool::SetGenTool( const string& sToolName, const ICurveComposite* pToolOutline, // Assegno le dimensioni dell'utensile m_dHeight = - Bounding.GetMin().y ; m_dRadius = Bounding.GetMax().x ; - + // Assegno il raggio di riferimento se non già assegnato + if ( m_dRefRadius < EPS_SMALL) + m_dRefRadius = 0.25 * m_dRadius ; + return true ; } diff --git a/Tool.h b/Tool.h index 459a5c6..a84d0cd 100644 --- a/Tool.h +++ b/Tool.h @@ -46,6 +46,8 @@ class Tool { return m_dTipRadius ; } double GetCornRadius() const { return m_dRCorner ; } + double GetRefRadius() const + { return m_dRefRadius ; } double GetMrtChsWidth() const { return m_dMrtChsWidth ; } double GetMrtChsThickness() const @@ -84,6 +86,7 @@ class Tool double m_dRadius ; double m_dRCorner ; double m_dTipRadius ; + double m_dRefRadius ; double m_dMrtChsWidth ; double m_dMrtChsThickness ; } ;