diff --git a/MachMgr.h b/MachMgr.h index 1b9a703..f84545f 100644 --- a/MachMgr.h +++ b/MachMgr.h @@ -406,6 +406,8 @@ class MachMgr : public IMachMgr // Operations bool GetOperationNewName( std::string& sName) const ; const ToolData* GetMachiningToolData( void) const ; + // Simulation + bool SimAddCollisionObj( int nInd, int nFrameId, int nType, double dPar1, double dPar2, double dPar3) ; // Machine bool GetHeadAbove( const std::string& sHead) const ; double GetAngDeltaMinForHome( void) const ; diff --git a/MachMgrSimulation.cpp b/MachMgrSimulation.cpp index 012aa90..471f986 100644 --- a/MachMgrSimulation.cpp +++ b/MachMgrSimulation.cpp @@ -147,3 +147,14 @@ MachMgr::SimExit( void) m_pSimul = nullptr ; return true ; } + +//---------------------------------------------------------------------------- +bool +MachMgr::SimAddCollisionObj( int nInd, int nFrameId, int nType, double dPar1, double dPar2, double dPar3) +{ + // verifico simulatore + if ( m_pSimul == nullptr) + return false ; + // aggiungo un oggetto da verificare per la collisione con il grezzo + return m_pSimul->AddCollisionObj( nInd, nFrameId, nType, dPar1, dPar2, dPar3) ; +} diff --git a/Machine.h b/Machine.h index 8d6c255..3ecda2e 100644 --- a/Machine.h +++ b/Machine.h @@ -303,6 +303,7 @@ class Machine static int LuaEmtSetOutstrokeInfo( lua_State* L) ; static int LuaEmtSetLastError( lua_State* L) ; static int LuaEmtSetWarning( lua_State* L) ; + static int LuaEmtAddCollisionObj( lua_State* L) ; } ; //---------------------------------------------------------------------------- diff --git a/MachineLua.cpp b/MachineLua.cpp index 04f44b7..420ede2 100644 --- a/MachineLua.cpp +++ b/MachineLua.cpp @@ -112,6 +112,8 @@ Machine::LuaInit( const string& sMachineName) m_LuaMgr.RegisterFunction( "EmtSetLastError", Machine::LuaEmtSetLastError) ; // registro la funzione per aggiungere informazioni di warning m_LuaMgr.RegisterFunction( "EmtSetWarning", Machine::LuaEmtSetWarning) ; + // registro la funzione per aggiungere un oggetto da verificare per la collisione in simulazione + m_LuaMgr.RegisterFunction( "EmtAddCollisionObj", Machine::LuaEmtAddCollisionObj) ; return true ; } @@ -1015,6 +1017,34 @@ Machine::LuaEmtSetWarning( lua_State* L) // assegno i dati bool bOk = ( m_pMchLua->m_pMchMgr != nullptr && m_pMchLua->m_pMchMgr->SetWarning( nWarnId, sWarnDesc)) ; // assegno risultato - LuaSetParam( L, true) ; + LuaSetParam( L, bOk) ; + return 1 ; +} + +//---------------------------------------------------------------------------- +int +Machine::LuaEmtAddCollisionObj( lua_State* L) +{ + // 6 parametri : nInd, nFrameId, nType, dPar1, dPar2, dPar3 + int nInd ; + LuaCheckParam( L, 1, nInd) + int nFrameId ; + LuaCheckParam( L, 2, nFrameId) + int nType ; + LuaCheckParam( L, 3, nType) + double dPar1 ; + LuaCheckParam( L, 4, dPar1) + double dPar2 ; + LuaCheckParam( L, 5, dPar2) + double dPar3 ; + LuaCheckParam( L, 6, dPar3) + LuaClearStack( L) ; + // verifico ci sia una macchina attiva + if ( m_pMchLua == nullptr) + return luaL_error( L, " Unknown Machine") ; + // assegno i dati + bool bOk = ( m_pMchLua->m_pMchMgr != nullptr && m_pMchLua->m_pMchMgr->SimAddCollisionObj( nInd, nFrameId, nType, dPar1, dPar2, dPar3)) ; + // assegno risultato + LuaSetParam( L, bOk) ; return 1 ; } diff --git a/OutputConst.h b/OutputConst.h index b7c0da0..fb35811 100644 --- a/OutputConst.h +++ b/OutputConst.h @@ -143,8 +143,9 @@ static const std::string GVAR_VMILL = ".VMILL" ; // (int) identifica static const std::string GVAR_SIM1ST = ".SIM1ST" ; // (bool) flag inizio simulazione static const std::string GVAR_SIMSTEP = ".SIMSTEP" ; // (num) step di movimento durante la simulazione static const std::string GVAR_SIMUISTAT = ".SIMUISTAT" ; // (num) stato simulazione imposto da utente - - +static const std::string GVAR_SAFEDIST = ".SAFEDIST" ; // (num) distanza di sicurezza per verifica di collisione +static const std::string GVAR_SIMVMID = ".SIMVMID" ; // (int) identificativo grezzo Vmill in collisione +static const std::string GVAR_SIMCOBIND = ".SIMCOBIND" ; // (int) indice oggetto in collisione // Funzioni generazione static const std::string ON_START = "OnStart" ; static const std::string ON_END = "OnEnd" ; @@ -204,12 +205,11 @@ static const std::string ON_SIMUL_PATH_END = "OnSimulPathEnd" ; static const std::string ON_SIMUL_PATH_START_AUX = "OnSimulPathStartAux" ; static const std::string ON_SIMUL_PATH_END_AUX = "OnSimulPathEndAux" ; static const std::string ON_SIMUL_MOVE_START = "OnSimulMoveStart" ; -static const std::string ON_SIMUL_MOVE = "OnSimulMove" ; static const std::string ON_SIMUL_MOVE_END = "OnSimulMoveEnd" ; +static const std::string ON_SIMUL_COLLISION = "OnSimulCollision" ; // Funzioni macchina static const std::string ON_RESET_MACHINE = "OnResetMachine" ; - //---------------------------------------------------------------------------- inline std::string GetGlobVarAxisValue( int nAx, const std::string& sVar = GLOB_VAR) diff --git a/Simulator.cpp b/Simulator.cpp index 94e045f..0b06318 100644 --- a/Simulator.cpp +++ b/Simulator.cpp @@ -23,6 +23,7 @@ #include "DllMain.h" #include "/EgtDev/Include/EGkVolZmap.h" #include "/EgtDev/Include/EGkStringUtils3d.h" +#include "/EgtDev/Include/EXeConst.h" #include "/EgtDev/Include/EMkToolConst.h" #include "/EgtDev/Include/EMkOperationConst.h" #include "/EgtDev/Include/EgtPerfCounter.h" @@ -38,6 +39,7 @@ static const double MIN_STEP = 1.0 ; static const double MAX_STEP = 100.0 ; static const double MID_STEP = 50.0 ; static const double COEFF_LIM = 0.999 ; +static const double SAFEDIST_STD = 5.0 ; //---------------------------------------------------------------------------- Simulator::Simulator( void) @@ -61,6 +63,7 @@ Simulator::Simulator( void) m_nAuxEInd = 0 ; m_dVmTdOffs = 0 ; m_dVmAdOffs = 0 ; + m_dSafeDist = SAFEDIST_STD ; m_bEnabAxes = true ; m_AxesName.reserve( 8) ; m_AxesToken.reserve( 8) ; @@ -191,6 +194,7 @@ Simulator::ResetInterpolation( void) m_VmId.clear() ; m_dVmTdOffs = 0 ; m_dVmAdOffs = 0 ; + m_dSafeDist = SAFEDIST_STD ; m_bEnabAxes = true ; return true ; } @@ -208,7 +212,7 @@ Simulator::Move( int& nStatus) // Recupero tempo impiegato (calcolo + visualizzazione precedenti) if ( m_pPerfCnt != nullptr && ExeGetDebugLevel() >= 5) { double dElapsed = m_pPerfCnt->Stop() ; - string sOut = "Elapsed=" + ToString( dElapsed, 0) ; + string sOut = "Elapsed=" + ToString( dElapsed, 1) ; LOG_DBG_INFO( GetEMkLogger(), sOut.c_str()) m_pPerfCnt->Start() ; } @@ -935,9 +939,11 @@ Simulator::ManageSingleMove( int& nStatus, double& dMove) m_pMachine->SetAxisPos( m_AxesName[i], 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) { - Point3d ptNoseF ; Vector3d vtDirF ; Vector3d vtAuxF ; Frame3d frVzmF ; - bool bOkF = GetHeadCurrPosDirAux( ptNoseF, vtDirF, vtAuxF) && m_pGeomDB->GetGlobFrame( m_VmId[j], frVzmF) ; ; + Frame3d frVzmF ; + bOkF = m_pGeomDB->GetGlobFrame( m_VmId[j], frVzmF) && bOkF ; ExecLineVmill( m_VmId[j], ptNoseI, vtDirI, vtAuxI, vFrVzmI[j], ptNoseF, vtDirF, vtAuxF, frVzmF) ; } } @@ -1016,16 +1022,20 @@ Simulator::ManageSingleMove( int& nStatus, double& dMove) m_pMachine->SetAxisPos( m_AuxAxesName[i], dVal) ; } - // Richiamo gestione evento movimento in corso - int nErr ; - if ( ! OnMove( nErr)) { - nStatus = ( Stopped() ? MCH_SIM_STOP : ( nErr == 1 ? MCH_SIM_COLLISION : MCH_SIM_ERR)) ; - // Se arrivato a fine interpolazione movimento, salvo posizioni - if ( m_dCoeff > COEFF_LIM && m_bEnabAxes) { - for ( size_t i = 0 ; i < m_AxesName.size() ; ++ i) - m_AxesVal[i] = AxesEnd[i] ; + // Eseguo eventuale verifica di collisione + int nVmInd, nObjInd ; + if ( ! ExecCollisionCheck( nVmInd, nObjInd)) { + // Richiamo funzione di convalida collisione + int nErr ; + if ( ! OnCollision( nVmInd, nObjInd, nErr)) { + nStatus = ( Stopped() ? MCH_SIM_STOP : ( nErr == 1 ? MCH_SIM_COLLISION : MCH_SIM_ERR)) ; + // Se arrivato a fine interpolazione movimento, salvo posizioni + if ( m_dCoeff > COEFF_LIM && m_bEnabAxes) { + for ( size_t i = 0 ; i < m_AxesName.size() ; ++ i) + m_AxesVal[i] = AxesEnd[i] ; + } + return false ; } - return false ; } // Se arrivato a fine interpolazione movimento, salvo posizioni e segnalo @@ -1099,6 +1109,48 @@ Simulator::ExecLineVmill( int nVmId, const Point3d& ptHi, const Vector3d& vtHi, return pVZM->MillingStep( ptHiL, vtHiL, vtAiL, ptHfL, vtHfL, vtAfL) ; } +//---------------------------------------------------------------------------- +bool +Simulator::ExecCollisionCheck( int& nVmInd, int& nObjInd) +{ + // se non ci sono oggetti da controllare o non c'è il grezzo lavorato, tutto bene + if ( m_CollObj.empty() || m_VmId.empty()) + return true ; + // ciclo sui grezzi + for ( int i = 0 ; i < int( m_VmId.size()) ; ++ i) { + const IVolZmap* pVZM = GetVolZmap( m_pGeomDB->GetGeoObj( m_VmId[i])) ; + if ( pVZM == nullptr) + continue ; + Frame3d frZM ; m_pGeomDB->GetGlobFrame( m_VmId[i], frZM) ; + // ciclo sugli oggetti da verificare + for ( int j = 0 ; j < int( m_CollObj.size()) ; ++ j) { + bool bOk = true ; + 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) ; + 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) ; + } + else if ( m_CollObj[j].nType == MCH_SIM_COB_CYL) { + bOk = pVZM->AvoidCylinder( frObj, m_CollObj[j].dPar1, m_CollObj[j].dPar2, m_dSafeDist) ; + } + else if ( m_CollObj[j].nType == MCH_SIM_COB_SPHE) { + bOk = pVZM->AvoidSphere( frObj.Orig(), m_CollObj[j].dPar1, m_dSafeDist) ; + } + if ( ! bOk) { + nVmInd = i ; + nObjInd = j ; + return false ; + } + } + } + return true ; +} + //---------------------------------------------------------------------------- bool Simulator::OnStart( bool bFirst) @@ -1200,6 +1252,8 @@ bool Simulator::OnToolSelect( const string& sTool, const string& sHead, int nExit, const string& sTcPos, bool bFirst) { + // reset oggetti per verifica collisione con grezzo + m_CollObj.clear() ; // assegno il nome dell'utensile, la testa e l'uscita if ( ! m_pMachine->LuaSetGlobVar( GLOB_VAR + GVAR_TOOL, sTool) || ! m_pMachine->LuaSetGlobVar( GLOB_VAR + GVAR_HEAD, sHead) || @@ -1230,6 +1284,8 @@ Simulator::OnToolSelect( const string& sTool, const string& sHead, int nExit, co bool bOk = m_pMachine->LuaCallFunction( ON_SIMUL_TOOL_SELECT) ; // forzo aggiornamento posizione assi (possono essere stati mossi nello script) UpdateAxesPos() ; + // recupero distanza di sicurezza per verifica collisione + m_pMachine->LuaGetGlobVar( GLOB_VAR + GVAR_SAFEDIST, m_dSafeDist) ; // recupero i dati di ritorno per assi ausiliari ResetAuxAxes() ; int nNumAuxAxes = 0 ; @@ -1511,26 +1567,6 @@ Simulator::OnMoveStart( const CamData* pCamData, int& nErr) return bOk ; } -//---------------------------------------------------------------------------- -bool -Simulator::OnMove( int& nErr) -{ - // reset stato di errore da script - nErr = 0 ; - // verifico esistenza funzione - if ( ! m_pMachine->LuaExistsFunction( ON_SIMUL_MOVE)) - return true ; - // reset stato di errore - if ( ! m_pMachine->LuaSetGlobVar( GLOB_VAR + GVAR_ERR, nErr)) - return false ; - // chiamo la funzione - if ( ! m_pMachine->LuaCallFunction( ON_SIMUL_MOVE)) - return false ; - // verifico codice di errore - m_pMachine->LuaGetGlobVar( GLOB_VAR + GVAR_ERR, nErr) ; - return ( nErr == 0) ; -} - //---------------------------------------------------------------------------- bool Simulator::OnMoveEnd( void) @@ -1545,6 +1581,31 @@ Simulator::OnMoveEnd( void) return bOk ; } +//---------------------------------------------------------------------------- +bool +Simulator::OnCollision( int nVmInd, int nObjInd, int& nErr) +{ + // reset stato di errore da script + nErr = 0 ; + // verifico esistenza funzione + if ( ! m_pMachine->LuaExistsFunction( ON_SIMUL_COLLISION)) + return true ; + // reset stato di errore + if ( ! m_pMachine->LuaSetGlobVar( GLOB_VAR + GVAR_ERR, nErr)) + return false ; + // assegno identificativi grezzo e oggetto in collisione + if ( ! m_pMachine->LuaSetGlobVar( GLOB_VAR + GVAR_SIMVMID, m_VmId[nVmInd])) + return false ; + if ( ! m_pMachine->LuaSetGlobVar( GLOB_VAR + GVAR_SIMCOBIND, m_CollObj[nObjInd].nInd)) + return false ; + // chiamo la funzione + if ( ! m_pMachine->LuaCallFunction( ON_SIMUL_COLLISION)) + return false ; + // verifico codice di errore + m_pMachine->LuaGetGlobVar( GLOB_VAR + GVAR_ERR, nErr) ; + return ( nErr == 0) ; +} + //---------------------------------------------------------------------------- bool Simulator::OnResetMachine( void) @@ -1555,3 +1616,31 @@ Simulator::OnResetMachine( void) // eseguo reset macchina return m_pMachine->LuaCallFunction( ON_RESET_MACHINE) ; } + +//---------------------------------------------------------------------------- +bool +Simulator::AddCollisionObj( int nInd, int nFrameId, int nType, double dPar1, double dPar2, double dPar3) +{ + // verifiche sui parametri + if ( nInd <= 0 || nFrameId <= GDB_ID_NULL) + return false ; + switch ( nType) { + case MCH_SIM_COB_BOX : + if ( abs( dPar1) < EPS_SMALL || abs( dPar2) < EPS_SMALL || abs( dPar3) < EPS_SMALL) + return false ; + break ; + case MCH_SIM_COB_CYL : + if ( abs( dPar1) < EPS_SMALL || abs( dPar2) < EPS_SMALL) + return false ; + break ; + case MCH_SIM_COB_SPHE : + if ( abs( dPar1) < EPS_SMALL) + return false ; + break ; + default : + return false ; + } + // aggiungo al vettore + m_CollObj.emplace_back( nInd, nFrameId, nType, dPar1, dPar2, dPar3) ; + return true ; +} diff --git a/Simulator.h b/Simulator.h index b0e1849..9133f79 100644 --- a/Simulator.h +++ b/Simulator.h @@ -37,6 +37,7 @@ class Simulator bool GetToolInfo( std::string& sName, double& dSpeed) const ; bool GetOperationInfo( std::string& sName, int& nType) const ; bool GetMoveInfo( int& nGmove, double& dFeed) const ; + bool AddCollisionObj( int nInd, int nFrameId, int nType, double dPar1, double dPar2, double dPar3) ; private : bool UpdateTool( bool bFirst = false) ; @@ -59,6 +60,7 @@ 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 ExecCollisionCheck( int& nVmInd, int& nObjInd) ; bool Stopped( void) { return ( m_nUiStatus == MCH_UISIM_STOP) ; } bool OnStart( bool bFirst) ; @@ -78,10 +80,24 @@ class Simulator const Vector3d& vtExtr, const Point3d& ptMin, const Point3d& ptMax, double dElev) ; bool OnPathEnd( int nAE) ; bool OnMoveStart( const CamData* pCamData, int& nErr) ; - bool OnMove( int& nErr) ; bool OnMoveEnd( void) ; + bool OnCollision( int nVmInd, int nObjInd, int& nErr) ; bool OnResetMachine( void) ; + private : + struct CollObj { + int nInd ; + int nFrameId ; + int nType ; + double dPar1 ; + double dPar2 ; + double dPar3 ; + CollObj( void) : nInd( 0), nFrameId( -1), nType( 0), dPar1( 0), dPar2( 0), dPar3( 0) {} + CollObj( int nI, int nF, int nT, double dP1, double dP2, double dP3) + : nInd( nI), nFrameId( nF), nType( nT), dPar1( dP1), dPar2( dP2), dPar3( dP3) {} + } ; + typedef std::vector< CollObj> COBVECTOR ; + private : MachMgr* m_pMchMgr ; // puntatore al gestore di tutte le lavorazioni IGeomDB* m_pGeomDB ; // puntatore al DB geometrico @@ -103,7 +119,9 @@ class Simulator std::string m_sTool ; // nome dell'utensile corrente INTVECTOR m_VmId ; // vettore identificativi Zmap per Virtual Milling double m_dVmTdOffs ; // offset utensile in direzione principale per VM - double m_dVmAdOffs ; // offset utensile in direzione ausiliaria per VM + double m_dVmAdOffs ; // offset utensile in direzione ausiliaria per VM + COBVECTOR m_CollObj ; // vettore oggetti da testare per collisione con grezzo + double m_dSafeDist ; // distanza di sicurezza per verifica collisioni bool m_bEnabAxes ; // flag abilitazione movimento assi attivi STRVECTOR m_AxesName ; // nomi degli assi macchina attivi STRVECTOR m_AxesToken ; // token degli assi macchina attivi