From 171c473e5b8ef3391eac2b5eff302f2a5f967575 Mon Sep 17 00:00:00 2001 From: Dario Sassi Date: Wed, 22 May 2024 08:20:56 +0200 Subject: [PATCH] EgtMachKernel : - nel calcolo degli assi macchina su percorsi ora si infittiscono opportunamente i punti quando necessario a 5 assi. --- GenMachining.cpp | 11 + GenMachining.h | 1 + Machine.h | 2 +- Operation.cpp | 555 +++++++++++++++++++++++++++++++---------------- Operation.h | 6 + 5 files changed, 386 insertions(+), 189 deletions(-) diff --git a/GenMachining.cpp b/GenMachining.cpp index 7ecae39..bebb25c 100644 --- a/GenMachining.cpp +++ b/GenMachining.cpp @@ -964,3 +964,14 @@ GenMachining::VerifyGeometry( SelData Id, int& nSubs, int& nType) else return false ; } + +//---------------------------------------------------------------------------- +double +GenMachining::GetApproxLinTol( void) const +{ + double dLinTol ; + if ( GetValInNotes( m_Params.m_sUserNotes, "LinTol", dLinTol)) + return dLinTol ; + else + return Operation::GetApproxLinTol() ; +} diff --git a/GenMachining.h b/GenMachining.h index d0ad5df..23c6c97 100644 --- a/GenMachining.h +++ b/GenMachining.h @@ -67,6 +67,7 @@ class GenMachining : public Machining private : bool VerifyGeometry( SelData Id, int& nSubs, int& nType) ; + double GetApproxLinTol( void) const override ; private : double GetSpeed() const diff --git a/Machine.h b/Machine.h index 9e6f0d7..f581845 100644 --- a/Machine.h +++ b/Machine.h @@ -147,7 +147,7 @@ class Machine bool GetNoseFromPositions( double dX, double dY, double dZ, const DBLVECTOR& vAng, Point3d& ptNose) const ; bool GetTipFromPositions( double dX, double dY, double dZ, const DBLVECTOR& vAng, - bool bBottom, bool bOverall, Point3d& ptTip) const ; + bool bOverall, bool bBottom, Point3d& ptTip) const ; bool GetToolDirFromAngles( const DBLVECTOR& vAng, Vector3d& vtDir) const ; bool GetAuxDirFromAngles( const DBLVECTOR& vAng, Vector3d& vtDir) const ; bool GetPartDirFromAngles( const Vector3d& vtPart, const DBLVECTOR& vAng, Vector3d& vtDir) const ; diff --git a/Operation.cpp b/Operation.cpp index d336d83..f9cbbf9 100644 --- a/Operation.cpp +++ b/Operation.cpp @@ -26,6 +26,7 @@ #include "/EgtDev/Include/EGkCurveArc.h" #include "/EgtDev/Include/EGkArcSpecial.h" #include "/EgtDev/Include/EGkCurveComposite.h" +#include "/EgtDev/Include/EGkDistPointLine.h" #include "/EgtDev/Include/EGkDistPointCurve.h" #include "/EgtDev/Include/EGkIntersCurves.h" #include "/EgtDev/Include/EGkIntersLineSurfTm.h" @@ -1857,13 +1858,16 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, { if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr) return false ; - // massima variazione secondo asse rotante per continuità - double dMaxDeltaR2 = 30 ; + Machine* pMachine = m_pMchMgr->GetCurrMachine() ; // predispongo variabile per valori assi DBLVECTOR vAxVal ; vAxVal.reserve( 8) ; - // variabili per coordinate lineari precedenti - double dXprec, dYprec, dZprec ; + // valori del precedente + Point3d ptPrec ; + Vector3d vtDirPrec ; + Vector3d vtAuxPrec ; + Vector3d vtCorrPrec ; + DBLVECTOR vAxPrec ; // ciclo su tutte le entità del percorso CL nOutStrC = 0 ; bool bOk = true ; @@ -1881,154 +1885,24 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, LOG_ERROR( GetEMkLogger(), "Error : first move in ClPath is an arc") return false ; } - // calcolo degli assi rotanti della macchina - int nRStat ; - DBLVECTOR vAng1, vAng2 ; - bool bROk = m_pMchMgr->GetCalcAngles( pCamData->GetToolDir(), pCamData->GetAuxDir(), nRStat, vAng1, vAng2) ; - if ( ! bROk || nRStat == 0) { + // calcolo gli assi rotanti della macchina + Vector3d vtDir = pCamData->GetToolDir() ; + Vector3d vtAux = pCamData->GetAuxDir() ; + Vector3d vtCorr = pCamData->GetCorrDir() ; + DBLVECTOR vAxRot ; + if ( ! CalculateRotAxesValues( bFirst, vtDir, vtAux, dRot1W, + bMaxDeltaR2OnFirst, bRotContOnNext, dAngDeltaMinForHome, vAxRotHome, vAxRotPrec, vAxRot)) { pCamData->SetAxes( CamData::AS_DIR_ERR, vAxVal) ; LOG_ERROR( GetEMkLogger(), "Error : tool direction unreachable") return false ; } - if ( abs( nRStat) == 1) { - // se primo movimento - if ( bFirst) { - // porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri - for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ; - if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ; - } - // se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente, limitandolo alla corsa - if ( nRStat < 0 && vAng1.size() >= 1) { - for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { - // ignoro gli assi bloccati - if ( m_pMchMgr->IsKinematicRotAxisBlocked( i)) - continue ; - // assegno il precedente ed esco - vAng1[i] = vAxRotPrec[i] ; - m_pMchMgr->LimitAngleToStroke( i, vAng1[i]) ; - break ; - } - } - } - // per movimenti successivi - else { - // scelgo gli angoli più vicini, per continuità non applico offset per stare nelle corse - for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { - if ( bRotContOnNext) - vAng1[i] = AngleNearAngle( vAng1[i], vAxRotPrec[i]) ; - else - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ; - if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ; - } - // se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente - if ( nRStat < 0 && vAng1.size() >= 1) { - for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { - // ignoro gli assi bloccati - if ( m_pMchMgr->IsKinematicRotAxisBlocked( i)) - continue ; - // assegno il precedente ed esco - vAng1[i] = vAxRotPrec[i] ; - break ; - } - } - } - } - if ( abs( nRStat) == 2) { - bool bAng1 = true ; - bool bAng2 = true ; - // se primo movimento - if ( bFirst) { - // porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri - for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ; - if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ; - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ; - if ( abs( vAng2[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ; - } - // se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente, limitandolo alla corsa - if ( nRStat < 0 && vAng1.size() >= 1) { - for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { - // ignoro gli assi bloccati - if ( m_pMchMgr->IsKinematicRotAxisBlocked( i)) - continue ; - // assegno il precedente ed esco - vAng1[i] = vAxRotPrec[i] ; - m_pMchMgr->LimitAngleToStroke( i, vAng1[i]) ; - vAng2[i] = vAng1[i] ; - break ; - } - } - } - else { - // scelgo gli angoli più vicini, per continuità non applico offset per stare nelle corse - for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { - if ( bRotContOnNext) - vAng1[i] = AngleNearAngle( vAng1[i], vAxRotPrec[i]) ; - else - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ; - if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ; - if ( bRotContOnNext) - vAng2[i] = AngleNearAngle( vAng2[i], vAxRotPrec[i]) ; - else - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ; - if ( abs( vAng2[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ; - } - // se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente - if ( nRStat < 0 && vAng1.size() >= 1) { - for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { - // ignoro gli assi bloccati - if ( m_pMchMgr->IsKinematicRotAxisBlocked( i)) - continue ; - // assegno il precedente ed esco - vAng1[i] = vAxRotPrec[i] ; - vAng2[i] = vAng1[i] ; - break ; - } - } - // verifico che le soluzioni siano nelle corse - for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { - if ( ! m_pMchMgr->VerifyAngleOutstroke( i, vAng1[i])) - bAng1 = false ; - if ( ! m_pMchMgr->VerifyAngleOutstroke( i, vAng2[i])) - bAng2 = false ; - } - } - // scelgo la soluzione più vicina alla precedente - double dDelta1 = 0 ; - double dDelta2 = 0 ; - for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { - // ignoro gli assi bloccati - if ( m_pMchMgr->IsKinematicRotAxisBlocked( i)) - continue ; - // calcolo i delta asse con eventuale peso - bool bFirst = ( i == 0) ; - dDelta1 += abs( vAng1[i] - vAxRotPrec[i]) * ( bFirst ? dRot1W : 1) ; - dDelta2 += abs( vAng2[i] - vAxRotPrec[i]) * ( bFirst ? dRot1W : 1) ; - } - if ( bAng2 && ( dDelta2 < dDelta1 - EPS_ANG_SMALL || ! bAng1)) - swap( vAng1, vAng2) ; - // se imposto limite su variazione di secondo asse rotante - if ( ( bMaxDeltaR2OnFirst || ! bFirst) && dMaxDeltaR2 > EPS_ANG_SMALL && vAng1.size() >= 2) { - double dR2Diff1 = vAng1[1] - vAxRotPrec[1] ; - double dR2Diff2 = vAng2[1] - vAxRotPrec[1] ; - if ( abs( dR2Diff1) > abs( dR2Diff2) && abs( dR2Diff1) > dMaxDeltaR2) - swap( vAng1, vAng2) ; - } - } // ricavo posizione con eventuali modifiche dipendenti dalla lavorazione Point3d ptP = pCamData->GetEndPoint() ; AdjustEndPointForAxesCalc( pCamData, ptP) ; // calcolo gli assi lineari della macchina int nLStat ; double dX, dY, dZ ; - bool bLOk = m_pMchMgr->GetCalcPositions( ptP, vAng1, nLStat, dX, dY, dZ) ; + bool bLOk = m_pMchMgr->GetCalcPositions( ptP, vAxRot, nLStat, dX, dY, dZ) ; if ( ! bLOk || nLStat != 0) { bOk = false ; pCamData->SetAxes( CamData::AS_ERR, vAxVal) ; @@ -2039,11 +1913,11 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, vAxVal.emplace_back( dX) ; vAxVal.emplace_back( dY) ; vAxVal.emplace_back( dZ) ; - for ( auto dAng : vAng1) + for ( auto dAng : vAxRot) vAxVal.emplace_back( dAng) ; // verifico i limiti di corsa degli assi int nStat ; - bool bOsOk = m_pMchMgr->VerifyOutstroke( dX, dY, dZ, vAng1, false, nStat) ; + bool bOsOk = m_pMchMgr->VerifyOutstroke( dX, dY, dZ, vAxRot, false, nStat) ; if ( ! bOsOk || nStat != 0) { bOk = false ; pCamData->SetAxes( CamData::AS_OUTSTROKE, vAxVal) ; @@ -2057,47 +1931,65 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, LOG_INFO( GetEMkLogger(), sInfo.c_str()) // calcolo e salvo il valore dell'asse ausiliario di testa riportato sul grezzo Vector3d vtBackAux ; - m_pMchMgr->GetCurrMachine()->GetBackAuxDirFromAngles( vAng1, vtBackAux) ; + pMachine->GetBackAuxDirFromAngles( vAxRot, vtBackAux) ; pCamData->SetBackAuxDir( vtBackAux) ; // imposto stato bFirst = false ; - // memorizzo i valori degli assi lineari come nuovi precedenti - dXprec = dX ; - dYprec = dY ; - dZprec = dZ ; + // memorizzo i precedenti + ptPrec = ptP ; + vtDirPrec = vtDir ; + vtAuxPrec = vtAux ; + vtCorrPrec = vtCorr ; + vAxPrec = vAxVal ; // memorizzo i valori degli angoli come nuovi precedenti - vAxRotPrec = vAng1 ; + vAxRotPrec = vAxRot ; continue ; } // salvo i valori degli assi pCamData->SetAxes( CamData::AS_OK, vAxVal) ; // calcolo e salvo il valore dell'asse ausiliario di testa riportato sul grezzo Vector3d vtBackAux ; - m_pMchMgr->GetCurrMachine()->GetBackAuxDirFromAngles( vAng1, vtBackAux) ; + pMachine->GetBackAuxDirFromAngles( vAxRot, vtBackAux) ; pCamData->SetBackAuxDir( vtBackAux) ; - // se esiste precedente, corrente è linea ed esistono aree protette, devo verificare anche i punti intermedi - if ( ! bFirst && pCamData->IsLine() && m_pMchMgr->ExistProtectedAreas()) { - // verifico i limiti di corsa dei punti lungo la linea - const int NUM_VERIF_STEP = 16 ; - Point3d ptPrec( dXprec, dYprec, dZprec) ; - Point3d ptP( dX, dY, dZ) ; - for ( int i = 1 ; i < NUM_VERIF_STEP ; ++ i) { - double dCoeff = double( i) / NUM_VERIF_STEP ; - Point3d ptCurr = Media( ptPrec, ptP, dCoeff) ; - DBLVECTOR vAng( vAng1.size()) ; - for ( size_t i = 0 ; i < vAng1.size() ; ++ i) - vAng[i] = vAxRotPrec[i] * ( 1 - dCoeff) + vAng1[i] * dCoeff ; - int nStat ; - bool bOsOk = m_pMchMgr->VerifyOutstroke( ptCurr.x, ptCurr.y, ptCurr.z, vAng, false, nStat) ; - if ( ! bOsOk || nStat != 0) { - bOk = false ; - pCamData->SetAxes( CamData::AS_OUTSTROKE, vAxVal) ; - string sInfo = "Outstroke : " + m_pMchMgr->GetOutstrokeInfo() ; - LOG_INFO( GetEMkLogger(), sInfo.c_str()) + // se punto finale di linea + if ( ! bFirst && pCamData->IsLine()) { + // Verifica ricorsiva del punto medio + bool bMidAdded ; + bool bAxesError ; + if ( ! VerifyLineMidPoint( ptPrec, vtDirPrec, vtAuxPrec, vtCorrPrec, vAxPrec, + ptP, vtDir, vtAux, vtCorr, vAxVal, + 0, nEntId, dRot1W, bMidAdded, bAxesError)) { + LOG_ERROR( GetEMkLogger(), "Error : VerifyLineMidPoint of CalculateClPathAxesValues failed") + return false ; + } + if ( bAxesError) { + bOk = false ; + continue ; + } + // Se esistono aree protette, devo comunque verificare i punti intermedi + if ( ! bMidAdded && m_pMchMgr->ExistProtectedAreas()) { + // verifico i limiti di corsa dei punti lungo la linea + const int NUM_VERIF_STEP = 16 ; + Point3d ptPrec( vAxPrec[0], vAxPrec[1], vAxPrec[2]) ; + Point3d ptP( dX, dY, dZ) ; + for ( int i = 1 ; i < NUM_VERIF_STEP ; ++ i) { + double dCoeff = double( i) / NUM_VERIF_STEP ; + Point3d ptCurr = Media( ptPrec, ptP, dCoeff) ; + DBLVECTOR vAng( vAxRot.size()) ; + for ( size_t i = 0 ; i < vAxRot.size() ; ++ i) + vAng[i] = vAxRotPrec[i] * ( 1 - dCoeff) + vAxRot[i] * dCoeff ; + int nStat ; + bool bOsOk = m_pMchMgr->VerifyOutstroke( ptCurr.x, ptCurr.y, ptCurr.z, vAng, false, nStat) ; + if ( ! bOsOk || nStat != 0) { + bOk = false ; + pCamData->SetAxes( CamData::AS_OUTSTROKE, vAxVal) ; + string sInfo = "Outstroke : " + m_pMchMgr->GetOutstrokeInfo() ; + LOG_INFO( GetEMkLogger(), sInfo.c_str()) + } } } } - // se arco devo calcolarne il centro in assi macchina + // se punto finale di arco -> devo calcolarne il centro in assi macchina else if ( pCamData->IsArc()) { // devo lavorare con arco schiacciato nel suo piano Point3d ptCen = pCamData->GetCenter() ; @@ -2109,9 +2001,9 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, Point3d ptMid = ptP - dDeltaN * vtN ; ptMid.Rotate( ptCen, vtN, - dAngCen / 2) ; // determino i valori degli assi al punto medio - DBLVECTOR vAngMid( vAng1.size()) ; - for ( size_t i = 0 ; i < vAng1.size() ; ++ i) - vAngMid[i] = ( vAxRotPrec[i] + vAng1[i]) / 2 ; + DBLVECTOR vAngMid( vAxRot.size()) ; + for ( size_t i = 0 ; i < vAxRot.size() ; ++ i) + vAngMid[i] = ( vAxRotPrec[i] + vAxRot[i]) / 2 ; int nLmidStat ; double dXmid, dYmid, dZmid ; bool bLmidOk = m_pMchMgr->GetCalcPositions( ptMid, vAngMid, nLmidStat, dXmid, dYmid, dZmid) ; @@ -2123,9 +2015,9 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, // ricavo punto finale nel piano dell'arco Point3d ptEnd = ptP - dDeltaN * vtN ; // determino i valori degli assi al punto finale - DBLVECTOR vAngEnd( vAng1.size()) ; - for ( size_t i = 0 ; i < vAng1.size() ; ++ i) - vAngEnd[i] = vAng1[i] ; + DBLVECTOR vAngEnd( vAxRot.size()) ; + for ( size_t i = 0 ; i < vAxRot.size() ; ++ i) + vAngEnd[i] = vAxRot[i] ; int nLendStat ; double dXend, dYend, dZend ; bool bLendOk = m_pMchMgr->GetCalcPositions( ptEnd, vAngEnd, nLendStat, dXend, dYend, dZend) ; @@ -2135,7 +2027,7 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, continue ; } // punti espressi in assi macchina - Point3d ptP1M( dXprec, dYprec, dZprec) ; + Point3d ptP1M( vAxPrec[0], vAxPrec[1], vAxPrec[2]) ; Point3d ptP2M( dXmid, dYmid, dZmid) ; Point3d ptP3M( dXend, dYend, dZend) ; // se coincidono @@ -2178,14 +2070,14 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, Point3d ptCen = pCamData->GetAxesCen() ; double dAngCenStep = pCamData->GetAxesAngCen() / NUM_VERIF_STEP ; Vector3d vtN = pCamData->GetAxesNormDir() ; - Vector3d vtCurr = Point3d( dXprec, dYprec, dZprec) - ptCen ; + Vector3d vtCurr = Point3d( vAxPrec[0], vAxPrec[1], vAxPrec[2]) - ptCen ; for ( int i = 1 ; i < NUM_VERIF_STEP ; ++ i) { vtCurr.Rotate( vtN, dAngCenStep) ; double dCoeff = double( i) / NUM_VERIF_STEP ; Point3d ptCurr = ptCen + vtCurr + vtN * dDeltaN * dCoeff ; - DBLVECTOR vAng( vAng1.size()) ; - for ( size_t i = 0 ; i < vAng1.size() ; ++ i) - vAng[i] = vAxRotPrec[i] * ( 1 - dCoeff) + vAng1[i] * dCoeff ; + DBLVECTOR vAng( vAxRot.size()) ; + for ( size_t i = 0 ; i < vAxRot.size() ; ++ i) + vAng[i] = vAxRotPrec[i] * ( 1 - dCoeff) + vAxRot[i] * dCoeff ; int nStat ; bool bOsOk = m_pMchMgr->VerifyOutstroke( ptCurr.x, ptCurr.y, ptCurr.z, vAng, false, nStat) ; if ( ! bOsOk || nStat != 0) { @@ -2210,17 +2102,304 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, } } bFirst = false ; - // memorizzo i valori degli assi lineari come nuovi precedenti - dXprec = dX ; - dYprec = dY ; - dZprec = dZ ; - // memorizzo i valori degli angoli come nuovi precedenti - vAxRotPrec = vAng1 ; + // memorizzo i precedenti + ptPrec = ptP ; + vtDirPrec = vtDir ; + vtAuxPrec = vtAux ; + vtCorrPrec = vtCorr ; + vAxPrec = vAxVal ; + vAxRotPrec = vAxRot ; } return bOk ; } +//---------------------------------------------------------------------------- +bool +Operation::CalculateRotAxesValues( bool bFirst, const Vector3d& vtTool, const Vector3d& vtAux, + double dRot1W, bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome, + const DBLVECTOR& vAxRotHome, const DBLVECTOR& vAxRotPrec, DBLVECTOR& vAxRot) +{ + // massima variazione secondo asse rotante per continuità + const double dMaxDeltaR2 = 30 ; + // calcolo degli assi rotanti della macchina + int nRStat ; + DBLVECTOR vAng1, vAng2 ; + bool bROk = m_pMchMgr->GetCalcAngles( vtTool, vtAux, nRStat, vAng1, vAng2) ; + if ( ! bROk || nRStat == 0) + return false ; + if ( abs( nRStat) == 1) { + // se primo movimento + if ( bFirst) { + // porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri + for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { + m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ; + if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) + m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ; + } + // se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente, limitandolo alla corsa + if ( nRStat < 0 && vAng1.size() >= 1) { + for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { + // ignoro gli assi bloccati + if ( m_pMchMgr->IsKinematicRotAxisBlocked( i)) + continue ; + // assegno il precedente ed esco + vAng1[i] = vAxRotPrec[i] ; + m_pMchMgr->LimitAngleToStroke( i, vAng1[i]) ; + break ; + } + } + } + // per movimenti successivi + else { + // scelgo gli angoli più vicini, per continuità non applico offset per stare nelle corse + for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { + if ( bRotContOnNext) + vAng1[i] = AngleNearAngle( vAng1[i], vAxRotPrec[i]) ; + else + m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ; + if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) + m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ; + } + // se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente + if ( nRStat < 0 && vAng1.size() >= 1) { + for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { + // ignoro gli assi bloccati + if ( m_pMchMgr->IsKinematicRotAxisBlocked( i)) + continue ; + // assegno il precedente ed esco + vAng1[i] = vAxRotPrec[i] ; + break ; + } + } + } + } + if ( abs( nRStat) == 2) { + bool bAng1 = true ; + bool bAng2 = true ; + // se primo movimento + if ( bFirst) { + // porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri + for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { + m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ; + if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) + m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ; + m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ; + if ( abs( vAng2[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) + m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ; + } + // se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente, limitandolo alla corsa + if ( nRStat < 0 && vAng1.size() >= 1) { + for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { + // ignoro gli assi bloccati + if ( m_pMchMgr->IsKinematicRotAxisBlocked( i)) + continue ; + // assegno il precedente ed esco + vAng1[i] = vAxRotPrec[i] ; + m_pMchMgr->LimitAngleToStroke( i, vAng1[i]) ; + vAng2[i] = vAng1[i] ; + break ; + } + } + } + else { + // scelgo gli angoli più vicini, per continuità non applico offset per stare nelle corse + for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { + if ( bRotContOnNext) + vAng1[i] = AngleNearAngle( vAng1[i], vAxRotPrec[i]) ; + else + m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ; + if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) + m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ; + if ( bRotContOnNext) + vAng2[i] = AngleNearAngle( vAng2[i], vAxRotPrec[i]) ; + else + m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ; + if ( abs( vAng2[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) + m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ; + } + // se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente + if ( nRStat < 0 && vAng1.size() >= 1) { + for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { + // ignoro gli assi bloccati + if ( m_pMchMgr->IsKinematicRotAxisBlocked( i)) + continue ; + // assegno il precedente ed esco + vAng1[i] = vAxRotPrec[i] ; + vAng2[i] = vAng1[i] ; + break ; + } + } + // verifico che le soluzioni siano nelle corse + for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { + if ( ! m_pMchMgr->VerifyAngleOutstroke( i, vAng1[i])) + bAng1 = false ; + if ( ! m_pMchMgr->VerifyAngleOutstroke( i, vAng2[i])) + bAng2 = false ; + } + } + // scelgo la soluzione più vicina alla precedente + double dDelta1 = 0 ; + double dDelta2 = 0 ; + for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { + // ignoro gli assi bloccati + if ( m_pMchMgr->IsKinematicRotAxisBlocked( i)) + continue ; + // calcolo i delta asse con eventuale peso + bool bFirst = ( i == 0) ; + dDelta1 += abs( vAng1[i] - vAxRotPrec[i]) * ( bFirst ? dRot1W : 1) ; + dDelta2 += abs( vAng2[i] - vAxRotPrec[i]) * ( bFirst ? dRot1W : 1) ; + } + if ( bAng2 && ( dDelta2 < dDelta1 - EPS_ANG_SMALL || ! bAng1)) + swap( vAng1, vAng2) ; + // se imposto limite su variazione di secondo asse rotante + if ( ( bMaxDeltaR2OnFirst || ! bFirst) && dMaxDeltaR2 > EPS_ANG_SMALL && vAng1.size() >= 2) { + double dR2Diff1 = vAng1[1] - vAxRotPrec[1] ; + double dR2Diff2 = vAng2[1] - vAxRotPrec[1] ; + if ( abs( dR2Diff1) > abs( dR2Diff2) && abs( dR2Diff1) > dMaxDeltaR2) + swap( vAng1, vAng2) ; + } + } + vAxRot = vAng1 ; + return true ; +} + +//---------------------------------------------------------------------------- +bool +Operation::VerifyLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDirPrec, const Vector3d& vtAuxPrec, const Vector3d& vtCorrPrec, const DBLVECTOR& vAxPrec, + const Point3d& ptP, const Vector3d& vtDir, const Vector3d& vtAux, const Vector3d& vtCorr, const DBLVECTOR& vAxVal, + int nCnt, int nEntId, double dRot1W, bool& bAdded, bool& bAxError) +{ + // impostazioni + bAdded = false ; + bAxError = false ; + Machine* pMachine = m_pMchMgr->GetCurrMachine() ; + + // se superato il limite di ricursioni, non devo fare alcunché + const int MAX_RECURSION = 6 ; + if ( nCnt > MAX_RECURSION) + return true ; + + // calcolo variazione angolare + double dDeltaAng = 0 ; + for ( int i = 3 ; i < int( vAxVal.size()) ; ++i) + dDeltaAng += abs( vAxVal[i] - vAxPrec[i]) ; + + // Se non cambia la direzione utensile, non devo fare alcunché + const double MAX_DELTA_ANG = 0.01 ; + if ( dDeltaAng <= MAX_DELTA_ANG) + return true ; + + // Determino i valori degli assi al punto medio + DBLVECTOR vAxMid( vAxVal.size()) ; + for ( int i = 0 ; i < int( vAxVal.size()) ; ++ i) + vAxMid[i] = ( vAxPrec[i] + vAxVal[i]) / 2 ; + DBLVECTOR vAxRotMid( int( vAxVal.size() - 3)) ; + for ( int i = 3 ; i < int( vAxVal.size()) ; ++ i) + vAxRotMid[i-3] = vAxMid[i] ; + // ricavo il punto medio del movimento + Point3d ptMoveMid ; + if ( ! pMachine->GetTipFromPositions( vAxMid[0], vAxMid[1], vAxMid[2], vAxRotMid, false, false, ptMoveMid)) + return false ; + // verifico le differenze + double dDist ; + if ( ! DistPointLine( ptMoveMid, ptPrec, ptP).GetDist( dDist)) + return false ; + // LOG_INFO( GetEMkLogger(), ( "Midpoint dist =" + ToString( dDist, 3)).c_str()) + double dLinTol = GetApproxLinTol() ; + + // Se posizione in tolleranza, non devo fare alcunché + if ( dDist <= dLinTol) + return true ; + + // Va inserito il punto medio + Point3d ptMid = Media( ptPrec, ptP) ; + Vector3d vtDirMid = Media( vtDirPrec, vtDir) ; + if ( ! vtDirMid.Normalize()) + return false ; + Vector3d vtAuxMid = Media( vtAuxPrec, vtAux) ; + if ( ! vtAuxPrec.IsSmall() && ! vtAux.IsSmall() && ! vtAuxMid.Normalize()) + return false ; + Vector3d vtCorrMid = Media( vtCorrPrec, vtCorr) ; + if ( ! vtCorrPrec.IsSmall() && ! vtCorr.IsSmall() && ! vtCorrMid.Normalize()) + return false ; + // inserisco nuova entità e modifico vecchia + int nMidEntId = m_pGeomDB->Copy( nEntId, GDB_ID_NULL, nEntId, GDB_BEFORE) ; + if ( nMidEntId == GDB_ID_NULL) + return false ; + ICurve* pCurve = GetCurve( m_pGeomDB->GetGeoObj( nEntId)) ; + if ( pCurve == nullptr || ! pCurve->ModifyStart( ptMid)) + return false ; + ICurve* pMidCurve = GetCurve( m_pGeomDB->GetGeoObj( nMidEntId)) ; + if ( pMidCurve == nullptr || ! pMidCurve->ModifyEnd( ptMid)) + return false ; + CamData* pMidCamData = GetCamData( m_pGeomDB->GetUserObj( nMidEntId)) ; + if ( pMidCamData == nullptr) + return false ; + pMidCamData->SetEndPoint( ptMid) ; + pMidCamData->SetToolDir( vtDirMid) ; + pMidCamData->SetAuxDir( vtAuxMid) ; + pMidCamData->SetFlag( 401) ; + // calcolo gli assi rotanti per il punto medio + DBLVECTOR vAxRotHome( int( vAxVal.size() - 3)) ; + DBLVECTOR vAxRotPrec( int( vAxVal.size() - 3)) ; + for ( int i = 3 ; i < int( vAxVal.size()) ; ++ i) + vAxRotPrec[i-3] = vAxVal[i] ; + if ( ! CalculateRotAxesValues(false, vtDirMid, vtAuxMid, dRot1W, false, true, INFINITO, vAxRotHome, vAxRotPrec, vAxRotMid)) { + pMidCamData->SetAxes( CamData::AS_DIR_ERR, vAxVal) ; + return false ; + } + for ( int i = 3 ; i < int( vAxVal.size()) ; ++ i) + vAxMid[i] = vAxRotMid[i-3] ; + // determino il nuovo valore degli assi lineari sul punto medio + int nMidLStat ; + double dMidX, dMidY, dMidZ ; + bool bMidLOk = m_pMchMgr->GetCalcPositions( ptMid, vAxRotMid, nMidLStat, dMidX, dMidY, dMidZ) ; + if ( ! bMidLOk || nMidLStat != 0) { + pMidCamData->SetAxes( CamData::AS_ERR, vAxMid) ; + bAxError = true ; + return true ; + } + // verifico i limiti di corsa degli assi + int nStat ; + bool bOsOk = m_pMchMgr->VerifyOutstroke( dMidX, dMidY, dMidZ, vAxRotMid, false, nStat) ; + if ( ! bOsOk || nStat != 0) { + pMidCamData->SetAxes( CamData::AS_OUTSTROKE, vAxMid) ; + bAxError = true ; + return true ; + } + // assegno i valori degli assi + vAxMid[0] = dMidX ; + vAxMid[1] = dMidY ; + vAxMid[2] = dMidZ ; + pMidCamData->SetAxes( CamData::AS_OK, vAxMid) ; + // calcolo e salvo il valore dell'asse ausiliario di testa riportato sul grezzo + Vector3d vtBackAux ; pMachine->GetBackAuxDirFromAngles( vAxRotMid, vtBackAux) ; + pMidCamData->SetBackAuxDir( vtBackAux) ; + + // devo verificare le due nuove parti + nCnt ++ ; + bool bAdded1, bAxError1, bAdded2, bAxError2 ; + if ( ! VerifyLineMidPoint( ptPrec, vtDirPrec, vtAuxPrec, vtCorrPrec, vAxPrec, + ptMid, vtDirMid, vtAuxMid, vtCorrMid, vAxMid, + nCnt, nMidEntId, dRot1W, bAdded1, bAxError1)) + return false ; + if ( bAxError1) { + bAxError = true ; + return true ; + } + if ( ! VerifyLineMidPoint( ptMid, vtDirMid, vtAuxMid, vtCorrMid, vAxMid, + ptP, vtDir, vtAux, vtCorr, vAxVal, + nCnt, nEntId, dRot1W, bAdded2, bAxError2)) + return false ; + if ( bAxError2) { + bAxError = true ; + return true ; + } + + return true ; +} + //---------------------------------------------------------------------------- bool Operation::CalculateClPathRobotAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W, diff --git a/Operation.h b/Operation.h index d734a5d..fbcd8a6 100644 --- a/Operation.h +++ b/Operation.h @@ -150,6 +150,12 @@ class Operation : public IUserObj bool CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W, bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome, const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec, int& nOutStrC) ; + bool CalculateRotAxesValues( bool bFirst, const Vector3d& vtTool, const Vector3d& vtAux, + double dRot1W, bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome, + const DBLVECTOR& vAxRotHome, const DBLVECTOR& vAxRotPrec, DBLVECTOR& vAxRot) ; + bool VerifyLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDirPrec, const Vector3d& vtAuxPrec, const Vector3d& vtCorrPrec, const DBLVECTOR& vAxPrec, + const Point3d& ptP, const Vector3d& vtDir, const Vector3d& vtAux, const Vector3d& vtCorr, const DBLVECTOR& vAxVal, + int nCnt, int nEntId, double dRot1W, bool& bAdded, bool& bAxError) ; bool CalculateClPathRobotAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W, bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome, const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec) ;