//---------------------------------------------------------------------------- // EgalTech 2015-2015 //---------------------------------------------------------------------------- // File : Machine.cpp Data : 06.05.15 Versione : 1.6e3 // Contenuto : Implementazione gestione macchina. // // // // Modifiche : 06.05.15 DS Creazione modulo. // // //---------------------------------------------------------------------------- //--------------------------- Include ---------------------------------------- #include "stdafx.h" #include "MachMgr.h" #include "DllMain.h" #include "Table.h" #include "Axis.h" #include "Head.h" #include "Exit.h" #include "/EgtDev/Include/EGkGeomDB.h" #include "/EgtDev/Include/EGkGeoVector3d.h" #include "/EgtDev/Include/EGkStringUtils3d.h" #include "/EgtDev/Include/EGnFileUtils.h" #include "/EgtDev/Include/EgtPointerOwner.h" using namespace std ; //---------------------------------------------------------------------------- Machine::Machine( void) { m_pMchMgr = nullptr ; m_pGeomDB = nullptr ; m_nGroupId = GDB_ID_NULL ; m_nTempGroupId = GDB_ID_NULL ; m_nCalcTabId = GDB_ID_NULL ; m_nCalcHeadId = GDB_ID_NULL ; m_nCalcExitId = GDB_ID_NULL ; m_nCalcToolId = GDB_ID_NULL ; m_dCalcRot1W = 1 ; m_nCalcSolCh = MCH_SCC_NONE ; } //---------------------------------------------------------------------------- Machine::~Machine( void) { Clear() ; } //---------------------------------------------------------------------------- void Machine::Clear( void) { // cancellazione eventuali gruppi geometrici associati if ( m_pGeomDB != nullptr) { if ( m_nGroupId != GDB_ID_NULL) m_pGeomDB->Erase( m_nGroupId) ; if ( m_nTempGroupId != GDB_ID_NULL) m_pGeomDB->Erase( m_nTempGroupId) ; } // pulizia interprete lua di macchina LuaExit() ; // reset membri m_pMchMgr = nullptr ; m_pGeomDB = nullptr ; m_sName.clear() ; m_sMachineDir.clear() ; m_nGroupId = GDB_ID_NULL ; m_nTempGroupId = GDB_ID_NULL ; m_mapGroups.clear() ; m_mapGroups.rehash( 20) ; } //---------------------------------------------------------------------------- bool Machine::Init( const string& sMachinesDir, const string& sMachineName, MachMgr* pMchMgr) { // pulisco Clear() ; // verifico ambiente if ( pMchMgr == nullptr || pMchMgr->GetContextId() == 0 || pMchMgr->GetGeomDB() == nullptr) return false ; m_pMchMgr = pMchMgr ; m_pGeomDB = m_pMchMgr->GetGeomDB() ; // verifico direttorio dati macchina m_sMachineDir = sMachinesDir + "\\" + sMachineName ; if ( ! ExistsDirectory( m_sMachineDir)) return false ; // creo il gruppo per la macchina int nId = m_pGeomDB->InsertGroup( GDB_ID_NULL, pMchMgr->GetMachAuxId(), GDB_LAST_SON, GLOB_FRM) ; if ( nId == GDB_ID_NULL) return false ; // imposto nome del gruppo m_pGeomDB->SetName( nId, sMachineName) ; // carico l'interprete lua dedicato alla macchina if ( ! LuaInit( sMachineName)) { m_pGeomDB->Erase( nId) ; return false ; } // salvo i dati m_nGroupId = nId ; m_sName = sMachineName ; // carico la macchina { string sMachineMde = m_sMachineDir + "\\" + sMachineName + ".Mde" ; bool bOk = LuaLoadMachine( sMachineMde) ; // cancello il gruppo temporaneo m_pGeomDB->Erase( m_nTempGroupId) ; m_nTempGroupId = GDB_ID_NULL ; // in caso di errore, cancello tutta la geometria if ( ! bOk) { m_pGeomDB->Erase( m_nGroupId) ; m_pGeomDB->Erase( m_nTempGroupId) ; m_nGroupId = GDB_ID_NULL ; m_nTempGroupId = GDB_ID_NULL ; m_sName.clear() ; m_sMachineDir.clear() ; } // metto tutti gli assi in posizione home bOk = bOk && ResetAllAxesPos() ; return bOk ; } //---------------------------------------------------------------------------- bool Machine::LoadMachineGeometry( const string& sMGeoName, const Vector3d& vtOffset) { // creo un sotto gruppo temporaneo in cui caricare la macchina m_nTempGroupId = m_pGeomDB->AddGroup( GDB_ID_NULL, m_pMchMgr->GetMachAuxId(), GLOB_FRM) ; if ( m_nTempGroupId == GDB_ID_NULL) return false ; // carico la macchina string sMachineNge = m_sMachineDir + "\\" + sMGeoName ; if ( ! m_pGeomDB->Load( sMachineNge, m_nTempGroupId)) return false ; // applico offset return m_pGeomDB->TranslateGlob( m_nTempGroupId, vtOffset) ; } //---------------------------------------------------------------------------- bool Machine::LoadMachineBase( const string& sName, const string& sGeo) { // recupero pezzo e layer della geometria originale della base string sPart, sLay ; Split( sGeo, "/", true, sPart, sLay) ; // cerco il gruppo nella geometria originale int nPart = m_pGeomDB->GetFirstNameInGroup( m_nTempGroupId, sPart) ; int nLay = m_pGeomDB->GetFirstNameInGroup( nPart, sLay) ; if ( nLay == GDB_ID_NULL) return false ; // lo sposto nella radice della macchina if ( ! m_pGeomDB->RelocateGlob( nLay, m_nGroupId, GDB_FIRST_SON)) return false ; // gli assegno il nome m_pGeomDB->SetName( nLay, sName) ; // lo inserisco nel dizionario dei gruppi della macchina return m_mapGroups.emplace( sName, nLay).second ; } //---------------------------------------------------------------------------- bool Machine::LoadMachineTable( const string& sName, const string& sParent, int nType, const Point3d& ptRef1, const string& sGeo) { // recupero pezzo e layer della geometria originale della tavola string sPart, sLay ; Split( sGeo, "/", true, sPart, sLay) ; // cerco il gruppo nella geometria originale int nPart = m_pGeomDB->GetFirstNameInGroup( m_nTempGroupId, sPart) ; int nLay = m_pGeomDB->GetFirstNameInGroup( nPart, sLay) ; if ( nLay == GDB_ID_NULL) return false ; // cerco il gruppo padre per spostarvelo int nParentId = GetGroup( sParent) ; if ( nParentId == GDB_ID_NULL || ! m_pGeomDB->RelocateGlob( nLay, nParentId, GDB_FIRST_SON)) return false ; // gli assegno il nome m_pGeomDB->SetName( nLay, sName) ; // aggiusto la posizione della tavola if ( ! AdjustTablePos( nLay, ptRef1)) return false ; // recupero l'area valida int nAreaId = m_pGeomDB->GetFirstNameInGroup( nLay, MCH_TAREA + "1") ; BBox3d b3Area1 ; if ( ! m_pGeomDB->GetGlobalBBox( nAreaId, b3Area1)) return false ; // installo e inizializzo il gestore della tavola Table* pTab = new(nothrow) Table ; if ( pTab == nullptr) return false ; pTab->Set( sName, nType, ptRef1, b3Area1) ; m_pGeomDB->SetUserObj( nLay, pTab) ; // lo inserisco nel dizionario dei gruppi della macchina return m_mapGroups.emplace( sName, nLay).second ; } //---------------------------------------------------------------------------- bool Machine::AdjustTablePos( int nLay, const Point3d& ptRef1) { // riferimento globale del gruppo tavola Frame3d frTable ; m_pGeomDB->GetGroupGlobFrame( nLay, frTable) ; // recupero il primo riferimento della tavola int nRef1 = m_pGeomDB->GetFirstNameInGroup( nLay, MCH_TREF + "1") ; if ( nRef1 == GDB_ID_NULL || m_pGeomDB->GetGeoType( nRef1) != GEO_FRAME3D) { string sOut = " Missing " + MCH_TREF + "1" ; LOG_ERROR( GetEMkLogger(), sOut.c_str()) ; return false ; } // recupero frame const Frame3d& frFrame = GetGeoFrame3d( m_pGeomDB->GetGeoObj( nRef1))->GetFrame() ; // ne calcolo l'origine in globale Point3d ptPos = frFrame.Orig() ; ptPos.ToGlob( frTable) ; // verifico eventuale aggiustamento di posizione Vector3d vtMove = ptRef1 - ptPos ; if ( ! vtMove.IsSmall()) { string sOut = " Move = (" + ToString( vtMove) + ")" ; LOG_ERROR( GetEMkLogger(), sOut.c_str()) ; return m_pGeomDB->TranslateGlob( nLay, vtMove) ; } else return true ; } //---------------------------------------------------------------------------- bool Machine::LoadMachineAxis( const string& sName, const string& sParent, int nType, const Point3d& ptPos, const Vector3d& vtDir, const STROKE& Stroke, const string& sGeo, double dHome) { // verifico sia di tipo ammesso if ( nType != MCH_AT_LINEAR && nType != MCH_AT_ROTARY) { LOG_ERROR( GetEMkLogger(), " Wrong Axis Type") ; return false ; } // recupero pezzo e layer della geometria originale dell'asse string sPart, sLay ; Split( sGeo, "/", true, sPart, sLay) ; // cerco il gruppo nella geometria originale int nPart = m_pGeomDB->GetFirstNameInGroup( m_nTempGroupId, sPart) ; int nLay = m_pGeomDB->GetFirstNameInGroup( nPart, sLay) ; if ( nLay == GDB_ID_NULL) { string sOut = " Missing Axis Part " + sPart ; LOG_ERROR( GetEMkLogger(), sOut.c_str()) ; return false ; } // cerco il gruppo padre per spostarvelo int nParentId = GetGroup( sParent) ; if ( nParentId == GDB_ID_NULL || ! m_pGeomDB->RelocateGlob( nLay, nParentId, GDB_FIRST_SON)) { string sOut = " Missing Parent Group " + sParent ; LOG_ERROR( GetEMkLogger(), sOut.c_str()) ; return false ; } // verifico che il valore di home sia nei limiti di corsa if ( dHome < Stroke.Min || dHome > Stroke.Max) { LOG_ERROR( GetEMkLogger(), " Home Position Out of stroke") ; return false ; } // gli assegno il nome m_pGeomDB->SetName( nLay, sName) ; // installo e inizializzo il gestore dell'asse Axis* pAxis = new(nothrow) Axis ; if ( pAxis == nullptr) return false ; pAxis->Set( sName, nType, ptPos, vtDir, Stroke, dHome) ; m_pGeomDB->SetUserObj( nLay, pAxis) ; // verifico il vettore rappresentativo dell'asse if ( ! AdjustAxisVector( nLay, sPart, sName, nType, ptPos, vtDir)) return false ; // lo inserisco nel dizionario dei gruppi della macchina return m_mapGroups.emplace( sName, nLay).second ; } //---------------------------------------------------------------------------- bool Machine::AdjustAxisVector( int nLay, const string& sPart, const string& sName, int nType, const Point3d& ptPos, const Vector3d& vtDir) { // verifico presenza vettore asse int nId = m_pGeomDB->GetFirstNameInGroup( nLay, sPart) ; if ( nId == GDB_ID_NULL || m_pGeomDB->GetGeoType( nId) != GEO_VECT3D) { string sOut = " Missing Axis Vector " + sPart ; LOG_ERROR( GetEMkLogger(), sOut.c_str()) ; return false ; } // lo sposto all'inizio del suo gruppo if ( ! m_pGeomDB->Relocate( nId, nLay, GDB_FIRST_SON)) return false ; // lo rinomino come l'asse if ( ! m_pGeomDB->SetName( nId, sName)) return false ; // lo nascondo if ( ! m_pGeomDB->SetStatus( nId, GDB_ST_OFF)) return false ; // riferimento globale del gruppo asse Frame3d frFrame ; m_pGeomDB->GetGroupGlobFrame( nLay, frFrame) ; // recupero punto base e vettore direzione IGeoVector3d* pGeoVct = GetGeoVector3d( m_pGeomDB->GetGeoObj( nId)) ; Point3d ptBase = pGeoVct->GetBase() ; Vector3d vtAxis = pGeoVct->GetVector() ; vtAxis.Normalize() ; // riassegno il vettore normalizzato pGeoVct->ChangeVector( vtAxis) ; // li porto in globale ptBase.ToGlob( frFrame) ; vtAxis.ToGlob( frFrame) ; // se asse lineare devo verificarne solo la direzione if ( nType == MCH_AT_LINEAR) { Vector3d vtDirN = vtDir ; if ( ! vtDirN.Normalize() || ! AreSameVectorApprox( vtAxis, vtDirN)) { string sOut = " Wrong Axis Vector or Dir " + sPart ; LOG_ERROR( GetEMkLogger(), sOut.c_str()) ; return false ; } return true ; } // altrimenti asse rotante, devo verificarne direzione e coassialitā else { Vector3d vtDirN = vtDir ; if ( ! vtDirN.Normalize() || ! AreSameVectorApprox( vtAxis, vtDirN)) { string sOut = " Wrong Axis Vector or Dir " + sPart ; LOG_ERROR( GetEMkLogger(), sOut.c_str()) ; return false ; } Vector3d vtDelta = ptPos - ptBase ; vtDelta -= ( vtDelta * vtDirN) * vtDirN ; if ( ! vtDelta.IsSmall()) { string sOut = " Wrong Axis Base move = (" + ToString( vtDelta) + ")" ; LOG_ERROR( GetEMkLogger(), sOut.c_str()) ; return false ; } return true ; } } //---------------------------------------------------------------------------- bool Machine::LoadMachineStdHead( const string& sName, const string& sParent, const std::string& sHSet, const Point3d& ptPos, const Vector3d& vtTDir, const Vector3d& vtADir, double dRot1W, const STROKE& Rot2Stroke, int nSolCh, const string& sGeo) { // recupero pezzo e layer della geometria originale dell'asse string sPart, sLay ; Split( sGeo, "/", true, sPart, sLay) ; // cerco il gruppo nella geometria originale int nPart = m_pGeomDB->GetFirstNameInGroup( m_nTempGroupId, sPart) ; int nLay = m_pGeomDB->GetFirstNameInGroup( nPart, sLay) ; if ( nLay == GDB_ID_NULL) return false ; // cerco il gruppo padre per spostarvelo int nParentId = GetGroup( sParent) ; if ( nParentId == GDB_ID_NULL || ! m_pGeomDB->RelocateGlob( nLay, nParentId, GDB_FIRST_SON)) return false ; // sistemo lo stato di visualizzazione bool bShow = ( sHSet == sName) ; m_pGeomDB->SetStatus( nLay, ( bShow ? GDB_ST_ON : GDB_ST_OFF)) ; // gli assegno il nome m_pGeomDB->SetName( nLay, sName) ; // installo e inizializzo il gestore della testa Head* pHead = new(nothrow) Head ; if ( pHead == nullptr) return false ; pHead->Set( sName, MCH_HT_STD, sHSet, vtADir, dRot1W, Rot2Stroke, nSolCh) ; m_pGeomDB->SetUserObj( nLay, pHead) ; // aggiorno la testa capostipite if ( ! AddHeadToSet( sHSet, sName)) return false ; // trasformazione del riferimento di uscita in gruppo di uscita MUEXITVECTOR vMuExit ; vMuExit.emplace_back( ptPos, vtTDir) ; if ( ! CreateExitGroups( nLay, vMuExit)) return false ; // lo inserisco nel dizionario dei gruppi della macchina return m_mapGroups.emplace( sName, nLay).second ; } //---------------------------------------------------------------------------- bool Machine::LoadMachineMultiHead( const string& sName, const string& sParent, const std::string& sHSet, const MUEXITVECTOR& vMuExit, const Vector3d& vtADir, double dRot1W, const STROKE& Rot2Stroke, int nSolCh, const string& sGeo) { // recupero pezzo e layer della geometria originale dell'asse string sPart, sLay ; Split( sGeo, "/", true, sPart, sLay) ; // cerco il gruppo nella geometria originale int nPart = m_pGeomDB->GetFirstNameInGroup( m_nTempGroupId, sPart) ; int nLay = m_pGeomDB->GetFirstNameInGroup( nPart, sLay) ; if ( nLay == GDB_ID_NULL) return false ; // cerco il gruppo padre per spostarvelo int nParentId = GetGroup( sParent) ; if ( nParentId == GDB_ID_NULL || ! m_pGeomDB->RelocateGlob( nLay, nParentId, GDB_FIRST_SON)) return false ; // sistemo lo stato di visualizzazione bool bShow = ( sHSet == sName) ; m_pGeomDB->SetStatus( nLay, ( bShow ? GDB_ST_ON : GDB_ST_OFF)) ; // gli assegno il nome m_pGeomDB->SetName( nLay, sName) ; // installo e inizializzo il gestore della testa Head* pHead = new(nothrow) Head ; if ( pHead == nullptr) return false ; pHead->Set( sName, MCH_HT_MULTI, sHSet, vtADir, dRot1W, Rot2Stroke, nSolCh) ; m_pGeomDB->SetUserObj( nLay, pHead) ; // aggiorno la testa capostipite if ( ! AddHeadToSet( sHSet, sName)) return false ; // trasformazione dei riferimenti di uscita in gruppi di uscita if ( ! CreateExitGroups( nLay, vMuExit)) return false ; // lo inserisco nel dizionario dei gruppi della macchina return m_mapGroups.emplace( sName, nLay).second ; } //---------------------------------------------------------------------------- int Machine::GetGroup( const string& sGroup) const { STRINT_UMAP::const_iterator Iter = m_mapGroups.find( sGroup) ; return (( Iter != m_mapGroups.end()) ? Iter->second : GDB_ID_NULL) ; } //---------------------------------------------------------------------------- Axis* Machine::GetAxis( int nGroup) const { return ( dynamic_cast( m_pGeomDB->GetUserObj( nGroup))) ; } //---------------------------------------------------------------------------- Table* Machine::GetTable( int nGroup) const { return ( dynamic_cast( m_pGeomDB->GetUserObj( nGroup))) ; } //---------------------------------------------------------------------------- Head* Machine::GetHead( int nGroup) const { return ( dynamic_cast( m_pGeomDB->GetUserObj( nGroup))) ; } //---------------------------------------------------------------------------- Exit* Machine::GetExit( int nGroup) const { return ( dynamic_cast( m_pGeomDB->GetUserObj( nGroup))) ; } //---------------------------------------------------------------------------- bool Machine::AddHeadToSet( const string& sHSet, const string& sName) { // se il capo-insieme coincide con la testa, non devo fare alcunchč if ( sHSet == sName) return true ; // recupero la testa capo-insieme Head* pHead = GetHead( GetGroup( sHSet)) ; if ( pHead == nullptr) return false ; // aggiungo questa testa all'insieme return pHead->AddHeadToHSet( sName) ; } //---------------------------------------------------------------------------- const STRVECTOR& Machine::GetHSet( const string& sHead) const { // vettore di stringhe vuoto, da restituire in caso di errore static const STRVECTOR vsNull ; // recupero la testa Head* pHead = GetHead( GetGroup( sHead)) ; if ( pHead == nullptr) return vsNull ; // recupero l'insieme di teste a cui appartiene const STRVECTOR& vsTmp = pHead->GetHSet() ; if ( vsTmp.empty()) return vsNull ; // se il primo membro coincide con la testa, allora č giā l'insieme di teste if ( vsTmp[0] == sHead) return vsTmp ; // altrimenti cerco l'insieme della prima testa return GetHSet( vsTmp[0]) ; } //---------------------------------------------------------------------------- bool Machine::EnableHeadInSet( const string& sHead) { // recupero l'insieme di teste const STRVECTOR& vsHSet = GetHSet( sHead) ; if ( vsHSet.empty()) return false ; // spengo tutte le teste tranne questa for ( size_t i = 0 ; i < vsHSet.size() ; ++ i) { int nH = GetGroup( vsHSet[i]) ; bool bShow = ( vsHSet[i] == sHead) ; m_pGeomDB->SetStatus( nH, ( bShow ? GDB_ST_ON : GDB_ST_OFF)) ; } return true ; } //---------------------------------------------------------------------------- bool Machine::CreateExitGroups( int nLay, const MUEXITVECTOR& vMuExit) { // riferimento globale del gruppo testa Frame3d frHead ; m_pGeomDB->GetGroupGlobFrame( nLay, frHead) ; // ciclo sulle uscite for ( int i = 0 ; i < int( vMuExit.size()) ; ++ i) { string sName = MCH_EXIT + ToString( i+1) ; // se trovo riferimento per uscita, lo sostituisco con gruppo equivalente int nT = m_pGeomDB->GetFirstNameInGroup( nLay, sName) ; if ( nT != GDB_ID_NULL && m_pGeomDB->GetGeoType( nT) == GEO_FRAME3D) { // recupero frame const Frame3d& frFrame = GetGeoFrame3d( m_pGeomDB->GetGeoObj( nT))->GetFrame() ; // verifico dati frame rispetto ad uscita (in globale) Point3d ptPos = frFrame.Orig() ; ptPos.ToGlob( frHead) ; Vector3d vtTDir = frFrame.VersZ() ; vtTDir.ToGlob(frHead) ; vtTDir.Normalize() ; Vector3d vtDelta = vMuExit[i].ptPos - ptPos ; if ( ! vtDelta.IsSmall()) { string sOut = " Wrong Exit Position " + sName + " move = (" + ToString( vtDelta) +")" ; LOG_ERROR( GetEMkLogger(), sOut.c_str()) ; return false ; } Vector3d vtDirN = vMuExit[i].vtTDir ; if ( ! vtDirN.Normalize() || ! AreSameVectorApprox( vtTDir, vtDirN)) { string sOut = " Wrong Exit Vector " + sName ; LOG_ERROR( GetEMkLogger(), sOut.c_str()) ; return false ; } // inserisco gruppo con riferimento del frame int nGT = m_pGeomDB->InsertGroup( GDB_ID_NULL, nT, GDB_BEFORE, frFrame) ; if ( nGT == GDB_ID_NULL) { string sOut = " Error inserting group " + sName ; LOG_ERROR( GetEMkLogger(), sOut.c_str()) ; return false ; } // cambio nome al riferimento e lo nascondo m_pGeomDB->SetName( nT, "_" + sName) ; m_pGeomDB->SetStatus( nT, GDB_ST_OFF) ; // assegno nome al gruppo m_pGeomDB->SetName( nGT, sName) ; // installo e inizializzo il gestore dell'uscita Exit* pExit = new(nothrow) Exit ; if ( pExit == nullptr) return false ; pExit->Set( sName, vMuExit[i].ptPos, vMuExit[i].vtTDir) ; m_pGeomDB->SetUserObj( nGT, pExit) ; } else { string sOut = " Error finding frame " + sName ; LOG_ERROR( GetEMkLogger(), sOut.c_str()) ; return false ; } } return true ; }