diff --git a/EgtMachKernel.rc b/EgtMachKernel.rc index a8e5e83..4fbd596 100644 Binary files a/EgtMachKernel.rc and b/EgtMachKernel.rc differ 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