From 1507f18b88d481eff274ffd3cf2cd5c37c6c0d9f Mon Sep 17 00:00:00 2001
From: DarioS <dario.sassi@egaltech.com>
Date: Tue, 17 Jan 2023 16:30:23 +0100
Subject: [PATCH] EgtMachKernel : - aggiunta gestione Vmill a EmtMoveAxes.

---
 Simulator.cpp | 76 +++++++++++++++++++++++++++++++++++++++------------
 1 file changed, 59 insertions(+), 17 deletions(-)

diff --git a/Simulator.cpp b/Simulator.cpp
index 271cdd6..26d796b 100644
--- a/Simulator.cpp
+++ b/Simulator.cpp
@@ -2049,33 +2049,72 @@ Simulator::MoveAxes( int nMoveType, const SAMVECTOR& vAxNaEpSt)
 {
   // Salvo lo step di simulazione corrente
    double dOrigStep = m_dStep ; 
-  // verifico ci siano assi da muovere
+  // Verifico ci siano assi da muovere
    if ( vAxNaEpSt.empty())
       return SIM_AXMV_RES_ERR ;
-  // numero assi da muovere
+  // Numero assi da muovere
    int nAxCount = int( vAxNaEpSt.size()) ;
-  // posizioni iniziali
+  // Posizioni iniziali
    DBLVECTOR vPrev( nAxCount) ;
    for ( int i = 0 ; i < nAxCount ; ++ i) {
       if ( ! m_pMchMgr->GetAxisPos( vAxNaEpSt[i].sName, vPrev[i]))
          return SIM_AXMV_RES_ERR ;
    }
-  // determino gli step di movimento
+  // Posizione e direzione attuali degli utensili e riferimenti dei pezzi (per Vmill)
+   bool bExecVmill = (( nMoveType != 0  ||  ! NeedCollisionCheck())  &&  ! m_VmTool.empty()  &&  ! m_VmId.empty()) ;
+   PNTVECTOR vPtNoseI ;
+   VCT3DVECTOR vVtDirI ;
+   VCT3DVECTOR vVtAuxI ;
+   vector<Frame3d> vFrVzmI ;
+   if ( bExecVmill) {
+      vPtNoseI.resize( m_VmTool.size()) ;
+      vVtDirI.resize( m_VmTool.size()) ;
+      vVtAuxI.resize( m_VmTool.size()) ;
+      vFrVzmI.resize( m_VmId.size()) ;
+      bool bOkI = true ;
+      for ( int i = 0 ; i < int( m_VmTool.size()) ; ++ i)
+         bOkI = GetHeadCurrPosDirAux( m_VmTool[i].sHead, m_VmTool[i].nExit, vPtNoseI[i], vVtDirI[i], vVtAuxI[i]) && bOkI ;
+      for ( int i = 0 ; i < int( m_VmId.size()) ; ++ i)
+         bOkI = m_pGeomDB->GetGlobFrame( m_VmId[i], vFrVzmI[i]) && bOkI ;  
+   }
+  // Determino gli step di movimento
    int nStep = 1 ;
    for ( int i = 0 ; i < nAxCount ; ++ i) {
       double dMove = abs( vAxNaEpSt[i].dEndPos - vPrev[i]) ;
       int nAxStep = int( dMove / max( vAxNaEpSt[i].dStep, 1.)) + 1 ;
       nStep = max( nStep, nAxStep) ;
    }
-  // eseguo il movimento
-   for ( int j = 1 ; j <= nStep ; ++ j) {
-     // muovo
+  // Eseguo il movimento
+   for ( int i = 1 ; i <= nStep ; ++ i) {
+     // Muovo
       bool bMoveOk = true ;
-      double dCoeff = double( j) / nStep ;
-      for ( int i = 0 ; i < nAxCount ; ++ i)
-         bMoveOk = m_pMchMgr->SetAxisPos( vAxNaEpSt[i].sName, ( 1 - dCoeff) * vPrev[i] + dCoeff * vAxNaEpSt[i].dEndPos) && bMoveOk ;
+      double dCoeff = double( i) / nStep ;
+      for ( int j = 0 ; j < nAxCount ; ++ j) {
+         bMoveOk = m_pMchMgr->SetAxisPos( vAxNaEpSt[j].sName, ( 1 - dCoeff) * vPrev[j] + dCoeff * vAxNaEpSt[j].dEndPos) && bMoveOk ;
+      }
+     // Eseguo eventuale Vmill
+      if ( bExecVmill) {
+         for ( int j = 0 ; j < int( m_VmTool.size()) ; ++ j) {
+            Point3d ptNoseF ; Vector3d vtDirF ; Vector3d vtAuxF ;
+            bool bOkF = GetHeadCurrPosDirAux( m_VmTool[j].sHead, m_VmTool[j].nExit, ptNoseF, vtDirF, vtAuxF) ;
+            for ( int k = 0 ; k < int( m_VmId.size()) ; ++ k) {
+               Frame3d frVzmF ;
+               bOkF = m_pGeomDB->GetGlobFrame( m_VmId[k], frVzmF) && bOkF ;
+               ExecLineVmill( m_VmId[k], j, m_VmTool[j].dTdOffs, m_VmTool[j].dAdOffs,
+                              vPtNoseI[j], vVtDirI[j], vVtAuxI[j], vFrVzmI[k], ptNoseF, vtDirF, vtAuxF, frVzmF) ;
+               // se ultimo utensile, salvo riferimento per prossimo inizio
+               if ( j == int( m_VmTool.size()) - 1)
+                  vFrVzmI[k] = frVzmF ;
+            }
+            // aggiorno prossimo inizio
+            vPtNoseI[j] = ptNoseF ;
+            vVtDirI[j] = vtDirF ;
+            vVtAuxI[j] = vtAuxF ;
+         }
+      }
+     // Aggiorno visualizzazione
       ExeDraw() ;
-     // verifica collisioni
+     // Verifico collisioni
       int nCdInd, nObjInd ;
       bool bCollCheck = ExecCollisionCheck( nCdInd, nObjInd, nMoveType) ;
       if ( ! bCollCheck) {
@@ -2092,7 +2131,7 @@ Simulator::MoveAxes( int nMoveType, const SAMVECTOR& vAxNaEpSt)
                m_nUiStatus = MCH_UISIM_PAUSE ;
                while ( m_nUiStatus == MCH_UISIM_PAUSE) {
                  // -11 notifica al simulatore pausa dopo collisione
-                  ExeProcessEvents( -11, 2) ;
+                  ExeProcessEvents( -11, 4) ;
                }
             }
          }
@@ -2100,22 +2139,25 @@ Simulator::MoveAxes( int nMoveType, const SAMVECTOR& vAxNaEpSt)
             m_pMachine->LuaSetGlobVar( GLOB_VAR + GVAR_ERR, nPrevErr) ;
          }
       }
+     // In caso di errore esco
       if ( ! bMoveOk)
          return SIM_AXMV_RES_ERR ;
+     // Gestione aggiornamenti...
       if ( ExeGetEnableUI()) {
         // Aggiornamento interfaccia
-         ExeProcessEvents( 0, 2) ;
+         ExeProcessEvents( 0, 4) ;
          while ( m_nUiStatus == MCH_UISIM_PAUSE) {
-            ExeProcessEvents( 0, 2) ;
+            ExeProcessEvents( 0, 4) ;
          }
         // Gestione Stop
          if ( m_nUiStatus == MCH_UISIM_STOP)
             return SIM_AXMV_RES_STOP ;
         // Se cambia lo step di simulazione in modo significativo ...
-         if ( abs( m_dStep - dOrigStep) > 5) {
+         if ( abs( ( m_dStep - dOrigStep) / ( m_dStep + dOrigStep)) > 0.2) {
             double dRat = m_dStep / dOrigStep ;
-            for ( int i = 0 ; i < nAxCount ; ++ i)
-               const_cast<double&>( vAxNaEpSt[i].dStep) *= dRat ;
+            for ( int j = 0 ; j < nAxCount ; ++ j) {
+               const_cast<double&>( vAxNaEpSt[j].dStep) *= dRat ;
+            }
             return MoveAxes( nMoveType, vAxNaEpSt) ;
          }
       }