diff --git a/EgtMachKernel.rc b/EgtMachKernel.rc
index 348990f..df97662 100644
Binary files a/EgtMachKernel.rc and b/EgtMachKernel.rc differ
diff --git a/EgtMachKernel.vcxproj b/EgtMachKernel.vcxproj
index f6f0d6a..99991b8 100644
--- a/EgtMachKernel.vcxproj
+++ b/EgtMachKernel.vcxproj
@@ -205,8 +205,10 @@ copy $(TargetPath) \EgtProg\Dll64
+
+
@@ -225,7 +227,9 @@ copy $(TargetPath) \EgtProg\Dll64
+
+
diff --git a/EgtMachKernel.vcxproj.filters b/EgtMachKernel.vcxproj.filters
index 846e1bd..cf0b85e 100644
--- a/EgtMachKernel.vcxproj.filters
+++ b/EgtMachKernel.vcxproj.filters
@@ -54,6 +54,12 @@
Source Files
+
+ Source Files
+
+
+ Source Files
+
@@ -77,6 +83,12 @@
Header Files
+
+ Header Files
+
+
+ Header Files
+
diff --git a/GeoCalc.cpp b/GeoCalc.cpp
new file mode 100644
index 0000000..3237097
--- /dev/null
+++ b/GeoCalc.cpp
@@ -0,0 +1,102 @@
+//----------------------------------------------------------------------------
+// EgalTech 2015-2015
+//----------------------------------------------------------------------------
+// File : GeoCalc.cpp Data : 12.05.15 Versione : 1.6e3
+// Contenuto : Funzioni varie e speciali di calcolo geometrico.
+//
+//
+//
+// Modifiche : 12.05.15 DS Creazione modulo.
+//
+//
+//----------------------------------------------------------------------------
+
+//--------------------------- Include ----------------------------------------
+#include "stdafx.h"
+#include "GeoCalc.h"
+
+//----------------------------------------------------------------------------
+int
+GetRotationComponent( const Vector3d& vtDir1, double dComp, const Vector3d& vtDir2, const Vector3d& vtRotAx,
+ double& dAng1Deg, double& dAng2Deg, bool& bDet)
+{
+ // Vettori normalizzati.
+ // Si rimanda alle note, con vtDir1 == vtT, vtDir2 == vtZ, vtRotAx == vtW, dComp == dTaz.
+
+ // calcolo della componenete di T0 lungo W
+ double dT0w = vtDir1 * vtRotAx ;
+
+ // calcolo dell'asse U del riferimento UVW, coincidente con X di XYZ
+ Vector3d vtPvZW = vtDir2 ^ vtRotAx ;
+
+ // se Z e W sono allineati (o quasi)
+ Vector3d vtU = vtPvZW ;
+ if ( ! vtU.Normalize()) {
+ // se le componenti concordano, angolo indeterminato
+ if ( fabs( dT0w - dComp) < 0.5 * SIN_EPS_ANG_SMALL) {
+ bDet = false ;
+ return 1 ;
+ }
+ // altrimenti, nessuna soluzione
+ else
+ return 0 ;
+ }
+
+ // calcolo della componenete di T0 lungo U
+ double dT0u = vtDir1 * vtU ;
+
+ // calcolo dell'asse V di UVW
+ Vector3d vtV = vtRotAx ^ vtU ;
+
+ // dovrebbe essere sempre normalizzabile, ma ...
+ if ( ! vtV.Normalize())
+ return 0 ;
+
+ // calcolo della componenete di T0 lungo V
+ double dT0v = vtDir1 * vtV ;
+
+ // calcolo coseno e seno dell'angolo comprezo tra vtZ e vtW, visto da vtU
+ double dCosG = vtDir2 * vtRotAx ;
+ double dSinG = vtPvZW * vtU ;
+
+ // calcolo della prima parte dell'angolo
+ double dOffsAngRad = PIGRECO / 2 - atan2( dT0v, dT0u) ;
+
+ // calcolo della seconda parte dell'angolo
+ // denominatore nullo ...
+ if ( ( fabs( dT0v) < EPS_ZERO && fabs( dT0u) < EPS_ZERO) || fabs( dSinG) < SIN_EPS_ANG_ZERO) {
+ // calcolo della componenete di T0 lungo Z
+ double dT0z = vtDir1 * vtDir2 ;
+ // se le componenti concordano, angolo indeterminato
+ if ( fabs( dT0z - dComp) < 0.5 * SIN_EPS_ANG_SMALL) {
+ bDet = false ;
+ return 1 ;
+ }
+ // altrimenti, nessuna soluzione
+ else
+ return 0 ;
+ }
+ double dT0uv = sqrt( dT0v * dT0v + dT0u * dT0u) ;
+ double dDenom = dT0uv * dSinG ;
+ double dNumer = dComp - dT0w * dCosG ;
+ // due angoli possibili
+ if ( fabs( dDenom) > fabs( dNumer)) {
+ double dDeltaAngRad = acos( dNumer / dDenom) ;
+ dAng1Deg = ( dOffsAngRad + dDeltaAngRad) * RADTODEG ;
+ dAng2Deg = ( dOffsAngRad - dDeltaAngRad) * RADTODEG ;
+ bDet = true ;
+ return 2 ;
+ }
+ // un angolo possibile
+ else if ( fabs( dDenom) > fabs( dNumer) - SIN_EPS_ANG_ZERO) {
+ if ( dDenom * dNumer > 0)
+ dAng1Deg = dOffsAngRad * RADTODEG ;
+ else
+ dAng1Deg = dOffsAngRad * RADTODEG + ANG_STRAIGHT ;
+ bDet = true ;
+ return 1 ;
+ }
+ // nessun angolo possibile
+ else
+ return 0 ;
+}
diff --git a/GeoCalc.h b/GeoCalc.h
new file mode 100644
index 0000000..8b1a265
--- /dev/null
+++ b/GeoCalc.h
@@ -0,0 +1,21 @@
+//----------------------------------------------------------------------------
+// EgalTech 2015-2015
+//----------------------------------------------------------------------------
+// File : GeoCalc.h Data : 12.05.15 Versione : 1.6e5
+// Contenuto : Prototipi funzioni di calcolo geometrico.
+//
+//
+//
+// Modifiche : 12.05.15 DS Creazione modulo.
+//
+//
+//----------------------------------------------------------------------------
+
+#pragma once
+
+#include "/EgtDev/Include/EGkVector3d.h"
+
+//----------------------------------------------------------------------------
+int GetRotationComponent( const Vector3d& vtDir1, double dComp, const Vector3d& vtDir2, const Vector3d& vtRotAx,
+ double& dAng1Deg, double& dAng2Deg, bool& bDet) ;
+
diff --git a/MachConst.h b/MachConst.h
index 8347366..330d8f1 100644
--- a/MachConst.h
+++ b/MachConst.h
@@ -1,5 +1,5 @@
//----------------------------------------------------------------------------
-// EgalTech 20135-2015
+// EgalTech 2015-2015
//----------------------------------------------------------------------------
// File : MachConst.h Data : 24.03.15 Versione : 1.6c6
// Contenuto : Costanti generali per calcoli lavorazioni.
diff --git a/MachMgr.h b/MachMgr.h
index 6fef0ca..6f9c76f 100644
--- a/MachMgr.h
+++ b/MachMgr.h
@@ -84,6 +84,13 @@ class MachMgr : public IMachMgr
virtual bool ResetAxisPos( const std::string& sAxis) ;
virtual bool LoadTool( const std::string& sHead, int nExit, const std::string& sTool) ;
virtual bool ResetHeadSet( const std::string& sHead) ;
+ virtual bool SetCalcTable( const std::string& sTable) ;
+ virtual bool SetCalcTool( const std::string& sTool, const std::string& sHead, int nExit) ;
+ virtual bool GetCalcAngles( const Vector3d& vtDirT,
+ int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2) ;
+ virtual bool GetCalcPositions( const Point3d& ptP, double dAngA, double dAngB,
+ int& nStat, double& dX, double& dY, double& dZ) ;
+ virtual bool VerifyOutOfStroke( double dX, double dY, double dZ, double dAngA, double dAngB, int& nStat) ;
public :
MachMgr( void) ;
diff --git a/MachMgrMachines.cpp b/MachMgrMachines.cpp
index 486e19d..f471c03 100644
--- a/MachMgrMachines.cpp
+++ b/MachMgrMachines.cpp
@@ -129,3 +129,55 @@ MachMgr::ResetHeadSet( const string& sHead)
return false ;
return m_vMachines[m_nCurrMch]->ResetHeadSet( sHead) ;
}
+
+//----------------------------------------------------------------------------
+bool
+MachMgr::SetCalcTable( const string& sTable)
+{
+ if ( m_nCurrMch < 0 || m_nCurrMch >= int( m_vMachines.size()) ||
+ m_vMachines[m_nCurrMch] == nullptr)
+ return false ;
+ return m_vMachines[m_nCurrMch]->SetCurrTable( sTable) ;
+}
+
+//----------------------------------------------------------------------------
+bool
+MachMgr::SetCalcTool( const string& sTool, const string& sHead, int nExit)
+{
+ if ( m_nCurrMch < 0 || m_nCurrMch >= int( m_vMachines.size()) ||
+ m_vMachines[m_nCurrMch] == nullptr)
+ return false ;
+ return m_vMachines[m_nCurrMch]->SetCurrTool( sTool, sHead, nExit) ;
+}
+
+//----------------------------------------------------------------------------
+bool
+MachMgr::GetCalcAngles( const Vector3d& vtDirT,
+ int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2)
+{
+ if ( m_nCurrMch < 0 || m_nCurrMch >= int( m_vMachines.size()) ||
+ m_vMachines[m_nCurrMch] == nullptr)
+ return false ;
+ return m_vMachines[m_nCurrMch]->GetAngles( vtDirT, nStat, dAngA1, dAngB1, dAngA2, dAngB2) ;
+}
+
+//----------------------------------------------------------------------------
+bool
+MachMgr::GetCalcPositions( const Point3d& ptP, double dAngA, double dAngB,
+ int& nStat, double& dX, double& dY, double& dZ)
+{
+ if ( m_nCurrMch < 0 || m_nCurrMch >= int( m_vMachines.size()) ||
+ m_vMachines[m_nCurrMch] == nullptr)
+ return false ;
+ return m_vMachines[m_nCurrMch]->GetPositions( ptP, dAngA, dAngB, nStat, dX, dY, dZ) ;
+}
+
+//----------------------------------------------------------------------------
+bool
+MachMgr::VerifyOutOfStroke( double dX, double dY, double dZ, double dAngA, double dAngB, int& nStat)
+{
+ if ( m_nCurrMch < 0 || m_nCurrMch >= int( m_vMachines.size()) ||
+ m_vMachines[m_nCurrMch] == nullptr)
+ return false ;
+ return m_vMachines[m_nCurrMch]->VerifyOutOfStroke( dX, dY, dZ, dAngA, dAngB, nStat) ;
+}
diff --git a/Machine.cpp b/Machine.cpp
index 2795e87..d5ae6be 100644
--- a/Machine.cpp
+++ b/Machine.cpp
@@ -280,14 +280,12 @@ Machine::LoadMachineStdHead( const string& sName, const string& sParent, const s
// gli assegno il tipo
string sInfo = MCH_HEAD + ToString( MCH_HT_STD) ;
m_pMchMgr->m_pGeomDB->SetInfo( nLay, "Type", sInfo) ;
- // gli assegno la posizione
- m_pMchMgr->m_pGeomDB->SetInfo( nLay, "Pos", ptPos) ;
- // gli assegno la direzione utensile
- m_pMchMgr->m_pGeomDB->SetInfo( nLay, "TDir", vtTDir) ;
// gli assegno la direzione ausiliaria
m_pMchMgr->m_pGeomDB->SetInfo( nLay, "ADir", vtADir) ;
// trasformazione del riferimento di uscita in gruppo di uscita
- if ( ! CreateExitGroups( nLay, 1))
+ 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 ;
@@ -296,7 +294,7 @@ Machine::LoadMachineStdHead( const string& sName, const string& sParent, const s
//----------------------------------------------------------------------------
bool
Machine::LoadMachineMultiHead( const string& sName, const string& sParent, const std::string& sHSet,
- MUEXITVECTOR vMuExit,
+ const MUEXITVECTOR& vMuExit,
const Vector3d& vtADir, const string& sGeo)
{
// recupero pezzo e layer della geometria originale dell'asse
@@ -317,24 +315,17 @@ Machine::LoadMachineMultiHead( const string& sName, const string& sParent, const
m_pMchMgr->m_pGeomDB->SetStatus( nLay, ( bShow ? GDB_ST_ON : GDB_ST_OFF)) ;
// gli assegno il nome
m_pMchMgr->m_pGeomDB->SetName( nLay, sName) ;
- // gli assegno il tipo
- string sInfo = MCH_HEAD + ToString( MCH_HT_MULTI) ;
- m_pMchMgr->m_pGeomDB->SetInfo( nLay, "Type", sInfo) ;
// gli assegno l'insieme teste e aggiorno il capostipite
m_pMchMgr->m_pGeomDB->SetInfo( nLay, "HSet", sHSet) ;
if ( ! AddHeadToSet( sHSet, sName))
return false ;
- // gli assegno le posizioni e direzioni delle uscite
- for ( int i = 0 ; i < int( vMuExit.size()) ; ++ i) {
- string sPos = "Pos" + ToString( i + 1) ;
- m_pMchMgr->m_pGeomDB->SetInfo( nLay, sPos, vMuExit[i].ptPos) ;
- string sTDir = "TDir" + ToString( i + 1) ;
- m_pMchMgr->m_pGeomDB->SetInfo( nLay, sTDir, vMuExit[i].vtTDir) ;
- }
+ // gli assegno il tipo
+ string sInfo = MCH_HEAD + ToString( MCH_HT_MULTI) ;
+ m_pMchMgr->m_pGeomDB->SetInfo( nLay, "Type", sInfo) ;
// gli assegno la direzione ausiliaria
m_pMchMgr->m_pGeomDB->SetInfo( nLay, "ADir", vtADir) ;
// trasformazione dei riferimenti di uscita in gruppi di uscita
- if ( ! CreateExitGroups( nLay, int( vMuExit.size())))
+ if ( ! CreateExitGroups( nLay, vMuExit))
return false ;
// lo inserisco nel dizionario dei gruppi della macchina
return m_mapGroups.emplace( sName, nLay).second ;
@@ -430,11 +421,11 @@ Machine::EnableHeadInSet( const string& sHead)
//----------------------------------------------------------------------------
bool
-Machine::CreateExitGroups( int nLay, int nExitNbr)
+Machine::CreateExitGroups( int nLay, const MUEXITVECTOR& vMuExit)
{
// ciclo sulle uscite
- for ( int i = 1 ; i <= nExitNbr ; ++ i) {
- string sName = "T" + ToString( i) ;
+ for ( int i = 0 ; i < int( vMuExit.size()) ; ++ i) {
+ string sName = "T" + ToString( i+1) ;
// se trovo riferimento per uscita, lo sostituisco con gruppo equivalente
int nT = m_pMchMgr->m_pGeomDB->GetFirstNameInGroup( nLay, sName) ;
if ( nT != GDB_ID_NULL && m_pMchMgr->m_pGeomDB->GetGeoType( nT) == GEO_FRAME3D) {
@@ -446,7 +437,12 @@ Machine::CreateExitGroups( int nLay, int nExitNbr)
return false ;
}
m_pMchMgr->m_pGeomDB->Erase( nT) ;
+ // assegno nome
m_pMchMgr->m_pGeomDB->SetName( nGT, sName) ;
+ // gli assegno la posizione
+ m_pMchMgr->m_pGeomDB->SetInfo( nGT, "Pos", vMuExit[i].ptPos) ;
+ // gli assegno la direzione utensile
+ m_pMchMgr->m_pGeomDB->SetInfo( nGT, "TDir", vMuExit[i].vtTDir) ;
}
else {
string sOut = "Error finding frame " + sName ;
diff --git a/Machine.h b/Machine.h
index bba6f6c..32e8563 100644
--- a/Machine.h
+++ b/Machine.h
@@ -37,6 +37,17 @@ struct MuExit {
} ;
typedef std::vector MUEXITVECTOR ;
+//----------------------------------------------------------------------------
+struct KinAxis {
+ int nGrpId ;
+ bool bLinear ;
+ bool bHead ;
+ Point3d ptPos ;
+ Vector3d vtDir ;
+ STROKE stroke ;
+};
+typedef std::vector KINAXISVECTOR ;
+
//----------------------------------------------------------------------------
class Machine
{
@@ -54,6 +65,13 @@ class Machine
bool GetAxisPos( const std::string& sAxis, double& dVal) ;
bool GetAxisHomePos( const std::string& sAxis, double& dHomeVal) ;
bool ResetAxisPos( const std::string& sAxis) ;
+ bool SetCurrTable( const std::string& sTable) ;
+ bool SetCurrTool( const std::string& sTool, const std::string& sHead, int nExit) ;
+ bool GetAngles( const Vector3d& vtDirT,
+ int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2) ;
+ bool GetPositions( const Point3d& ptP, double dA, double dB,
+ int& nStat, double& dX, double& dY, double& dZ) ;
+ bool VerifyOutOfStroke( double dX, double dY, double dZ, double dAngA, double dAngB, int& nStat) ;
private :
void Clear( void) ;
@@ -68,7 +86,7 @@ class Machine
const Point3d& ptPos, const Vector3d& vtTDir,
const Vector3d& vtADir, const std::string& sGeo) ;
bool LoadMachineMultiHead( const std::string& sName, const std::string& sParent, const std::string& sHSet,
- MUEXITVECTOR vMuExit,
+ const MUEXITVECTOR& vMuExit,
const Vector3d& vtADir, const std::string& sGeo) ;
int GetGroup( const std::string& sGroup) ;
bool IsAxisGroup( int nGroup) ;
@@ -77,7 +95,9 @@ class Machine
bool AddHeadToSet( const std::string& sHSet, const std::string& sName) ;
bool GetHSet( const std::string& sHead, STRVECTOR& vsHSet) ;
bool EnableHeadInSet( const std::string& sHead) ;
- bool CreateExitGroups( int nLay, int nExitNbr) ;
+ bool CreateExitGroups( int nLay, const MUEXITVECTOR& vMuExit) ;
+ bool CalculateKinematicChain( void) ;
+ bool AddKinematicAxis( bool bOnHead, int nId) ;
bool LuaInit( const std::string& sMachineName) ;
bool LuaExit( void) ;
@@ -87,13 +107,22 @@ class Machine
typedef std::unordered_map< std::string, int> STRINT_UMAP ;
private :
- MachMgr* m_pMchMgr ; // puntatore al gestore di tutte le lavorazioni
- LuaMgr m_LuaMgr ; // interprete lua della macchina
- std::string m_sName ; // nome della macchina
- std::string m_sMachineDir ; // direttorio della macchina
- int m_nGroupId ; // Id del gruppo della macchina
- int m_nTempGroupId ; // Id del gruppo temporaneo macchina per carico
- STRINT_UMAP m_mapGroups ; // dizionario dei gruppi della macchina
+ MachMgr* m_pMchMgr ; // puntatore al gestore di tutte le lavorazioni
+ LuaMgr m_LuaMgr ; // interprete lua della macchina
+ std::string m_sName ; // nome della macchina
+ std::string m_sMachineDir ; // direttorio della macchina
+ int m_nGroupId ; // Id del gruppo della macchina
+ int m_nTempGroupId ; // Id del gruppo temporaneo macchina per carico
+ STRINT_UMAP m_mapGroups ; // dizionario dei gruppi della macchina
+ int m_nCalcTabId ; // tavola corrente per calcoli
+ int m_nCalcHeadId ; // testa corrente per calcoli
+ int m_nCalcExitId ; // uscita corrente per calcoli
+ int m_nCalcToolId ; // utensile corrente per calcoli
+ Point3d m_ptCalcPos ; // posizione utensile a riposo per calcoli
+ Vector3d m_vtCalcDir ; // direzione utensile a riposo per calcoli
+ double m_dCalcTLen ; // lunghezza utensile corrente per calcoli
+ KINAXISVECTOR m_vCalcLinAx ; // vettore assi lineari attivi per calcoli
+ KINAXISVECTOR m_vCalcRotAx ; // vettore assi rotanti attivi per calcoli
// Static per interprete Lua di macchina
private :
diff --git a/MachineCalc.cpp b/MachineCalc.cpp
new file mode 100644
index 0000000..adaaf80
--- /dev/null
+++ b/MachineCalc.cpp
@@ -0,0 +1,363 @@
+//----------------------------------------------------------------------------
+// EgalTech 2015-2015
+//----------------------------------------------------------------------------
+// File : MachineCalc.cpp Data : 12.05.15 Versione : 1.6e3
+// Contenuto : Implementazione gestione macchina : funzioni di calcolo.
+//
+//
+//
+// Modifiche : 12.05.15 DS Creazione modulo.
+//
+//
+//----------------------------------------------------------------------------
+
+//--------------------------- Include ----------------------------------------
+#include "stdafx.h"
+#include "MachMgr.h"
+#include "GeoCalc.h"
+#include "DllMain.h"
+#include "/EgtDev/Include/EGkGeoVector3d.h"
+#include "/EgtDev/Include/EGnStringUtils.h"
+#include "/EgtDev/Include/EGnFileUtils.h"
+
+using namespace std ;
+
+//----------------------------------------------------------------------------
+bool
+Machine::SetCurrTable( const string& sTable)
+{
+ // recupero il gruppo della tavola
+ m_nCalcTabId = GetGroup( sTable) ;
+ if ( m_nCalcTabId == GDB_ID_NULL || ! IsTableGroup( m_nCalcTabId)) {
+ m_nCalcTabId = GDB_ID_NULL ;
+ return false ;
+ }
+ return true ;
+}
+
+//----------------------------------------------------------------------------
+bool
+Machine::SetCurrTool( const string& sTool, const string& sHead, int nExit)
+{
+ // azzero tutto
+ m_nCalcHeadId = GDB_ID_NULL ;
+ m_nCalcExitId = GDB_ID_NULL ;
+ m_nCalcToolId = GDB_ID_NULL ;
+ m_dCalcTLen = 0 ;
+ // recupero il gruppo della testa
+ int nHeadId = GetGroup( sHead) ;
+ if ( nHeadId == GDB_ID_NULL || ! IsHeadGroup( nHeadId))
+ return false ;
+ // recupero il gruppo dell'uscita
+ string sExit = "T" + ToString( nExit) ;
+ int nExitId = m_pMchMgr->m_pGeomDB->GetFirstNameInGroup( nHeadId, sExit) ;
+ if ( nExitId == GDB_ID_NULL || m_pMchMgr->m_pGeomDB->GetGdbType( nExitId) != GDB_TY_GROUP)
+ return false ;
+ // recupero posizione e direzione a riposo
+ Point3d ptPos ;
+ if ( ! m_pMchMgr->m_pGeomDB->GetInfo( nExitId, "Pos", ptPos))
+ return false ;
+ Vector3d vtDir ;
+ if ( ! m_pMchMgr->m_pGeomDB->GetInfo( nExitId, "TDir", vtDir))
+ return false ;
+ // recupero i dati dell'utensile
+ if ( ! LoadTool( sHead, nExit, sTool))
+ return false ;
+ int nToolId = m_pMchMgr->m_pGeomDB->GetFirstNameInGroup( nExitId, sTool) ;
+ if ( nToolId == GDB_ID_NULL || m_pMchMgr->m_pGeomDB->GetGdbType( nToolId) != GDB_TY_GROUP)
+ return false ;
+ double dTLen ;
+ if ( ! m_pMchMgr->m_pGeomDB->GetInfo( nToolId, "L", dTLen))
+ return false ;
+ // assegno tutti i dati
+ m_nCalcHeadId = nHeadId ;
+ m_nCalcExitId = nExitId ;
+ m_nCalcToolId = nToolId ;
+ m_ptCalcPos = ptPos ;
+ m_vtCalcDir = vtDir ;
+ m_dCalcTLen = dTLen ;
+ // determino la catena cinematica
+ return CalculateKinematicChain() ;
+}
+
+//----------------------------------------------------------------------------
+bool
+Machine::CalculateKinematicChain( void)
+{
+ // azzero tutti gli assi della catena cinematica
+ m_vCalcLinAx.clear() ;
+ m_vCalcRotAx.clear() ;
+ // recupero gli assi di tavola
+ if ( m_nCalcTabId == GDB_ID_NULL)
+ return false ;
+ int nTParId = m_pMchMgr->m_pGeomDB->GetParentId( m_nCalcTabId) ;
+ if ( nTParId == GDB_ID_NULL)
+ return false ;
+ while ( IsAxisGroup( nTParId)) {
+ if ( ! AddKinematicAxis( false, nTParId))
+ return false ;
+ nTParId = m_pMchMgr->m_pGeomDB->GetParentId( nTParId) ;
+ }
+ // recupero gli assi di testa
+ if ( m_nCalcHeadId == GDB_ID_NULL)
+ return false ;
+ int nHParId = m_pMchMgr->m_pGeomDB->GetParentId( m_nCalcHeadId) ;
+ if ( nHParId == GDB_ID_NULL)
+ return false ;
+ while ( IsAxisGroup( nHParId)) {
+ if ( ! AddKinematicAxis( true, nHParId))
+ return false ;
+ nHParId = m_pMchMgr->m_pGeomDB->GetParentId( nHParId) ;
+ }
+ // verifiche sugli assi lineari :
+ // devono essere 3
+ if ( m_vCalcLinAx.size() != 3)
+ return false ;
+ // devono essere ordinabili come XYZ
+ if ( ! m_vCalcLinAx[0].vtDir.IsXplus()) {
+ if ( m_vCalcLinAx[1].vtDir.IsXplus())
+ swap( m_vCalcLinAx[0], m_vCalcLinAx[1]) ;
+ else if ( m_vCalcLinAx[2].vtDir.IsXplus())
+ swap( m_vCalcLinAx[0], m_vCalcLinAx[2]) ;
+ else
+ return false ;
+ }
+ if ( ! m_vCalcLinAx[1].vtDir.IsYplus()) {
+ if ( m_vCalcLinAx[2].vtDir.IsYplus())
+ swap( m_vCalcLinAx[1], m_vCalcLinAx[2]) ;
+ else
+ return false ;
+ }
+ // verifiche sugli assi rotanti :
+ // se 0 o 1 va bene
+ if ( m_vCalcRotAx.size() <= 1)
+ return true ;
+ // se 2 va bene
+ if ( m_vCalcRotAx.size() == 2) {
+ // se entrambi di testa devo invertirne l'ordine
+ if ( m_vCalcRotAx[0].bHead && m_vCalcRotAx[1].bHead)
+ swap( m_vCalcRotAx[0], m_vCalcRotAx[1]) ;
+ return true ;
+ }
+ // altrimenti non ancora gestito, quindi errore
+ return false ;
+}
+
+//----------------------------------------------------------------------------
+bool
+Machine::AddKinematicAxis( bool bOnHead, int nId)
+{
+ KinAxis kAx ;
+ // assegno id
+ kAx.nGrpId = nId ;
+ // recupero il tipo di asse
+ string sType ;
+ if ( ! m_pMchMgr->m_pGeomDB->GetInfo( nId, "Type", sType))
+ return false ;
+ kAx.bLinear = ( sType != MCH_AXIS + ToString( MCH_AT_ROTARY)) ;
+ // assegno posizione su catena cinematica
+ kAx.bHead = bOnHead ;
+ // recupero la posizione
+ if ( ! m_pMchMgr->m_pGeomDB->GetInfo( nId, "Pos", kAx.ptPos))
+ return false ;
+ // recupero la direzione e la normalizzo
+ if ( ! m_pMchMgr->m_pGeomDB->GetInfo( nId, "Dir", kAx.vtDir) || ! kAx.vtDir.Normalize())
+ return false ;
+ // recupero i limiti di corsa
+ string sStroke ;
+ if ( ! m_pMchMgr->m_pGeomDB->GetInfo( nId, "Stroke", sStroke) || ! FromString( sStroke, kAx.stroke.v))
+ return false ;
+ // se lineare di tavola, devo invertirlo
+ if ( kAx.bLinear && ! kAx.bHead)
+ kAx.vtDir.Invert() ;
+ // lo inserisco nella opportuna lista degli assi
+ if ( kAx.bLinear)
+ m_vCalcLinAx.emplace_back( kAx) ;
+ else
+ m_vCalcRotAx.emplace_back( kAx) ;
+ return true ;
+}
+
+//----------------------------------------------------------------------------
+bool
+Machine::GetAngles( const Vector3d& vtDirT,
+ int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2)
+{
+ // annullo tutti gli angoli
+ nStat = 0 ; dAngA1 = 0 ; dAngB1 = 0 ; dAngA2 = 0 ; dAngB2 = 0 ;
+ // se nessun asse rotante, non c'è alcunchè da calcolare
+ if ( m_vCalcRotAx.size() == 0) {
+ nStat = 1 ;
+ return true ;
+ }
+ // direzione fresa normalizzata
+ Vector3d vtDirTn = vtDirT ;
+ if ( ! vtDirTn.Normalize())
+ return false ;
+ // direzione fresa su testa a riposo
+ Vector3d vtDirH = m_vtCalcDir ;
+ // direzione primo asse rotante
+ Vector3d vtAx1 = m_vCalcRotAx[0].vtDir ;
+ // se asse di tavola, ne inverto la direzione
+ if ( ! m_vCalcRotAx[0].bHead)
+ vtAx1.Invert() ;
+ // componente versore fresa desiderato su direzione primo asse rotante
+ double dCompTSuAxR1 = vtDirTn * vtAx1 ;
+ // se c'è secondo asse rotante, si calcola angolo per avere il componente appena calcolato
+ bool bDet = true ;
+ Vector3d vtDirH1, vtDirH2 ;
+ if ( m_vCalcRotAx.size() == 2) {
+ // direzione secondo asse rotante
+ Vector3d vtAx2 = m_vCalcRotAx[1].vtDir ;
+ // se asse di tavola, ne inverto la direzione
+ if ( ! m_vCalcRotAx[1].bHead)
+ vtAx2.Invert() ;
+ // calcolo secondo angolo di rotazione
+ nStat = GetRotationComponent( vtDirH, dCompTSuAxR1, vtAx1, vtAx2, dAngB1, dAngB2, bDet) ;
+ // aggiornamento direzione fresa su testa
+ if ( nStat >= 1) {
+ // se indeterminato lo azzero
+ if ( ! bDet)
+ dAngB1 = 0 ;
+ // eseguo aggiornamento
+ vtDirH1 = vtDirH ;
+ vtDirH1.Rotate( vtAx2, dAngB1) ;
+ }
+ if ( nStat == 2) {
+ vtDirH2 = vtDirH ;
+ vtDirH2.Rotate( vtAx2, dAngB2) ;
+ }
+ }
+ // altrimenti verifico se compatibili
+ else {
+ // componente versore utensile su direzione primo asse
+ double dCompHSuAxR1 = vtDirH * vtAx1 ;
+ // componenti versori fresa e utensile perpendicolari direzione primo asse
+ double dTemp = 1 - dCompTSuAxR1 * dCompTSuAxR1 ;
+ double dCompTOrtAxR1 = ( ( dTemp > EPS_ZERO) ? sqrt( dTemp) : 0) ;
+ dTemp = 1 - dCompHSuAxR1 * dCompHSuAxR1 ;
+ double dCompHOrtAxR1 = ( ( dTemp > EPS_ZERO) ? sqrt( dTemp) : 0) ;
+ // verifica ( max delta angolare < 0.002 deg)
+ const double SIN_ANG_ERROR = sin( 0.002 * DEGTORAD) ;
+ if ( fabs( dCompTOrtAxR1 * dCompHSuAxR1 - dCompHOrtAxR1 * dCompTSuAxR1) < SIN_ANG_ERROR) {
+ nStat = 1 ;
+ vtDirH1 = vtDirH ;
+ // reset secondo angolo
+ dAngB1 = 0 ;
+ }
+ }
+
+ // calcolo primo angolo di rotazione per seconda soluzione
+ if ( nStat == 2) {
+ bool bDet ;
+ if ( ! vtDirH2.GetRotation( vtDirTn, vtAx1, dAngA2, bDet) )
+ nStat = 1 ;
+ else {
+ // se indeterminato ...
+ if ( ! bDet) {
+ nStat = - 2 ;
+ dAngA2 = 0 ;
+ }
+ }
+ }
+
+ // calcolo primo angolo di rotazione per prima soluzione
+ if ( nStat >= 1) {
+ bool bDet ;
+ if ( ! vtDirH1.GetRotation( vtDirTn, vtAx1, dAngA1, bDet) )
+ nStat = 0 ;
+ else {
+ // se indeterminato ...
+ if ( ! bDet) {
+ nStat = - nStat ;
+ dAngA1 = 0 ;
+ }
+ }
+ }
+
+ return true ;
+}
+
+//----------------------------------------------------------------------------
+bool
+Machine::GetPositions( const Point3d& ptP, double dAngA, double dAngB,
+ int& nStat, double& dX, double& dY, double& dZ)
+{
+ // la posizione deve essere espressa rispetto allo ZERO MACCHINA
+ // per ora gestisco solo gli assi di testa
+
+ // posizione e direzione fresa su testa a riposo
+ Point3d ptPosH = m_ptCalcPos ;
+ Vector3d vtDirH = m_vtCalcDir ;
+
+ // se c'è secondo asse rotante di testa
+ if ( m_vCalcRotAx.size() >= 2 && m_vCalcRotAx[1].bHead) {
+ // posizione e direzione primo asse rotante
+ Point3d ptAx2 = m_vCalcRotAx[1].ptPos ;
+ Vector3d vtAx2 = m_vCalcRotAx[1].vtDir ;
+ // ruoto dati a riposo
+ ptPosH.Rotate( ptAx2, vtAx2, dAngB) ;
+ vtDirH.Rotate( vtAx2, dAngB) ;
+ }
+
+ // se c'è primo asse rotante di testa
+ if ( m_vCalcRotAx.size() >= 1 && m_vCalcRotAx[0].bHead) {
+ // posizione e direzione primo asse rotante
+ Point3d ptAx1 = m_vCalcRotAx[0].ptPos ;
+ Vector3d vtAx1 = m_vCalcRotAx[0].vtDir ;
+ // ruoto dati a riposo
+ ptPosH.Rotate( ptAx1, vtAx1, dAngA) ;
+ vtDirH.Rotate( vtAx1, dAngA) ;
+ }
+
+ // calcolo il recupero degli assi : è l'opposto dello spostamento della posizione
+ Vector3d vtDtAx = m_ptCalcPos - ptPosH ;
+
+ // calcolo il recupero di lunghezza utensile
+ Vector3d vtDtTL = vtDirH * m_dCalcTLen ;
+
+ // calcolo le posizioni degli assi lineari
+ dX = ptP.x + vtDtAx.x + vtDtTL.x ;
+ dY = ptP.y + vtDtAx.y + vtDtTL.y ;
+ dZ = ptP.z + vtDtAx.z + vtDtTL.z ;
+
+ return true ;
+}
+
+//----------------------------------------------------------------------------
+bool
+Machine::VerifyOutOfStroke( double dX, double dY, double dZ, double dAngA, double dAngB, int& nStat)
+{
+ // default tutto ok
+ nStat = 0 ;
+ // primo lineare
+ if ( dX < m_vCalcLinAx[0].stroke.Min)
+ nStat += 1 ;
+ else if( dX > m_vCalcLinAx[0].stroke.Max)
+ nStat += 2 ;
+ // secondo lineare
+ if ( dY < m_vCalcLinAx[1].stroke.Min)
+ nStat += 4 ;
+ else if( dY > m_vCalcLinAx[1].stroke.Max)
+ nStat += 8 ;
+ // terzo lineare
+ if ( dZ < m_vCalcLinAx[2].stroke.Min)
+ nStat += 16 ;
+ else if( dZ > m_vCalcLinAx[2].stroke.Max)
+ nStat += 32 ;
+ // eventuale primo rotante
+ if ( abs( nStat) >= 1) {
+ if ( dAngA < m_vCalcRotAx[0].stroke.Min)
+ nStat += 64 ;
+ else if( dAngA > m_vCalcRotAx[0].stroke.Max)
+ nStat += 128 ;
+ }
+ // eventuale secondo rotante
+ if ( abs( nStat) >= 2) {
+ if ( dAngB < m_vCalcLinAx[1].stroke.Min)
+ nStat += 256 ;
+ else if( dAngB > m_vCalcLinAx[1].stroke.Max)
+ nStat += 512 ;
+ }
+ return true ;
+}
\ No newline at end of file
diff --git a/MachineHeads.cpp b/MachineHeads.cpp
index 570e6b5..2603663 100644
--- a/MachineHeads.cpp
+++ b/MachineHeads.cpp
@@ -46,6 +46,13 @@ Machine::LoadTool( const string& sHead, int nExit, const string& sTool)
// inserisco l'utensile nel gruppo
if ( ! m_pMchMgr->m_pGeomDB->Load( sToolFile, nExGrp))
return false ;
+ int nTGrpId = m_pMchMgr->m_pGeomDB->GetFirstGroupInGroup( nExGrp) ;
+ int nSolidId = m_pMchMgr->m_pGeomDB->GetFirstNameInGroup( nTGrpId, "SOLID") ;
+ if ( nSolidId == GDB_ID_NULL)
+ return false ;
+ m_pMchMgr->m_pGeomDB->RelocateGlob( nSolidId, nExGrp, GDB_FIRST_SON) ;
+ m_pMchMgr->m_pGeomDB->Erase( nTGrpId) ;
+ m_pMchMgr->m_pGeomDB->SetName( nSolidId, sTool) ;
// lo ruoto 90 deg attorno alla X locale
int nT = m_pMchMgr->m_pGeomDB->GetFirstGroupInGroup( nExGrp) ;
if ( nT == GDB_ID_NULL)