From 5d71cb243a554ba8e29de18fd392e85a3b530f21 Mon Sep 17 00:00:00 2001 From: Dario Sassi Date: Thu, 23 Jul 2020 15:06:11 +0000 Subject: [PATCH] =?UTF-8?q?EgtMachKernel=202.2g5=20:=20-=20in=20simulazion?= =?UTF-8?q?e=20la=20verifica=20collisione=20pu=C3=B2=20lavorare=20anche=20?= =?UTF-8?q?con=20trimesh=20chiuse=20invece=20delle=20sole=20Zmap=20-=20in?= =?UTF-8?q?=20simulazione=20si=20=C3=A8=20affinata=20la=20verifica=20di=20?= =?UTF-8?q?collisione=20per=20oggetti=20piccoli=20-=20in=20simulazione=20s?= =?UTF-8?q?i=20salta=20la=20verifica=20di=20collisione=20tra=20utensile=20?= =?UTF-8?q?in=20lavoro=20e=20grezzo=20lavorato=20anche=20se=20entrambi=20d?= =?UTF-8?q?a=20verificare.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- EgtMachKernel.rc | Bin 11774 -> 11774 bytes Simulator.cpp | 135 +++++++++++++++++++++++++++++++++++------------ Simulator.h | 4 ++ 3 files changed, 104 insertions(+), 35 deletions(-) diff --git a/EgtMachKernel.rc b/EgtMachKernel.rc index a8e5e83bc3c80036c12b10dc5684833e0b409e46..4fbd5966b2b03934c04f1af9cf5e812134bc9769 100644 GIT binary patch delta 97 zcmewt{V#gMFE&Qg&A-`fnHfzdKa|wnoW?bQ1uSxrY4Qod0+^@}R1`^_2jk{d(jGv4 NK<#LXK~l;gTmUO4BcK2P delta 97 zcmewt{V#gMFE&P#&A-`fnHfzcKa|wnoW?bQ1uSxrY4Qod0+^@}R1`^_2jk{d(jGv4 NK<#LXK~l;gTmUI`BbWdH diff --git a/Simulator.cpp b/Simulator.cpp index 38cdaeb..a3a74f4 100644 --- a/Simulator.cpp +++ b/Simulator.cpp @@ -21,6 +21,9 @@ #include "MachiningConst.h" #include "OutputConst.h" #include "DllMain.h" +#include "/EgtDev/Include/EGkCDeBoxClosedSurfTm.h" +#include "/EgtDev/Include/EGkCDeCylClosedSurfTm.h" +#include "/EgtDev/Include/EGkCDeSpheClosedSurfTm.h" #include "/EgtDev/Include/EGkVolZmap.h" #include "/EgtDev/Include/EGkStringUtils3d.h" #include "/EgtDev/Include/EXeConst.h" @@ -61,6 +64,7 @@ Simulator::Simulator( void) m_nAuxSInd = 0 ; m_nAuxETot = 0 ; m_nAuxEInd = 0 ; + m_nExit = 0 ; m_dVmTdOffs = 0 ; m_dVmAdOffs = 0 ; m_dSafeDist = SAFEDIST_STD ; @@ -438,6 +442,8 @@ Simulator::UpdateTool( bool bFirst) if ( ! m_pMchMgr->SetCalcTool( sTool, sHead, nExit)) return false ; m_sTool = sTool ; + m_sHead = sHead ; + m_nExit = nExit ; // aggiorno gli assi macchina if ( ! UpdateAxes()) return false ; @@ -480,6 +486,8 @@ Simulator::UpdateTool( bool bFirst) if ( ! m_pMchMgr->SetCalcTool( sTool, sHead, nExit)) return false ; m_sTool = sTool ; + m_sHead = sHead ; + m_nExit = nExit ; // aggiorno gli assi macchina if ( ! UpdateAxes()) return false ; @@ -905,6 +913,10 @@ Simulator::ManageSingleMove( int& nStatus, double& dMove) } } + // Dati per eventuale verifica di collisione + int nCdInd, nObjInd ; + bool bCollCheck = true ; + if ( m_bEnabAxes) { // Calcolo distanza di movimento double dSqDist = 0 ; @@ -934,18 +946,34 @@ Simulator::ManageSingleMove( int& nStatus, double& dMove) // Eseguo movimento rapido o lineare if ( nMoveType != 2 && nMoveType != 3) { - // assegno posizioni finali - for ( int i = 0 ; i < int( m_AxesName.size()) ; ++ i) { - double dVal = m_AxesVal[i] * ( 1 - m_dCoeff) + AxesEnd[i] * m_dCoeff ; - m_pMachine->SetAxisPos( m_AxesName[i], dVal) ; + // se attivo CollisionCheck approssimo movimento con più step + int nStep = 1 ; + if ( ! m_VmId.empty() || NeedCollisionCheck()) { + const double LEN_STEP = 10. ; + nStep = max( int( dDist / LEN_STEP), 1) ; } - // eseguo eventuale Vmill - Point3d ptNoseF ; Vector3d vtDirF ; Vector3d vtAuxF ; - bool bOkF = GetHeadCurrPosDirAux( ptNoseF, vtDirF, vtAuxF) ; - for ( int j = 0 ; j < int( m_VmId.size()) ; ++ j) { - Frame3d frVzmF ; - bOkF = m_pGeomDB->GetGlobFrame( m_VmId[j], frVzmF) && bOkF ; - ExecLineVmill( m_VmId[j], ptNoseI, vtDirI, vtAuxI, vFrVzmI[j], ptNoseF, vtDirF, vtAuxF, frVzmF) ; + for ( int i = 1 ; i <= nStep ; ++ i) { + double dCurrCoeff = dPrevCoeff + ( m_dCoeff - dPrevCoeff) / nStep * i ; + // assegno posizioni finali + for ( int j = 0 ; j < int( m_AxesName.size()) ; ++ j) { + double dVal = m_AxesVal[j] * ( 1 - dCurrCoeff) + AxesEnd[j] * dCurrCoeff ; + m_pMachine->SetAxisPos( m_AxesName[j], dVal) ; + } + // muovo eventuali assi ausiliari + for ( int j = 0 ; j < int( m_AuxAxesName.size()) ; ++ j) { + double dVal = m_AuxAxesVal[j] * ( 1 - dCurrCoeff) + m_AuxAxesEnd[j] * dCurrCoeff ; + m_pMachine->SetAxisPos( m_AuxAxesName[j], dVal) ; + } + // eseguo eventuale Vmill + Point3d ptNoseF ; Vector3d vtDirF ; Vector3d vtAuxF ; + bool bOkF = GetHeadCurrPosDirAux( ptNoseF, vtDirF, vtAuxF) ; + for ( int j = 0 ; j < int( m_VmId.size()) ; ++ j) { + Frame3d frVzmF ; + bOkF = m_pGeomDB->GetGlobFrame( m_VmId[j], frVzmF) && bOkF ; + ExecLineVmill( m_VmId[j], ptNoseI, vtDirI, vtAuxI, vFrVzmI[j], ptNoseF, vtDirF, vtAuxF, frVzmF) ; + } + // eseguo eventuale collision check + bCollCheck = bCollCheck && ExecCollisionCheck( nCdInd, nObjInd) ; } } // Eseguo movimento su arco @@ -959,9 +987,9 @@ Simulator::ManageSingleMove( int& nStatus, double& dMove) double dDiffAng = ( m_dCoeff - dPrevCoeff) * dAngCen ; Vector3d vtRot = Point3d( m_AxesVal[0], m_AxesVal[1], m_AxesVal[2]) - ptCen ; vtRot.Rotate( vtN, dPrevCoeff * dAngCen) ; - // approssimo movimento con 1 o più step a seconda ci sia Vmill + // se attivi Vmill o CollisionCheck approssimo movimento con più step int nStep = 1 ; - if ( ! m_VmId.empty()) { + if ( ! m_VmId.empty() || NeedCollisionCheck()) { const double LEN_STEP = 2. ; nStep = max( int( abs( dDiffAng * DEGTORAD * dRad) / LEN_STEP), 1) ; const double ANG_STEP = 5. ; @@ -980,6 +1008,11 @@ Simulator::ManageSingleMove( int& nStatus, double& dMove) double dVal = m_AxesVal[j] * ( 1 - dCurrCoeff) + AxesEnd[j] * dCurrCoeff ; m_pMachine->SetAxisPos( m_AxesName[j], dVal) ; } + // eventuali assi ausiliari + for ( int j = 0 ; j < int( m_AuxAxesName.size()) ; ++ j) { + double dVal = m_AuxAxesVal[j] * ( 1 - dCurrCoeff) + m_AuxAxesEnd[j] * dCurrCoeff ; + m_pMachine->SetAxisPos( m_AuxAxesName[j], dVal) ; + } // eseguo eventuale Vmill Point3d ptNoseF ; Vector3d vtDirF ; Vector3d vtAuxF ; bool bOkF = GetHeadCurrPosDirAux( ptNoseF, vtDirF, vtAuxF) ; @@ -990,6 +1023,8 @@ Simulator::ManageSingleMove( int& nStatus, double& dMove) // salvo riferimento per prossimo inizio vFrVzmI[k] = frVzmF ; } + // eseguo eventuale collision check + bCollCheck = bCollCheck && ExecCollisionCheck( nCdInd, nObjInd) ; // aggiorno prossimo inizio ptNoseI = ptNoseF ; vtDirI = vtDirF ; @@ -999,8 +1034,8 @@ Simulator::ManageSingleMove( int& nStatus, double& dMove) else { // Calcolo distanza di movimento double dSqDist = 0 ; - for ( size_t i = 0 ; i < m_AuxAxesName.size() ; ++ i) { - // coefficiente moltiplicativo per differenziare assi lineari e rotanti + for ( int i = 0 ; i < int( m_AuxAxesName.size()) ; ++ i) { + // coefficiente moltiplicativo per differenziare assi lineari e rotanti double dSqCoeff = ( m_AuxAxesLinear[i] ? 1 : 100) ; dSqDist += dSqCoeff * ( m_AuxAxesEnd[i] - m_AuxAxesVal[i]) * ( m_AuxAxesEnd[i] - m_AuxAxesVal[i]) ; } @@ -1015,17 +1050,17 @@ Simulator::ManageSingleMove( int& nStatus, double& dMove) else m_dCoeff = 1 ; dMove = ( m_dCoeff - dPrevCoeff) * dDist ; + // Muovo eventuali assi ausiliari + for ( int i = 0 ; i < int( m_AuxAxesName.size()) ; ++ i) { + double dVal = m_AuxAxesVal[i] * ( 1 - m_dCoeff) + m_AuxAxesEnd[i] * m_dCoeff ; + m_pMachine->SetAxisPos( m_AuxAxesName[i], dVal) ; + } + // eseguo eventuale collision check + bCollCheck = bCollCheck && ExecCollisionCheck( nCdInd, nObjInd) ; } - // Muovo eventuali assi ausiliari - for ( size_t i = 0 ; i < m_AuxAxesName.size() ; ++ i) { - double dVal = m_AuxAxesVal[i] * ( 1 - m_dCoeff) + m_AuxAxesEnd[i] * m_dCoeff ; - m_pMachine->SetAxisPos( m_AuxAxesName[i], dVal) ; - } - - // Eseguo eventuale verifica di collisione - int nCdInd, nObjInd ; - if ( ! ExecCollisionCheck( nCdInd, nObjInd)) { + // Se riscontrata collisione + if ( ! bCollCheck) { // Richiamo funzione di convalida collisione int nErr ; if ( ! OnCollision( nCdInd, nObjInd, nErr)) { @@ -1114,33 +1149,63 @@ Simulator::ExecLineVmill( int nVmId, const Point3d& ptHi, const Vector3d& vtHi, bool Simulator::ExecCollisionCheck( int& nCdInd, int& nObjInd) { - // se non ci sono oggetti da controllare o non c'è il grezzo lavorato, tutto bene - if ( m_CollObj.empty() || m_CdId.empty()) + // se non ci sono oggetti da controllare o non ci sono i grezzi o equivalenti, tutto bene + if ( ! NeedCollisionCheck()) return true ; - // ciclo sui grezzi + // ciclo sui grezzi o equivalenti for ( int i = 0 ; i < int( m_CdId.size()) ; ++ i) { - const IVolZmap* pVZM = GetVolZmap( m_pGeomDB->GetGeoObj( m_CdId[i])) ; - if ( pVZM == nullptr) + // se modo hidden è da saltare + int nMode ; + if ( m_pGeomDB->GetCalcMode( m_CdId[i], nMode) && nMode == GDB_MD_HIDDEN) continue ; - Frame3d frZM ; m_pGeomDB->GetGlobFrame( m_CdId[i], frZM) ; - // ciclo sugli oggetti da verificare + // verifico se è il grezzo in lavorazione + bool bIsRaw = ( find( m_VmId.begin(), m_VmId.end(), m_CdId[i]) != m_VmId.end()) ; + // recupero solido o trimesh, se altro lo salto + const IVolZmap* pVZM = GetVolZmap( m_pGeomDB->GetGeoObj( m_CdId[i])) ; + const ISurfTriMesh* pSTM = GetSurfTriMesh( m_pGeomDB->GetGeoObj( m_CdId[i])) ; + if ( pVZM == nullptr && pSTM == nullptr) + continue ; + // recupero il riferimento + Frame3d frCd ; m_pGeomDB->GetGlobFrame( m_CdId[i], frCd) ; + // ciclo sugli oggetti con cui verificare for ( int j = 0 ; j < int( m_CollObj.size()) ; ++ j) { bool bOk = true ; + // se modo hidden è da saltare + int nMode ; + if ( m_pGeomDB->GetCalcMode( m_CollObj[j].nFrameId, nMode) && nMode == GDB_MD_HIDDEN) + continue ; + // verifico se è l'utensile attivo + string sHead ; m_pGeomDB->GetName( m_pGeomDB->GetParentId( m_CollObj[j].nFrameId), sHead) ; + string sExit ; m_pGeomDB->GetName( m_CollObj[j].nFrameId, sExit) ; + bool bIsTool = ( sHead == m_sHead && sExit == string( "_T") + ToString( m_nExit)) ; + // se utensile attivo su grezzo in lavoro, salto + if ( bIsTool && bIsRaw) + continue ; + // se riferimento non trovato, si salta const IGeoFrame3d* pGeoFrame = GetGeoFrame3d( m_pGeomDB->GetGeoObj( m_CollObj[j].nFrameId)) ; if ( pGeoFrame == nullptr) continue ; Frame3d frObj = pGeoFrame->GetFrame() ; Frame3d frParent ; m_pGeomDB->GetGlobFrame( m_CollObj[j].nFrameId, frParent) ; - frObj.LocToLoc( frParent, frZM) ; + frObj.LocToLoc( frParent, frCd) ; if ( m_CollObj[j].nType == MCH_SIM_COB_BOX) { Vector3d vtDiag( m_CollObj[j].dPar1, m_CollObj[j].dPar2, m_CollObj[j].dPar3) ; - bOk = pVZM->AvoidBox( frObj, vtDiag, m_dSafeDist) ; + if ( pVZM != nullptr) + bOk = pVZM->AvoidBox( frObj, vtDiag, m_dSafeDist) ; + else + bOk = ! CDeBoxClosedSurfTm( frObj, vtDiag, m_dSafeDist, *pSTM) ; } else if ( m_CollObj[j].nType == MCH_SIM_COB_CYL) { - bOk = pVZM->AvoidCylinder( frObj, m_CollObj[j].dPar1, m_CollObj[j].dPar2, m_dSafeDist) ; + if ( pVZM != nullptr) + bOk = pVZM->AvoidCylinder( frObj, m_CollObj[j].dPar1, m_CollObj[j].dPar2, m_dSafeDist) ; + else + bOk = ! CDeCylClosedSurfTm( frObj, m_CollObj[j].dPar1, m_CollObj[j].dPar2, m_dSafeDist, *pSTM) ; } else if ( m_CollObj[j].nType == MCH_SIM_COB_SPHE) { - bOk = pVZM->AvoidSphere( frObj.Orig(), m_CollObj[j].dPar1, m_dSafeDist) ; + if ( pVZM != nullptr) + bOk = pVZM->AvoidSphere( frObj.Orig(), m_CollObj[j].dPar1, m_dSafeDist) ; + else + bOk = ! CDeSpheClosedSurfTm( frObj.Orig(), m_CollObj[j].dPar1, m_dSafeDist, *pSTM) ; } if ( ! bOk) { nCdInd = i ; diff --git a/Simulator.h b/Simulator.h index 6fb157e..c98670c 100644 --- a/Simulator.h +++ b/Simulator.h @@ -60,6 +60,8 @@ class Simulator bool GetHeadCurrPosDirAux( Point3d& ptH, Vector3d& vtH, Vector3d& vtA) ; bool ExecLineVmill( int nVmId, const Point3d& ptHi, const Vector3d& vtHi, const Vector3d& vtAi, const Frame3d& frVzmI, const Point3d& ptHf, const Vector3d& vtHf, const Vector3d& vtAf, const Frame3d& frVzmF) ; + bool NeedCollisionCheck( void) const + { return ( ! m_CollObj.empty() && ! m_CdId.empty()) ; } bool ExecCollisionCheck( int& nCdInd, int& nObjInd) ; bool Stopped( void) { return ( m_nUiStatus == MCH_UISIM_STOP) ; } @@ -117,6 +119,8 @@ class Simulator int m_nAuxETot ; // numero totale movimenti ausiliari di fine percorso int m_nAuxEInd ; // indice del movimento ausiliario di fine percorso corrente std::string m_sTool ; // nome dell'utensile corrente + std::string m_sHead ; // nome della testa corrente + int m_nExit ; // indice dell'uscita corrente INTVECTOR m_VmId ; // vettore identificativi Zmap per Virtual Milling INTVECTOR m_CdId ; // vettore identificativi Zmap per Collision Detection double m_dVmTdOffs ; // offset utensile in direzione principale per VM