Egalware/EgtMachKernel
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

EgtMachKernel :

Browse Source 
- migliorie nella verifica collisioni della simulazione.
This commit is contained in:
Dario Sassi
2020-07-24 07:18:05 +00:00
parent 5d71cb243a
commit b456f89be5
2 changed files with 25 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Simulator.cpp
+24 -14
View File
@@ -65,6 +65,7 @@ Simulator::Simulator( void)
m_nAuxETot = 0 ;
m_nAuxEInd = 0 ;
m_nExit = 0 ;
m_dTDiam = 0 ;
m_dVmTdOffs = 0 ;
m_dVmAdOffs = 0 ;
m_dSafeDist = SAFEDIST_STD ;
@@ -124,6 +125,9 @@ Simulator::Start( bool bFirst)
// Reset utensile, interpolazione e assi correnti
m_sTool.clear() ;
m_sHead.clear() ;
m_nExit = 0 ;
m_dTDiam = 0 ;
ResetInterpolation() ;
ResetAxes() ;
ResetAuxAxes() ;
@@ -444,6 +448,7 @@ Simulator::UpdateTool( bool bFirst)
m_sTool = sTool ;
m_sHead = sHead ;
m_nExit = nExit ;
m_pMchMgr->TdbGetCurrToolParam( TPA_DIAM, m_dTDiam) ;
// aggiorno gli assi macchina
if ( ! UpdateAxes())
return false ;
@@ -488,6 +493,7 @@ Simulator::UpdateTool( bool bFirst)
m_sTool = sTool ;
m_sHead = sHead ;
m_nExit = nExit ;
m_pMchMgr->TdbGetCurrToolParam( TPA_DIAM, m_dTDiam) ;
// aggiorno gli assi macchina
if ( ! UpdateAxes())
return false ;
@@ -928,7 +934,8 @@ Simulator::ManageSingleMove( int& nStatus, double& dMove)
double dPrevCoeff = m_dCoeff ;
double dDist = sqrt( dSqDist) ;
if ( dDist > EPS_SMALL) {
int nStep = int( max( dDist / ( ( nMoveType == 0 ? 2 : 1) * m_dStep), 1.)) ;
double dStep = min( ( nMoveType == 0 ? 2 : 1) * m_dStep, MAX_STEP) ;
int nStep = int( max( dDist / dStep, 1.)) ;
m_dCoeff += 1. / nStep ;
if ( m_dCoeff > 1)
m_dCoeff = 1 ;
@@ -948,7 +955,7 @@ Simulator::ManageSingleMove( int& nStatus, double& dMove)
if ( nMoveType != 2 && nMoveType != 3) {
// se attivo CollisionCheck approssimo movimento con più step
int nStep = 1 ;
if ( ! m_VmId.empty() || NeedCollisionCheck()) {
if ( NeedCollisionCheck()) {
const double LEN_STEP = 10. ;
nStep = max( int( dDist / LEN_STEP), 1) ;
}
@@ -964,17 +971,17 @@ Simulator::ManageSingleMove( int& nStatus, double& dMove)
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 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 movimento su arco
else {
@@ -990,8 +997,10 @@ Simulator::ManageSingleMove( int& nStatus, double& dMove)
// se attivi Vmill o CollisionCheck approssimo movimento con più step
int nStep = 1 ;
if ( ! m_VmId.empty() || NeedCollisionCheck()) {
const double LEN_STEP = 2. ;
nStep = max( int( abs( dDiffAng * DEGTORAD * dRad) / LEN_STEP), 1) ;
const double MIN_LEN_STEP = 2. ;
const double MAX_LEN_STEP = 10. ;
double dLenStep = Clamp( 0.1 * m_dTDiam, MIN_LEN_STEP, MAX_LEN_STEP) ;
nStep = max( int( abs( dDiffAng * DEGTORAD * dRad) / dLenStep), 1) ;
const double ANG_STEP = 5. ;
if ( abs( dDiffAng) / nStep > ANG_STEP)
nStep = int( abs( dDiffAng) / ANG_STEP) ;
@@ -1149,7 +1158,7 @@ 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 ci sono i grezzi o equivalenti, tutto bene
// se non ci sono oggetti da controllare o non ci sono grezzi o equivalenti, tutto bene
if ( ! NeedCollisionCheck())
return true ;
// ciclo sui grezzi o equivalenti
@@ -1169,7 +1178,6 @@ Simulator::ExecCollisionCheck( int& nCdInd, int& nObjInd)
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)
@@ -1185,6 +1193,8 @@ Simulator::ExecCollisionCheck( int& nCdInd, int& nObjInd)
const IGeoFrame3d* pGeoFrame = GetGeoFrame3d( m_pGeomDB->GetGeoObj( m_CollObj[j].nFrameId)) ;
if ( pGeoFrame == nullptr)
continue ;
// esecuzione controlli di collisione
bool bOk = true ;
Frame3d frObj = pGeoFrame->GetFrame() ;
Frame3d frParent ; m_pGeomDB->GetGlobFrame( m_CollObj[j].nFrameId, frParent) ;
frObj.LocToLoc( frParent, frCd) ;