Dario Sassi
0095847b41
- modifiche a tagli con lama per eliminare approcci/retrazioni con attacco e uscita fuori grezzo e tipologia ToAndFrom - aggiunte gestione nuove costanti (SURFFIN_SUB_Z_CONST e SURFFIN_SUB_OPTIMAL) per tipo finitura superfici.
4609 lines
191 KiB
C++
4609 lines
191 KiB
C++
//----------------------------------------------------------------------------
|
|
// EgalTech 2016-2016
|
|
//----------------------------------------------------------------------------
|
|
// File : Operation.cpp Data : 29.04.16 Versione : 1.6p4
|
|
// Contenuto : Implementazione gestione base operazioni.
|
|
//
|
|
//
|
|
//
|
|
// Modifiche : 29.04.16 DS Creazione modulo.
|
|
//
|
|
//
|
|
//----------------------------------------------------------------------------
|
|
|
|
//--------------------------- Include ----------------------------------------
|
|
#include "stdafx.h"
|
|
#include "DllMain.h"
|
|
#include "Operation.h"
|
|
#include "OperationConst.h"
|
|
#include "MachMgr.h"
|
|
#include "OutputConst.h"
|
|
#include "GeoConst.h"
|
|
#include "/EgtDev/Include/EGkAngle.h"
|
|
#include "/EgtDev/Include/EGkBBox1d.h"
|
|
#include "/EgtDev/Include/EGkGeoPoint3d.h"
|
|
#include "/EgtDev/Include/EGkCurveLine.h"
|
|
#include "/EgtDev/Include/EGkCurveArc.h"
|
|
#include "/EgtDev/Include/EGkArcSpecial.h"
|
|
#include "/EgtDev/Include/EGkCurveComposite.h"
|
|
#include "/EgtDev/Include/EGkDistPointLine.h"
|
|
#include "/EgtDev/Include/EGkDistPointCurve.h"
|
|
#include "/EgtDev/Include/EGkIntersCurves.h"
|
|
#include "/EgtDev/Include/EGkIntersLineSurfTm.h"
|
|
#include "/EgtDev/Include/EGkGeomDB.h"
|
|
#include "/EgtDev/Include/EGkCDeBoxClosedSurfTm.h"
|
|
#include "/EgtDev/Include/EGkCDeCylClosedSurfTm.h"
|
|
#include "/EgtDev/Include/EGkCAvToolSurfTm.h"
|
|
#include "/EgtDev/Include/EGkStringUtils3d.h"
|
|
#include "/EgtDev/Include/EXeConst.h"
|
|
#include "/EgtDev/Include/EgtNumUtils.h"
|
|
#include "/EgtDev/Include/EgtPointerOwner.h"
|
|
|
|
using namespace std ;
|
|
|
|
//----------------------------------------------------------------------------
|
|
const double MAX_DIST_RAW = 200.0 ;
|
|
const double MAX_ROBOT_G0_LEN = 299.0 ;
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::SetOwner( int nId, IGeomDB* pGDB)
|
|
{
|
|
m_nOwnerId = nId ;
|
|
m_pGeomDB = pGDB ;
|
|
return ( m_nOwnerId != GDB_ID_NULL && m_pGeomDB != nullptr) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
int
|
|
Operation::GetOwner( void) const
|
|
{
|
|
return m_nOwnerId ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
IGeomDB*
|
|
Operation::GetGeomDB( void) const
|
|
{
|
|
return m_pGeomDB ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
static bool
|
|
IsActiveOperation( int nId, IGeomDB* pGDB)
|
|
{
|
|
int nMode ;
|
|
return ( pGDB != nullptr && pGDB->GetCalcMode( nId, nMode) && nMode == GDB_MD_STD) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::Removing( int nParentId, int nNextId)
|
|
{
|
|
// acconsento alla rimozione anche se non inizializzato
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return true ;
|
|
// imposto da verificare operazione attiva successiva
|
|
int nActId = nNextId ;
|
|
while ( nActId != GDB_ID_NULL && ! IsActiveOperation( nActId, m_pGeomDB))
|
|
nActId = m_pGeomDB->GetNextGroup( nActId) ;
|
|
// se non trovata, imposto da verificare operazione attiva precedente
|
|
if ( nActId == GDB_ID_NULL) {
|
|
nActId = m_pGeomDB->GetPrevGroup( m_nOwnerId) ;
|
|
while ( nActId != GDB_ID_NULL && ! IsActiveOperation( nActId, m_pGeomDB))
|
|
nActId = m_pGeomDB->GetPrevGroup( nActId) ;
|
|
}
|
|
// imposto stato operazione
|
|
Operation* pOpe = GetOperation( m_pGeomDB->GetUserObj( nActId)) ;
|
|
if ( pOpe != nullptr)
|
|
pOpe->UpdateStatus( MCH_ST_TO_VERIFY) ;
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::Relocate( int nOrigParentId, int nOrigNextId, int nRefId, int nSonBeforeAfter, bool bGlob)
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// imposto da verificare operazione
|
|
UpdateStatus( MCH_ST_TO_VERIFY) ;
|
|
// imposto da verificare operazione attiva successiva in posizione originale
|
|
while ( nOrigNextId != GDB_ID_NULL && ! IsActiveOperation( nOrigNextId, m_pGeomDB))
|
|
nOrigNextId = m_pGeomDB->GetNextGroup( nOrigNextId) ;
|
|
Operation* pOpe = GetOperation( m_pGeomDB->GetUserObj( nOrigNextId)) ;
|
|
if ( pOpe != nullptr)
|
|
pOpe->UpdateStatus( MCH_ST_TO_VERIFY) ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::Init( MachMgr* pMchMgr)
|
|
{
|
|
m_pMchMgr = pMchMgr ;
|
|
if ( m_pMchMgr == nullptr)
|
|
return false ;
|
|
if ( m_pMchMgr->GetGeomDB() != m_pGeomDB)
|
|
return false ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
//----------------------------------------------------------------------------
|
|
Operation::Operation( void)
|
|
: m_nOwnerId( GDB_ID_NULL), m_pGeomDB( nullptr), m_pMchMgr( nullptr), m_nPhase( 1),
|
|
m_nPathId( GDB_ID_NULL), m_bCurr( false), m_ptCurr(), m_vtTool(), m_vtCorr(), m_vtAux(),
|
|
m_dFeed( 0), m_nFlag( 0), m_nFlag2( 0), m_nIndex( 0)
|
|
{
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::UpdateFollowingOperationsStatus(int nModif)
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// modifico lo stato delle operazioni attive successive
|
|
int nOpeId = m_pMchMgr->GetNextActiveOperation( m_nOwnerId) ;
|
|
while ( nOpeId != GDB_ID_NULL) {
|
|
Operation* pOpe = GetOperation( m_pGeomDB->GetUserObj( nOpeId)) ;
|
|
if ( pOpe != nullptr)
|
|
pOpe->UpdateStatus( nModif) ;
|
|
nOpeId = m_pMchMgr->GetNextActiveOperation( nOpeId) ;
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
string
|
|
Operation::GetName( void) const
|
|
{
|
|
string sName ;
|
|
if ( m_pGeomDB == nullptr || ! m_pGeomDB->GetName( m_nOwnerId, sName))
|
|
sName = "___" ;
|
|
return sName ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetElevation( int nPhase, const Point3d& ptP,
|
|
const Vector3d& vtDir, double& dElev) const
|
|
{
|
|
Vector3d vtNorm ;
|
|
INTVECTOR vRawStmId ;
|
|
return GetElevation( nPhase, ptP, vtDir, dElev, vtNorm, vRawStmId) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetElevation( int nPhase, const Point3d& ptP,
|
|
const Vector3d& vtDir, double& dElev, Vector3d& vtNorm) const
|
|
{
|
|
INTVECTOR vRawStmId ;
|
|
return GetElevation( nPhase, ptP, vtDir, dElev, vtNorm, vRawStmId) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetElevation( int nPhase, const Point3d& ptP,
|
|
const Vector3d& vtDir, double& dElev, INTVECTOR& vRawStmId) const
|
|
{
|
|
Vector3d vtNorm ;
|
|
return GetElevation( nPhase, ptP, vtDir, dElev, vtNorm, vRawStmId) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetElevation( int nPhase, const Point3d& ptP,
|
|
const Vector3d& vtDir, double& dElev, Vector3d& vtNorm, INTVECTOR& vRawStmId) const
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// inizializzo elevazione
|
|
dElev = INFINITO ;
|
|
// inizializzo la normale
|
|
vtNorm = V_NULL ;
|
|
// ciclo sui grezzi
|
|
bool bFound = false ;
|
|
int nRawId = m_pMchMgr->GetFirstRawPart() ;
|
|
while ( nRawId != GDB_ID_NULL) {
|
|
// verifico che il grezzo compaia nella fase
|
|
if ( m_pMchMgr->VerifyRawPartPhase( nRawId, nPhase)) {
|
|
// intersezione del raggio dal punto alla trimesh del grezzo
|
|
const double RAY_LEN = 100000 ;
|
|
int nStmId = m_pGeomDB->GetFirstNameInGroup( nRawId, MACH_RAW_SOLID) ;
|
|
ISurfTriMesh* pStm = GetSurfTriMesh( m_pGeomDB->GetGeoObj( nStmId)) ;
|
|
if ( pStm != nullptr) {
|
|
bFound = true ;
|
|
// indice del triangolo intersecato
|
|
int nTria = SVT_NULL ;
|
|
// recupero il riferimento della trimesh
|
|
Frame3d frStm ;
|
|
m_pGeomDB->GetGlobFrame( nStmId, frStm) ;
|
|
// porto il raggio in questo riferimento
|
|
Point3d ptPL = ptP ;
|
|
ptPL.ToLoc( frStm) ;
|
|
Vector3d vtDirL = vtDir ;
|
|
vtDirL.ToLoc( frStm) ;
|
|
ILSIVECTOR vInfo ;
|
|
if ( IntersLineSurfTm( ptPL, vtDirL, RAY_LEN, *pStm, vInfo)) {
|
|
for ( int i = 0 ; i < int( vInfo.size()) ; ++ i) {
|
|
const IntLinStmInfo& Info = vInfo[i] ;
|
|
// se tratto di intersezione coincidente, considero la posizione più lontana, ovvero 2
|
|
if ( Info.nILTT == ILTT_SEGM || Info.nILTT == ILTT_SEGM_ON_EDGE) {
|
|
// se prosegue un tratto precedente, non devo controllare sia minimo
|
|
if ( abs( dElev - Info.dU) < EPS_SMALL) {
|
|
dElev = Info.dU2 ;
|
|
nTria = Info.nT ;
|
|
}
|
|
else if ( Info.dU2 < dElev) {
|
|
dElev = Info.dU2 ;
|
|
nTria = Info.nT ;
|
|
}
|
|
}
|
|
// se altrimenti intersezione puntuale, verifico che esca (coseno >= 0)
|
|
else if ( Info.dCosDN > - COS_ORTO_ANG_ZERO) {
|
|
if ( Info.dU < dElev) {
|
|
dElev = Info.dU ;
|
|
nTria = Info.nT ;
|
|
}
|
|
}
|
|
// se altrimenti è la prima ed entra
|
|
else if ( i == 0 && Info.dCosDN < - COS_ORTO_ANG_ZERO) {
|
|
// se una sola o distante salto tutto
|
|
if ( vInfo.size() == 1 || Info.dU > ( dElev > INFINITO - 1 ? 0 : dElev) + MAX_DIST_RAW)
|
|
break ;
|
|
// altrimenti reset
|
|
else
|
|
dElev = INFINITO ;
|
|
}
|
|
}
|
|
}
|
|
// se c'è triangolo di intersezione, ne recupero la normale e salvo Id del grezzo
|
|
if ( nTria != SVT_NULL) {
|
|
Triangle3d Tria ;
|
|
if ( pStm->GetTriangle( nTria, Tria))
|
|
vtNorm = Tria.GetN() ;
|
|
if ( find( vRawStmId.begin(), vRawStmId.end(), nStmId) == vRawStmId.end())
|
|
vRawStmId.emplace_back( nStmId) ;
|
|
}
|
|
}
|
|
}
|
|
// passo al grezzo successivo
|
|
nRawId = m_pMchMgr->GetNextRawPart( nRawId) ;
|
|
}
|
|
// se non trovate intersezioni, elevazione nulla
|
|
if ( dElev > INFINITO - 1)
|
|
dElev = 0 ;
|
|
return bFound ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetElevation( int nPhase, const Point3d& ptP1, const Point3d& ptP2,
|
|
const Vector3d& vtDir, double& dElev) const
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// inizializzo elevazione
|
|
dElev = INFINITO ;
|
|
// ciclo sui grezzi
|
|
bool bFound = false ;
|
|
int nRawId = m_pMchMgr->GetFirstRawPart() ;
|
|
while ( nRawId != GDB_ID_NULL) {
|
|
// verifico che il grezzo compaia nella fase
|
|
if ( m_pMchMgr->VerifyRawPartPhase( nRawId, nPhase)) {
|
|
// intersezione del raggio dal punto alla trimesh del grezzo
|
|
const double RAY_LEN = 10000 ;
|
|
int nStmId = m_pGeomDB->GetFirstNameInGroup( nRawId, MACH_RAW_SOLID) ;
|
|
ISurfTriMesh* pStm = GetSurfTriMesh( m_pGeomDB->GetGeoObj( nStmId)) ;
|
|
if ( pStm != nullptr) {
|
|
bFound = true ;
|
|
// recupero il riferimento della trimesh
|
|
Frame3d frStm ;
|
|
m_pGeomDB->GetGlobFrame( nStmId, frStm) ;
|
|
// porto il segmento e la direzione in questo riferimento
|
|
Point3d ptP1L = ptP1 ;
|
|
ptP1L.ToLoc( frStm) ;
|
|
Point3d ptP2L = ptP2 ;
|
|
ptP2L.ToLoc( frStm) ;
|
|
Vector3d vtDirL = vtDir ;
|
|
vtDirL.ToLoc( frStm) ;
|
|
ILSIVECTOR vInfo ;
|
|
// inizializzo elevazione della superficie
|
|
double dElevS = 0 ;
|
|
// faccio test con un insieme di punti ( !!! sostituire con intersezione tra rettangolo e trimesh !!!)
|
|
const double STEP = 50 ;
|
|
int nStep = max( (int) ceil( ApproxDist( ptP1L, ptP2L) / STEP), 2) ;
|
|
for ( int i = 0 ; i <= nStep ; ++ i) {
|
|
// calcolo punto di test
|
|
double dFraz = i / (double) nStep ;
|
|
Point3d ptPL = Media( ptP1L, ptP2L, dFraz) ;
|
|
// inizializzo elevazione del punto
|
|
double dElevP = INFINITO ;
|
|
// calcolo elevazione sul punto
|
|
if ( IntersLineSurfTm( ptPL, vtDirL, RAY_LEN, *pStm, vInfo)) {
|
|
for ( int i = 0 ; i < int( vInfo.size()) ; ++ i) {
|
|
const IntLinStmInfo& Info = vInfo[i] ;
|
|
// se tratto di intersezione coincidente, considero la posizione più lontana, ovvero 2
|
|
if ( Info.nILTT == ILTT_SEGM || Info.nILTT == ILTT_SEGM_ON_EDGE) {
|
|
// se prosegue un tratto precedente, non devo controllare sia minimo
|
|
if ( abs( dElevP - Info.dU) < EPS_SMALL)
|
|
dElevP = Info.dU2 ;
|
|
else
|
|
dElevP = min( dElevP, Info.dU2) ;
|
|
}
|
|
// se altrimenti intersezione puntuale, verifico che esca (coseno >= 0)
|
|
else if ( Info.dCosDN > - COS_ORTO_ANG_ZERO) {
|
|
dElevP = min( dElevP, Info.dU) ;
|
|
}
|
|
// se altrimenti è la prima, distante ed entra, salto tutto
|
|
else if ( i == 0 && Info.dU > MAX_DIST_RAW && Info.dCosDN < - COS_ORTO_ANG_ZERO)
|
|
break ;
|
|
}
|
|
}
|
|
// se elevazione calcolata, aggiorno quella della superficie
|
|
if ( dElevP < INFINITO - 1)
|
|
dElevS = max( dElevS, dElevP) ;
|
|
}
|
|
// aggiorno elevazione complessiva
|
|
if ( dElevS > 0)
|
|
dElev = min( dElev, dElevS) ;
|
|
}
|
|
}
|
|
// passo al grezzo successivo
|
|
nRawId = m_pMchMgr->GetNextRawPart( nRawId) ;
|
|
}
|
|
// se non trovate intersezioni, elevazione nulla
|
|
if ( dElev > INFINITO - 1)
|
|
dElev = 0 ;
|
|
return bFound ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetElevation( int nPhase, const Point3d& ptP1, const Point3d& ptP2, const Point3d& ptP3,
|
|
const Vector3d& vtDir, double& dElev) const
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// approssimo l'arco con due linee
|
|
double dElev1, dElev2 ;
|
|
if ( ! GetElevation( nPhase, ptP1, ptP2, vtDir, dElev1))
|
|
return false ;
|
|
if ( ! GetElevation( nPhase, ptP2, ptP3, vtDir, dElev2))
|
|
return false ;
|
|
dElev = max( dElev1, dElev2) ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetElevation( int nPhase, const Point3d& ptP, const Vector3d& vtTool, double dRad,
|
|
const Vector3d& vtDir, double& dElev) const
|
|
{
|
|
const double STD_TOOL_LEN = 100 ;
|
|
return GetElevation( nPhase, ptP, vtTool, dRad, STD_TOOL_LEN, 0, vtDir, dElev) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetElevation( int nPhase, const Point3d& ptP, const Vector3d& vtTool, double dRad, double dLen,
|
|
const Vector3d& vtDir, double& dElev) const
|
|
{
|
|
return GetElevation( nPhase, ptP, vtTool, dRad, dLen, 0, vtDir, dElev) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetElevation( int nPhase, const Point3d& ptP, const Vector3d& vtTool, double dRad, double dLen, double dSafeDist,
|
|
const Vector3d& vtDir, double& dElev) const
|
|
{
|
|
// risultato di default
|
|
dElev = 0 ;
|
|
// verifico oggetti base
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// aggiustamenti per distanza di sicurezza
|
|
Point3d ptQ = ptP ;
|
|
if ( dSafeDist > EPS_SMALL) {
|
|
ptQ += -vtTool * dSafeDist ;
|
|
dRad += dSafeDist ;
|
|
dLen += 2 * dSafeDist ;
|
|
}
|
|
// inizializzo elevazioni per ogni grezzo
|
|
INTDBLVECTOR vRawElev ;
|
|
// ciclo sui grezzi della fase
|
|
int nRawId = m_pMchMgr->GetFirstRawPart() ;
|
|
while ( nRawId != GDB_ID_NULL) {
|
|
// verifico che il grezzo compaia nella fase
|
|
if ( m_pMchMgr->VerifyRawPartPhase( nRawId, nPhase)) {
|
|
// recupero la trimesh del grezzo
|
|
int nStmId = m_pGeomDB->GetFirstNameInGroup( nRawId, MACH_RAW_SOLID) ;
|
|
ISurfTriMesh* pStm = GetSurfTriMesh( m_pGeomDB->GetGeoObj( nStmId)) ;
|
|
if ( pStm != nullptr) {
|
|
// recupero il riferimento della trimesh
|
|
Frame3d frStm ;
|
|
m_pGeomDB->GetGlobFrame( nStmId, frStm) ;
|
|
// porto posizione e direzioni in questo riferimento
|
|
Point3d ptQL = GetToLoc( ptQ, frStm) ;
|
|
Vector3d vtToolL = GetToLoc( vtTool, frStm) ;
|
|
Vector3d vtDirL = GetToLoc( vtDir, frStm) ;
|
|
// determino quanto allontanarsi
|
|
PtrOwner<ICAvToolSurfTm> pCAvTlStm( CreateCAvToolSurfTm()) ;
|
|
if ( IsNull( pCAvTlStm))
|
|
return false ;
|
|
pCAvTlStm->SetStdTool( dLen, dRad, dSafeDist) ;
|
|
pCAvTlStm->SetSurfTm( *pStm) ;
|
|
double dDist = 0 ;
|
|
// non è ammesso un angolo oltre 90deg tra direzione utensile e vettore movimento
|
|
if ( vtToolL * vtDirL < 0) {
|
|
// il tip utensile è qui il naso mandrino per Collision Avoid
|
|
if ( ! pCAvTlStm->TestPosition( ptQL, -vtToolL, vtDirL, dDist))
|
|
return false ;
|
|
}
|
|
else {
|
|
// per Collision Avoid il punto di riferimento non è il tip dell'utensile ma il naso mandrino
|
|
if ( ! pCAvTlStm->TestPosition( ptQL + dLen * vtToolL, vtToolL, vtDirL, dDist))
|
|
return false ;
|
|
}
|
|
if ( dDist > EPS_SMALL)
|
|
vRawElev.emplace_back( nStmId, dDist) ;
|
|
}
|
|
}
|
|
// passo al grezzo successivo
|
|
nRawId = m_pMchMgr->GetNextRawPart( nRawId) ;
|
|
}
|
|
// se trovate elevazioni
|
|
if ( ! vRawElev.empty()) {
|
|
// ordino il vettore secondo l'elevazione crescente
|
|
sort( vRawElev.begin(), vRawElev.end(), []( const INTDBL& a, const INTDBL& b)
|
|
{ return a.second < b.second ; }) ;
|
|
// box dell'utensile nella posizione iniziale
|
|
BBox3d b3Tool ;
|
|
b3Tool.Add( ptQ) ;
|
|
b3Tool.Add( ptQ + dLen * vtTool) ;
|
|
if ( vtTool.IsX())
|
|
b3Tool.Expand( 0, dRad, dRad) ;
|
|
else if ( vtTool.IsY())
|
|
b3Tool.Expand( dRad, 0, dRad) ;
|
|
else if ( vtTool.IsZ())
|
|
b3Tool.Expand( dRad, dRad, 0) ;
|
|
else {
|
|
double dExpandX = dRad * sqrt( 1 - vtTool.x * vtTool.x) ;
|
|
double dExpandY = dRad * sqrt( 1 - vtTool.y * vtTool.y) ;
|
|
double dExpandZ = dRad * sqrt( 1 - vtTool.z * vtTool.z) ;
|
|
b3Tool.Expand( dExpandX, dExpandY, dExpandZ) ;
|
|
}
|
|
b3Tool.Expand( MAX_DIST_RAW) ;
|
|
// verifico la reale interferenza dell'utensile con i diversi grezzi
|
|
for ( int i = 0 ; i < int( vRawElev.size()) ; ++ i) {
|
|
// box del grezzo
|
|
BBox3d b3Raw ;
|
|
m_pGeomDB->GetGlobalBBox( vRawElev[i].first, b3Raw) ;
|
|
// confronto con il box dell'utensile nella posizione precedente
|
|
BBox3d b3CurrTool = b3Tool ;
|
|
b3CurrTool.Translate( dElev * vtDir) ;
|
|
if ( b3Raw.Overlaps( b3CurrTool))
|
|
dElev = vRawElev[i].second ;
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetElevation( int nPhase, const Point3d& ptS, const Point3d& ptE, const Vector3d& vtTool, double dRad, double dLen,
|
|
const Vector3d& vtDir, double& dElev) const
|
|
{
|
|
// risultato di default
|
|
bool bOk = false ;
|
|
dElev = 0 ;
|
|
// faccio test sui punti dall'inizio alla fine
|
|
const double STEP = 50 ;
|
|
int nStep = max( (int) ceil( ApproxDist( ptS, ptE) / STEP), 2) ;
|
|
for ( int i = 0 ; i <= nStep ; ++ i) {
|
|
// calcolo punto di test
|
|
double dFraz = i / (double) nStep ;
|
|
Point3d ptP = Media( ptS, ptE, dFraz) ;
|
|
// eseguo il calcolo
|
|
double dCurrElev ;
|
|
if ( GetElevation( nPhase, ptP, vtTool, dRad, dLen, vtDir, dCurrElev)) {
|
|
bOk = true ;
|
|
dElev = max( dElev, dCurrElev) ;
|
|
}
|
|
}
|
|
return bOk ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetAhPointUnderRaw( const Point3d& ptP, const Vector3d& vtTool, double dToolRad, double dToolRadForElev,
|
|
double dToolLen, bool bIsSaw, double dSafeZ, const Vector3d& vtDir, double& dElev) const
|
|
{
|
|
// punto a metà lunghezza utensile
|
|
Point3d ptQ = ptP + vtTool * dToolLen / 2 ;
|
|
// box di ingombro dell'utensile
|
|
BBox3d b3Tool ;
|
|
b3Tool.Set( ptP) ;
|
|
b3Tool.Add( ptQ) ;
|
|
double dExpandX = sqrt( 1 - vtTool.x * vtTool.x) * dToolRadForElev ;
|
|
double dExpandY = sqrt( 1 - vtTool.y * vtTool.y) * dToolRadForElev ;
|
|
double dExpandZ = sqrt( 1 - vtTool.z * vtTool.z) * dToolRadForElev ;
|
|
b3Tool.Expand( dExpandX, dExpandY, dExpandZ) ;
|
|
// extra ingombro reale in Z
|
|
double dExtraZ = sqrt( 1 - vtTool.z * vtTool.z) * dToolRad ;
|
|
// direzione di fuga per l'elevazione
|
|
Vector3d vtMyDir = vtDir ;
|
|
if ( bIsSaw || vtMyDir.z < - EPS_SMALL)
|
|
vtMyDir.z = 0 ;
|
|
vtMyDir.Normalize() ;
|
|
// determino la posizione del punto rispetto al grezzo
|
|
// ciclo sui grezzi
|
|
int nRawId = m_pMchMgr->GetFirstRawPart() ;
|
|
while ( nRawId != GDB_ID_NULL) {
|
|
// se il grezzo compare nella fase
|
|
if ( m_pMchMgr->VerifyRawPartPhase( nRawId, m_nPhase)) {
|
|
int nStmId = m_pGeomDB->GetFirstNameInGroup( nRawId, MACH_RAW_SOLID) ;
|
|
BBox3d b3Raw ;
|
|
m_pGeomDB->GetGlobalBBox( nStmId, b3Raw) ;
|
|
b3Raw.Expand( 0.5 * dSafeZ, 0.5 * dSafeZ, 0) ;
|
|
if ( ! b3Raw.IsEmpty() && b3Raw.OverlapsXY( b3Tool) && ptP.z + dExtraZ < b3Raw.GetMin().z - 10 * EPS_SMALL) {
|
|
double dRawCentZ = ( b3Raw.GetMin().z + b3Raw.GetMax().z) / 2 ;
|
|
double dRawDimZ = b3Raw.GetMax().z - b3Raw.GetMin().z ;
|
|
double dToolDimZ = b3Tool.GetMax().z - b3Tool.GetMin().z ;
|
|
// determino elevazione del punto rispetto al grezzo a metà altezza
|
|
Point3d ptTest( ptP.x, ptP.y, dRawCentZ) ;
|
|
double dNewElev ;
|
|
if ( GetElevation( m_nPhase, ptTest, vtTool, dToolRadForElev, vtMyDir, dNewElev))
|
|
dElev = max( dElev, dNewElev) ;
|
|
// se direzione di fuga quasi orizzontale (max 30 deg) e ingombro utensile rilevante rispetto ad altezza grezzo
|
|
if ( vtMyDir.z < 0.5 && dToolDimZ > dRawDimZ / 2) {
|
|
// determino elevazione del punto rispetto al grezzo a metà altezza più metà ingombro utensile
|
|
ptTest = Point3d( ptP.x, ptP.y, dRawCentZ + dToolDimZ / 2) ;
|
|
if ( GetElevation( m_nPhase, ptTest, vtTool, dToolRadForElev, vtMyDir, dNewElev))
|
|
dElev = max( dElev, dNewElev) ;
|
|
// determino elevazione del punto rispetto al grezzo a metà altezza meno metà ingombro utensile
|
|
ptTest = Point3d( ptP.x, ptP.y, dRawCentZ - dToolDimZ / 2) ;
|
|
if ( GetElevation( m_nPhase, ptTest, vtTool, dToolRadForElev, vtMyDir, dNewElev))
|
|
dElev = max( dElev, dNewElev) ;
|
|
}
|
|
return true ;
|
|
}
|
|
}
|
|
nRawId = m_pMchMgr->GetNextRawPart( nRawId) ;
|
|
}
|
|
return false ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetUhPointAboveRaw( const Point3d& ptP, const Vector3d& vtTool, double dToolRad, double dToolRadForElev,
|
|
double dToolLen, bool bIsSaw, double dSafeZ, const Vector3d& vtDir, double& dElev) const
|
|
{
|
|
// punto a metà lunghezza utensile
|
|
Point3d ptQ = ptP + vtTool * dToolLen / 2 ;
|
|
// box di ingombro dell'utensile
|
|
BBox3d b3Tool ;
|
|
b3Tool.Set( ptP) ;
|
|
b3Tool.Add( ptQ) ;
|
|
double dExpandX = sqrt( 1 - vtTool.x * vtTool.x) * dToolRadForElev ;
|
|
double dExpandY = sqrt( 1 - vtTool.y * vtTool.y) * dToolRadForElev ;
|
|
double dExpandZ = sqrt( 1 - vtTool.z * vtTool.z) * dToolRadForElev ;
|
|
b3Tool.Expand( dExpandX, dExpandY, dExpandZ) ;
|
|
// extra ingombro reale in Z
|
|
double dExtraZ = sqrt( 1 - vtTool.z * vtTool.z) * dToolRad ;
|
|
// direzione di fuga per l'elevazione
|
|
Vector3d vtMyDir = vtDir ;
|
|
if ( bIsSaw || vtMyDir.z > EPS_SMALL)
|
|
vtMyDir.z = 0 ;
|
|
vtMyDir.Normalize() ;
|
|
// determino la posizione del punto rispetto al grezzo
|
|
// ciclo sui grezzi
|
|
int nRawId = m_pMchMgr->GetFirstRawPart() ;
|
|
while ( nRawId != GDB_ID_NULL) {
|
|
// se il grezzo compare nella fase
|
|
if ( m_pMchMgr->VerifyRawPartPhase( nRawId, m_nPhase)) {
|
|
int nStmId = m_pGeomDB->GetFirstNameInGroup( nRawId, MACH_RAW_SOLID) ;
|
|
BBox3d b3Raw ;
|
|
m_pGeomDB->GetGlobalBBox( nStmId, b3Raw) ;
|
|
b3Raw.Expand( 0.5 * dSafeZ, 0.5 * dSafeZ, 0) ;
|
|
if ( ! b3Raw.IsEmpty() && b3Raw.OverlapsXY( b3Tool) && ptP.z - dExtraZ > b3Raw.GetMax().z + 10 * EPS_SMALL) {
|
|
double dRawCentZ = ( b3Raw.GetMin().z + b3Raw.GetMax().z) / 2 ;
|
|
double dRawDimZ = b3Raw.GetMax().z - b3Raw.GetMin().z ;
|
|
double dToolDimZ = b3Tool.GetMax().z - b3Tool.GetMin().z ;
|
|
// determino elevazione del punto rispetto al grezzo a metà altezza
|
|
Point3d ptTest( ptP.x, ptP.y, dRawCentZ) ;
|
|
double dNewElev ;
|
|
if ( GetElevation( m_nPhase, ptTest, vtTool, dToolRadForElev, vtMyDir, dNewElev))
|
|
dElev = max( dElev, dNewElev) ;
|
|
// se direzione di fuga quasi orizzontale (max 30 deg) e ingombro utensile rilevante rispetto ad altezza grezzo
|
|
if ( vtMyDir.z > -0.5 && dToolDimZ > dRawDimZ / 2) {
|
|
// determino elevazione del punto rispetto al grezzo a metà altezza più metà ingombro utensile
|
|
ptTest = Point3d( ptP.x, ptP.y, dRawCentZ + dToolDimZ / 2) ;
|
|
if ( GetElevation( m_nPhase, ptTest, vtTool, dToolRadForElev, vtMyDir, dNewElev))
|
|
dElev = max( dElev, dNewElev) ;
|
|
// determino elevazione del punto rispetto al grezzo a metà altezza meno metà ingombro utensile
|
|
ptTest = Point3d( ptP.x, ptP.y, dRawCentZ - dToolDimZ / 2) ;
|
|
if ( GetElevation( m_nPhase, ptTest, vtTool, dToolRadForElev, vtMyDir, dNewElev))
|
|
dElev = max( dElev, dNewElev) ;
|
|
}
|
|
return true ;
|
|
}
|
|
}
|
|
nRawId = m_pMchMgr->GetNextRawPart( nRawId) ;
|
|
}
|
|
return false ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetDistanceFromRawSide( int nPhase, const Point3d& ptP, const Vector3d& vtDir, double& dDist) const
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// recupero il grezzo più vicino al punto (entro 100 mm)
|
|
const double EXPAND_LEN = 100 ;
|
|
BBox3d b3Pnt( ptP) ;
|
|
b3Pnt.Expand( EXPAND_LEN, EXPAND_LEN, 0) ;
|
|
double dMinDist = INFINITO ;
|
|
int nRawId = GDB_ID_NULL ;
|
|
int nCurrRawId = m_pMchMgr->GetFirstRawPart() ;
|
|
while ( nCurrRawId != GDB_ID_NULL) {
|
|
// verifico che il grezzo compaia nella fase
|
|
if ( m_pMchMgr->VerifyRawPartPhase( nCurrRawId, nPhase)) {
|
|
// pre-filtro con il box
|
|
BBox3d b3Raw ;
|
|
int nRawSolidId = m_pGeomDB->GetFirstNameInGroup( nCurrRawId, MACH_RAW_SOLID) ;
|
|
if ( m_pGeomDB->GetGlobalBBox( nRawSolidId, b3Raw) && b3Pnt.OverlapsXY( b3Raw)) {
|
|
// porto il punto nel riferimento del grezzo (a Z=0 perchè ivi è il contorno)
|
|
Frame3d frRaw ;
|
|
m_pGeomDB->GetGroupGlobFrame( nCurrRawId, frRaw) ;
|
|
Point3d ptPL = ptP ;
|
|
ptPL.ToLoc( frRaw) ;
|
|
ptPL.z = 0 ;
|
|
// verifica con il contorno
|
|
int nOutCrvId = m_pGeomDB->GetFirstNameInGroup( nCurrRawId, MACH_RAW_OUTLINE) ;
|
|
const ICurve* pCurve = GetCurve( m_pGeomDB->GetGeoObj( nOutCrvId)) ;
|
|
if ( pCurve != nullptr) {
|
|
int nSide ;
|
|
double dCurrDist ;
|
|
DistPointCurve distPC( ptPL, *pCurve) ;
|
|
if ( distPC.GetDist( dCurrDist)) {
|
|
if ( dCurrDist < 100 * EPS_SMALL ||
|
|
(distPC.GetSideAtMinDistPoint( 0, Z_AX, nSide) && nSide != MDS_RIGHT)) {
|
|
nRawId = nCurrRawId ;
|
|
break ;
|
|
}
|
|
else if ( dCurrDist < dMinDist) {
|
|
dMinDist = dCurrDist ;
|
|
nRawId = nCurrRawId ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
// passo al grezzo successivo
|
|
nCurrRawId = m_pMchMgr->GetNextRawPart( nCurrRawId) ;
|
|
}
|
|
// se il punto non è interno ad alcun grezzo, distanza nulla
|
|
if ( nRawId == GDB_ID_NULL) {
|
|
dDist = 0 ;
|
|
return true ;
|
|
}
|
|
// recupero il contorno del grezzo
|
|
int nOutId = m_pGeomDB->GetFirstNameInGroup( nRawId, MACH_RAW_OUTLINE) ;
|
|
ICurve* pOut = ::GetCurve( m_pGeomDB->GetGeoObj( nOutId)) ;
|
|
if ( pOut == nullptr)
|
|
return false ;
|
|
// recupero il riferimento del contorno e verifico che abbia Z verticale
|
|
Frame3d frStm ;
|
|
if ( ! m_pGeomDB->GetGlobFrame( nOutId, frStm) ||
|
|
( frStm.GetZType() != Frame3d::TOP && frStm.GetZType() != Frame3d::BOTTOM))
|
|
return false ;
|
|
// interseco il raggio di test (portato nel riferimento del contorno) con il contorno del grezzo
|
|
const double RAY_LEN = 10000 ;
|
|
const double EXTRA_LEN = EXPAND_LEN ;
|
|
PtrOwner<ICurveLine> pRay( CreateCurveLine()) ;
|
|
if ( IsNull( pRay) || ! pRay->SetPVL( ptP - vtDir * EXTRA_LEN, vtDir, RAY_LEN))
|
|
return false ;
|
|
pRay->ToLoc( frStm) ;
|
|
IntersCurveCurve intCC( *pRay, *pOut) ;
|
|
int nInters = intCC.GetIntersCount() ;
|
|
IntCrvCrvInfo aInfo ;
|
|
if ( nInters > 0 && intCC.GetIntCrvCrvInfo( nInters - 1, aInfo)) {
|
|
if ( ! aInfo.bOverlap)
|
|
dDist = aInfo.IciA[0].dU * RAY_LEN - EXTRA_LEN ;
|
|
else
|
|
dDist = aInfo.IciA[1].dU * RAY_LEN - EXTRA_LEN ;
|
|
}
|
|
else
|
|
dDist = 0 ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetAggrBottDistanceFromRawSide( int nPhase, const Point3d& ptP, const Vector3d& vtDir, double dEncSafeH, double& dDist) const
|
|
{
|
|
double dDistB1, dDistB2, dDistB3 ;
|
|
Vector3d vtOffs = dEncSafeH * GetRotate( vtDir, Z_AX, 90) ;
|
|
if ( ! GetDistanceFromRawSide( m_nPhase, ptP, vtDir, dDistB1))
|
|
dDistB1 = 0 ;
|
|
if ( ! GetDistanceFromRawSide( m_nPhase, ptP + vtOffs, vtDir, dDistB2))
|
|
dDistB2 = 0 ;
|
|
if ( ! GetDistanceFromRawSide( m_nPhase, ptP - vtOffs, vtDir, dDistB3))
|
|
dDistB3 = 0 ;
|
|
dDist = max( { dDistB1, dDistB2, dDistB3}) ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetMinDistanceFromRawSide( int nPhase, const Point3d& ptP, double dExpand,
|
|
double& dDist, Vector3d& vtDir) const
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// recupero il grezzo in cui è incluso il punto
|
|
BBox3d b3Pnt( ptP) ;
|
|
double dTotExpand = dExpand + 10 ;
|
|
b3Pnt.Expand( dTotExpand, dTotExpand, 0) ;
|
|
int nRawId = m_pMchMgr->GetFirstRawPart() ;
|
|
while ( nRawId != GDB_ID_NULL) {
|
|
// verifico che il grezzo compaia nella fase
|
|
if ( m_pMchMgr->VerifyRawPartPhase( nRawId, nPhase)) {
|
|
// pre-filtro con il box
|
|
BBox3d b3Raw ;
|
|
int nRawSolidId = m_pGeomDB->GetFirstNameInGroup( nRawId, MACH_RAW_SOLID) ;
|
|
if ( m_pGeomDB->GetGlobalBBox( nRawSolidId, b3Raw) && b3Pnt.OverlapsXY( b3Raw)) {
|
|
// porto il punto nel riferimento del grezzo (a Z=0 perchè ivi è il contorno)
|
|
Frame3d frRaw ;
|
|
m_pGeomDB->GetGroupGlobFrame( nRawId, frRaw) ;
|
|
Point3d ptPL = ptP ;
|
|
ptPL.ToLoc( frRaw) ;
|
|
ptPL.z = 0 ;
|
|
// verifica con il contorno
|
|
int nOutCrvId = m_pGeomDB->GetFirstNameInGroup( nRawId, MACH_RAW_OUTLINE) ;
|
|
const ICurve* pCurve = GetCurve( m_pGeomDB->GetGeoObj( nOutCrvId)) ;
|
|
if ( pCurve != nullptr) {
|
|
int nSide ;
|
|
DistPointCurve distPC( ptPL, *pCurve) ;
|
|
// se punto praticamente sul bordo del grezzo, prendo la normale esterna a questo bordo
|
|
if ( distPC.GetDist( dDist) && dDist < 100 * EPS_SMALL) {
|
|
double dU ;
|
|
int nFlag ;
|
|
Point3d ptML ;
|
|
Vector3d vtTgL ;
|
|
if ( distPC.GetParamAtMinDistPoint( 0, dU, nFlag) &&
|
|
pCurve->GetPointTang( dU, ICurve::FROM_MINUS, ptML, vtTgL)) {
|
|
vtDir = vtTgL ;
|
|
vtDir.Rotate( Z_AX, 0, -1) ;
|
|
vtDir.ToGlob( frRaw) ;
|
|
vtDir.Normalize() ;
|
|
break ;
|
|
}
|
|
}
|
|
// altrimenti se punto interno, prendo la direzione dal punto al bordo
|
|
else if ( distPC.GetSideAtMinDistPoint( 0, Z_AX, nSide) && nSide != MDS_RIGHT) {
|
|
int nFlag ;
|
|
Point3d ptML ;
|
|
if ( distPC.GetMinDistPoint( 0, ptML, nFlag)) {
|
|
vtDir = ptML - ptPL ;
|
|
vtDir.ToGlob( frRaw) ;
|
|
vtDir.z = 0 ;
|
|
vtDir.Normalize() ;
|
|
break ;
|
|
}
|
|
}
|
|
// altrimenti punto esterno ma entro espansione (recupero la normale verso l'esterno)
|
|
else if ( dDist < dTotExpand) {
|
|
int nFlag ;
|
|
Point3d ptML ;
|
|
if ( distPC.GetMinDistPoint( 0, ptML, nFlag)) {
|
|
dDist = - dDist ;
|
|
vtDir = ptPL - ptML ;
|
|
vtDir.ToGlob( frRaw) ;
|
|
vtDir.z = 0 ;
|
|
vtDir.Normalize() ;
|
|
break ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
// passo al grezzo successivo
|
|
nRawId = m_pMchMgr->GetNextRawPart( nRawId) ;
|
|
}
|
|
// se il punto non è interno ad alcun grezzo, distanza nulla
|
|
if ( nRawId == GDB_ID_NULL)
|
|
dDist = 0 ;
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetMinDistanceFromRawSide( int nPhase, const Point3d& ptP, double dExpand,
|
|
const Vector3d& vtMainDir, double dCosMaxDelta,
|
|
double& dDist, Vector3d& vtDir) const
|
|
{
|
|
// calcolo senza limiti sulla direzione
|
|
if ( ! GetMinDistanceFromRawSide( nPhase, ptP, dExpand, dDist, vtDir))
|
|
return false ;
|
|
// se esterno, non previsti limiti o direzione nei limiti, esco
|
|
if ( dDist < EPS_SMALL || vtMainDir.IsSmall() || dCosMaxDelta <= -1 || ( vtMainDir * vtDir) > dCosMaxDelta)
|
|
return true ;
|
|
// limito la direzione
|
|
Vector3d vtPar = vtMainDir * dCosMaxDelta ;
|
|
Vector3d vtOrt = OrthoCompo( vtDir, vtMainDir) ;
|
|
double dSqOrt = 1 - dCosMaxDelta * dCosMaxDelta ;
|
|
if ( vtOrt.Normalize() && dSqOrt > 0)
|
|
vtDir = vtPar + vtOrt * sqrt( dSqOrt) ;
|
|
else
|
|
vtDir = vtMainDir ;
|
|
// calcolo con direzione imposta
|
|
return GetDistanceFromRawSide( nPhase, ptP, vtDir, dDist) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetDistanceFromRawBottom( int nPhase, int nPathId, double dToler, double& dRbDist, double& dAllRbDist) const
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// recupero distanza da fondo dei grezzi interessati o no dal percorso
|
|
dRbDist = 0 ;
|
|
dAllRbDist = 0 ;
|
|
BBox3d b3Compo ;
|
|
if ( ! m_pGeomDB->GetGlobalBBox( nPathId, b3Compo))
|
|
return false ;
|
|
b3Compo.Expand( dToler, dToler, 0) ;
|
|
int nRawId = m_pMchMgr->GetFirstRawPart() ;
|
|
while ( nRawId != GDB_ID_NULL) {
|
|
// verifico che il grezzo compaia nella fase
|
|
if ( m_pMchMgr->VerifyRawPartPhase( nRawId, nPhase)) {
|
|
BBox3d b3Raw ;
|
|
int nRawSolidId = m_pGeomDB->GetFirstNameInGroup( nRawId, MACH_RAW_SOLID) ;
|
|
if ( m_pGeomDB->GetGlobalBBox( nRawSolidId, b3Raw)) {
|
|
double dDist = b3Compo.GetMax().z - b3Raw.GetMin().z ;
|
|
if ( b3Compo.OverlapsXY( b3Raw) && dDist > dRbDist)
|
|
dRbDist = dDist ;
|
|
if ( dDist > dAllRbDist)
|
|
dAllRbDist = dDist ;
|
|
}
|
|
}
|
|
// passo al grezzo successivo
|
|
nRawId = m_pMchMgr->GetNextRawPart( nRawId) ;
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetRawGlobBox( int nPhase, int nPathId, double dToler, BBox3d& b3Raw) const
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// inizializzo box
|
|
b3Raw.Reset() ;
|
|
// ricerco grezzi interessati dal percorso
|
|
BBox3d b3Compo ;
|
|
if ( ! m_pGeomDB->GetGlobalBBox( nPathId, b3Compo))
|
|
return false ;
|
|
return GetRawGlobBox( nPhase, b3Compo, dToler, b3Raw) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetRawGlobBox( int nPhase, const BBox3d& b3Test, double dToler, BBox3d& b3Raw) const
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// inizializzo box
|
|
b3Raw.Reset() ;
|
|
// allargo box di test
|
|
BBox3d b3Compo = b3Test ;
|
|
b3Compo.Expand( dToler, dToler, 0) ;
|
|
int nRawId = m_pMchMgr->GetFirstRawPart() ;
|
|
while ( nRawId != GDB_ID_NULL) {
|
|
// verifico che il grezzo compaia nella fase
|
|
if ( m_pMchMgr->VerifyRawPartPhase( nRawId, nPhase)) {
|
|
BBox3d b3OneRaw ;
|
|
int nRawSolidId = m_pGeomDB->GetFirstNameInGroup( nRawId, MACH_RAW_SOLID) ;
|
|
if ( m_pGeomDB->GetGlobalBBox( nRawSolidId, b3OneRaw) && b3Compo.OverlapsXY( b3OneRaw)) {
|
|
b3Raw.Add( b3OneRaw) ;
|
|
}
|
|
}
|
|
// passo al grezzo successivo
|
|
nRawId = m_pMchMgr->GetNextRawPart( nRawId) ;
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetCurrRawsGlobBox( BBox3d& b3Raw) const
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// inizializzo box
|
|
b3Raw.Reset() ;
|
|
// Ciclo sui grezzi attivi
|
|
int nRawId = m_pMchMgr->GetFirstRawPart() ;
|
|
while ( nRawId != GDB_ID_NULL) {
|
|
if ( m_pMchMgr->VerifyRawPartPhase( nRawId, m_nPhase)) {
|
|
BBox3d b3OneRaw ;
|
|
int nRawSolidId = m_pGeomDB->GetFirstNameInGroup( nRawId, MACH_RAW_SOLID) ;
|
|
if ( m_pGeomDB->GetGlobalBBox( nRawSolidId, b3OneRaw)) {
|
|
b3Raw.Add( b3OneRaw) ;
|
|
}
|
|
}
|
|
nRawId = m_pMchMgr->GetNextRawPart( nRawId) ;
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
//----------------------------------------------------------------------------
|
|
inline Vector3d
|
|
GetDirInFacePerpSide( int nFaceUse, const Vector3d& vtFaceUse, const Vector3d& vtN, ICurveComposite* pCrvCompo)
|
|
{
|
|
// determino la direzione di riferimento proiettata nella faccia
|
|
Vector3d vtRef ;
|
|
switch ( nFaceUse) {
|
|
case FACE_DOWN : vtRef = Z_AX ; break ;
|
|
case FACE_TOP : vtRef = -Z_AX ; break ;
|
|
case FACE_FRONT : vtRef = Y_AX ; break ;
|
|
case FACE_BACK : vtRef = -Y_AX ; break ;
|
|
case FACE_LEFT : vtRef = X_AX ; break ;
|
|
case FACE_RIGHT : vtRef = -X_AX ; break ;
|
|
case FACE_VERSOR : vtRef = vtFaceUse ; break ;
|
|
}
|
|
vtRef -= vtRef * vtN * vtN ;
|
|
if ( ! vtRef.Normalize())
|
|
vtRef = V_NULL ;
|
|
// sostituisco questa direzione con quella perpendicolare alla curva più adatta
|
|
double dCosMax = -1 ;
|
|
Vector3d vtPerpMax ;
|
|
const ICurve* pCrv = pCrvCompo->GetFirstCurve() ;
|
|
while ( pCrv != nullptr) {
|
|
Vector3d vtDir ; pCrv->GetMidDir( vtDir) ;
|
|
Vector3d vtPerp = vtN ^ vtDir ;
|
|
double dCos = vtPerp * vtRef ;
|
|
if ( dCos > dCosMax) {
|
|
dCosMax = dCos ;
|
|
vtPerpMax = vtPerp ;
|
|
}
|
|
pCrv = pCrvCompo->GetNextCurve() ;
|
|
}
|
|
// richiedo non più di 85deg di deviazione
|
|
if ( dCosMax > 0.087)
|
|
vtRef = vtPerpMax ;
|
|
return vtRef ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::AdjustCurveFromSurf( ICurveComposite* pCrvCompo, int nToolDir, int nFaceUse, const Vector3d& vtFaceUse,
|
|
double dToolThick, int nGrade)
|
|
{
|
|
// copia della curva originale
|
|
PtrOwner<ICurveComposite> pCopy( pCrvCompo->Clone()) ;
|
|
|
|
// versore normale alla faccia (estrusione della curva)
|
|
Vector3d vtN ;
|
|
pCrvCompo->GetExtrusion( vtN) ;
|
|
|
|
// direzione di riferimento
|
|
Vector3d vtRef ;
|
|
|
|
// direzioni lati precedente e successivo
|
|
Vector3d vtDirPrev ;
|
|
Vector3d vtDirNext ;
|
|
|
|
// se richiesto contorno
|
|
if ( nFaceUse == FACE_CONT) {
|
|
// cerco l'estremo più alto e lo imposto come inizio
|
|
double dU = 0 ;
|
|
double dUmax = 0 ;
|
|
double dZmax = - INFINITO ;
|
|
Point3d ptP ;
|
|
while ( pCrvCompo->GetPointD1D2( dU, ICurve::FROM_MINUS, ptP)) {
|
|
if ( ptP.z > dZmax + EPS_SMALL) {
|
|
dZmax = ptP.z ;
|
|
dUmax = dU ;
|
|
}
|
|
dU += 1 ;
|
|
}
|
|
if ( dUmax != 0)
|
|
pCrvCompo->ChangeStartPoint( dUmax) ;
|
|
}
|
|
// altrimenti
|
|
else {
|
|
// determino la direzione di riferimento
|
|
vtRef = GetDirInFacePerpSide( nFaceUse, vtFaceUse, vtN, pCrvCompo) ;
|
|
// la curva gira in senso antiorario attorno al contorno faccia vista dalla normale uscente
|
|
// elimino i segmenti che hanno la direzione di riferimento a destra o quasi (16 deg o 31 deg o 46deg)
|
|
nGrade = Clamp( nGrade, 1, 3) ;
|
|
const double COS_ANG_MAX = ( nGrade == 1 ? cos( 16.01 * DEGTORAD) : ( nGrade == 2 ? cos( 31.01 * DEGTORAD) : cos( 46.01 * DEGTORAD))) ;
|
|
// cerco primo elemento del contorno non valido e vi pongo inizio/fine della curva
|
|
int i = 0 ;
|
|
const ICurve* pCrv = pCrvCompo->GetFirstCurve() ;
|
|
while ( pCrv != nullptr) {
|
|
Vector3d vtDir ; pCrv->GetMidDir( vtDir) ;
|
|
if ( ( vtN ^ vtDir) * vtRef < COS_ANG_MAX) {
|
|
vtDirPrev = vtDir ;
|
|
vtDirNext = vtDir ;
|
|
pCrvCompo->ChangeStartPoint( i) ;
|
|
break ;
|
|
}
|
|
pCrv = pCrvCompo->GetNextCurve() ;
|
|
++ i ;
|
|
}
|
|
// a partire da questo elimino elementi non validi in avanti e indietro
|
|
pCrv = pCrvCompo->GetFirstCurve() ;
|
|
while ( pCrv != nullptr) {
|
|
Vector3d vtDir ; pCrv->GetMidDir( vtDir) ;
|
|
if ( ( vtN ^ vtDir) * vtRef < COS_ANG_MAX) {
|
|
delete( pCrvCompo->RemoveFirstOrLastCurve( false)) ;
|
|
pCrv = pCrvCompo->GetFirstCurve() ;
|
|
vtDirPrev = vtDir ;
|
|
}
|
|
else
|
|
break ;
|
|
}
|
|
pCrv = pCrvCompo->GetLastCurve() ;
|
|
while ( pCrv != nullptr) {
|
|
Vector3d vtDir ; pCrv->GetMidDir( vtDir) ;
|
|
if ( ( vtN ^ vtDir) * vtRef < COS_ANG_MAX) {
|
|
delete( pCrvCompo->RemoveFirstOrLastCurve( true)) ;
|
|
pCrv = pCrvCompo->GetLastCurve() ;
|
|
vtDirNext = vtDir ;
|
|
}
|
|
else
|
|
break ;
|
|
}
|
|
// elimino le curve estreme molto corte ( 0.5 mm)
|
|
const double REF_LEN = 10.0 ;
|
|
double dLen ;
|
|
if ( pCrvCompo->GetLength( dLen) && dLen > REF_LEN) {
|
|
const double MIN_LEN = 0.5 ;
|
|
double dCrvLen ;
|
|
pCrv = pCrvCompo->GetFirstCurve() ;
|
|
if ( pCrv != nullptr && pCrv->GetLength( dCrvLen) && dCrvLen < MIN_LEN)
|
|
delete( pCrvCompo->RemoveFirstOrLastCurve( false)) ;
|
|
pCrv = pCrvCompo->GetLastCurve() ;
|
|
if ( pCrv != nullptr && pCrv->GetLength( dCrvLen) && dCrvLen < MIN_LEN)
|
|
delete( pCrvCompo->RemoveFirstOrLastCurve( true)) ;
|
|
}
|
|
}
|
|
// determino il versore estrusione (utensile)
|
|
if ( nToolDir == TOOL_ORTHO)
|
|
pCrvCompo->SetThickness( 0) ;
|
|
else if ( nToolDir == TOOL_ORTUP) {
|
|
if ( vtN.z < - EPS_SMALL) {
|
|
pCrvCompo->Translate( vtN * dToolThick) ;
|
|
pCrvCompo->SetExtrusion( - vtN) ;
|
|
}
|
|
pCrvCompo->SetThickness( 0) ;
|
|
}
|
|
else { // nToolDir == TOOL_PARAL o TOOL_PAR_SLANT
|
|
// sistemo versore
|
|
Vector3d vtExtr ;
|
|
Vector3d vtTm ; pCrvCompo->GetMidDir( vtTm) ;
|
|
Vector3d vtEm = vtN ^ vtTm ;
|
|
if ( vtEm.Normalize())
|
|
vtExtr = vtEm ;
|
|
Vector3d vtTs ; pCrvCompo->GetStartDir( vtTs) ;
|
|
Vector3d vtEs = vtN ^ vtTs ;
|
|
if ( vtEs.Normalize() && vtEs * vtRef > vtExtr * vtRef)
|
|
vtExtr = vtEs ;
|
|
Vector3d vtTe ; pCrvCompo->GetEndDir( vtTe) ;
|
|
Vector3d vtEe = vtN ^ vtTe ;
|
|
if ( vtEe.Normalize() && vtEe * vtRef > vtExtr * vtRef)
|
|
vtExtr = vtEe ;
|
|
if ( nToolDir == TOOL_PAR_SLANT) {
|
|
Vector3d vtDirPN = -vtDirPrev + vtDirNext ;
|
|
// se le direzioni prima e dopo non sono troppo diverse tra loro (15deg) e la loro media è normalizzabile
|
|
if ( ( -vtDirPrev * vtDirNext) > cos( 15 * DEGTORAD) && vtDirPN.Normalize()) {
|
|
if ( vtDirPN * vtExtr > 0)
|
|
vtExtr = vtDirPN ;
|
|
else
|
|
vtExtr = -vtDirPN ;
|
|
}
|
|
}
|
|
pCrvCompo->SetExtrusion( vtExtr) ;
|
|
// elimino eventuali parti iniziali e/o finali dirette circa come il versore
|
|
const double MAX_ALL_COS = cos( 25 * DEGTORAD) ;
|
|
const ICurve* pCrv = pCrvCompo->GetFirstCurve() ;
|
|
while ( pCrv != nullptr) {
|
|
Vector3d vtDir ; pCrv->GetStartDir( vtDir) ;
|
|
if ( abs( vtDir * vtExtr) > MAX_ALL_COS) {
|
|
ICurve* pErase = pCrvCompo->RemoveFirstOrLastCurve( false) ;
|
|
delete( pErase) ;
|
|
pCrv = pCrvCompo->GetFirstCurve() ;
|
|
}
|
|
else if ( pCrvCompo->IsClosed()) {
|
|
pCrvCompo->ChangeStartPoint( 1) ;
|
|
pCrv = pCrvCompo->GetFirstCurve() ;
|
|
}
|
|
else
|
|
break ;
|
|
}
|
|
pCrv = pCrvCompo->GetLastCurve() ;
|
|
while ( pCrv != nullptr) {
|
|
Vector3d vtDir ; pCrv->GetStartDir( vtDir) ;
|
|
if ( abs( vtDir * vtExtr) > MAX_ALL_COS) {
|
|
ICurve* pErase = pCrvCompo->RemoveFirstOrLastCurve( true) ;
|
|
delete( pErase) ;
|
|
pCrv = pCrvCompo->GetLastCurve() ;
|
|
}
|
|
else
|
|
break ;
|
|
}
|
|
// determino lo spessore misurato lungo la direzione
|
|
if ( ! IsNull( pCopy)) {
|
|
Frame3d frExtr ;
|
|
frExtr.Set( ORIG, vtExtr) ;
|
|
frExtr.Invert() ;
|
|
BBox3d b3Extr ; pCopy->GetBBox( frExtr, b3Extr) ;
|
|
if ( ! b3Extr.IsEmpty()) {
|
|
double dThick = b3Extr.GetMax().z - b3Extr.GetMin().z ;
|
|
Point3d ptStart, ptEnd ;
|
|
if ( nToolDir == TOOL_PAR_SLANT && pCrvCompo->IsALine( LIN_TOL_STD, ptStart, ptEnd)) {
|
|
BBox3d b3Crv ; pCrvCompo->GetBBox( frExtr, b3Crv) ;
|
|
if ( ! b3Crv.IsEmpty())
|
|
dThick -= b3Crv.GetMax().z - b3Crv.GetMin().z ;
|
|
}
|
|
pCrvCompo->SetThickness( dThick) ;
|
|
}
|
|
}
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::ApproxWithArcsIfUseful( ICurveComposite* pCompo, bool bCareTempProp) const
|
|
{
|
|
// recupero estrusione e spessore
|
|
Vector3d vtExtr = Z_AX ;
|
|
pCompo->GetExtrusion( vtExtr) ;
|
|
double dThick = 0 ;
|
|
pCompo->GetThickness( dThick) ;
|
|
// verifico se ci sono tante linee corte
|
|
int nSmallLineCnt = 0 ;
|
|
const ICurve* pCrv = pCompo->GetFirstCurve() ;
|
|
while ( pCrv != nullptr) {
|
|
double dLen ;
|
|
if ( pCrv->GetLength( dLen) && dLen < LIN_FEA_STD / 2)
|
|
++ nSmallLineCnt ;
|
|
pCrv = pCompo->GetNextCurve() ;
|
|
}
|
|
if ( nSmallLineCnt < 10)
|
|
return true ;
|
|
// limito l'approssimazione alle curve piane
|
|
Frame3d frRef ;
|
|
frRef.Set( ORIG, vtExtr) ;
|
|
Frame3d frInvRef = frRef ; frInvRef.Invert() ;
|
|
BBox3d b3Crv ;
|
|
if ( ! pCompo->GetBBox( frInvRef, b3Crv) || abs( b3Crv.GetMax().z - b3Crv.GetMin().z - dThick) > 100 * EPS_SMALL)
|
|
return true ;
|
|
// porto la curva nel suo piano perchè ApproxWithArcsEx funziona bene solo nel piano XY (all'uscita va ripristinato il rif. originale)
|
|
pCompo->ToLoc( frRef) ;
|
|
// se posso ignorare la proprietà temporanea
|
|
if ( ! bCareTempProp) {
|
|
// calcolo approssimazione con archi (se possibile)
|
|
PolyArc PA ;
|
|
if ( ! pCompo->ApproxWithArcsEx( LIN_TOL_MID, ANG_TOL_STD_DEG, LIN_FEA_STD, PA)) {
|
|
pCompo->ToGlob( frRef) ;
|
|
return true ;
|
|
}
|
|
// sostituisco gli archi alle curve originali
|
|
pCompo->Clear() ;
|
|
if ( ! pCompo->FromPolyArc( PA)) {
|
|
pCompo->ToGlob( frRef) ;
|
|
return false ;
|
|
}
|
|
}
|
|
// altrimenti divido la curva in parti con la stessa proprietà e approssimo separatamente ciascuna parte
|
|
else {
|
|
// determino gli intervalli in cui dividere la curva
|
|
INTINTVECTOR vInt ;
|
|
int nPrevProp = - 1 ;
|
|
int nCount = 0 ;
|
|
const ICurve* pCrv = pCompo->GetFirstCurve() ;
|
|
while ( pCrv != nullptr) {
|
|
int nProp = pCrv->GetTempProp() ;
|
|
if ( nPrevProp != -1 && nProp != nPrevProp)
|
|
vInt.emplace_back( nCount, nPrevProp) ;
|
|
// passo alla curva successiva
|
|
nPrevProp = nProp ;
|
|
++ nCount ;
|
|
pCrv = pCompo->GetNextCurve() ;
|
|
}
|
|
if ( nCount > 0)
|
|
vInt.emplace_back( nCount, nPrevProp) ;
|
|
// divido la curva negli intervalli e li interpolo
|
|
ICURVEPOVECTOR vpCrvs ;
|
|
vpCrvs.reserve( vInt.size()) ;
|
|
for ( int i = 0 ; i < int( vInt.size()) ; ++ i) {
|
|
// recupero l'intervallo
|
|
vpCrvs.emplace_back( pCompo->Clone()) ;
|
|
if ( IsNull( vpCrvs.back())) {
|
|
pCompo->ToGlob( frRef) ;
|
|
return false ;
|
|
}
|
|
vpCrvs.back()->TrimEndAtParam( vInt[i].first) ;
|
|
if ( i > 0)
|
|
vpCrvs.back()->TrimStartAtParam( vInt[i-1].first) ;
|
|
// calcolo approssimazione con archi (se possibile)
|
|
PolyArc PA ;
|
|
if ( vpCrvs.back()->ApproxWithArcsEx( LIN_TOL_MID, ANG_TOL_STD_DEG, LIN_FEA_STD, PA)) {
|
|
// sostituisco gli archi alle curve originali
|
|
PtrOwner<ICurveComposite> pCC( CreateCurveComposite()) ;
|
|
if ( IsNull( pCC) || ! pCC->FromPolyArc( PA)) {
|
|
pCompo->ToGlob( frRef) ;
|
|
return false ;
|
|
}
|
|
// assegno la proprietà originale ad ogni curva semplice della composita
|
|
for ( int j = 0 ; j < pCC->GetCurveCount() ; ++ j)
|
|
pCC->SetCurveTempProp( j, vInt[i].second) ;
|
|
// reinserisco nel vettore di curve
|
|
vpCrvs.back().Set( pCC) ;
|
|
}
|
|
}
|
|
// riassemblo la curva e assegno le opportune proprietà
|
|
pCompo->Clear() ;
|
|
for ( int i = 0 ; i < int( vpCrvs.size()) ; ++ i) {
|
|
pCompo->AddCurve( Release( vpCrvs[i])) ;
|
|
}
|
|
}
|
|
// riporto in globale
|
|
pCompo->ToGlob( frRef) ;
|
|
|
|
// riassegno estrusione e spessore
|
|
pCompo->SetExtrusion( vtExtr) ;
|
|
pCompo->SetThickness( dThick) ;
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::ApproxWithLines( ICurveComposite* pCompo, bool bFeed) const
|
|
{
|
|
// recupero estrusione e spessore
|
|
Vector3d vtExtr = Z_AX ;
|
|
pCompo->GetExtrusion( vtExtr) ;
|
|
double dThick = 0 ;
|
|
pCompo->GetThickness( dThick) ;
|
|
// calcolo approssimazione lineare
|
|
PolyLine PL ;
|
|
double dLinTol = GetApproxLinTol() ;
|
|
if ( ! pCompo->ApproxWithLines( dLinTol, ANG_TOL_MID_DEG, ICurve::APL_SPECIAL, PL))
|
|
return false ;
|
|
// se prevista massima lunghezza
|
|
double dMaxLen = GetMaxSplitLen( true, bFeed) ;
|
|
if ( dMaxLen > 99 * EPS_SMALL)
|
|
PL.AdjustForMaxSegmentLen( dMaxLen) ;
|
|
// sostituisco le linee alle curve originali
|
|
pCompo->Clear() ;
|
|
pCompo->FromPolyLine( PL) ;
|
|
// riassegno estrusione e spessore
|
|
pCompo->SetExtrusion( vtExtr) ;
|
|
pCompo->SetThickness( dThick) ;
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::VerifyArcs( ICurveComposite* pCompo, double dMaxAngCen) const
|
|
{
|
|
// verifiche sull'ampiezza dell'angolo al centro degli eventuali archi
|
|
int nMaxInd = pCompo->GetCurveCount() - 1 ;
|
|
for ( int i = 0 ; i <= nMaxInd ; ) {
|
|
// se arco con angolo al centro oltre il limite, lo divido a metà
|
|
const ICurveArc* pArc = GetCurveArc( pCompo->GetCurve( i)) ;
|
|
if ( pArc != nullptr && abs( pArc->GetAngCenter()) > dMaxAngCen) {
|
|
pCompo->AddJoint( i + 0.5) ;
|
|
++ nMaxInd ;
|
|
}
|
|
else
|
|
++ i ;
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::CalcAndSetBBox( int nClId)
|
|
{
|
|
if ( m_pGeomDB == nullptr)
|
|
return false ;
|
|
// assegno ingombri dei vari percorsi di lavorazione e della lavorazione nel suo complesso
|
|
BBox3d b3AllMch ;
|
|
int nClPathId = m_pGeomDB->GetFirstInGroup( nClId) ;
|
|
while ( nClPathId != GDB_ID_NULL) {
|
|
BBox3d b3Grp ;
|
|
for ( int nEntId = m_pGeomDB->GetFirstInGroup( nClPathId) ;
|
|
nEntId != GDB_ID_NULL ;
|
|
nEntId = m_pGeomDB->GetNext( nEntId)) {
|
|
// recupero i dati Cam dell'entità
|
|
CamData* pCamData = GetCamData( m_pGeomDB->GetUserObj( nEntId)) ;
|
|
if ( pCamData == nullptr)
|
|
continue ;
|
|
// recupero il punto finale e lo uso per aggiornare l'ingombro
|
|
Point3d ptP = pCamData->GetEndPoint() ;
|
|
AdjustEndPointForAxesCalc( pCamData, ptP) ;
|
|
b3Grp.Add( ptP) ;
|
|
// se arco, determino anche alcuni punti intermedi
|
|
if ( pCamData->IsArc()) {
|
|
// dati dell'arco
|
|
Point3d ptCen = pCamData->GetCenter() ;
|
|
AdjustArcCenterForAxesCalc( pCamData, ptCen) ;
|
|
double dAngCen = pCamData->GetAngCen() ;
|
|
Vector3d vtN = pCamData->GetNormDir() ;
|
|
double dDeltaN = pCamData->GetDeltaN() ;
|
|
// ricavo i punti
|
|
const double ANG_STEP = 4.9 ;
|
|
int nStep = 1 + int( abs( dAngCen) / ANG_STEP) ;
|
|
double dCoeff = -1. / nStep ;
|
|
double dCosRot = cos( dCoeff * dAngCen * DEGTORAD) ;
|
|
double dSinRot = sin( dCoeff * dAngCen * DEGTORAD) ;
|
|
Point3d ptT = ptP ;
|
|
for ( int i = 1 ; i < nStep ; ++ i) {
|
|
ptT.Rotate( ptCen, vtN, dCosRot, dSinRot) ;
|
|
ptT.Translate( vtN * ( dCoeff * dDeltaN)) ;
|
|
b3Grp.Add( ptT) ;
|
|
}
|
|
}
|
|
}
|
|
if ( ! b3Grp.IsEmpty()) {
|
|
m_pGeomDB->SetInfo( nClPathId, KEY_PMIN, b3Grp.GetMin()) ;
|
|
m_pGeomDB->SetInfo( nClPathId, KEY_PMAX, b3Grp.GetMax()) ;
|
|
b3AllMch.Add( b3Grp) ;
|
|
}
|
|
nClPathId = m_pGeomDB->GetNext( nClPathId) ;
|
|
}
|
|
if ( ! b3AllMch.IsEmpty()) {
|
|
m_pGeomDB->SetInfo( nClId, KEY_MMIN, b3AllMch.GetMin()) ;
|
|
m_pGeomDB->SetInfo( nClId, KEY_MMAX, b3AllMch.GetMax()) ;
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::CalcAndSetAxesBBox( void)
|
|
{
|
|
if ( m_pGeomDB == nullptr || m_pMchMgr == nullptr)
|
|
return false ;
|
|
// recupero gruppo della geometria di lavorazione (Cutter Location)
|
|
int nClId = m_pGeomDB->GetFirstNameInGroup( m_nOwnerId, MCH_CL) ;
|
|
if ( nClId == GDB_ID_NULL)
|
|
return false ;
|
|
// recupero numero totale assi correnti
|
|
int nLinAxes = m_pMchMgr->GetCurrLinAxes() ;
|
|
int nRotAxes = m_pMchMgr->GetCurrRotAxes() ;
|
|
int nTotAxes = nLinAxes + nRotAxes ;
|
|
// assegno estremi assi dei vari percorsi di lavorazione e della lavorazione nel suo complesso
|
|
vector<BBox1d> vMmAxAll( nTotAxes) ;
|
|
int nClPathId = m_pGeomDB->GetFirstInGroup( nClId) ;
|
|
while ( nClPathId != GDB_ID_NULL) {
|
|
vector<BBox1d> vMmAxGrp( nTotAxes) ;
|
|
for ( int nEntId = m_pGeomDB->GetFirstInGroup( nClPathId) ;
|
|
nEntId != GDB_ID_NULL ;
|
|
nEntId = m_pGeomDB->GetNext( nEntId)) {
|
|
// recupero i dati Cam dell'entità
|
|
CamData* pCamData = GetCamData( m_pGeomDB->GetUserObj( nEntId)) ;
|
|
if ( pCamData == nullptr)
|
|
continue ;
|
|
// movimenti in rapido da ignorare
|
|
if ( pCamData->GetMoveType() == 0) {
|
|
// se posizione in home
|
|
if ( pCamData->GetFlag() == 4)
|
|
continue ;
|
|
// se posizione in alto
|
|
string sName ;
|
|
if ( m_pGeomDB->GetName( nEntId, sName) && ( sName == MCH_CL_CLIMB || sName == MCH_CL_RISE))
|
|
continue ;
|
|
}
|
|
// recupero gli assi macchina e li uso per aggiornare l'ingombro
|
|
DBLVECTOR AxesVal = pCamData->GetAxesVal() ;
|
|
for ( int i = 0 ; i < nTotAxes ; ++ i)
|
|
vMmAxGrp[i].Add( AxesVal[i]) ;
|
|
Point3d ptAxEnd( AxesVal[0], AxesVal[1], AxesVal[2]) ;
|
|
// se arco, determino anche alcuni valori intermedi
|
|
if ( pCamData->IsArc()) {
|
|
// dati dell'arco
|
|
Point3d ptCen = pCamData->GetAxesCen() ;
|
|
double dAngCen = pCamData->GetAxesAngCen() ;
|
|
Vector3d vtN = pCamData->GetAxesNormDir() ;
|
|
double dDeltaN = ( ptAxEnd - ptCen) * vtN ;
|
|
// ricavo i punti
|
|
const double ANG_STEP = 4.9 ;
|
|
int nStep = 1 + int( abs( dAngCen) / ANG_STEP) ;
|
|
double dCoeff = -1. / nStep ;
|
|
double dCosRot = cos( dCoeff * dAngCen * DEGTORAD) ;
|
|
double dSinRot = sin( dCoeff * dAngCen * DEGTORAD) ;
|
|
Point3d ptAxT = ptAxEnd ;
|
|
for ( int i = 1 ; i < nStep ; ++ i) {
|
|
// assi lineari
|
|
ptAxT.Rotate( ptCen, vtN, dCosRot, dSinRot) ;
|
|
ptAxT.Translate( vtN * ( dCoeff * dDeltaN)) ;
|
|
for ( int i = 0 ; i < min( nLinAxes, 3) ; ++ i)
|
|
vMmAxGrp[i].Add( ptAxT.v[i]) ;
|
|
}
|
|
}
|
|
}
|
|
DBLVECTOR vMinAxGrp( nTotAxes, +INFINITO), vMaxAxGrp( nTotAxes, -INFINITO) ;
|
|
for ( int i = 0 ; i < nTotAxes ; ++ i) {
|
|
vMmAxGrp[i].GetMinMax( vMinAxGrp[i], vMaxAxGrp[i]) ;
|
|
vMmAxAll[i].Add( vMmAxGrp[i]) ;
|
|
}
|
|
m_pGeomDB->SetInfo( nClPathId, KEY_PAXMIN, vMinAxGrp) ;
|
|
m_pGeomDB->SetInfo( nClPathId, KEY_PAXMAX, vMaxAxGrp) ;
|
|
nClPathId = m_pGeomDB->GetNext( nClPathId) ;
|
|
}
|
|
DBLVECTOR vMinAxAll( nTotAxes, +INFINITO), vMaxAxAll( nTotAxes, -INFINITO) ;
|
|
for ( int i = 0 ; i < nTotAxes ; ++ i)
|
|
vMmAxAll[i].GetMinMax( vMinAxAll[i], vMaxAxAll[i]) ;
|
|
m_pGeomDB->SetInfo( nClId, KEY_MAXMIN, vMinAxAll) ;
|
|
m_pGeomDB->SetInfo( nClId, KEY_MAXMAX, vMaxAxAll) ;
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::CalcMirrorByDouble( int nClId, const std::string& sUserNotes)
|
|
{
|
|
// verifico se prevista lavorazione in doppio
|
|
int nDouble ;
|
|
if ( ! GetValInNotes( sUserNotes, "DOUBLE", nDouble) ||
|
|
( nDouble != 1 && nDouble != 2 && nDouble != 3))
|
|
return true ;
|
|
|
|
// copio gruppo di lavorazione e cambio nome
|
|
int nDblId = m_pGeomDB->Copy( nClId, GDB_ID_NULL, nClId, GDB_AFTER) ;
|
|
if ( nDblId == GDB_ID_NULL)
|
|
return false ;
|
|
m_pGeomDB->SetName( nDblId, MCH_DBL) ;
|
|
// assegno colore di copia
|
|
int nClPathId = m_pGeomDB->GetFirstGroupInGroup( nDblId) ;
|
|
while ( nClPathId != GDB_ID_NULL) {
|
|
m_pGeomDB->SetMaterial( nClPathId, GRAY) ;
|
|
nClPathId = m_pGeomDB->GetNextGroup( nClPathId) ;
|
|
}
|
|
|
|
// determino posizione del piano di mirroring
|
|
BBox3d b3Raw ;
|
|
Point3d ptOn ;
|
|
if ( ! GetCurrRawsGlobBox( b3Raw) || ! b3Raw.GetCenter( ptOn))
|
|
return false ;
|
|
Vector3d vtNorm ;
|
|
switch ( nDouble) {
|
|
case 1 : vtNorm = X_AX ; break ;
|
|
case 2 : vtNorm = Y_AX ; break ;
|
|
case 3 : vtNorm = Z_AX ; break ;
|
|
}
|
|
double dPlanePos ;
|
|
if ( GetValInNotes( sUserNotes, "MirrorAx", dPlanePos))
|
|
ptOn = b3Raw.GetMin() + dPlanePos * vtNorm ;
|
|
|
|
// eseguo mirroring rispetto a piano opportuno
|
|
m_pGeomDB->MirrorGroup( nDblId, ptOn, vtNorm) ;
|
|
|
|
// se presente una differenza in Z, eseguo spostamento
|
|
double dDeltaZ ;
|
|
if ( GetValInNotes( sUserNotes, "DeltaZ", dDeltaZ) && abs( dDeltaZ) > EPS_SMALL)
|
|
m_pGeomDB->TranslateGlob( nDblId, Vector3d( 0, 0, dDeltaZ)) ;
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetInitialAxesValues( bool bSkipClimb, DBLVECTOR& vAxVal) const
|
|
{
|
|
const CamData* pCamData = GetInitialCamData( bSkipClimb) ;
|
|
vAxVal = ( pCamData != nullptr ? pCamData->GetAxesVal() : DBLVECTOR()) ;
|
|
return ( vAxVal.size() > 0) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetClPathInitialAxesValues( int nClPathId, bool bSkipClimb, DBLVECTOR& vAxVal) const
|
|
{
|
|
const CamData* pCamData = GetClPathInitialCamData( nClPathId, bSkipClimb) ;
|
|
vAxVal = ( pCamData != nullptr ? pCamData->GetAxesVal() : DBLVECTOR()) ;
|
|
return ( vAxVal.size() > 0) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetFinalAxesValues( bool bSkipRise, DBLVECTOR& vAxVal) const
|
|
{
|
|
const CamData* pCamData = GetFinalCamData( bSkipRise) ;
|
|
vAxVal = ( pCamData != nullptr ? pCamData->GetAxesVal() : DBLVECTOR()) ;
|
|
return ( vAxVal.size() > 0) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetClPathFinalAxesValues( int nClPathId, bool bSkipRise, DBLVECTOR& vAxVal) const
|
|
{
|
|
const CamData* pCamData = GetClPathFinalCamData( nClPathId, bSkipRise) ;
|
|
vAxVal = ( pCamData != nullptr ? pCamData->GetAxesVal() : DBLVECTOR()) ;
|
|
return ( vAxVal.size() > 0) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetInitialToolDir( bool bSkipClimb, Vector3d& vtTool) const
|
|
{
|
|
const CamData* pCamData = GetInitialCamData( bSkipClimb) ;
|
|
vtTool = ( pCamData != nullptr ? pCamData->GetToolDir() : V_NULL) ;
|
|
return ( ! vtTool.IsSmall()) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetClPathInitialToolDir( int nClPathId, bool bSkipClimb, Vector3d& vtTool) const
|
|
{
|
|
const CamData* pCamData = GetClPathInitialCamData( nClPathId, bSkipClimb) ;
|
|
vtTool = ( pCamData != nullptr ? pCamData->GetToolDir() : V_NULL) ;
|
|
return ( ! vtTool.IsSmall()) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetFinalToolDir( bool bSkipRise, Vector3d& vtTool) const
|
|
{
|
|
const CamData* pCamData = GetFinalCamData( bSkipRise) ;
|
|
vtTool = ( pCamData != nullptr ? pCamData->GetToolDir() : V_NULL) ;
|
|
return ( ! vtTool.IsSmall()) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetClPathFinalToolDir( int nClPathId, bool bSkipRise, Vector3d& vtTool) const
|
|
{
|
|
const CamData* pCamData = GetClPathFinalCamData( nClPathId, bSkipRise) ;
|
|
vtTool = ( pCamData != nullptr ? pCamData->GetToolDir() : V_NULL) ;
|
|
return ( ! vtTool.IsSmall()) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
const CamData*
|
|
Operation::GetInitialCamData( bool bSkipClimb) const
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return nullptr ;
|
|
// recupero gruppo per geometria di lavorazione (Cutter Location)
|
|
int nClId = m_pGeomDB->GetFirstNameInGroup( GetOwner(), MCH_CL) ;
|
|
if ( nClId == GDB_ID_NULL)
|
|
return nullptr ;
|
|
// recupero il primo percorso CL
|
|
int nClPathId = m_pGeomDB->GetFirstGroupInGroup( nClId) ;
|
|
// recupero i dati Cam della prima entità del percorso
|
|
return GetClPathInitialCamData( nClPathId, bSkipClimb) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
const CamData*
|
|
Operation::GetClPathInitialCamData( int nClPathId, bool bSkipClimb) const
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return nullptr ;
|
|
// recupero la prima entità di questo percorso
|
|
int nEntId = m_pGeomDB->GetFirstInGroup( nClPathId) ;
|
|
// se richiesto di ignorare le entità CLIMB
|
|
if ( bSkipClimb) {
|
|
while ( nEntId != GDB_ID_NULL) {
|
|
string sName ;
|
|
if ( ! m_pGeomDB->GetName( nEntId, sName) || sName != MCH_CL_CLIMB)
|
|
break ;
|
|
nEntId = m_pGeomDB->GetNext( nEntId) ;
|
|
}
|
|
}
|
|
// recupero i dati Cam dell'entità
|
|
return GetCamData( m_pGeomDB->GetUserObj( nEntId)) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
const CamData*
|
|
Operation::GetFinalCamData( bool bSkipRise) const
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return nullptr ;
|
|
// recupero gruppo per geometria di lavorazione (Cutter Location)
|
|
int nClId = m_pGeomDB->GetFirstNameInGroup( GetOwner(), MCH_CL) ;
|
|
if ( nClId == GDB_ID_NULL)
|
|
return nullptr ;
|
|
// recupero l'ultimo percorso CL
|
|
int nClPathId = m_pGeomDB->GetLastGroupInGroup( nClId) ;
|
|
// recupero i dati Cam dell'ultima entità del percorso
|
|
return GetClPathFinalCamData( nClPathId, bSkipRise) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
const CamData*
|
|
Operation::GetClPathFinalCamData( int nClPathId, bool bSkipRise) const
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return nullptr ;
|
|
// recupero l'ultima entità di questo percorso
|
|
int nEntId = m_pGeomDB->GetLastInGroup( nClPathId) ;
|
|
// se richiesto di ignorare le entità RISE
|
|
if ( bSkipRise) {
|
|
while ( nEntId != GDB_ID_NULL) {
|
|
string sName ;
|
|
if ( ! m_pGeomDB->GetName( nEntId, sName) || sName != MCH_CL_RISE)
|
|
break ;
|
|
nEntId = m_pGeomDB->GetPrev( nEntId) ;
|
|
}
|
|
}
|
|
// recupero i dati Cam dell'entità
|
|
return GetCamData( m_pGeomDB->GetUserObj( nEntId)) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
string
|
|
Operation::ExtractInfo( const string& sNotes, const string& sKey) const
|
|
{
|
|
string sInfo ;
|
|
STRVECTOR vsCmd ;
|
|
Tokenize( sNotes, ";", vsCmd) ;
|
|
for ( const auto& sCmd : vsCmd) {
|
|
// se chiave trovata
|
|
if ( sCmd.find( sKey) != string::npos) {
|
|
sInfo = sCmd ;
|
|
ReplaceString( sInfo, sKey, "") ;
|
|
break ;
|
|
}
|
|
}
|
|
return sInfo ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
string
|
|
Operation::ExtractHint( const string& sNotes) const
|
|
{
|
|
return ExtractInfo( sNotes, "Hint:") ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::SetBlockedRotAxis( const string& sBlockedAxis) const
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// cancello bloccaggio attuale
|
|
m_pMchMgr->ClearRotAxisBlock() ;
|
|
// imposto l'utensile per i calcoli macchina
|
|
if ( ! m_pMchMgr->SetCalcTool( GetToolName(), GetHeadName(), GetExitNbr()))
|
|
return false ;
|
|
// se non ci sono bloccaggi, esco
|
|
if ( sBlockedAxis.empty())
|
|
return true ;
|
|
// recupero token/nome asse e valore
|
|
string sKey, sVal ;
|
|
Split( sBlockedAxis, "=", true, sKey, sVal) ;
|
|
if ( sKey.empty() || sVal.empty())
|
|
return true ;
|
|
double dVal = 0 ;
|
|
if ( ! FromString( sVal, dVal))
|
|
return true ;
|
|
// recupero numero assi lineari e rotanti correnti
|
|
int nLinAxes = m_pMchMgr->GetCurrLinAxes() ;
|
|
int nRotAxes = m_pMchMgr->GetCurrRotAxes() ;
|
|
// lo cerco tra i token degli assi rotanti correnti
|
|
for ( int i = 0 ; i < nRotAxes ; ++ i) {
|
|
string sAxToken ;
|
|
if ( m_pMchMgr->GetCurrMachine()->GetCurrAxisToken( i + nLinAxes, sAxToken) &&
|
|
sKey == Trim( sAxToken, " \t\r\n=")) {
|
|
string sAxis ;
|
|
m_pMchMgr->GetCurrMachine()->GetCurrAxisName( i + nLinAxes, sAxis) ;
|
|
return m_pMchMgr->SetRotAxisBlock( sAxis, dVal) ;
|
|
}
|
|
}
|
|
// lo cerco tra gli assi rotanti della macchina
|
|
return m_pMchMgr->SetRotAxisBlock( sKey, dVal) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::CalculateAxesValues( const string& sHint, bool bRotContOnNext, bool bSolChExact)
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// recupero l'operazione precedente non vuota o che richiede home precedente
|
|
int nPrevOpId = m_pMchMgr->GetPrevActiveOperation( m_nOwnerId) ;
|
|
Operation* pPrevOp = GetOperation( m_pGeomDB->GetUserObj( nPrevOpId)) ;
|
|
while ( pPrevOp != nullptr) {
|
|
if ( ! pPrevOp->IsEmpty() || pPrevOp->NeedPrevHome())
|
|
break ;
|
|
else {
|
|
nPrevOpId = m_pMchMgr->GetPrevActiveOperation( nPrevOpId) ;
|
|
pPrevOp = GetOperation( m_pGeomDB->GetUserObj( nPrevOpId)) ;
|
|
}
|
|
}
|
|
// recupero l'utensile precedente
|
|
string sPrevTool ;
|
|
if ( pPrevOp != nullptr && ! pPrevOp->IsEmpty())
|
|
sPrevTool = pPrevOp->GetToolName() ;
|
|
|
|
// imposto l'utensile per i calcoli macchina
|
|
if ( ! m_pMchMgr->SetCalcTool( GetToolName(), GetHeadName(), GetExitNbr()))
|
|
return false ;
|
|
|
|
// rimuovo posizionamento home da lavorazione precedente se non richiesto (non può più essere l'ultima)
|
|
if ( pPrevOp != nullptr && ! NeedPrevHome())
|
|
pPrevOp->RemoveHome() ;
|
|
|
|
// recupero il numero di assi lineari e rotanti attivi
|
|
int nLinAxes = m_pMchMgr->GetCurrLinAxes() ;
|
|
int nRotAxes = m_pMchMgr->GetCurrRotAxes() ;
|
|
|
|
// recupero gli angoli home
|
|
DBLVECTOR vAxRotHome( nRotAxes, 0.) ;
|
|
DBLVECTOR vAxHome ;
|
|
m_pMchMgr->GetAllCurrAxesHomePos( vAxHome) ;
|
|
for ( int i = 0 ; i < nRotAxes ; ++ i)
|
|
vAxRotHome[i] = vAxHome[nLinAxes + i] ;
|
|
|
|
// Assegno gli angoli iniziali
|
|
DBLVECTOR vAxRotPrec( nRotAxes, 0.) ;
|
|
DBLVECTOR vAxVal ;
|
|
// se non richiesti angoli Home e utensile non cambiato, uso gli angoli finali della lavorazione precedente
|
|
if ( GetUserNotesZmax() != 2 && ! sPrevTool.empty() && ! ToolChangeNeeded( *pPrevOp, *this) &&
|
|
pPrevOp->GetFinalAxesValues( true, vAxVal)) {
|
|
for ( int i = 0 ; i < nRotAxes ; ++ i)
|
|
vAxRotPrec[i] = vAxVal[nLinAxes + i] ;
|
|
}
|
|
// altrimenti uso gli angoli home
|
|
else {
|
|
vAxRotPrec = vAxRotHome ;
|
|
}
|
|
// se ci sono suggerimenti validi per gli assi rotanti, li applico
|
|
if ( ! sHint.empty()) {
|
|
STRVECTOR vsTok ;
|
|
Tokenize( sHint, ",;", vsTok) ;
|
|
for ( const auto& sTok : vsTok) {
|
|
string szKey, szVal ;
|
|
Split( sTok, "=", true, szKey, szVal) ;
|
|
Trim( szKey) ;
|
|
for ( int i = 0 ; i < nRotAxes ; ++ i) {
|
|
string sAxToken ;
|
|
if ( m_pMchMgr->GetCurrMachine()->GetCurrAxisToken( nLinAxes + i, sAxToken) &&
|
|
szKey == Trim( sAxToken, " \t\r\n=")) {
|
|
double dVal ;
|
|
if ( FromString( szVal, dVal)) {
|
|
double dOffset = 0 ;
|
|
m_pMchMgr->GetCurrMachine()->GetCurrAxisOffset( nLinAxes + i, dOffset) ;
|
|
if ( abs( dOffset) > EPS_ANG_SMALL)
|
|
dVal -= dOffset ;
|
|
bool bInvert = false ;
|
|
m_pMchMgr->GetCurrMachine()->GetCurrAxisInvert( nLinAxes + i, bInvert) ;
|
|
if ( bInvert)
|
|
dVal = -dVal ;
|
|
vAxRotPrec[i] = dVal ;
|
|
}
|
|
break ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// recupero peso primo asse rotante di testa
|
|
double dRot1W = m_pMchMgr->GetCalcRot1W() ;
|
|
|
|
// recupero flag controllo delta massimo secondo asse rotante anche su primo movimento di lavorazione
|
|
bool bMaxDeltaR2OnFirst = m_pMchMgr->GetCalcMaxDeltaR2OnFirst() ;
|
|
|
|
// recupero la minima differenza angolare da posizione precedente per stare vicino a posizione home
|
|
double dAngDeltaMinForHome = m_pMchMgr->GetAngDeltaMinForHome() ;
|
|
|
|
// applico eventuali blocchi di assi rotanti
|
|
m_pMchMgr->ApplyRotAxisBlock() ;
|
|
|
|
// applico il criterio di scelta della soluzione quando molteplici
|
|
m_pMchMgr->SetCalcSolCh( GetSolCh(), bSolChExact) ;
|
|
|
|
// recupero gruppo della geometria di lavorazione (Cutter Location)
|
|
int nClId = m_pGeomDB->GetFirstNameInGroup( GetOwner(), MCH_CL) ;
|
|
if ( nClId == GDB_ID_NULL)
|
|
return false ;
|
|
|
|
// calcolo il valore degli assi macchina di tutti i movimenti
|
|
bool bOk = true ;
|
|
if ( m_pMchMgr->GetCurrIsCenter()) {
|
|
int nClPathId = m_pGeomDB->GetFirstGroupInGroup( nClId) ;
|
|
while ( nClPathId != GDB_ID_NULL) {
|
|
if ( ! CalculateClPathCenterAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst,
|
|
bRotContOnNext, dAngDeltaMinForHome, vAxRotHome, vAxRotPrec))
|
|
bOk = false ;
|
|
nClPathId = m_pGeomDB->GetNextGroup( nClPathId) ;
|
|
}
|
|
}
|
|
else if ( m_pMchMgr->GetCurrIsRobot()) {
|
|
int nClPathId = m_pGeomDB->GetFirstGroupInGroup( nClId) ;
|
|
while ( nClPathId != GDB_ID_NULL) {
|
|
if ( ! CalculateClPathRobotAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst,
|
|
bRotContOnNext, dAngDeltaMinForHome, vAxRotHome, vAxRotPrec))
|
|
bOk = false ;
|
|
nClPathId = m_pGeomDB->GetNextGroup( nClPathId) ;
|
|
}
|
|
}
|
|
|
|
return bOk ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::CalculateClPathCenterAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W,
|
|
bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome,
|
|
const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec)
|
|
{
|
|
DBLVECTOR vAxRotPrecOri = vAxRotPrec ;
|
|
int nOutStrC = 0 ;
|
|
if ( ! CalculateClPathAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst,
|
|
bRotContOnNext, dAngDeltaMinForHome, vAxRotHome, vAxRotPrec, nOutStrC)) {
|
|
// se attivata scelta angolo iniziale vicino ad home ed extra corsa C, provo a rifarlo disattivando
|
|
if ( dAngDeltaMinForHome < ANG_FULL && nOutStrC != 0) {
|
|
m_pMchMgr->GetCurrMachine()->ResetOutstrokeInfo() ;
|
|
if ( CalculateClPathAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst,
|
|
bRotContOnNext, INFINITO, vAxRotHome, vAxRotPrec, nOutStrC))
|
|
return true ;
|
|
}
|
|
// se extracorsa dell'asse C, provo a precaricarlo al contrario
|
|
if ( nOutStrC != 0) {
|
|
vAxRotPrec = vAxRotPrecOri ;
|
|
vAxRotPrec[0] = vAxRotPrecOri[0] + ( nOutStrC > 0 ? - ANG_FULL : ANG_FULL) ;
|
|
m_pMchMgr->GetCurrMachine()->ResetOutstrokeInfo() ;
|
|
if ( ! CalculateClPathAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst,
|
|
bRotContOnNext, INFINITO, vAxRotHome, vAxRotPrec, nOutStrC))
|
|
return false ;
|
|
}
|
|
else
|
|
return false ;
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W,
|
|
bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome,
|
|
const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec, int& nOutStrC)
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
Machine* pMachine = m_pMchMgr->GetCurrMachine() ;
|
|
// predispongo variabile per valori assi
|
|
DBLVECTOR vAxVal ;
|
|
vAxVal.reserve( 8) ;
|
|
// valori del precedente
|
|
Point3d ptPrec ;
|
|
Vector3d vtDirPrec ;
|
|
Vector3d vtAuxPrec ;
|
|
Vector3d vtCorrPrec ;
|
|
DBLVECTOR vAxPrec ;
|
|
// ciclo su tutte le entità del percorso CL
|
|
nOutStrC = 0 ;
|
|
bool bOk = true ;
|
|
bool bFirst = true ;
|
|
for ( int nEntId = m_pGeomDB->GetFirstInGroup( nClPathId) ;
|
|
nEntId != GDB_ID_NULL ;
|
|
nEntId = m_pGeomDB->GetNext( nEntId)) {
|
|
// recupero i dati Cam dell'entità
|
|
CamData* pCamData = GetCamData( m_pGeomDB->GetUserObj( nEntId)) ;
|
|
if ( pCamData == nullptr)
|
|
continue ;
|
|
// verifico che il primo movimento non sia un arco
|
|
if ( bFirst && pCamData->IsArc()) {
|
|
pCamData->ResetAxes() ;
|
|
LOG_ERROR( GetEMkLogger(), "Error : first move in ClPath is an arc")
|
|
return false ;
|
|
}
|
|
// calcolo gli assi rotanti della macchina
|
|
Vector3d vtDir = pCamData->GetToolDir() ;
|
|
Vector3d vtAux = pCamData->GetAuxDir() ;
|
|
Vector3d vtCorr = pCamData->GetCorrDir() ;
|
|
DBLVECTOR vAxRot ;
|
|
if ( ! CalculateRotAxesValues( bFirst, vtDir, vtAux, dRot1W,
|
|
bMaxDeltaR2OnFirst, bRotContOnNext, dAngDeltaMinForHome, vAxRotHome, vAxRotPrec, vAxRot)) {
|
|
pCamData->SetAxes( CamData::AS_DIR_ERR, vAxVal) ;
|
|
LOG_ERROR( GetEMkLogger(), "Error : tool direction unreachable")
|
|
return false ;
|
|
}
|
|
// ricavo posizione con eventuali modifiche dipendenti dalla lavorazione
|
|
Point3d ptP = pCamData->GetEndPoint() ;
|
|
AdjustEndPointForAxesCalc( pCamData, ptP) ;
|
|
// calcolo gli assi lineari della macchina
|
|
int nLStat ;
|
|
double dX, dY, dZ ;
|
|
bool bLOk = m_pMchMgr->GetCalcPositions( ptP, vAxRot, nLStat, dX, dY, dZ) ;
|
|
if ( ! bLOk || nLStat != 0) {
|
|
bOk = false ;
|
|
pCamData->SetAxes( CamData::AS_ERR, vAxVal) ;
|
|
continue ;
|
|
}
|
|
// assegno valori assi
|
|
vAxVal.clear() ;
|
|
vAxVal.emplace_back( dX) ;
|
|
vAxVal.emplace_back( dY) ;
|
|
vAxVal.emplace_back( dZ) ;
|
|
for ( auto dAng : vAxRot)
|
|
vAxVal.emplace_back( dAng) ;
|
|
// verifico i limiti di corsa degli assi
|
|
int nStat ;
|
|
bool bOsOk = m_pMchMgr->VerifyOutstroke( dX, dY, dZ, vAxRot, false, nStat) ;
|
|
if ( ! bOsOk || nStat != 0) {
|
|
bOk = false ;
|
|
pCamData->SetAxes( CamData::AS_OUTSTROKE, vAxVal) ;
|
|
if ( nOutStrC == 0) {
|
|
if ( (nStat & 64) != 0)
|
|
nOutStrC = - 1 ;
|
|
else if ( (nStat & 128) != 0)
|
|
nOutStrC = 1 ;
|
|
}
|
|
string sInfo = "Outstroke : " + m_pMchMgr->GetOutstrokeInfo() ;
|
|
LOG_INFO( GetEMkLogger(), sInfo.c_str())
|
|
// calcolo e salvo il valore dell'asse ausiliario di testa riportato sul grezzo
|
|
Vector3d vtBackAux ;
|
|
pMachine->GetBackAuxDirFromAngles( vAxRot, vtBackAux) ;
|
|
pCamData->SetBackAuxDir( vtBackAux) ;
|
|
// imposto stato
|
|
bFirst = false ;
|
|
// memorizzo i precedenti
|
|
ptPrec = ptP ;
|
|
vtDirPrec = vtDir ;
|
|
vtAuxPrec = vtAux ;
|
|
vtCorrPrec = vtCorr ;
|
|
vAxPrec = vAxVal ;
|
|
// memorizzo i valori degli angoli come nuovi precedenti
|
|
vAxRotPrec = vAxRot ;
|
|
continue ;
|
|
}
|
|
// salvo i valori degli assi
|
|
pCamData->SetAxes( CamData::AS_OK, vAxVal) ;
|
|
// calcolo e salvo il valore dell'asse ausiliario di testa riportato sul grezzo
|
|
Vector3d vtBackAux ;
|
|
pMachine->GetBackAuxDirFromAngles( vAxRot, vtBackAux) ;
|
|
pCamData->SetBackAuxDir( vtBackAux) ;
|
|
// tipo di movimento
|
|
int nMoveType = pCamData->GetMoveType() ;
|
|
// se punto finale di linea
|
|
if ( ! bFirst && pCamData->IsLine()) {
|
|
// Verifica ricorsiva del punto medio
|
|
bool bMidAdded ;
|
|
bool bAxesError ;
|
|
if ( ! VerifyLineMidPoint( ptPrec, vtDirPrec, vtAuxPrec, vtCorrPrec, vAxPrec,
|
|
ptP, vtDir, vtAux, vtCorr, vAxVal,
|
|
0, nEntId, dRot1W, nMoveType, bMidAdded, bAxesError)) {
|
|
LOG_ERROR( GetEMkLogger(), "Error : VerifyLineMidPoint of CalculateClPathAxesValues failed")
|
|
return false ;
|
|
}
|
|
if ( bAxesError) {
|
|
bOk = false ;
|
|
continue ;
|
|
}
|
|
// Se esistono aree protette, devo comunque verificare i punti intermedi
|
|
if ( ! bMidAdded && m_pMchMgr->ExistProtectedAreas()) {
|
|
// verifico i limiti di corsa dei punti lungo la linea
|
|
const int NUM_VERIF_STEP = 16 ;
|
|
Point3d ptPrec( vAxPrec[0], vAxPrec[1], vAxPrec[2]) ;
|
|
Point3d ptP( dX, dY, dZ) ;
|
|
for ( int i = 1 ; i < NUM_VERIF_STEP ; ++ i) {
|
|
double dCoeff = double( i) / NUM_VERIF_STEP ;
|
|
Point3d ptCurr = Media( ptPrec, ptP, dCoeff) ;
|
|
DBLVECTOR vAng( vAxRot.size()) ;
|
|
for ( size_t i = 0 ; i < vAxRot.size() ; ++ i)
|
|
vAng[i] = vAxRotPrec[i] * ( 1 - dCoeff) + vAxRot[i] * dCoeff ;
|
|
int nStat ;
|
|
bool bOsOk = m_pMchMgr->VerifyOutstroke( ptCurr.x, ptCurr.y, ptCurr.z, vAng, false, nStat) ;
|
|
if ( ! bOsOk || nStat != 0) {
|
|
bOk = false ;
|
|
pCamData->SetAxes( CamData::AS_OUTSTROKE, vAxVal) ;
|
|
string sInfo = "Outstroke : " + m_pMchMgr->GetOutstrokeInfo() ;
|
|
LOG_INFO( GetEMkLogger(), sInfo.c_str())
|
|
}
|
|
}
|
|
}
|
|
}
|
|
// se punto finale di arco -> devo calcolarne il centro in assi macchina
|
|
else if ( pCamData->IsArc()) {
|
|
// devo lavorare con arco schiacciato nel suo piano
|
|
Point3d ptCen = pCamData->GetCenter() ;
|
|
AdjustArcCenterForAxesCalc( pCamData, ptCen) ;
|
|
double dAngCen = pCamData->GetAngCen() ;
|
|
Vector3d vtN = pCamData->GetNormDir() ;
|
|
double dDeltaN = pCamData->GetDeltaN() ;
|
|
// ricavo punto medio nel piano dell'arco
|
|
Point3d ptMid = ptP - dDeltaN * vtN ;
|
|
ptMid.Rotate( ptCen, vtN, - dAngCen / 2) ;
|
|
// determino i valori degli assi al punto medio
|
|
DBLVECTOR vAngMid( vAxRot.size()) ;
|
|
for ( size_t i = 0 ; i < vAxRot.size() ; ++ i)
|
|
vAngMid[i] = ( vAxRotPrec[i] + vAxRot[i]) / 2 ;
|
|
int nLmidStat ;
|
|
double dXmid, dYmid, dZmid ;
|
|
bool bLmidOk = m_pMchMgr->GetCalcPositions( ptMid, vAngMid, nLmidStat, dXmid, dYmid, dZmid) ;
|
|
if ( ! bLmidOk || nLmidStat != 0) {
|
|
bOk = false ;
|
|
pCamData->SetAxes( CamData::AS_ERR, vAxVal) ;
|
|
continue ;
|
|
}
|
|
// ricavo punto finale nel piano dell'arco
|
|
Point3d ptEnd = ptP - dDeltaN * vtN ;
|
|
// determino i valori degli assi al punto finale
|
|
DBLVECTOR vAngEnd( vAxRot.size()) ;
|
|
for ( size_t i = 0 ; i < vAxRot.size() ; ++ i)
|
|
vAngEnd[i] = vAxRot[i] ;
|
|
int nLendStat ;
|
|
double dXend, dYend, dZend ;
|
|
bool bLendOk = m_pMchMgr->GetCalcPositions( ptEnd, vAngEnd, nLendStat, dXend, dYend, dZend) ;
|
|
if ( ! bLendOk || nLendStat != 0) {
|
|
bOk = false ;
|
|
pCamData->SetAxes( CamData::AS_ERR, vAxVal) ;
|
|
continue ;
|
|
}
|
|
// punti espressi in assi macchina
|
|
Point3d ptP1M( vAxPrec[0], vAxPrec[1], vAxPrec[2]) ;
|
|
Point3d ptP2M( dXmid, dYmid, dZmid) ;
|
|
Point3d ptP3M( dXend, dYend, dZend) ;
|
|
// se coincidono
|
|
if ( AreSamePointApprox( ptP1M, ptP2M) && AreSamePointApprox( ptP1M, ptP3M)) {
|
|
// lo considero un arco di raggio nullo
|
|
pCamData->SetAxesCen( ptP2M) ;
|
|
pCamData->SetAxesRad( 0) ;
|
|
pCamData->SetAxesAngCen( 0) ;
|
|
pCamData->SetAxesNormDir( V_NULL) ;
|
|
// il movimento diventa lineare
|
|
pCamData->SetMoveType( 1) ;
|
|
// non è necessario verificare i limiti di corsa degli assi
|
|
}
|
|
// altrimenti calcolo l'arco cui appartengono
|
|
else {
|
|
// calcolo arco per i tre punti espressi in assi macchina
|
|
PtrOwner<ICurve> pCurve( GetArc3P( ptP1M, ptP2M, ptP3M, false)) ;
|
|
if ( IsNull( pCurve)) {
|
|
LOG_ERROR( GetEMkLogger(), "Error : arc on machine not calculable")
|
|
return false ;
|
|
}
|
|
// se realmente arco
|
|
if ( pCurve->GetType() == CRV_ARC) {
|
|
ICurveArc* pArc = GetCurveArc( pCurve) ;
|
|
// verifico il piano (il vettore va portato nel riferimento degli assi lineari)
|
|
Vector3d vtRef ;
|
|
m_pMchMgr->GetCalcPartDirFromAngles( pCamData->GetNormDir(), vAngEnd, vtRef) ;
|
|
vtRef.ToLoc( m_pMchMgr->GetCurrLinAxesFrame()) ;
|
|
if ( pArc->GetNormVersor() * vtRef < 0)
|
|
pArc->InvertN() ;
|
|
// assegno il centro e il raggio di questo arco ai dati cam
|
|
pCamData->SetAxesCen( pArc->GetCenter()) ;
|
|
pCamData->SetAxesRad( pArc->GetRadius()) ;
|
|
pCamData->SetAxesAngCen( pArc->GetAngCenter()) ;
|
|
pCamData->SetAxesNormDir( pArc->GetNormVersor()) ;
|
|
// aggiorno il tipo di arco
|
|
pCamData->SetMoveType( pArc->GetAngCenter() > 0 ? 3 : 2) ;
|
|
// verifico i limiti di corsa dei punti lungo l'arco
|
|
const int NUM_VERIF_STEP = 16 ;
|
|
Point3d ptCen = pCamData->GetAxesCen() ;
|
|
double dAngCenStep = pCamData->GetAxesAngCen() / NUM_VERIF_STEP ;
|
|
Vector3d vtN = pCamData->GetAxesNormDir() ;
|
|
Vector3d vtCurr = Point3d( vAxPrec[0], vAxPrec[1], vAxPrec[2]) - ptCen ;
|
|
for ( int i = 1 ; i < NUM_VERIF_STEP ; ++ i) {
|
|
vtCurr.Rotate( vtN, dAngCenStep) ;
|
|
double dCoeff = double( i) / NUM_VERIF_STEP ;
|
|
Point3d ptCurr = ptCen + vtCurr + vtN * dDeltaN * dCoeff ;
|
|
DBLVECTOR vAng( vAxRot.size()) ;
|
|
for ( size_t i = 0 ; i < vAxRot.size() ; ++ i)
|
|
vAng[i] = vAxRotPrec[i] * ( 1 - dCoeff) + vAxRot[i] * dCoeff ;
|
|
int nStat ;
|
|
bool bOsOk = m_pMchMgr->VerifyOutstroke( ptCurr.x, ptCurr.y, ptCurr.z, vAng, false, nStat) ;
|
|
if ( ! bOsOk || nStat != 0) {
|
|
bOk = false ;
|
|
pCamData->SetAxes( CamData::AS_OUTSTROKE, vAxVal) ;
|
|
string sInfo = "Outstroke : " + m_pMchMgr->GetOutstrokeInfo() ;
|
|
LOG_INFO( GetEMkLogger(), sInfo.c_str())
|
|
}
|
|
}
|
|
}
|
|
// altrimenti linea
|
|
else {
|
|
// lo considero un arco di raggio infinito
|
|
pCamData->SetAxesCen( ORIG) ;
|
|
pCamData->SetAxesRad( 0) ;
|
|
pCamData->SetAxesAngCen( 0) ;
|
|
pCamData->SetAxesNormDir( V_NULL) ;
|
|
// il movimento diventa lineare
|
|
pCamData->SetMoveType( 1) ;
|
|
// non è necessario verificare i limiti di corsa degli assi
|
|
}
|
|
}
|
|
}
|
|
bFirst = false ;
|
|
// memorizzo i precedenti
|
|
ptPrec = ptP ;
|
|
vtDirPrec = vtDir ;
|
|
vtAuxPrec = vtAux ;
|
|
vtCorrPrec = vtCorr ;
|
|
vAxPrec = vAxVal ;
|
|
vAxRotPrec = vAxRot ;
|
|
}
|
|
|
|
return bOk ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::CalculateRotAxesValues( bool bFirst, const Vector3d& vtTool, const Vector3d& vtAux,
|
|
double dRot1W, bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome,
|
|
const DBLVECTOR& vAxRotHome, const DBLVECTOR& vAxRotPrec, DBLVECTOR& vAxRot)
|
|
{
|
|
// massima variazione secondo asse rotante per continuità
|
|
const double dMaxDeltaR2 = 30 ;
|
|
// calcolo degli assi rotanti della macchina
|
|
int nRStat ;
|
|
DBLVECTOR vAng1, vAng2 ;
|
|
bool bROk = m_pMchMgr->GetCalcAngles( vtTool, vtAux, nRStat, vAng1, vAng2) ;
|
|
if ( ! bROk || nRStat == 0)
|
|
return false ;
|
|
if ( abs( nRStat) == 1) {
|
|
// se primo movimento
|
|
if ( bFirst) {
|
|
// porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri
|
|
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
|
|
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ;
|
|
if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
|
|
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ;
|
|
}
|
|
// se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente, limitandolo alla corsa
|
|
if ( nRStat < 0 && vAng1.size() >= 1) {
|
|
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
|
|
// ignoro gli assi bloccati
|
|
if ( m_pMchMgr->IsKinematicRotAxisBlocked( i))
|
|
continue ;
|
|
// assegno il precedente ed esco
|
|
vAng1[i] = vAxRotPrec[i] ;
|
|
m_pMchMgr->LimitAngleToStroke( i, vAng1[i]) ;
|
|
break ;
|
|
}
|
|
}
|
|
}
|
|
// per movimenti successivi
|
|
else {
|
|
// scelgo gli angoli più vicini, per continuità non applico offset per stare nelle corse
|
|
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
|
|
if ( bRotContOnNext)
|
|
vAng1[i] = AngleNearAngle( vAng1[i], vAxRotPrec[i]) ;
|
|
else
|
|
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ;
|
|
if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
|
|
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ;
|
|
}
|
|
// se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente
|
|
if ( nRStat < 0 && vAng1.size() >= 1) {
|
|
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
|
|
// ignoro gli assi bloccati
|
|
if ( m_pMchMgr->IsKinematicRotAxisBlocked( i))
|
|
continue ;
|
|
// assegno il precedente ed esco
|
|
vAng1[i] = vAxRotPrec[i] ;
|
|
break ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
if ( abs( nRStat) == 2) {
|
|
bool bAng1 = true ;
|
|
bool bAng2 = true ;
|
|
// se primo movimento
|
|
if ( bFirst) {
|
|
// porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri
|
|
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
|
|
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ;
|
|
if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
|
|
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ;
|
|
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ;
|
|
if ( abs( vAng2[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
|
|
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ;
|
|
}
|
|
// se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente, limitandolo alla corsa
|
|
if ( nRStat < 0 && vAng1.size() >= 1) {
|
|
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
|
|
// ignoro gli assi bloccati
|
|
if ( m_pMchMgr->IsKinematicRotAxisBlocked( i))
|
|
continue ;
|
|
// assegno il precedente ed esco
|
|
vAng1[i] = vAxRotPrec[i] ;
|
|
m_pMchMgr->LimitAngleToStroke( i, vAng1[i]) ;
|
|
vAng2[i] = vAng1[i] ;
|
|
break ;
|
|
}
|
|
}
|
|
}
|
|
else {
|
|
// scelgo gli angoli più vicini, per continuità non applico offset per stare nelle corse
|
|
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
|
|
if ( bRotContOnNext)
|
|
vAng1[i] = AngleNearAngle( vAng1[i], vAxRotPrec[i]) ;
|
|
else
|
|
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ;
|
|
if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
|
|
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ;
|
|
if ( bRotContOnNext)
|
|
vAng2[i] = AngleNearAngle( vAng2[i], vAxRotPrec[i]) ;
|
|
else
|
|
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ;
|
|
if ( abs( vAng2[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
|
|
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ;
|
|
}
|
|
// se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente
|
|
if ( nRStat < 0 && vAng1.size() >= 1) {
|
|
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
|
|
// ignoro gli assi bloccati
|
|
if ( m_pMchMgr->IsKinematicRotAxisBlocked( i))
|
|
continue ;
|
|
// assegno il precedente ed esco
|
|
vAng1[i] = vAxRotPrec[i] ;
|
|
vAng2[i] = vAng1[i] ;
|
|
break ;
|
|
}
|
|
}
|
|
// verifico che le soluzioni siano nelle corse
|
|
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
|
|
if ( ! m_pMchMgr->VerifyAngleOutstroke( i, vAng1[i]))
|
|
bAng1 = false ;
|
|
if ( ! m_pMchMgr->VerifyAngleOutstroke( i, vAng2[i]))
|
|
bAng2 = false ;
|
|
}
|
|
}
|
|
// scelgo la soluzione più vicina alla precedente
|
|
double dDelta1 = 0 ;
|
|
double dDelta2 = 0 ;
|
|
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
|
|
// ignoro gli assi bloccati
|
|
if ( m_pMchMgr->IsKinematicRotAxisBlocked( i))
|
|
continue ;
|
|
// calcolo i delta asse con eventuale peso
|
|
bool bFirst = ( i == 0) ;
|
|
dDelta1 += abs( vAng1[i] - vAxRotPrec[i]) * ( bFirst ? dRot1W : 1) ;
|
|
dDelta2 += abs( vAng2[i] - vAxRotPrec[i]) * ( bFirst ? dRot1W : 1) ;
|
|
}
|
|
if ( bAng2 && ( dDelta2 < dDelta1 - EPS_ANG_SMALL || ! bAng1))
|
|
swap( vAng1, vAng2) ;
|
|
// se imposto limite su variazione di secondo asse rotante
|
|
if ( ( bMaxDeltaR2OnFirst || ! bFirst) && dMaxDeltaR2 > EPS_ANG_SMALL && vAng1.size() >= 2) {
|
|
double dR2Diff1 = vAng1[1] - vAxRotPrec[1] ;
|
|
double dR2Diff2 = vAng2[1] - vAxRotPrec[1] ;
|
|
if ( abs( dR2Diff1) > abs( dR2Diff2) && abs( dR2Diff1) > dMaxDeltaR2)
|
|
swap( vAng1, vAng2) ;
|
|
}
|
|
}
|
|
vAxRot = vAng1 ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::VerifyLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDirPrec, const Vector3d& vtAuxPrec, const Vector3d& vtCorrPrec, const DBLVECTOR& vAxPrec,
|
|
const Point3d& ptP, const Vector3d& vtDir, const Vector3d& vtAux, const Vector3d& vtCorr, const DBLVECTOR& vAxVal,
|
|
int nCnt, int nEntId, double dRot1W, int nMoveType, bool& bAdded, bool& bAxError)
|
|
{
|
|
// impostazioni
|
|
bAdded = false ;
|
|
bAxError = false ;
|
|
Machine* pMachine = m_pMchMgr->GetCurrMachine() ;
|
|
|
|
// se disposizione non vanno fatti controlli
|
|
if ( GetType() == OPER_DISP)
|
|
return true ;
|
|
|
|
// se superato il limite di ricursioni, non devo fare alcunché
|
|
const int MAX_RECURSION = 6 ;
|
|
if ( nCnt > MAX_RECURSION)
|
|
return true ;
|
|
|
|
// calcolo variazione angolare
|
|
double dDeltaAng = 0 ;
|
|
for ( int i = 3 ; i < int( vAxVal.size()) ; ++i)
|
|
dDeltaAng += abs( vAxVal[i] - vAxPrec[i]) ;
|
|
|
|
// Se non cambia la direzione utensile, non devo fare alcunché
|
|
const double MAX_DELTA_ANG = 0.01 ;
|
|
if ( dDeltaAng <= MAX_DELTA_ANG)
|
|
return true ;
|
|
|
|
// Determino i valori degli assi al punto medio
|
|
DBLVECTOR vAxMid( vAxVal.size()) ;
|
|
for ( int i = 0 ; i < int( vAxVal.size()) ; ++ i)
|
|
vAxMid[i] = ( vAxPrec[i] + vAxVal[i]) / 2 ;
|
|
DBLVECTOR vAxRotMid( int( vAxVal.size() - 3)) ;
|
|
for ( int i = 3 ; i < int( vAxVal.size()) ; ++ i)
|
|
vAxRotMid[i-3] = vAxMid[i] ;
|
|
// ricavo il punto medio del movimento
|
|
Point3d ptMoveMid ;
|
|
if ( ! pMachine->GetTipFromPositions( vAxMid[0], vAxMid[1], vAxMid[2], vAxRotMid, false, false, true, ptMoveMid))
|
|
return false ;
|
|
// verifico le differenze
|
|
double dDist ;
|
|
if ( ! DistPointLine( ptMoveMid, ptPrec, ptP).GetDist( dDist))
|
|
return false ;
|
|
// LOG_INFO( GetEMkLogger(), ( "Midpoint dist =" + ToString( dDist, 3)).c_str())
|
|
double dLinTol = GetApproxLinTol() ;
|
|
if ( nMoveType == 0)
|
|
dLinTol = max( 10 * dLinTol, LIN_TOL_RAW) ;
|
|
|
|
// Se posizione in tolleranza, non devo fare alcunché
|
|
if ( dDist <= dLinTol)
|
|
return true ;
|
|
|
|
// Va inserito il punto medio
|
|
Point3d ptMid = Media( ptPrec, ptP) ;
|
|
Vector3d vtDirMid = Media( vtDirPrec, vtDir) ;
|
|
if ( ! vtDirMid.Normalize()) {
|
|
if ( ! pMachine->GetBackToolDirFromAngles( vAxRotMid, vtDirMid))
|
|
return ( nMoveType == 0) ;
|
|
}
|
|
Vector3d vtAuxMid = Media( vtAuxPrec, vtAux) ;
|
|
if ( ! vtAuxPrec.IsSmall() && ! vtAux.IsSmall() && ! vtAuxMid.Normalize()) {
|
|
if ( ! pMachine->GetBackAuxDirFromAngles( vAxRotMid, vtAuxMid))
|
|
return ( nMoveType == 0) ;
|
|
}
|
|
Vector3d vtCorrMid = Media( vtCorrPrec, vtCorr) ;
|
|
if ( ! vtCorrPrec.IsSmall() && ! vtCorr.IsSmall() && ! vtCorrMid.Normalize()) {
|
|
// riferimenti precedente e sul medio da direzioni fresa (Z) e ausiliaria (X)
|
|
Frame3d refPrec, refMid ;
|
|
if ( ! refPrec.Set( ORIG, vtDirPrec, vtAuxPrec) ||
|
|
! refMid.Set( ORIG, vtDirMid, vtAuxMid))
|
|
return ( nMoveType == 0) ;
|
|
// versore correzione in locale al precente coincide con quello in locale al corrente
|
|
vtCorrMid = vtCorrPrec ;
|
|
vtCorrMid.ToLoc( refPrec) ;
|
|
vtCorrMid.ToGlob( refMid) ;
|
|
}
|
|
// inserisco nuova entità e modifico vecchia
|
|
int nMidEntId = m_pGeomDB->Copy( nEntId, GDB_ID_NULL, nEntId, GDB_BEFORE) ;
|
|
if ( nMidEntId == GDB_ID_NULL)
|
|
return false ;
|
|
ICurve* pCurve = GetCurve( m_pGeomDB->GetGeoObj( nEntId)) ;
|
|
if ( pCurve == nullptr || ! pCurve->ModifyStart( ptMid))
|
|
return false ;
|
|
ICurve* pMidCurve = GetCurve( m_pGeomDB->GetGeoObj( nMidEntId)) ;
|
|
if ( pMidCurve == nullptr || ! pMidCurve->ModifyEnd( ptMid))
|
|
return false ;
|
|
CamData* pMidCamData = GetCamData( m_pGeomDB->GetUserObj( nMidEntId)) ;
|
|
if ( pMidCamData == nullptr)
|
|
return false ;
|
|
pMidCamData->SetEndPoint( ptMid) ;
|
|
pMidCamData->SetToolDir( vtDirMid) ;
|
|
pMidCamData->SetAuxDir( vtAuxMid) ;
|
|
pMidCamData->SetCorrDir( vtCorrMid) ;
|
|
pMidCamData->SetFlag2( -1) ;
|
|
// calcolo gli assi rotanti per il punto medio
|
|
DBLVECTOR vAxRotHome( int( vAxVal.size() - 3)) ;
|
|
DBLVECTOR vAxRotPrec( int( vAxVal.size() - 3)) ;
|
|
for ( int i = 3 ; i < int( vAxVal.size()) ; ++ i)
|
|
vAxRotPrec[i-3] = vAxVal[i] ;
|
|
if ( ! CalculateRotAxesValues(false, vtDirMid, vtAuxMid, dRot1W, false, true, INFINITO, vAxRotHome, vAxRotPrec, vAxRotMid)) {
|
|
pMidCamData->SetAxes( CamData::AS_DIR_ERR, vAxVal) ;
|
|
return false ;
|
|
}
|
|
for ( int i = 3 ; i < int( vAxVal.size()) ; ++ i)
|
|
vAxMid[i] = vAxRotMid[i-3] ;
|
|
// determino il nuovo valore degli assi lineari sul punto medio
|
|
int nMidLStat ;
|
|
double dMidX, dMidY, dMidZ ;
|
|
bool bMidLOk = m_pMchMgr->GetCalcPositions( ptMid, vAxRotMid, nMidLStat, dMidX, dMidY, dMidZ) ;
|
|
if ( ! bMidLOk || nMidLStat != 0) {
|
|
pMidCamData->SetAxes( CamData::AS_ERR, vAxMid) ;
|
|
bAxError = true ;
|
|
return true ;
|
|
}
|
|
// verifico i limiti di corsa degli assi
|
|
int nStat ;
|
|
bool bOsOk = m_pMchMgr->VerifyOutstroke( dMidX, dMidY, dMidZ, vAxRotMid, false, nStat) ;
|
|
if ( ! bOsOk || nStat != 0) {
|
|
pMidCamData->SetAxes( CamData::AS_OUTSTROKE, vAxMid) ;
|
|
bAxError = true ;
|
|
return true ;
|
|
}
|
|
// assegno i valori degli assi
|
|
vAxMid[0] = dMidX ;
|
|
vAxMid[1] = dMidY ;
|
|
vAxMid[2] = dMidZ ;
|
|
pMidCamData->SetAxes( CamData::AS_OK, vAxMid) ;
|
|
// calcolo e salvo il valore dell'asse ausiliario di testa riportato sul grezzo
|
|
Vector3d vtBackAux ; pMachine->GetBackAuxDirFromAngles( vAxRotMid, vtBackAux) ;
|
|
pMidCamData->SetBackAuxDir( vtBackAux) ;
|
|
|
|
// devo verificare le due nuove parti
|
|
nCnt ++ ;
|
|
bool bAdded1, bAxError1, bAdded2, bAxError2 ;
|
|
if ( ! VerifyLineMidPoint( ptPrec, vtDirPrec, vtAuxPrec, vtCorrPrec, vAxPrec,
|
|
ptMid, vtDirMid, vtAuxMid, vtCorrMid, vAxMid,
|
|
nCnt, nMidEntId, dRot1W, nMoveType, bAdded1, bAxError1))
|
|
return false ;
|
|
if ( bAxError1) {
|
|
bAxError = true ;
|
|
return true ;
|
|
}
|
|
if ( ! VerifyLineMidPoint( ptMid, vtDirMid, vtAuxMid, vtCorrMid, vAxMid,
|
|
ptP, vtDir, vtAux, vtCorr, vAxVal,
|
|
nCnt, nEntId, dRot1W, nMoveType, bAdded2, bAxError2))
|
|
return false ;
|
|
if ( bAxError2) {
|
|
bAxError = true ;
|
|
return true ;
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::CalculateClPathRobotAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W,
|
|
bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome,
|
|
const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec)
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// eventuale direzione Aux preferenziale
|
|
bool bUseRefAux = true ;
|
|
Vector3d vtRefAux = Y_AX ;
|
|
switch ( GetSolCh()) {
|
|
case MCH_SCC_ADIR_XP : vtRefAux = X_AX ; break ;
|
|
case MCH_SCC_ADIR_XM : vtRefAux = -X_AX ; break ;
|
|
case MCH_SCC_ADIR_YP : vtRefAux = Y_AX ; break ;
|
|
case MCH_SCC_ADIR_YM : vtRefAux = -Y_AX ; break ;
|
|
case MCH_SCC_ADIR_ZP : vtRefAux = Z_AX ; break ;
|
|
case MCH_SCC_ADIR_ZM : vtRefAux = -Z_AX ; break ;
|
|
default : bUseRefAux = false ; break ;
|
|
}
|
|
// predispongo variabile per valori assi
|
|
DBLVECTOR vAxVal ;
|
|
vAxVal.reserve( 8) ;
|
|
// ciclo su tutte le entità del percorso CL
|
|
bool bOk = true ;
|
|
bool bFirst = true ;
|
|
for ( int nEntId = m_pGeomDB->GetFirstInGroup( nClPathId) ;
|
|
nEntId != GDB_ID_NULL ;
|
|
nEntId = m_pGeomDB->GetNext( nEntId)) {
|
|
// recupero i dati Cam dell'entità
|
|
CamData* pCamData = GetCamData( m_pGeomDB->GetUserObj( nEntId)) ;
|
|
if ( pCamData == nullptr)
|
|
continue ;
|
|
// verifico che il movimento non sia un arco
|
|
if ( bFirst && pCamData->IsArc()) {
|
|
pCamData->ResetAxes() ;
|
|
LOG_ERROR( GetEMkLogger(), "Error : move in ClPath for Robot is an arc")
|
|
return false ;
|
|
}
|
|
// ricavo posizione con eventuali modifiche dipendenti dalla lavorazione
|
|
Point3d ptP = pCamData->GetEndPoint() ;
|
|
AdjustEndPointForAxesCalc( pCamData, ptP) ;
|
|
// calcolo gli assi del robot
|
|
Vector3d vtTool = pCamData->GetToolDir() ;
|
|
Vector3d vtAux ;
|
|
if ( ! bUseRefAux) {
|
|
Frame3d frTool ;
|
|
frTool.Set( ORIG, vtTool) ;
|
|
frTool.Rotate( frTool.Orig(), frTool.VersZ(), 0, 1) ;
|
|
vtAux = frTool.VersX() ;
|
|
}
|
|
else {
|
|
vtAux = OrthoCompo( vtRefAux, vtTool) ;
|
|
vtAux.Normalize() ;
|
|
}
|
|
pCamData->SetAuxDir( vtAux) ;
|
|
DBLVECTOR vAng1, vAng2 ;
|
|
bool bRaOk = m_pMchMgr->GetCurrMachine()->GetRobotAngles( ptP, vtTool, vtAux, vAng1, vAng2) ;
|
|
if ( ! bRaOk) {
|
|
pCamData->SetAxes( CamData::AS_ERR, vAxVal) ;
|
|
return false ;
|
|
}
|
|
// se inizio
|
|
if ( bFirst) {
|
|
// porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri
|
|
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
|
|
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ;
|
|
if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
|
|
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ;
|
|
}
|
|
for ( int i = 0 ; i < int( vAng2.size()) ; ++ i) {
|
|
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ;
|
|
if ( abs( vAng2[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
|
|
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ;
|
|
}
|
|
bFirst = false ;
|
|
}
|
|
// altrimenti movimenti successivi
|
|
else {
|
|
// porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri
|
|
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
|
|
if ( bRotContOnNext)
|
|
vAng1[i] = AngleNearAngle( vAng1[i], vAxRotPrec[i]) ;
|
|
else
|
|
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ;
|
|
}
|
|
for ( int i = 0 ; i < int( vAng2.size()) ; ++ i) {
|
|
if ( bRotContOnNext)
|
|
vAng2[i] = AngleNearAngle( vAng2[i], vAxRotPrec[i]) ;
|
|
else
|
|
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ;
|
|
}
|
|
}
|
|
// Scelgo la soluzione più vicina
|
|
// minimizzo rotazione polso
|
|
if ( vAng2.empty() || abs( vAng1[3] - vAxRotPrec[3]) < abs( vAng2[3] - vAxRotPrec[3]) + 10 * EPS_ANG_SMALL)
|
|
vAxVal = vAng1 ;
|
|
else
|
|
vAxVal = vAng2 ;
|
|
// controllo continuità di R6 dalla posizione precedente
|
|
m_pMchMgr->GetNearestAngleInStroke( 5, vAxRotPrec[5], vAxVal[5]) ;
|
|
// verifico di essere nelle corse degli assi
|
|
int nStat ;
|
|
bool bOsOk = m_pMchMgr->VerifyOutstroke( 0, 0, 0, vAxVal, false, nStat) ;
|
|
// salvo il risultato
|
|
if ( ! bOsOk || nStat != 0) {
|
|
bOk = false ;
|
|
pCamData->SetAxes( CamData::AS_OUTSTROKE, vAxVal) ;
|
|
}
|
|
else {
|
|
pCamData->SetAxes( CamData::AS_OK, vAxVal) ;
|
|
}
|
|
// aggiorno valori precedenti
|
|
vAxRotPrec = vAxVal ;
|
|
}
|
|
|
|
return bOk ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::AdjustStartEndMovements( bool bVerifyPreviousLink)
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr || m_pMchMgr->GetCurrMachine() == nullptr)
|
|
return false ;
|
|
// recupero gruppo della geometria di lavorazione (Cutter Location)
|
|
int nClId = m_pGeomDB->GetFirstNameInGroup( m_nOwnerId, MCH_CL) ;
|
|
if ( nClId == GDB_ID_NULL)
|
|
return false ;
|
|
|
|
// recupero ultima operazione precedente non vuota o che richiede home precedente
|
|
int nPrevOpId = m_pMchMgr->GetPrevActiveOperation( m_nOwnerId) ;
|
|
Operation* pPrevOp = GetOperation( m_pGeomDB->GetUserObj( nPrevOpId)) ;
|
|
while ( pPrevOp != nullptr) {
|
|
if ( ! pPrevOp->IsEmpty() || pPrevOp->NeedPrevHome())
|
|
break ;
|
|
else {
|
|
nPrevOpId = m_pMchMgr->GetPrevActiveOperation( nPrevOpId) ;
|
|
pPrevOp = GetOperation( m_pGeomDB->GetUserObj( nPrevOpId)) ;
|
|
}
|
|
}
|
|
// recupero l'utensile precedente e i dati della sua testa
|
|
string sPrevTool ;
|
|
if ( pPrevOp != nullptr && ! pPrevOp->IsEmpty())
|
|
sPrevTool = pPrevOp->GetToolName() ;
|
|
|
|
// determino posizione precedente assi
|
|
DBLVECTOR vAxVal ;
|
|
bool bMaxZ = false ;
|
|
// se primo utensile o richiesto, uso la posizione home
|
|
if ( sPrevTool.empty() || NeedPrevHome()) {
|
|
// imposto l'utensile per i calcoli macchina
|
|
if ( ! m_pMchMgr->SetCalcTool( GetToolName(), GetHeadName(), GetExitNbr()))
|
|
return false ;
|
|
// recupero posizione home
|
|
if ( ! m_pMchMgr->GetAllCurrAxesHomePos( vAxVal))
|
|
return false ;
|
|
// si parte da Z massima
|
|
bMaxZ = true ;
|
|
}
|
|
// se utensile non cambiato o su diversa uscita della stessa testa, uso posizione finale della lavorazione precedente
|
|
else if ( ! ToolChangeNeeded( *pPrevOp, *this)) {
|
|
// imposto l'utensile per i calcoli macchina
|
|
if ( ! m_pMchMgr->SetCalcTool( GetToolName(), GetHeadName(), GetExitNbr()))
|
|
return false ;
|
|
// se richiesta verifica collegamento con lavorazione precedente
|
|
if ( bVerifyPreviousLink) {
|
|
// cancello eventuale risalita finale lavorazione precedente
|
|
pPrevOp->RemoveRise() ;
|
|
// recupero posizione finale lavorazione precedente
|
|
if ( ! pPrevOp->GetFinalAxesValues( false, vAxVal))
|
|
return false ;
|
|
// elimino eventuali CLIMB già presenti
|
|
RemoveClimb( m_pGeomDB->GetFirstGroupInGroup( nClId)) ;
|
|
// recupero posizione iniziale lavorazione
|
|
DBLVECTOR vAxIni ;
|
|
if ( ! GetInitialAxesValues( false, vAxIni))
|
|
return false ;
|
|
// se centro di lavoro
|
|
if ( m_pMchMgr->GetCurrIsCenter()) {
|
|
// recupero se ZHome è in basso
|
|
bool bZHomeDown = GetZHomeDown() ;
|
|
// recupero quota di home in Z
|
|
double dHomeZ ;
|
|
if ( ! m_pMchMgr->GetCurrAxisHomePos( 2, dHomeZ))
|
|
return false ;
|
|
// se necessaria, aggiungo risalita parziale
|
|
double dDeltaZ = ( vAxIni[2] - vAxVal[2]) ;
|
|
if ( ( ! bZHomeDown && dDeltaZ > 100 * EPS_SMALL && vAxIni[2] <= dHomeZ) ||
|
|
( bZHomeDown && dDeltaZ < -100 * EPS_SMALL && vAxIni[2] >= dHomeZ)) {
|
|
if ( ! pPrevOp->AddRise( vAxVal, dDeltaZ, GDB_ID_NULL, bZHomeDown))
|
|
return false ;
|
|
// aggiorno quota in Z della posizione iniziale ( anche se verrà aggiornata solo in seguito)
|
|
vAxIni[2] = vAxVal[2] ;
|
|
}
|
|
// Verifico non ci sia collisione a HomeZ
|
|
bool bToZmax = false ;
|
|
bool bToMyHomeZ = false ;
|
|
DBLVECTOR vAxVal2 = vAxVal ; vAxVal2[2] = dHomeZ ;
|
|
DBLVECTOR vAxIni2 = vAxIni ; vAxIni2[2] = dHomeZ ;
|
|
if ( ! TestCollisionAvoid( vAxVal2, vAxIni2)) {
|
|
// se ammessa risalita aggiuntiva
|
|
Vector3d vtTprev ; pPrevOp->GetFinalToolDir( false, vtTprev) ;
|
|
Vector3d vtTcurr ; GetInitialToolDir( false, vtTcurr) ;
|
|
double dExtraZ ;
|
|
if ( GetExtraZ( vAxVal2, vtTprev, vAxIni2, vtTcurr, dHomeZ, dExtraZ)) {
|
|
if ( abs( dExtraZ) > EPS_SMALL) {
|
|
double dMyHomeZ = dHomeZ + dExtraZ ;
|
|
vAxVal2[2] = dMyHomeZ ;
|
|
vAxIni2[2] = dMyHomeZ ;
|
|
if ( TestCollisionAvoid( vAxVal2, vAxIni2)) {
|
|
dHomeZ = dMyHomeZ ;
|
|
bToMyHomeZ = true ;
|
|
}
|
|
else
|
|
bToZmax = true ;
|
|
}
|
|
else
|
|
bToZmax = true ;
|
|
}
|
|
// se altrimenti ammessa rotazione a Zmax e richiesta rotazione
|
|
else if ( GetRotationAtZmax() &&
|
|
(( vAxVal2.size() >= 4 && abs( vAxVal2[3] - vAxIni2[3]) > 5) ||
|
|
( vAxVal2.size() >= 5 && abs( vAxVal2[4] - vAxIni2[4]) > 5))) {
|
|
// verifico se va bene rotazione assi iniziale e poi movimento
|
|
DBLVECTOR vAxMid2 = vAxVal2 ;
|
|
vAxMid2[3] = vAxIni2[3] ;
|
|
if ( vAxVal2.size() >= 5)
|
|
vAxMid2[4] = vAxIni2[4] ;
|
|
if ( TestCollisionAvoid( vAxVal2, vAxMid2) &&
|
|
TestCollisionAvoid( vAxMid2, vAxIni2) &&
|
|
pPrevOp->AddRise( vAxVal2) &&
|
|
pPrevOp->AddSpecialRise( vAxMid2, true, GDB_ID_NULL, 5)) {
|
|
vAxVal = vAxMid2 ;
|
|
bMaxZ = true ;
|
|
}
|
|
else
|
|
return false ;
|
|
}
|
|
// altrimenti impossibile
|
|
else
|
|
return false ;
|
|
}
|
|
// Recupero box complessivo dei grezzi attivi
|
|
BBox3d b3Raws ;
|
|
GetCurrRawsGlobBox( b3Raws) ;
|
|
// Se già a Zmax
|
|
if ( bMaxZ) {
|
|
// non devo fare alcunché
|
|
}
|
|
// se altrimenti richiesta risalita a Zmax
|
|
else if ( bToZmax || ForcedZmax( vAxVal, vAxIni, b3Raws)) {
|
|
// cancello eventuale risalita parziale della lavorazione precedente
|
|
pPrevOp->RemoveRise() ;
|
|
// aggiungo risalita a Zmax
|
|
if ( ! pPrevOp->AddRise( vAxVal))
|
|
return false ;
|
|
// si parte da Z massima
|
|
bMaxZ = true ;
|
|
}
|
|
// altrimenti, verifico se la testa interferisce con i pezzi o i bloccaggi sulla tavola
|
|
else if ( bToMyHomeZ || ! TestCollisionAvoid( vAxVal, vAxIni)) {
|
|
// riprovo con risalita parziale
|
|
bool bPartRise = false ;
|
|
double dSafeZ = dHomeZ ;
|
|
double dUnsafeZ = ( bZHomeDown ? min( vAxVal[2], vAxIni[2]) : max( vAxVal[2], vAxIni[2])) ;
|
|
if ( ( ! bZHomeDown && dUnsafeZ < dSafeZ) || ( bZHomeDown && dUnsafeZ > dSafeZ)) {
|
|
DBLVECTOR vAxVal2 = vAxVal ;
|
|
DBLVECTOR vAxIni2 = vAxIni ;
|
|
vAxVal2[2] = dUnsafeZ ;
|
|
vAxIni2[2] = dUnsafeZ ;
|
|
if ( TestCollisionAvoid( vAxVal2, vAxIni2)) {
|
|
bPartRise = true ;
|
|
dSafeZ = dUnsafeZ ;
|
|
}
|
|
else {
|
|
for ( int i = 1 ; i <= 3 ; ++ i) {
|
|
double dTestSafeZ = ( dUnsafeZ + dSafeZ) / 2 ;
|
|
vAxVal2[2] = dTestSafeZ ;
|
|
vAxIni2[2] = dTestSafeZ ;
|
|
if ( TestCollisionAvoid( vAxVal2, vAxIni2)) {
|
|
bPartRise = true ;
|
|
dSafeZ = dTestSafeZ ;
|
|
}
|
|
else {
|
|
dUnsafeZ = dTestSafeZ ;
|
|
}
|
|
}
|
|
}
|
|
// se necessario, riprovo appena prima di Zmax
|
|
if ( ! bPartRise) {
|
|
double dOffsZ = 1 * ( bZHomeDown ? 1 : -1) ;
|
|
vAxVal2[2] = dHomeZ + dOffsZ ;
|
|
vAxIni2[2] = dHomeZ + dOffsZ ;
|
|
if ( TestCollisionAvoid( vAxVal2, vAxIni2)) {
|
|
bPartRise = true ;
|
|
dSafeZ = dHomeZ + dOffsZ ;
|
|
}
|
|
}
|
|
}
|
|
if ( ! bPartRise) {
|
|
// cancello eventuale risalita parziale della lavorazione precedente
|
|
pPrevOp->RemoveRise() ;
|
|
// aggiungo risalita a Zmax
|
|
if ( ! pPrevOp->AddRise( vAxVal))
|
|
return false ;
|
|
// si parte da Z massima
|
|
bMaxZ = true ;
|
|
}
|
|
else {
|
|
// cancello eventuale risalita parziale della lavorazione precedente
|
|
pPrevOp->RemoveRise() ;
|
|
// recupero le nuove quote finali (per calcolare il corretto delta)
|
|
pPrevOp->GetFinalAxesValues( false, vAxVal) ;
|
|
// aggiungo risalita a Zsafe
|
|
double dDelta = ( ! bZHomeDown ? max( dSafeZ - vAxVal[2], 0.) : min( dSafeZ - vAxVal[2], 0.)) ;
|
|
if ( ! pPrevOp->AddRise( vAxVal, dDelta, GDB_ID_NULL, bZHomeDown))
|
|
return false ;
|
|
// aggiorno quota iniziale in Z
|
|
vAxIni[2] = dSafeZ ;
|
|
}
|
|
}
|
|
// se ci sono aree protette...
|
|
if ( m_pMchMgr->GetCurrMachine()->ExistProtectedAreas()) {
|
|
// verifico se il collegamento le attraversa
|
|
const double PA_VERIF_LEN = 10 ;
|
|
Point3d ptStart( vAxVal[0], vAxVal[1], vAxVal[2]) ;
|
|
Point3d ptEnd( vAxIni[0], vAxIni[1], vAxIni[2]) ;
|
|
double dLen = DistXY( ptStart, ptEnd) ;
|
|
int nStep = int( dLen / PA_VERIF_LEN + 1) ;
|
|
bool bOutstroke = false ;
|
|
for ( int i = 1 ; i < nStep ; ++ i) {
|
|
Point3d ptCurr = Media( ptStart, ptEnd, double( i) / nStep) ;
|
|
int nStat = 0 ;
|
|
if ( ! m_pMchMgr->GetCurrMachine()->VerifyProtectedAreas( ptCurr.x, ptCurr.y, ptCurr.z, {}, nStat) || nStat != 0) {
|
|
bOutstroke = true ;
|
|
break ;
|
|
}
|
|
}
|
|
// in caso di attraversamento dell'area proibita
|
|
if ( bOutstroke) {
|
|
// chiamo funzione script OnSpecialRapidMove per avere nuovo punto intermedio
|
|
DBLVECTOR vAxNew ;
|
|
bool bModif = false ;
|
|
bool bOk = ( SpecialMoveRapid( vAxVal, vAxIni, vAxNew, bModif) && bModif) ;
|
|
// inserisco il nuovo punto alla fine della lavorazione precedente
|
|
if ( bOk)
|
|
vAxVal = vAxNew ;
|
|
if ( ! pPrevOp->AddSpecialRise( vAxVal, bOk))
|
|
return false ;
|
|
}
|
|
}
|
|
}
|
|
// se robot
|
|
else if ( m_pMchMgr->GetCurrIsRobot()) {
|
|
// recupero dati posizione finale lavorazione precedente
|
|
const CamData* pCdEnd = pPrevOp->GetFinalCamData( false) ;
|
|
const Point3d& ptPEnd = pCdEnd->GetEndPoint() ;
|
|
const Vector3d& vtDirEnd = pCdEnd->GetToolDir() ;
|
|
const Vector3d& vtAuxEnd = pCdEnd->GetAuxDir() ;
|
|
// recupero dati posizione iniziale lavorazione corrente
|
|
const CamData* pCdStart = GetInitialCamData( false) ;
|
|
const Point3d& ptPStart = pCdStart->GetEndPoint() ;
|
|
const Vector3d& vtDirStart = pCdStart->GetToolDir() ;
|
|
const Vector3d& vtAuxStart = pCdStart->GetAuxDir() ;
|
|
// verifico se collegamento diretto tra le due posizioni (muovendosi alla Z maggiore) è possibile senza collisioni
|
|
int nLowerPos = 0 ;
|
|
DBLVECTOR vAxEnd = vAxVal ;
|
|
DBLVECTOR vAxStart = vAxIni ;
|
|
if ( ptPEnd.z < ptPStart.z - 100 * EPS_SMALL) {
|
|
nLowerPos = - 1 ;
|
|
if ( ! CalcRobotAxesAbovePos( ptPEnd, vtDirEnd, vtAuxEnd, ptPStart.z - ptPEnd.z, vAxEnd))
|
|
return false ;
|
|
}
|
|
else if ( ptPEnd.z > ptPStart.z + 100 * EPS_SMALL) {
|
|
nLowerPos = 1 ;
|
|
if ( ! CalcRobotAxesAbovePos( ptPStart, vtDirStart, vtAuxStart, ptPEnd.z - ptPStart.z, vAxStart))
|
|
return false ;
|
|
}
|
|
if ( TestCollisionAvoid( vAxEnd, vAxStart)) {
|
|
if ( nLowerPos == -1) {
|
|
// aggiungo risalita a Z opportuna
|
|
if ( ! pPrevOp->AddRise( vAxVal, ptPStart.z - ptPEnd.z))
|
|
return false ;
|
|
}
|
|
}
|
|
// altrimenti aggiungo risalita a Zmax
|
|
else {
|
|
if ( ! pPrevOp->AddRise( vAxVal))
|
|
return false ;
|
|
// si parte da Z massima
|
|
bMaxZ = true ;
|
|
}
|
|
}
|
|
}
|
|
// altrimenti non richiesta verifica collegamento con lavorazione precedente
|
|
else {
|
|
// recupero posizione finale lavorazione precedente
|
|
if ( ! pPrevOp->GetFinalAxesValues( false, vAxVal))
|
|
return false ;
|
|
}
|
|
}
|
|
// altrimenti aggiungo uscita per cambio utensile alla lavorazione precedente e parto da questa
|
|
else {
|
|
// imposto la lavorazione precedente come corrente
|
|
m_pMchMgr->SetCurrMachining( pPrevOp->GetOwner()) ;
|
|
// imposto l'utensile precedente per i calcoli macchina
|
|
if ( ! m_pMchMgr->SetCalcTool( pPrevOp->GetToolName(), pPrevOp->GetHeadName(), pPrevOp->GetExitNbr()))
|
|
return false ;
|
|
// ricalcolo risalita di lavorazione precedente (se già calcolata)
|
|
if ( pPrevOp->RemoveRise()) {
|
|
if ( ! pPrevOp->AddRise( vAxVal))
|
|
return false ;
|
|
}
|
|
else {
|
|
// recupero posizione home
|
|
if ( ! m_pMchMgr->GetAllCurrAxesHomePos( vAxVal))
|
|
return false ;
|
|
}
|
|
// ripristino la lavorazione corrente
|
|
m_pMchMgr->SetCurrMachining( GetOwner()) ;
|
|
// imposto l'utensile per i calcoli macchina
|
|
if ( ! m_pMchMgr->SetCalcTool( GetToolName(), GetHeadName(), GetExitNbr()))
|
|
return false ;
|
|
// si parte da Z massima
|
|
bMaxZ = true ;
|
|
}
|
|
|
|
// aggiusto l'inizio di ogni percorso di lavoro
|
|
bool bOk = true ;
|
|
int nPrevClPathId = GDB_ID_NULL ;
|
|
int nClPathId = m_pGeomDB->GetFirstGroupInGroup( nClId) ;
|
|
while ( bOk && nClPathId != GDB_ID_NULL) {
|
|
// se richiesta verifica collegamento con lavorazione precedente, sistemo inizio
|
|
if ( bVerifyPreviousLink) {
|
|
if ( ! AdjustOneStartMovement( nClPathId, nPrevClPathId, pPrevOp, vAxVal, bMaxZ))
|
|
bOk = false ;
|
|
}
|
|
bMaxZ = false ;
|
|
// recupero nuovi finali
|
|
RemoveRise( nClPathId) ;
|
|
if ( ! GetClPathFinalAxesValues( nClPathId, false, vAxVal))
|
|
bOk = false ;
|
|
// passo al successivo
|
|
nPrevClPathId = nClPathId ;
|
|
nClPathId = m_pGeomDB->GetNextGroup( nClPathId) ;
|
|
pPrevOp = nullptr ;
|
|
}
|
|
|
|
// aggiungo risalita finale
|
|
bOk = bOk && AddRise( vAxVal) ;
|
|
|
|
// se ultima operazione o la successiva lo richiede, vado in home
|
|
int nNextOpId = m_pMchMgr->GetNextActiveOperation( m_nOwnerId) ;
|
|
Operation* pNextOp = GetOperation( m_pGeomDB->GetUserObj( nNextOpId)) ;
|
|
if ( pNextOp == nullptr || pNextOp->NeedPrevHome())
|
|
bOk = bOk && AddHome() ;
|
|
|
|
return bOk ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::AdjustOneStartMovement( int nClPathId, int nPrevClPathId, Operation* pPrevOp, const DBLVECTOR& vAxPrev, bool bMaxZ)
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// elimino eventuali CLIMB già presenti
|
|
RemoveClimb( nClPathId) ;
|
|
// recupero la prima entità di questo percorso
|
|
int nEntId = m_pGeomDB->GetFirstInGroup( nClPathId) ;
|
|
// recupero i dati Cam dell'entità
|
|
CamData* pCamData = GetCamData( m_pGeomDB->GetUserObj( nEntId)) ;
|
|
if ( pCamData == nullptr)
|
|
return false ;
|
|
// recupero i valori degli assi
|
|
const DBLVECTOR vAxCurr = pCamData->GetAxesVal() ;
|
|
// se provengo da Z massima
|
|
if ( bMaxZ) {
|
|
// se centro di lavoro
|
|
if ( m_pMchMgr->GetCurrIsCenter()) {
|
|
// copio l'entità
|
|
if ( m_pGeomDB->Copy( nEntId, GDB_ID_NULL, nEntId, GDB_AFTER) == GDB_ID_NULL)
|
|
return false ;
|
|
// recupero HomeZ
|
|
double dHomeZ ;
|
|
if ( ! m_pMchMgr->GetCurrAxisHomePos( 2, dHomeZ))
|
|
return false ;
|
|
// aggiungo eventuale ExtraZ
|
|
const CamData* pCamDataPrev = GetClPathFinalCamData( nPrevClPathId, true) ;
|
|
const DBLVECTOR& vAxPrev = ( pCamDataPrev != nullptr ? pCamDataPrev->GetAxesVal() : DBLVECTOR()) ;
|
|
const Vector3d& vtTprev = ( pCamDataPrev != nullptr ? pCamDataPrev->GetToolDir() : V_NULL) ;
|
|
const Vector3d& vtTcurr = pCamData->GetToolDir() ;
|
|
double dExtraZ ;
|
|
if ( GetExtraZ( vAxPrev, vtTprev, vAxCurr, vtTcurr, dHomeZ, dExtraZ)) {
|
|
if ( abs( dExtraZ) > EPS_SMALL)
|
|
dHomeZ += dExtraZ ;
|
|
}
|
|
// modifico quella originale (è la prima del percorso)
|
|
m_pGeomDB->SetName( nEntId, MCH_CL_CLIMB) ;
|
|
DBLVECTOR vAxNew = vAxCurr ;
|
|
vAxNew[2] = dHomeZ ;
|
|
int nFlagNew = 2 ;
|
|
int nFlag2New = 1 ;
|
|
// eventuali aggiustamenti speciali dipendenti dalla macchina
|
|
DBLVECTOR vAxTmp = vAxNew ;
|
|
Vector3d vtTool = pCamData->GetToolDir() ;
|
|
int nFlagTmp = nFlagNew ;
|
|
int nFlag2Tmp = nFlag2New ;
|
|
bool bModif ;
|
|
if ( ! SpecialMoveZup( vAxTmp, vtTool, nFlagTmp, nFlag2Tmp, bModif))
|
|
return false ;
|
|
if ( bModif) {
|
|
vAxNew = vAxTmp ;
|
|
pCamData->SetToolDir( vtTool) ;
|
|
Vector3d vtAux = pCamData->GetAuxDir() ;
|
|
if ( ! vtAux.IsSmall()) {
|
|
DBLVECTOR vAng( vAxNew.begin() + 3, vAxNew.end()) ;
|
|
if ( m_pMchMgr->GetCalcAuxDirFromAngles( vAng, vtAux))
|
|
pCamData->SetAuxDir( vtAux) ;
|
|
}
|
|
nFlagNew = nFlagTmp ;
|
|
nFlag2New = nFlag2Tmp ;
|
|
}
|
|
// applico le modifiche (non forzo emissione della Z che è già massima)
|
|
pCamData->SetAxes( CamData::AS_OK, vAxNew) ;
|
|
pCamData->ChangeAxesMask( CamData::MSK_L1 | CamData::MSK_L2 | CamData::MSK_R1 | CamData::MSK_R2 | CamData::MSK_R3) ;
|
|
pCamData->SetFlag( nFlagNew) ;
|
|
pCamData->SetFlag2( nFlag2New) ;
|
|
}
|
|
// se altrimenti robot
|
|
else if ( m_pMchMgr->GetCurrIsRobot()) {
|
|
// inserisco la posizione iniziale a Zmax
|
|
if ( ! AddRobotClimb( nEntId, -1))
|
|
return false ;
|
|
}
|
|
}
|
|
// verifico se la testa interferisce con i pezzi o i bloccaggi sulla tavola
|
|
else {
|
|
// se centro di lavoro
|
|
if ( m_pMchMgr->GetCurrIsCenter()) {
|
|
// Recupero box complessivo dei grezzi attivi
|
|
BBox3d b3Raws ;
|
|
GetCurrRawsGlobBox( b3Raws) ;
|
|
// recupero se ZHome è in basso
|
|
bool bZHomeDown = GetZHomeDown() ;
|
|
// determino la Z più alta tra le due posizioni
|
|
double dTopZ = ( bZHomeDown ? min( vAxPrev[2], vAxCurr[2]) : max( vAxPrev[2], vAxCurr[2])) ;
|
|
// per il test uso posizioni temporanee con questa Z
|
|
DBLVECTOR vAxPrevTmp = vAxPrev ; vAxPrevTmp[2] = dTopZ ;
|
|
DBLVECTOR vAxCurrTmp = vAxCurr ; vAxCurrTmp[2] = dTopZ ;
|
|
// verifico se forzata risalita a Zmax
|
|
bool bForcedZMax = ForcedZmax( vAxPrevTmp, vAxCurrTmp, b3Raws) ;
|
|
// se interferisce
|
|
if ( bForcedZMax || ! TestCollisionAvoid( vAxPrevTmp, vAxCurrTmp)) {
|
|
// recupero HomeZ
|
|
double dHomeZ ;
|
|
if ( ! m_pMchMgr->GetCurrAxisHomePos( 2, dHomeZ))
|
|
return false ;
|
|
bool bPartRise = false ;
|
|
double dSafeZ = dHomeZ ;
|
|
// se non forzata Zmax
|
|
if ( ! bForcedZMax) {
|
|
// riprovo con una Z intermedia
|
|
double dUnsafeZ = dTopZ ;
|
|
for ( int i = 1 ; i <= 3 ; ++ i) {
|
|
double dTestSafeZ = ( dUnsafeZ + dSafeZ) / 2 ;
|
|
vAxPrevTmp[2] = dTestSafeZ ;
|
|
vAxCurrTmp[2] = dTestSafeZ ;
|
|
if ( TestCollisionAvoid( vAxPrevTmp, vAxCurrTmp)) {
|
|
bPartRise = true ;
|
|
dSafeZ = dTestSafeZ ;
|
|
}
|
|
else
|
|
dUnsafeZ = dTestSafeZ ;
|
|
}
|
|
// se necessario, riprovo appena prima di Zmax
|
|
if ( ! bPartRise) {
|
|
double dOffsZ = 5 * ( bZHomeDown ? 1 : -1) ;
|
|
vAxPrevTmp[2] = dHomeZ + dOffsZ ;
|
|
vAxCurrTmp[2] = dHomeZ + dOffsZ ;
|
|
if ( TestCollisionAvoid( vAxPrevTmp, vAxCurrTmp)) {
|
|
bPartRise = true ;
|
|
dSafeZ = dHomeZ + dOffsZ ;
|
|
}
|
|
}
|
|
}
|
|
// risalita sopra fine percorso precedente
|
|
DBLVECTOR vAxNew1 ;
|
|
if ( nPrevClPathId == GDB_ID_NULL) {
|
|
if ( pPrevOp == nullptr || ! pPrevOp->RemoveRise())
|
|
return false ;
|
|
DBLVECTOR vAxPrevNoRise ;
|
|
pPrevOp->GetFinalAxesValues( false, vAxPrevNoRise) ;
|
|
double dDelta = ( ! bZHomeDown ? max( dSafeZ - vAxPrevNoRise[2], 0.) : min( dSafeZ - vAxPrevNoRise[2], 0.)) ;
|
|
if ( ! pPrevOp->AddRise( vAxNew1, dDelta, GDB_ID_NULL, bZHomeDown))
|
|
return false ;
|
|
}
|
|
else {
|
|
double dDelta = ( ! bZHomeDown ? max( dSafeZ - vAxPrev[2], 0.) : min( dSafeZ - vAxPrev[2], 0.)) ;
|
|
if ( ! AddRise( vAxNew1, dDelta, nPrevClPathId, bZHomeDown))
|
|
return false ;
|
|
}
|
|
// aggiungo posizione elevata prima dell'inizio del percorso corrente
|
|
if ( m_pGeomDB->Copy( nEntId, GDB_ID_NULL, nEntId, GDB_AFTER) == GDB_ID_NULL)
|
|
return false ;
|
|
// modifico l'entità originale (è la prima del percorso)
|
|
m_pGeomDB->SetName( nEntId, MCH_CL_CLIMB) ;
|
|
DBLVECTOR vAxNew = vAxCurr ;
|
|
vAxNew[2] = vAxNew1[2] ;
|
|
int nFlagNew = 2 ;
|
|
int nFlag2New = ( bPartRise ? 0 : 1) ;
|
|
// eventuali aggiustamenti speciali dipendenti dalla macchina
|
|
DBLVECTOR vAxTmp = vAxNew ;
|
|
Vector3d vtTool = pCamData->GetToolDir() ;
|
|
int nFlagTmp = nFlagNew ;
|
|
int nFlag2Tmp = nFlag2New ;
|
|
bool bModif ;
|
|
if ( ! SpecialMoveZup( vAxTmp, vtTool, nFlagTmp, nFlag2Tmp, bModif))
|
|
return false ;
|
|
if ( bModif) {
|
|
vAxNew = vAxTmp ;
|
|
pCamData->SetToolDir( vtTool) ;
|
|
Vector3d vtAux = pCamData->GetAuxDir() ;
|
|
if ( ! vtAux.IsSmall()) {
|
|
DBLVECTOR vAng( vAxNew.begin() + 3, vAxNew.end()) ;
|
|
if ( m_pMchMgr->GetCalcAuxDirFromAngles( vAng, vtAux))
|
|
pCamData->SetAuxDir( vtAux) ;
|
|
}
|
|
nFlagNew = nFlagTmp ;
|
|
nFlag2New = nFlag2Tmp ;
|
|
}
|
|
// applico le modifiche (non forzo emissione della Z che è già alta)
|
|
pCamData->SetAxes( CamData::AS_OK, vAxNew) ;
|
|
pCamData->ChangeAxesMask( CamData::MSK_L1 | CamData::MSK_L2 | CamData::MSK_R1 | CamData::MSK_R2 | CamData::MSK_R3) ;
|
|
pCamData->SetFlag( nFlagNew) ;
|
|
pCamData->SetFlag2( nFlag2New) ;
|
|
}
|
|
// altrimenti
|
|
else {
|
|
// se Z pressochè uguali non devo fare alcunché
|
|
if ( abs( vAxCurr[2] - vAxPrev[2]) <= 100 * EPS_SMALL)
|
|
return true ;
|
|
// se Z corrente maggiore della precedente o viceversa se HomeZ sotto
|
|
else if ( ( ! bZHomeDown && vAxCurr[2] > vAxPrev[2]) ||
|
|
( bZHomeDown && vAxCurr[2] < vAxPrev[2]) ) {
|
|
// se vengo da precedente lavorazione non devo fare alcunché
|
|
if ( nPrevClPathId == GDB_ID_NULL)
|
|
return true ;
|
|
// aggiungo risalita alla fine del precedente percorso
|
|
DBLVECTOR vAxNew ;
|
|
double dDelta = ( ! bZHomeDown ? max( vAxCurr[2] - vAxPrev[2], 0.) : min( vAxCurr[2] - vAxPrev[2], 0.)) ;
|
|
if ( ! AddRise( vAxNew, dDelta, nPrevClPathId, bZHomeDown))
|
|
return false ;
|
|
}
|
|
// altrimenti Z corrente minore della precedente
|
|
else {
|
|
// aggiungo posizione elevata prima dell'inizio del percorso corrente
|
|
if ( m_pGeomDB->Copy( nEntId, GDB_ID_NULL, nEntId, GDB_AFTER) == GDB_ID_NULL)
|
|
return false ;
|
|
// modifico l'entità originale (è la prima del percorso)
|
|
m_pGeomDB->SetName( nEntId, MCH_CL_CLIMB) ;
|
|
DBLVECTOR vAxNew = vAxCurr ;
|
|
vAxNew[2] = vAxPrev[2] ;
|
|
int nFlagNew = 2 ;
|
|
int nFlag2New = 0 ;
|
|
// eventuali aggiustamenti speciali dipendenti dalla macchina
|
|
DBLVECTOR vAxTmp = vAxNew ;
|
|
Vector3d vtTool = pCamData->GetToolDir() ;
|
|
int nFlagTmp = nFlagNew ;
|
|
int nFlag2Tmp = nFlag2New ;
|
|
bool bModif ;
|
|
if ( ! SpecialMoveZup( vAxTmp, vtTool, nFlagTmp, nFlag2Tmp, bModif))
|
|
return false ;
|
|
if ( bModif) {
|
|
vAxNew = vAxTmp ;
|
|
pCamData->SetToolDir( vtTool) ;
|
|
Vector3d vtAux = pCamData->GetAuxDir() ;
|
|
if ( ! vtAux.IsSmall()) {
|
|
DBLVECTOR vAng( vAxNew.begin() + 3, vAxNew.end()) ;
|
|
if ( m_pMchMgr->GetCalcAuxDirFromAngles( vAng, vtAux))
|
|
pCamData->SetAuxDir( vtAux) ;
|
|
}
|
|
nFlagNew = nFlagTmp ;
|
|
nFlag2New = nFlag2Tmp ;
|
|
}
|
|
// applico le modifiche (non forzo emissione della Z che è già alta)
|
|
pCamData->SetAxes( CamData::AS_OK, vAxNew) ;
|
|
pCamData->ChangeAxesMask( CamData::MSK_L1 | CamData::MSK_L2 | CamData::MSK_R1 | CamData::MSK_R2 | CamData::MSK_R3) ;
|
|
pCamData->SetFlag( nFlagNew) ;
|
|
pCamData->SetFlag2( nFlag2New) ;
|
|
}
|
|
}
|
|
}
|
|
// se robot
|
|
else if ( m_pMchMgr->GetCurrIsRobot()) {
|
|
// recupero dati posizione finale lavorazione precedente
|
|
const CamData* pCamDataPrev = GetClPathFinalCamData( nPrevClPathId, true) ;
|
|
if ( nPrevClPathId == GDB_ID_NULL && pPrevOp != nullptr)
|
|
pCamDataPrev = pPrevOp->GetFinalCamData( true) ;
|
|
if ( pCamDataPrev == nullptr)
|
|
return false ;
|
|
const DBLVECTOR& vAxPrev = pCamDataPrev->GetAxesVal() ;
|
|
const Point3d& ptPEnd = pCamDataPrev->GetEndPoint() ;
|
|
const Vector3d& vtDirEnd = pCamDataPrev->GetToolDir() ;
|
|
const Vector3d& vtAuxEnd = pCamDataPrev->GetAuxDir() ;
|
|
// recupero dati posizione iniziale lavorazione corrente
|
|
// vAxCurr già recuperati sopra
|
|
const Point3d& ptPStart = pCamData->GetEndPoint() ;
|
|
const Vector3d& vtDirStart = pCamData->GetToolDir() ;
|
|
const Vector3d& vtAuxStart = pCamData->GetAuxDir() ;
|
|
// verifico se collegamento diretto tra le due posizioni (muovendosi alla Z maggiore) è possibile senza collisioni
|
|
int nLowerPos = 0 ;
|
|
DBLVECTOR vAxEnd = vAxPrev ;
|
|
DBLVECTOR vAxStart = vAxCurr ;
|
|
if ( ptPEnd.z < ptPStart.z - 100 * EPS_SMALL) {
|
|
nLowerPos = - 1 ;
|
|
if ( ! CalcRobotAxesAbovePos( ptPEnd, vtDirEnd, vtAuxEnd, ptPStart.z - ptPEnd.z, vAxEnd))
|
|
return false ;
|
|
}
|
|
else if ( ptPEnd.z > ptPStart.z + 100 * EPS_SMALL) {
|
|
nLowerPos = 1 ;
|
|
if ( ! CalcRobotAxesAbovePos( ptPStart, vtDirStart, vtAuxStart, ptPEnd.z - ptPStart.z, vAxStart))
|
|
return false ;
|
|
}
|
|
if ( TestCollisionAvoid( vAxEnd, vAxStart)) {
|
|
// se precedente più basso
|
|
if ( nLowerPos == -1) {
|
|
// se precedente appartiene ad altra operazione
|
|
if ( nPrevClPathId == GDB_ID_NULL) {
|
|
// elimino eventuali risalite finali
|
|
if ( pPrevOp == nullptr || ! pPrevOp->RemoveRise())
|
|
return false ;
|
|
// aggiungo risalita a Z opportuna alla fine del percorso precedente
|
|
DBLVECTOR vAxNew = vAxPrev ;
|
|
if ( ! pPrevOp->AddRise( vAxNew, ptPStart.z - ptPEnd.z))
|
|
return false ;
|
|
}
|
|
// altrimenti precedente appartiene alla stessa operazione
|
|
else {
|
|
// eventuali risalite finali già eliminate dal chiamante
|
|
// aggiungo risalita a Z opportuna alla fine del percorso precedente
|
|
DBLVECTOR vAxNew = vAxPrev ;
|
|
if ( ! AddRise( vAxNew, ptPStart.z - ptPEnd.z, nPrevClPathId))
|
|
return false ;
|
|
}
|
|
}
|
|
// se corrente più basso
|
|
else if ( nLowerPos == 1) {
|
|
// inserisco la posizione a Z alta
|
|
if ( ! AddRobotClimb( nEntId, ptPEnd.z - ptPStart.z))
|
|
return false ;
|
|
}
|
|
// altrimenti non devo fare alcunché
|
|
}
|
|
// altrimenti aggiungo risalita a Zmax
|
|
else {
|
|
// se precedente appartiene ad altra operazione
|
|
if ( nPrevClPathId == GDB_ID_NULL) {
|
|
// elimino eventuali risalite finali
|
|
if ( pPrevOp == nullptr || ! pPrevOp->RemoveRise())
|
|
return false ;
|
|
// aggiungo risalita a Z max alla fine della lavorazione precedente
|
|
DBLVECTOR vAxNew = vAxPrev ;
|
|
if ( ! pPrevOp->AddRise( vAxNew))
|
|
return false ;
|
|
}
|
|
// altrimenti precedente appartiene alla stessa operazione
|
|
else {
|
|
// eventuali risalite finali già eliminate dal chiamante
|
|
// aggiungo risalita a Z max alla fine del percorso precedente
|
|
DBLVECTOR vAxNew = vAxPrev ;
|
|
if ( ! AddRise( vAxNew, -1, nPrevClPathId))
|
|
return false ;
|
|
}
|
|
// inserisco la posizione iniziale a Zmax in questo percorso
|
|
if ( ! AddRobotClimb( nEntId, -1))
|
|
return false ;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::ToolChangeNeeded( const Operation& Op1, const Operation& Op2) const
|
|
{
|
|
// se non cambia l'utensile, non cambia la testa fisica
|
|
if ( EqualNoCase( Op1.GetToolName(), Op2.GetToolName()))
|
|
return false ;
|
|
// se non hanno TcPos e stanno sulla stessa testa con uscita diversa, non cambia la testa fisica
|
|
if ( Op1.GetToolTcPos().empty() && Op2.GetToolTcPos().empty() &&
|
|
EqualNoCase( Op1.GetHeadName(), Op2.GetHeadName()) && Op1.GetExitNbr() != Op2.GetExitNbr())
|
|
return false ;
|
|
// se hanno lo stesso TcPos e stanno sulla stessa testa con uscita diversa, non cambia la testa fisica
|
|
if ( EqualNoCase( Op1.GetToolTcPos(), Op2.GetToolTcPos()) &&
|
|
EqualNoCase( Op1.GetHeadName(), Op2.GetHeadName()) && Op1.GetExitNbr() != Op2.GetExitNbr())
|
|
return false ;
|
|
// altrimenti necessario un cambio
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::RemoveClimb( int nClPathId)
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// elimino tutte le entità CLIMB all'inizio del percorso
|
|
int nId = m_pGeomDB->GetFirstNameInGroup( nClPathId, MCH_CL_CLIMB) ;
|
|
while ( nId != GDB_ID_NULL) {
|
|
m_pGeomDB->Erase( nId) ;
|
|
nId = m_pGeomDB->GetFirstNameInGroup( nClPathId, MCH_CL_CLIMB) ;
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::AddRise( DBLVECTOR& vAxVal, double dDelta, int nClPathId, bool bZHomeDown)
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// recupero gruppo per geometria di lavorazione (Cutter Location)
|
|
int nClId = m_pGeomDB->GetFirstNameInGroup( GetOwner(), MCH_CL) ;
|
|
if ( nClId == GDB_ID_NULL)
|
|
return false ;
|
|
// se percorso CL specificato, verifico appartenga al gruppo sopra ricavato
|
|
if ( nClPathId != GDB_ID_NULL) {
|
|
if ( m_pGeomDB->GetParentId( nClPathId) != nClId)
|
|
return false ;
|
|
}
|
|
// altrimenti, recupero l'ultimo percorso CL della operazione
|
|
else {
|
|
nClPathId = m_pGeomDB->GetLastGroupInGroup( nClId) ;
|
|
if ( nClPathId == GDB_ID_NULL)
|
|
return false ;
|
|
}
|
|
// recupero l'ultima entità del percorso
|
|
int nEntId = m_pGeomDB->GetLastInGroup( nClPathId) ;
|
|
if ( nEntId == GDB_ID_NULL)
|
|
return false ;
|
|
// ne recupero i dati Cam
|
|
const CamData* pCamData = GetCamData( m_pGeomDB->GetUserObj( nEntId)) ;
|
|
if ( pCamData == nullptr)
|
|
return false ;
|
|
// se risalita praticamente nulla, esco con successo
|
|
if ( abs( dDelta) < 10 * EPS_SMALL) {
|
|
// recupero i valori degli assi
|
|
vAxVal = pCamData->GetAxesVal() ;
|
|
return ( vAxVal.size() >= 3) ;
|
|
}
|
|
// creo oggetto punto per DB geometrico
|
|
PtrOwner<IGeoPoint3d> pGP( CreateGeoPoint3d()) ;
|
|
if ( IsNull( pGP))
|
|
return false ;
|
|
// assegno le coordinate del punto
|
|
pGP->Set( pCamData->GetEndPoint()) ;
|
|
// inserisco l'oggetto nel DB geometrico
|
|
int nId = m_pGeomDB->AddGeoObj( GDB_ID_NULL, nClPathId, Release( pGP)) ;
|
|
if ( nId == GDB_ID_NULL)
|
|
return false ;
|
|
m_pGeomDB->SetName( nId, MCH_CL_RISE) ;
|
|
// creo oggetto dati Cam da associare al punto
|
|
PtrOwner<CamData> pCam( pCamData->Clone()) ;
|
|
if ( IsNull( pCam))
|
|
return false ;
|
|
// recupero i valori degli assi
|
|
vAxVal = pCam->GetAxesVal() ;
|
|
// flag per tipo di movimento in rapido
|
|
int nFlag = 0 ;
|
|
int nFlag2 = 0 ;
|
|
bool bOk = false ;
|
|
// se centro di lavoro
|
|
if ( m_pMchMgr->GetCurrIsCenter()) {
|
|
// devono esserci almeno i tre assi lineari
|
|
if ( vAxVal.size() < 3)
|
|
return false ;
|
|
// se delta positivo lo uso come incremento di Z
|
|
if ( dDelta > 0 || bZHomeDown) {
|
|
vAxVal[2] += dDelta ;
|
|
nFlag = 0 ; // movimento standard
|
|
}
|
|
// altrimenti uso la Z home
|
|
else {
|
|
// recupero posizione home
|
|
DBLVECTOR vAxHome ;
|
|
m_pMchMgr->GetAllCurrAxesHomePos( vAxHome) ;
|
|
vAxVal[2] = vAxHome[2] ;
|
|
nFlag = 3 ; // movimento a Zmax
|
|
nFlag2 = 1 ;
|
|
}
|
|
// eventuale aggiustamenti speciali dipendenti dalla macchina
|
|
DBLVECTOR vAxTmp = vAxVal ;
|
|
Vector3d vtTool = pCam->GetToolDir() ;
|
|
int nFlagTmp = nFlag ;
|
|
int nFlag2Tmp = nFlag2 ;
|
|
bool bModif ;
|
|
if ( ! SpecialMoveZup( vAxTmp, vtTool, nFlagTmp, nFlag2Tmp, bModif))
|
|
return false ;
|
|
if ( bModif) {
|
|
vAxVal = vAxTmp ;
|
|
pCam->SetToolDir( vtTool) ;
|
|
Vector3d vtAux = pCam->GetAuxDir() ;
|
|
if ( ! vtAux.IsSmall()) {
|
|
DBLVECTOR vAng( vAxVal.begin() + 3, vAxVal.end()) ;
|
|
if ( m_pMchMgr->GetCalcAuxDirFromAngles( vAng, vtAux))
|
|
pCam->SetAuxDir( vtAux) ;
|
|
}
|
|
nFlag = nFlagTmp ;
|
|
nFlag2 = nFlag2Tmp ;
|
|
}
|
|
// verifico extra-corsa asse Z
|
|
double dL1 = vAxVal[0] ;
|
|
double dL2 = vAxVal[1] ;
|
|
double dL3 = vAxVal[2] ;
|
|
DBLVECTOR vAng( vAxVal.begin() + 3, vAxVal.end()) ;
|
|
int nStat ;
|
|
bOk = ( m_pMchMgr->VerifyOutstroke( dL1, dL2, dL3, vAng, false, nStat) && nStat == 0) ;
|
|
// assegno i dati
|
|
pCam->SetAxes( ( bOk ? CamData::AS_OK : CamData::AS_OUTSTROKE), vAxVal) ;
|
|
pCam->SetMoveType( 0) ;
|
|
pCam->SetFeed( 0) ;
|
|
pCam->SetFlag( nFlag) ;
|
|
pCam->SetFlag2( nFlag2) ;
|
|
// associo questo oggetto a quello geometrico
|
|
m_pGeomDB->SetUserObj( nId, Release( pCam)) ;
|
|
}
|
|
// se altrimenti robot
|
|
else if ( m_pMchMgr->GetCurrIsRobot()) {
|
|
// devono esserci i sei assi rotanti
|
|
if ( vAxVal.size() < 6)
|
|
return false ;
|
|
// verifico se richiesta Zmax
|
|
bool bZmax = ( dDelta < 0) ;
|
|
// calcolo gli assi (compresa verifica corse)
|
|
double dNewDeltaZ ;
|
|
Point3d ptEnd = pCamData->GetEndPoint() ;
|
|
Vector3d vtTool = pCamData->GetToolDir() ;
|
|
Vector3d vtAux = pCamData->GetAuxDir() ;
|
|
bOk = CalcRobotAxesAbovePos( ptEnd, vtTool, vtAux, dDelta, vAxVal, &dNewDeltaZ) ;
|
|
// assegno i dati
|
|
pCam->SetAxes( ( bOk ? CamData::AS_OK : CamData::AS_OUTSTROKE), vAxVal) ;
|
|
pCam->SetMoveType( 0) ;
|
|
pCam->SetFeed( 0) ;
|
|
pCam->SetFlag( ( bZmax ? 3 : 0)) ;
|
|
pCam->SetFlag2( ( bZmax ? 1 : 0)) ;
|
|
// associo questo oggetto a quello geometrico
|
|
m_pGeomDB->SetUserObj( nId, Release( pCam)) ;
|
|
// aggiungo eventuali posizioni intermedie per garantire movimento lineare entro tolleranze
|
|
int nStep = 1 + int( dNewDeltaZ / MAX_ROBOT_G0_LEN) ;
|
|
double dStep = dNewDeltaZ / nStep ;
|
|
for ( int i = 1 ; i < nStep ; ++ i) {
|
|
// copio l'entità
|
|
int nNewId = m_pGeomDB->Copy( nId, GDB_ID_NULL, nId, GDB_BEFORE) ;
|
|
if ( nNewId == GDB_ID_NULL)
|
|
return false ;
|
|
// calcolo la posizione sopra il punto a Zmax
|
|
DBLVECTOR vAxNew = vAxVal ;
|
|
if ( ! CalcRobotAxesAbovePos( ptEnd, vtTool, vtAux, i * dStep, vAxNew))
|
|
return false ;
|
|
// modifico l'entità copiata
|
|
CamData* pCdNew = GetCamData( m_pGeomDB->GetUserObj( nNewId)) ;
|
|
if ( pCdNew == nullptr)
|
|
return false ;
|
|
pCdNew->SetAxes( CamData::AS_OK, vAxNew) ;
|
|
pCdNew->SetFlag( 0) ;
|
|
pCdNew->SetFlag2( 0) ;
|
|
}
|
|
}
|
|
|
|
return bOk ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::AddSpecialRise( const DBLVECTOR& vAxVal, bool bOk, int nClPathId, int nFlag)
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// recupero gruppo per geometria di lavorazione (Cutter Location)
|
|
int nClId = m_pGeomDB->GetFirstNameInGroup( GetOwner(), MCH_CL) ;
|
|
if ( nClId == GDB_ID_NULL)
|
|
return false ;
|
|
// se percorso CL specificato, verifico appartenga al gruppo sopra ricavato
|
|
if ( nClPathId != GDB_ID_NULL) {
|
|
if ( m_pGeomDB->GetParentId( nClPathId) != nClId)
|
|
return false ;
|
|
}
|
|
// altrimenti, recupero l'ultimo percorso CL della operazione
|
|
else {
|
|
nClPathId = m_pGeomDB->GetLastGroupInGroup( nClId) ;
|
|
if ( nClPathId == GDB_ID_NULL)
|
|
return false ;
|
|
}
|
|
// recupero l'ultima entità del percorso
|
|
int nEntId = m_pGeomDB->GetLastInGroup( nClPathId) ;
|
|
if ( nEntId == GDB_ID_NULL)
|
|
return false ;
|
|
// ne recupero i dati Cam
|
|
CamData* pCamData = GetCamData( m_pGeomDB->GetUserObj( nEntId)) ;
|
|
if ( pCamData == nullptr)
|
|
return false ;
|
|
// creo oggetto punto per DB geometrico
|
|
PtrOwner<IGeoPoint3d> pGP( CreateGeoPoint3d()) ;
|
|
if ( IsNull( pGP))
|
|
return false ;
|
|
// assegno le coordinate del punto
|
|
pGP->Set( pCamData->GetEndPoint()) ;
|
|
// inserisco l'oggetto nel DB geometrico
|
|
int nId = m_pGeomDB->AddGeoObj( GDB_ID_NULL, nClPathId, Release( pGP)) ;
|
|
if ( nId == GDB_ID_NULL)
|
|
return false ;
|
|
m_pGeomDB->SetName( nId, MCH_CL_RISE) ;
|
|
// creo oggetto dati Cam da associare al punto
|
|
PtrOwner<CamData> pCam( pCamData->Clone()) ;
|
|
if ( IsNull( pCam))
|
|
return false ;
|
|
// assegno i dati
|
|
pCam->SetAxes( ( bOk ? CamData::AS_OK : CamData::AS_OUTSTROKE), vAxVal) ;
|
|
pCam->SetMoveType( 0) ;
|
|
pCam->SetFeed( 0) ;
|
|
pCam->SetFlag( nFlag) ;
|
|
pCam->SetFlag2( 0) ;
|
|
// associo questo oggetto a quello geometrico
|
|
m_pGeomDB->SetUserObj( nId, Release( pCam)) ;
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::RemoveRise( int nClPathId)
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// recupero gruppo per geometria di lavorazione (Cutter Location)
|
|
int nClId = m_pGeomDB->GetFirstNameInGroup( GetOwner(), MCH_CL) ;
|
|
if ( nClId == GDB_ID_NULL)
|
|
return false ;
|
|
// se percorso CL specificato, verifico appartenga al gruppo sopra ricavato
|
|
if ( nClPathId != GDB_ID_NULL) {
|
|
if ( m_pGeomDB->GetParentId( nClPathId) != nClId)
|
|
return false ;
|
|
}
|
|
// altrimenti, recupero l'ultimo percorso CL della operazione
|
|
else {
|
|
nClPathId = m_pGeomDB->GetLastGroupInGroup( nClId) ;
|
|
if ( nClPathId == GDB_ID_NULL)
|
|
return false ;
|
|
}
|
|
// elimino tutte le entità RISE alla fine del percorso
|
|
int nId = m_pGeomDB->GetFirstNameInGroup( nClPathId, MCH_CL_RISE) ;
|
|
while ( nId != GDB_ID_NULL) {
|
|
m_pGeomDB->Erase( nId) ;
|
|
nId = m_pGeomDB->GetFirstNameInGroup( nClPathId, MCH_CL_RISE) ;
|
|
}
|
|
// elimino a maggior ragione eventuali entità HOME
|
|
nId = m_pGeomDB->GetFirstNameInGroup( nClPathId, MCH_CL_HOME) ;
|
|
while ( nId != GDB_ID_NULL) {
|
|
m_pGeomDB->Erase( nId) ;
|
|
nId = m_pGeomDB->GetFirstNameInGroup( nClPathId, MCH_CL_HOME) ;
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::AddHome( void)
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// recupero gruppo per geometria di lavorazione (Cutter Location)
|
|
int nClId = m_pGeomDB->GetFirstNameInGroup( GetOwner(), MCH_CL) ;
|
|
if ( nClId == GDB_ID_NULL)
|
|
return false ;
|
|
// recupero l'ultimo percorso
|
|
int nLastPxClId = m_pGeomDB->GetLastGroupInGroup( nClId) ;
|
|
// recupero l'ultima entità del percorso di nome RISE
|
|
int nEntId = m_pGeomDB->GetLastNameInGroup( nLastPxClId, MCH_CL_RISE) ;
|
|
if ( nEntId == GDB_ID_NULL)
|
|
return false ;
|
|
// ne recupero i dati Cam
|
|
CamData* pCamData = GetCamData( m_pGeomDB->GetUserObj( nEntId)) ;
|
|
if ( pCamData == nullptr)
|
|
return false ;
|
|
// creo oggetto punto per DB geometrico
|
|
PtrOwner<IGeoPoint3d> pGP( CreateGeoPoint3d()) ;
|
|
if ( IsNull( pGP))
|
|
return false ;
|
|
// assegno le coordinate del punto
|
|
pGP->Set( pCamData->GetEndPoint()) ;
|
|
// inserisco l'oggetto nel DB geometrico
|
|
int nId = m_pGeomDB->AddGeoObj( GDB_ID_NULL, nLastPxClId, Release( pGP)) ;
|
|
if ( nId == GDB_ID_NULL)
|
|
return false ;
|
|
m_pGeomDB->SetName( nId, MCH_CL_HOME) ;
|
|
// creo oggetto dati Cam
|
|
PtrOwner<CamData> pCam( pCamData->Clone()) ;
|
|
if ( IsNull( pCam))
|
|
return false ;
|
|
// flag per tipo di movimento in rapido (rapido a Home)
|
|
int nFlag = 4 ;
|
|
// recupero coordinate home
|
|
DBLVECTOR vAxHome ;
|
|
m_pMchMgr->GetAllCurrAxesHomePos( vAxHome) ;
|
|
// assegno i dati
|
|
pCam->SetAxes( CamData::AS_OK, vAxHome) ;
|
|
pCam->SetMoveType( 0) ;
|
|
pCam->SetFeed( 0) ;
|
|
pCam->SetFlag( nFlag) ;
|
|
// associo questo oggetto a quello geometrico
|
|
m_pGeomDB->SetUserObj( nId, Release( pCam)) ;
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::RemoveHome( void)
|
|
{
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// recupero gruppo per geometria di lavorazione (Cutter Location)
|
|
int nClId = m_pGeomDB->GetFirstNameInGroup( GetOwner(), MCH_CL) ;
|
|
if ( nClId == GDB_ID_NULL)
|
|
return false ;
|
|
// recupero l'ultimo percorso CL
|
|
int nClPathId = m_pGeomDB->GetLastGroupInGroup( nClId) ;
|
|
// elimino tutte le entità HOME alla fine del percorso
|
|
int nId = m_pGeomDB->GetFirstNameInGroup( nClPathId, MCH_CL_HOME) ;
|
|
while ( nId != GDB_ID_NULL) {
|
|
m_pGeomDB->Erase( nId) ;
|
|
nId = m_pGeomDB->GetFirstNameInGroup( nClPathId, MCH_CL_HOME) ;
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::RemoveClimbRiseHome( void)
|
|
{
|
|
// elimino le entità CLIMB, RISE e HOME delle diverse path dell'operazione
|
|
int nClId = m_pGeomDB->GetFirstNameInGroup( m_nOwnerId, MCH_CL) ;
|
|
int nClPathId = m_pGeomDB->GetFirstGroupInGroup( nClId) ;
|
|
while ( nClPathId != GDB_ID_NULL) {
|
|
RemoveClimb( nClPathId) ;
|
|
RemoveRise( nClPathId) ;
|
|
nClPathId = m_pGeomDB->GetNextGroup( nClPathId) ;
|
|
}
|
|
return ( nClId != GDB_ID_NULL) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::AddRobotClimb( int nEntId, double dDeltaZ)
|
|
{
|
|
// recupero i dati Cam dell'entità
|
|
CamData* pCdEnt = GetCamData( m_pGeomDB->GetUserObj( nEntId)) ;
|
|
if ( pCdEnt == nullptr)
|
|
return false ;
|
|
DBLVECTOR vAxVal = pCdEnt->GetAxesVal() ;
|
|
// verifico se richiesta Zmax
|
|
bool bZmax = ( dDeltaZ < 0) ;
|
|
// calcolo la posizione sopra il punto a Z richiesta
|
|
DBLVECTOR vAxClimb = vAxVal ;
|
|
double dNewDeltaZ ;
|
|
if ( ! CalcRobotAxesAbovePos( pCdEnt->GetEndPoint(), pCdEnt->GetToolDir(), pCdEnt->GetAuxDir(), dDeltaZ, vAxClimb, &dNewDeltaZ))
|
|
return false ;
|
|
// aggiungo l'entità solo se necessario
|
|
if ( dNewDeltaZ < 100 * EPS_SMALL)
|
|
return true ;
|
|
// copio l'entità
|
|
if ( m_pGeomDB->Copy( nEntId, GDB_ID_NULL, nEntId, GDB_AFTER) == GDB_ID_NULL)
|
|
return false ;
|
|
// modifico quella originale (è la prima del percorso)
|
|
m_pGeomDB->SetName( nEntId, MCH_CL_CLIMB) ;
|
|
pCdEnt->SetAxes( CamData::AS_OK, vAxClimb) ;
|
|
pCdEnt->SetFlag( 2) ;
|
|
pCdEnt->SetFlag2( ( bZmax ? 1 : 0)) ;
|
|
// aggiungo eventuali posizioni intermedie per garantire movimento lineare entro tolleranze
|
|
int nStep = 1 + int( dNewDeltaZ / MAX_ROBOT_G0_LEN) ;
|
|
double dStep = dNewDeltaZ / nStep ;
|
|
for ( int i = 1 ; i < nStep ; ++ i) {
|
|
// copio l'entità
|
|
int nNewId = m_pGeomDB->Copy( nEntId, GDB_ID_NULL, nEntId, GDB_AFTER) ;
|
|
if ( nNewId == GDB_ID_NULL)
|
|
return false ;
|
|
// calcolo la posizione sopra il punto a Zmax
|
|
DBLVECTOR vAxNew = vAxVal ;
|
|
if ( ! CalcRobotAxesAbovePos( pCdEnt->GetEndPoint(), pCdEnt->GetToolDir(), pCdEnt->GetAuxDir(), i * dStep, vAxNew))
|
|
return false ;
|
|
// modifico l'entità copiata
|
|
CamData* pCdNew = GetCamData( m_pGeomDB->GetUserObj( nNewId)) ;
|
|
if ( pCdNew == nullptr)
|
|
return false ;
|
|
pCdNew->SetAxes( CamData::AS_OK, vAxNew) ;
|
|
pCdNew->SetFlag( 0) ;
|
|
pCdNew->SetFlag2( 0) ;
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::CalcRobotAxesAbovePos( const Point3d& ptP, const Vector3d& vtT, const Vector3d& vtA, double dDeltaZ,
|
|
DBLVECTOR& vAx, double* pdNewDeltaZ) const
|
|
{
|
|
// Recupero macchina corrente
|
|
Machine* pMch = ( m_pMchMgr != nullptr ? m_pMchMgr->GetCurrMachine() : nullptr) ;
|
|
if ( pMch == nullptr) {
|
|
if ( pdNewDeltaZ != nullptr)
|
|
*pdNewDeltaZ = 0 ;
|
|
return false ;
|
|
}
|
|
// se DeltaZ non specificata vado a Zmax
|
|
if ( dDeltaZ < 0 ) {
|
|
// recupero quota tavola
|
|
Point3d ptRef1 ;
|
|
pMch->GetCurrTableRef1( ptRef1) ;
|
|
// recupero Zmax sopra tavola
|
|
double dMaxOnTabZ ;
|
|
if ( ! m_pGeomDB->GetInfo( pMch->GetCurrTable(), "MaxOnTabZ", dMaxOnTabZ))
|
|
dMaxOnTabZ = 600 ;
|
|
// assegno la variazione di Z
|
|
dDeltaZ = ptRef1.z + dMaxOnTabZ - ptP.z ;
|
|
}
|
|
if ( pdNewDeltaZ != nullptr)
|
|
*pdNewDeltaZ = dDeltaZ ;
|
|
// calcolo assi macchina
|
|
Point3d ptNew( ptP.x, ptP.y, ptP.z + dDeltaZ) ;
|
|
DBLVECTOR vAng1, vAng2 ;
|
|
if ( ! m_pMchMgr->GetCurrMachine()->GetRobotAngles( ptNew, vtT, vtA, vAng1, vAng2))
|
|
return false ;
|
|
// se non definita posizione precedente, scelgo la prima soluzione
|
|
if ( vAx.size() < 6) {
|
|
vAx = vAng1 ;
|
|
}
|
|
// altrimenti, scelgo la soluzione più vicina
|
|
else {
|
|
// porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri
|
|
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i)
|
|
m_pMchMgr->GetNearestAngleInStroke( i, vAx[i], vAng1[i]) ;
|
|
for ( int i = 0 ; i < int( vAng2.size()) ; ++ i)
|
|
m_pMchMgr->GetNearestAngleInStroke( i, vAx[i], vAng2[i]) ;
|
|
// salvo precedente R6
|
|
double dPrevR6 = vAx[5] ;
|
|
// minimizzo rotazione polso
|
|
if ( vAng2.empty() || abs( vAng1[3] - vAx[3]) < abs( vAng2[3] - vAx[3]) + 10 * EPS_ANG_SMALL)
|
|
vAx = vAng1 ;
|
|
else
|
|
vAx = vAng2 ;
|
|
// controllo continuità di R6 dalla posizione precedente
|
|
m_pMchMgr->GetNearestAngleInStroke( 5, dPrevR6, vAx[5]) ;
|
|
}
|
|
// verifico di essere nelle corse degli assi
|
|
int nStat ;
|
|
if ( ! m_pMchMgr->VerifyOutstroke( 0, 0, 0, vAx, false, nStat) || nStat != 0)
|
|
return false ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::CalcDeltaZForHeadRotation( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd, double& dDeltaZ) const
|
|
{
|
|
if ( m_pMchMgr == nullptr)
|
|
return false ;
|
|
// il numero di assi deve essere costante
|
|
if ( vAxStart.size() != vAxEnd.size())
|
|
return false ;
|
|
// se ci sono solo assi lineari, pongo delta Z nullo
|
|
if ( vAxStart.size() <= 3) {
|
|
dDeltaZ = 0 ;
|
|
return true ;
|
|
}
|
|
// determino gli estremi del movimento rotatorio
|
|
double dR1Start = ( vAxStart.size() >= 4 ? vAxStart[3] : 0) ;
|
|
double dR2Start = ( vAxStart.size() >= 5 ? vAxStart[4] : 0) ;
|
|
double dR3Start = ( vAxStart.size() >= 6 ? vAxStart[5] : 0) ;
|
|
double dR1End = ( vAxEnd.size() >= 4 ? vAxEnd[3] : 0) ;
|
|
double dR2End = ( vAxEnd.size() >= 5 ? vAxEnd[4] : 0) ;
|
|
double dR3End = ( vAxEnd.size() >= 6 ? vAxEnd[5] : 0) ;
|
|
// determino le variazioni di ciascun asse
|
|
double dR1Delta = dR1End - dR1Start ;
|
|
double dR2Delta = dR2End - dR2Start ;
|
|
double dR3Delta = dR3End - dR3Start ;
|
|
// divido il movimento in step di massimo 5 gradi
|
|
int nStep = int( max( abs( dR1Delta), max( abs( dR2Delta), abs( dR3Delta))) / 5.0) + 1 ;
|
|
double dR1Step = dR1Delta / nStep ;
|
|
double dR2Step = dR2Delta / nStep ;
|
|
double dR3Step = dR3Delta / nStep ;
|
|
// calcolo
|
|
double dL1 = vAxStart[0] ;
|
|
double dL2 = vAxStart[1] ;
|
|
double dL3 = vAxStart[2] ;
|
|
double dZStart, dZmin ;
|
|
DBLVECTOR vAng( 3) ;
|
|
for ( int i = 0 ; i <= nStep ; ++ i) {
|
|
vAng[0] = dR1Start + i * dR1Step ;
|
|
vAng[1] = dR2Start + i * dR2Step ;
|
|
vAng[2] = dR3Start + i * dR3Step ;
|
|
Point3d ptTip ;
|
|
if ( ! m_pMchMgr->GetCalcTipFromPositions( dL1, dL2, dL3, vAng, true, true, ptTip))
|
|
return false ;
|
|
if ( i == 0) {
|
|
dZStart = ptTip.z ;
|
|
dZmin = dZStart ;
|
|
}
|
|
else if ( ptTip.z < dZmin)
|
|
dZmin = ptTip.z ;
|
|
}
|
|
dDeltaZ = dZStart - dZmin ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetExtraZ( const DBLVECTOR& vAx1, const Vector3d& vtTool1,
|
|
const DBLVECTOR& vAx2, const Vector3d& vtTool2,
|
|
double dHomeZ, double& dExtraZ) const
|
|
{
|
|
// Recupero macchina corrente
|
|
Machine* pMch = ( m_pMchMgr != nullptr ? m_pMchMgr->GetCurrMachine() : nullptr) ;
|
|
if ( pMch == nullptr)
|
|
return false ;
|
|
|
|
// Flag per posizione sicura in alto o in basso
|
|
int nHeadId = pMch->GetCurrHead() ;
|
|
bool bZHomeDown ;
|
|
if ( ! m_pGeomDB->GetInfo( nHeadId, MCH_ZHOMEDOWN, bZHomeDown))
|
|
bZHomeDown = false ;
|
|
|
|
// Estremi dell'asse Z (asse 3, quindi indice 2)
|
|
double dAxZmax ; pMch->GetCurrAxisMax( 2, dAxZmax) ;
|
|
double dAxZmin ; pMch->GetCurrAxisMin( 2, dAxZmin) ;
|
|
|
|
// Se definito e correttamente eseguito script SpecialGetMaxZ
|
|
double dMaxZ ;
|
|
if ( SpecialGetMaxZ( vAx1, vtTool1, vAx2, vtTool2, dMaxZ)) {
|
|
// se caso standard
|
|
if ( ! bZHomeDown) {
|
|
dExtraZ = min( dMaxZ, dAxZmax) - dHomeZ ;
|
|
return ( dExtraZ > EPS_SMALL) ;
|
|
}
|
|
// altrimenti caso con posizione sicura in basso (teste da sotto)
|
|
else {
|
|
dExtraZ = max( dMaxZ, dAxZmin) - dHomeZ ;
|
|
return ( dExtraZ < EPS_SMALL) ;
|
|
}
|
|
}
|
|
|
|
// Altrimenti verifico se testa ha Info ZEXTRA
|
|
DBLVECTOR vdVal ;
|
|
if ( ! m_pGeomDB->GetInfo( nHeadId, MCH_ZEXTRA, vdVal) || vdVal.empty() || vdVal[0] < EPS_SMALL)
|
|
return false ;
|
|
// angolo verticale limite per applicare questo allontanamento extra
|
|
double dMaxAngV = 30 ;
|
|
if ( vdVal.size() >= 2)
|
|
dMaxAngV = vdVal[1] ;
|
|
// angolo verticale
|
|
double dAngV = 0 ;
|
|
if ( vAx1.size() >= 5 && vAx2.size() >= 5)
|
|
dAngV = max( abs( vAx1[4]), abs( vAx2[4])) ;
|
|
else if ( vAx2.size() >= 5)
|
|
dAngV = abs( vAx2[4]) ;
|
|
// verifica angolo verticale
|
|
if ( dAngV < dMaxAngV) {
|
|
// se caso standard
|
|
if ( ! bZHomeDown) {
|
|
double dZmax ;
|
|
pMch->GetCurrAxisMax( 2, dZmax) ;
|
|
dExtraZ = min( vdVal[0], dZmax - dHomeZ) ;
|
|
return ( dExtraZ > EPS_SMALL) ;
|
|
}
|
|
// altrimenti caso con posizione sicura in basso (teste da sotto)
|
|
else {
|
|
double dZmin ;
|
|
pMch->GetCurrAxisMin( 2, dZmin) ;
|
|
dExtraZ = max( vdVal[0], dZmin - dHomeZ) ;
|
|
return ( dExtraZ < EPS_SMALL) ;
|
|
}
|
|
}
|
|
// Lascio provare a Zmax
|
|
dExtraZ = 0 ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::SpecialGetMaxZ( const DBLVECTOR& vAx1, const Vector3d& vtTool1,
|
|
const DBLVECTOR& vAx2, const Vector3d& vtTool2,
|
|
double& dMaxZ) const
|
|
{
|
|
// recupero la macchina corrente
|
|
Machine* pMch = ( m_pMchMgr != nullptr ? m_pMchMgr->GetCurrMachine() : nullptr) ;
|
|
if ( pMch == nullptr)
|
|
return false ;
|
|
|
|
// verifico numero assi
|
|
int nNumAx1 = int( vAx1.size()) ;
|
|
int nNumAx2 = int( vAx2.size()) ;
|
|
if ( nNumAx1 != nNumAx2 && nNumAx1 != 0 && ! vtTool1.IsSmall())
|
|
return false ;
|
|
|
|
// costanti
|
|
static const string EMC_VAR = "EMC" ; // tabella variabili locali per calcolo
|
|
static const string EVAR_ERROR = ".ERR" ; // OUT (int) codice di errore ( 0 = ok)
|
|
static const string EVAR_MAXZ = ".MAXZ" ; // OUT (double) valore Zmax
|
|
static const string ON_SPECIAL_GETMAXZ = "OnSpecialGetMaxZ" ;
|
|
|
|
// verifico definizione funzione
|
|
if ( ! pMch->LuaExistsFunction( ON_SPECIAL_GETMAXZ))
|
|
return false ;
|
|
|
|
// avvio
|
|
bool bOk = true ;
|
|
int nErr = 99 ;
|
|
// imposto valori parametri
|
|
bOk = bOk && pMch->LuaCreateGlobTable( EMC_VAR) ;
|
|
bOk = bOk && pMch->LuaSetGlobVar( EMC_VAR + GVAR_TOOL, GetToolName()) ;
|
|
bOk = bOk && pMch->LuaSetGlobVar( EMC_VAR + GVAR_HEAD, GetHeadName()) ;
|
|
bOk = bOk && pMch->LuaSetGlobVar( EMC_VAR + GVAR_EXIT, GetExitNbr()) ;
|
|
bOk = bOk && pMch->LuaSetGlobVar( EMC_VAR + GVAR_TCPOS, GetToolTcPos()) ;
|
|
bOk = bOk && pMch->LuaSetGlobVar( EMC_VAR + GVAR_MCHID, GetOwner()) ;
|
|
// valori degli assi
|
|
bool bIsRobot = m_pMchMgr->GetCurrIsRobot() ;
|
|
for ( int i = 1 ; i <= nNumAx1 ; ++ i)
|
|
bOk = bOk && pMch->LuaSetGlobVar( GetGlobVarAxisPrev( i, EMC_VAR, bIsRobot), vAx1[i-1]) ;
|
|
for ( int i = 1 ; i <= nNumAx2 ; ++ i)
|
|
bOk = bOk && pMch->LuaSetGlobVar( GetGlobVarAxisValue( i, EMC_VAR, bIsRobot), vAx2[i-1]) ;
|
|
// direzioni utensile
|
|
bOk = bOk && pMch->LuaSetGlobVar( EMC_VAR + GVAR_TDIRP, vtTool1) ;
|
|
bOk = bOk && pMch->LuaSetGlobVar( EMC_VAR + GVAR_TDIR, vtTool2) ;
|
|
// eseguo
|
|
bOk = bOk && pMch->LuaCallFunction( ON_SPECIAL_GETMAXZ, false) ;
|
|
// recupero valori parametri obbligatori
|
|
bOk = bOk && pMch->LuaGetGlobVar( EMC_VAR + EVAR_ERROR, nErr) ;
|
|
bOk = bOk && pMch->LuaGetGlobVar( EMC_VAR + EVAR_MAXZ, dMaxZ) ;
|
|
// reset
|
|
bOk = bOk && pMch->LuaResetGlobVar( EMC_VAR) ;
|
|
// segnalo errori
|
|
if ( nErr != 0) {
|
|
bOk = false ;
|
|
string sOut = " Error in " + ON_SPECIAL_GETMAXZ + " (" + ToString( nErr) + ")" ;
|
|
LOG_ERROR( GetEMkLogger(), sOut.c_str())
|
|
}
|
|
return bOk ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetRotationAtZmax( void) const
|
|
{
|
|
// Recupero macchina corrente
|
|
Machine* pMch = ( m_pMchMgr != nullptr ? m_pMchMgr->GetCurrMachine() : nullptr) ;
|
|
if ( pMch == nullptr)
|
|
return false ;
|
|
// Se testa con Info ROTATZMAX la recupero e restituisco abilitazione rotazione a Zmax
|
|
int nHeadId = pMch->GetCurrHead() ;
|
|
int nVal = 0 ;
|
|
return ( m_pGeomDB->GetInfo( nHeadId, MCH_ROTATZMAX, nVal) && nVal != 0) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::ForcedZmax( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd, const BBox3d& b3Raws) const
|
|
{
|
|
// Recupero macchina corrente
|
|
Machine* pMch = ( m_pMchMgr != nullptr ? m_pMchMgr->GetCurrMachine() : nullptr) ;
|
|
if ( pMch == nullptr)
|
|
return true ;
|
|
// Recupero numero assi e ne verifico la costanza
|
|
int nLinAxes = m_pMchMgr->GetCurrLinAxes() ;
|
|
int nRotAxes = m_pMchMgr->GetCurrRotAxes() ;
|
|
if ( nLinAxes + nRotAxes != vAxStart.size() || nLinAxes + nRotAxes != vAxEnd.size())
|
|
return true ;
|
|
// Verifico se richiesta risalita da note utente della lavorazione
|
|
if ( GetUserNotesZmax() > 0)
|
|
return true ;
|
|
// Se testa con Info ZMAXONROT != 0 e movimento assi rotanti -> risalita a Zmax
|
|
int nHeadId = pMch->GetCurrHead() ;
|
|
DBLVECTOR vdVal ;
|
|
if ( m_pGeomDB->GetInfo( nHeadId, MCH_ZMAXONROT, vdVal) && vdVal.size() >= 1 && lround(vdVal[0]) > 0) {
|
|
// controllo altezza minima grezzi
|
|
double dHmin = 100 * EPS_SMALL ;
|
|
if ( vdVal.size() >= 3)
|
|
dHmin = max( vdVal[2], dHmin) ;
|
|
if ( b3Raws.GetDimZ() < dHmin)
|
|
return false ;
|
|
// controllo variazione angolare minima
|
|
double dAngTol = 100 * EPS_ANG_SMALL ;
|
|
if ( vdVal.size() >= 2)
|
|
dAngTol = max( vdVal[1], dAngTol) ;
|
|
for ( int i = nLinAxes ; i < nLinAxes + nRotAxes ; ++ i) {
|
|
if ( abs( vAxEnd[i] - vAxStart[i]) > dAngTol)
|
|
return true ;
|
|
}
|
|
}
|
|
// Non forzata risalita a Zmax
|
|
return false ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
int
|
|
Operation::GetUserNotesZmax( void) const
|
|
{
|
|
string sUserNotes ;
|
|
if ( m_pMchMgr != nullptr && m_pMchMgr->GetMachiningParam( MPA_USERNOTES, sUserNotes)) {
|
|
int nVal ;
|
|
if ( FromString( ExtractInfo( sUserNotes, "StartZmax="), nVal))
|
|
return nVal ;
|
|
}
|
|
return 0 ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
double
|
|
Operation::GetDeltaSafeZ( const Vector3d& vtTool) const
|
|
{
|
|
// Recupero macchina corrente
|
|
Machine* pMch = ( m_pMchMgr != nullptr ? m_pMchMgr->GetCurrMachine() : nullptr) ;
|
|
if ( pMch == nullptr)
|
|
return false ;
|
|
// Leggo da testa Info ZSAFEDELTA
|
|
DBLVECTOR vdVal ;
|
|
if ( ! m_pGeomDB->GetInfo( pMch->GetCurrHead(), MCH_ZSAFEDELTA, vdVal) || vdVal.empty())
|
|
return 0 ;
|
|
if ( vdVal.size() == 1)
|
|
return vdVal[0] ;
|
|
return ( vtTool.z > MIN_ZDIR_VERT_CHSAW ? vdVal[0] : vdVal[1]) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetZHomeDown( void) const
|
|
{
|
|
// Recupero macchina corrente
|
|
Machine* pMch = ( m_pMchMgr != nullptr ? m_pMchMgr->GetCurrMachine() : nullptr) ;
|
|
if ( pMch == nullptr)
|
|
return false ;
|
|
// Leggo da testa Info ZHOMEDOWN (0=falso[default], altrimenti vero)
|
|
bool bZHomeDown ;
|
|
if ( ! m_pGeomDB->GetInfo( pMch->GetCurrHead(), MCH_ZHOMEDOWN, bZHomeDown))
|
|
bZHomeDown = false ;
|
|
return bZHomeDown ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
static bool
|
|
ResetMachine( Machine* pMch)
|
|
{
|
|
// macchina non definita
|
|
if ( pMch == nullptr)
|
|
return false ;
|
|
// Riporto la macchina in home
|
|
pMch->ResetAllAxesPos( true, false) ;
|
|
// Sgancio pezzi, grezzi e sottopezzi dalla tavola corrente
|
|
pMch->UnlinkAllPartsFromGroups() ;
|
|
pMch->UnlinkAllRawPartsFromGroups() ;
|
|
pMch->UnlinkAllFixturesFromGroups() ;
|
|
// tutto bene
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::TestCollisionAvoid( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd) const
|
|
{
|
|
// Recupero macchina corrente
|
|
Machine* pMch = ( m_pMchMgr != nullptr ? m_pMchMgr->GetCurrMachine() : nullptr) ;
|
|
if ( pMch == nullptr)
|
|
return false ;
|
|
// Recupero tavola corrente
|
|
string sTable ;
|
|
if ( ! pMch->GetCurrTable( sTable))
|
|
return false ;
|
|
// Recupero assi correnti e ne verifico la costanza
|
|
STRVECTOR vAxName ;
|
|
pMch->GetAllCurrAxesNames( vAxName) ;
|
|
if ( vAxName.size() != vAxStart.size() || vAxName.size() != vAxEnd.size())
|
|
return false ;
|
|
|
|
// Porto la macchina in home
|
|
pMch->ResetAllAxesPos( true, false) ;
|
|
// Elenco grezzi attivi
|
|
INTVECTOR vRawId ;
|
|
int nRawId = m_pMchMgr->GetFirstRawPart() ;
|
|
while ( nRawId != GDB_ID_NULL) {
|
|
if ( m_pMchMgr->VerifyRawPartPhase( nRawId, m_nPhase))
|
|
vRawId.emplace_back( nRawId) ;
|
|
nRawId = m_pMchMgr->GetNextRawPart( nRawId) ;
|
|
}
|
|
// Aggancio pezzi attivi alla tavola corrente
|
|
for ( const auto nRawId : vRawId)
|
|
pMch->LinkRawPartToGroup( nRawId, sTable) ;
|
|
// Elenco bloccaggi attivi
|
|
INTVECTOR vFxtId ;
|
|
int nFxtId = m_pMchMgr->GetFirstFixture() ;
|
|
while ( nFxtId != GDB_ID_NULL) {
|
|
vFxtId.emplace_back( nFxtId) ;
|
|
nFxtId = m_pMchMgr->GetNextFixture( nFxtId) ;
|
|
}
|
|
// Aggancio bloccaggi alla tavola corrente
|
|
for ( const auto nFxtId : vFxtId)
|
|
pMch->LinkFixtureToGroup( nFxtId, sTable) ;
|
|
|
|
// Lancio la verifica custom
|
|
int nRes = SpecialTestCollisionAvoid( vAxStart, vAxEnd) ;
|
|
if ( nRes != SCAV_TOTEST) {
|
|
ResetMachine( pMch) ;
|
|
return ( nRes == SCAV_AVOID) ;
|
|
}
|
|
|
|
// Verifica standard
|
|
bool bCollAvoid ;
|
|
int nLinkAxisOrder = LKAO_INTERP ;
|
|
// se ordine stabilito tra gli assi di tavola e testa (LKAO_HEAD_BEFORE(-1)=prima testa, LKAO_HEAD_AFTER(+1)=dopo testa)
|
|
if ( nLinkAxisOrder == LKAO_HEAD_BEFORE || nLinkAxisOrder == LKAO_HEAD_AFTER) {
|
|
// assegno il valore degli assi nella posizione intermedia opportuna
|
|
DBLVECTOR vAxMid ;
|
|
GetAxisMidForTestCollisionAvoid( vAxStart, vAxEnd, nLinkAxisOrder, pMch, vAxMid) ;
|
|
// eseguo i test
|
|
bCollAvoid = OneMoveTestCollisionAvoid( vAxName, vAxStart, vAxMid, pMch, vRawId, vFxtId) &&
|
|
OneMoveTestCollisionAvoid( vAxName, vAxMid, vAxEnd, pMch, vRawId, vFxtId) ;
|
|
}
|
|
// se verifico entrambi gli ordini degli assi di tavola e testa LKAO_HEAD_BOTH(11)
|
|
else if ( nLinkAxisOrder == LKAO_HEAD_BOTH) {
|
|
// assegno il valore degli assi nelle posizioni intermedie
|
|
DBLVECTOR vAxMidB, vAxMidA ;
|
|
GetAxisMidForTestCollisionAvoid( vAxStart, vAxEnd, LKAO_HEAD_BEFORE, pMch, vAxMidB) ;
|
|
GetAxisMidForTestCollisionAvoid( vAxStart, vAxEnd, LKAO_HEAD_AFTER, pMch, vAxMidA) ;
|
|
// eseguo i test
|
|
bCollAvoid = OneMoveTestCollisionAvoid( vAxName, vAxStart, vAxMidB, pMch, vRawId, vFxtId) &&
|
|
OneMoveTestCollisionAvoid( vAxName, vAxMidB, vAxEnd, pMch, vRawId, vFxtId) &&
|
|
OneMoveTestCollisionAvoid( vAxName, vAxStart, vAxMidA, pMch, vRawId, vFxtId) &&
|
|
OneMoveTestCollisionAvoid( vAxName, vAxMidA, vAxEnd, pMch, vRawId, vFxtId) ;
|
|
}
|
|
// altrimenti interpolo tutti gli assi tra loro LKAO_INTERP(0)
|
|
else {
|
|
bCollAvoid = OneMoveTestCollisionAvoid( vAxName, vAxStart, vAxEnd, pMch, vRawId, vFxtId) ;
|
|
}
|
|
|
|
// Ripristino lo stato macchina
|
|
ResetMachine( pMch) ;
|
|
|
|
return bCollAvoid ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetAxisMidForTestCollisionAvoid( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd, int nAxisOrder, Machine* pMch,
|
|
DBLVECTOR& vAxMid) const
|
|
{
|
|
// reset del risultato
|
|
vAxMid.clear() ;
|
|
// verifico che i vettori degli assi abbiano la stessa dimensione e che la macchina sia definita
|
|
if ( vAxStart.size() != vAxEnd.size() || pMch == nullptr)
|
|
return false ;
|
|
// assegno dimensione al risultato
|
|
vAxMid.resize( vAxStart.size()) ;
|
|
// ciclo sugli assi correnti, assegno opportunamente valori a seconda del tipo (testa o tavola)
|
|
for ( int i = 0 ; i < int( vAxStart.size()) ; ++ i) {
|
|
bool bLinear, bHead ; pMch->GetCurrAxisType( i, bLinear, bHead) ;
|
|
if ( nAxisOrder == -1)
|
|
vAxMid[i] = ( bHead ? vAxEnd[i] : vAxStart[i]) ;
|
|
else if ( nAxisOrder == +1) {
|
|
vAxMid[i] = ( bHead ? vAxStart[i] : vAxEnd[i]) ;
|
|
}
|
|
}
|
|
// tutto bene
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::OneMoveTestCollisionAvoid( const STRVECTOR& vAxName, const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd,
|
|
Machine* pMch, const INTVECTOR& vRawId, const INTVECTOR& vFxtId) const
|
|
{
|
|
// Parti di tavola da considerare per la collisione
|
|
INTVECTOR vTabCollId ;
|
|
pMch->GetCurrTableCollGroups( vTabCollId) ;
|
|
// Parti di testa e collegati da considerare per la collisione
|
|
INTVECTOR vHeadCollId ;
|
|
pMch->GetCurrHeadCollGroups( vHeadCollId) ;
|
|
if ( vHeadCollId.empty())
|
|
return true ;
|
|
// Recupero utensili veri e propri e li nascondo
|
|
INTVECTOR vCutterId ;
|
|
int nExitId = m_pGeomDB->GetFirstGroupInGroup( vHeadCollId[0]) ;
|
|
while ( nExitId != GDB_ID_NULL) {
|
|
int nExitStat ;
|
|
if ( m_pGeomDB->GetStatus( nExitId, nExitStat) && nExitStat != GDB_ST_OFF) {
|
|
int nToolId = m_pGeomDB->GetFirstGroupInGroup( nExitId) ;
|
|
while ( nToolId != GDB_ID_NULL) {
|
|
int nToolStat ;
|
|
if ( m_pGeomDB->GetStatus( nToolId, nToolStat) && nToolStat != GDB_ST_OFF) {
|
|
int nCutterId = m_pGeomDB->GetFirstNameInGroup( nToolId, TNA_TOOL_START + "*") ;
|
|
while ( nCutterId != GDB_ID_NULL) {
|
|
vCutterId.emplace_back( nCutterId) ;
|
|
m_pGeomDB->SetStatus( nCutterId, GDB_ST_OFF) ;
|
|
nCutterId = m_pGeomDB->GetNextName( nCutterId, TNA_TOOL_START + "*") ;
|
|
}
|
|
}
|
|
nToolId = m_pGeomDB->GetNextGroup( nToolId) ;
|
|
}
|
|
}
|
|
nExitId = m_pGeomDB->GetNextGroup( nExitId) ;
|
|
}
|
|
|
|
// distanza di sicurezza
|
|
const double TOL_SAFEDIST = 5.0 ;
|
|
double dSafeDist = GetSafeZ() ;
|
|
dSafeDist = max( dSafeDist - TOL_SAFEDIST, MIN_SAFEDIST) ;
|
|
// Vado nelle posizioni da controllare
|
|
const int COLL_STEP_MIN = 1 ;
|
|
const int COLL_STEP_STD = 16 ;
|
|
double dMoveLen = 0 ;
|
|
for ( int j = 0 ; j < int( vAxName.size()) ; ++ j) {
|
|
bool bLinear, bHead ; pMch->GetCurrAxisType( j, bLinear, bHead) ;
|
|
dMoveLen += abs( vAxEnd[j] - vAxStart[j]) * ( bLinear ? 1 : 10) ;
|
|
}
|
|
int nStep = ( dMoveLen < 10 ? COLL_STEP_MIN : COLL_STEP_STD) ;
|
|
bool bCollide = false ;
|
|
for ( int i = 0 ; i <= nStep && ! bCollide ; ++ i) {
|
|
// Imposto la posizione
|
|
double dCoeff = double( i) / nStep ;
|
|
for ( int j = 0 ; j < int( vAxName.size()) ; ++ j) {
|
|
double dPos = ( 1 - dCoeff) * vAxStart[j] + dCoeff * vAxEnd[j] ;
|
|
pMch->SetAxisPos( vAxName[j], dPos) ;
|
|
}
|
|
// eventuale emissione Log
|
|
if ( ExeGetDebugLevel() >= 5) {
|
|
string sAxes = "Test Collision at " ;
|
|
for ( int j = 0 ; j < int( vAxName.size()) ; ++ j) {
|
|
double dPos ;
|
|
pMch->GetAxisPos( vAxName[j], dPos) ;
|
|
sAxes += vAxName[j] + "=" + ToString( dPos, 1) + " " ;
|
|
}
|
|
LOG_DBG_INFO( GetEMkLogger(), sAxes.c_str()) ;
|
|
}
|
|
// Determino sottobox e box della testa e degli altri oggetti da considerare per la collisione
|
|
BOXIVECTOR vbiSH ;
|
|
BBox3d b3Head ;
|
|
for ( int nCId : vHeadCollId) {
|
|
int nHId = m_pGeomDB->GetFirstInGroup( nCId) ;
|
|
while ( nHId != GDB_ID_NULL) {
|
|
BBox3d b3Tmp ;
|
|
m_pGeomDB->GetGlobalBBox( nHId, b3Tmp, BBF_ONLY_VISIBLE) ;
|
|
if ( ! b3Tmp.IsEmpty()) {
|
|
b3Tmp.Expand( dSafeDist) ;
|
|
b3Head.Add( b3Tmp) ;
|
|
vbiSH.emplace_back( b3Tmp, nHId) ;
|
|
}
|
|
nHId = m_pGeomDB->GetNext( nHId) ;
|
|
}
|
|
// eventuale emissione Log
|
|
if ( ExeGetDebugLevel() >= 5) {
|
|
string sName ;
|
|
if ( ! m_pGeomDB->GetName( nCId, sName))
|
|
sName = "---" ;
|
|
string sOut = " " + sName + " : " + ToString( b3Head, 1) ;
|
|
LOG_DBG_INFO( GetEMkLogger(), sOut.c_str()) ;
|
|
}
|
|
}
|
|
// Aggiungo eventuali utensili veri e propri
|
|
for ( int nCutterId : vCutterId) {
|
|
BBox3d b3Tmp ;
|
|
m_pGeomDB->GetGlobalBBox( nCutterId, b3Tmp, BBF_STANDARD) ;
|
|
if ( ! b3Tmp.IsEmpty()) {
|
|
b3Tmp.Expand( dSafeDist) ;
|
|
b3Head.Add( b3Tmp) ;
|
|
vbiSH.emplace_back( b3Tmp, nCutterId) ;
|
|
}
|
|
// eventuale emissione Log
|
|
if ( ExeGetDebugLevel() >= 5) {
|
|
string sName ;
|
|
if ( ! m_pGeomDB->GetName( nCutterId, sName))
|
|
sName = "---" ;
|
|
string sOut = " " + sName + " : " + ToString( b3Head, 1) ;
|
|
LOG_DBG_INFO( GetEMkLogger(), sOut.c_str()) ;
|
|
}
|
|
}
|
|
// Li confronto con i grezzi
|
|
for ( const auto nRawId : vRawId) {
|
|
// verifico i box
|
|
BBox3d b3Raw ;
|
|
int nRawSolidId = m_pGeomDB->GetFirstNameInGroup( nRawId, MACH_RAW_SOLID) ;
|
|
m_pGeomDB->GetGlobalBBox( nRawSolidId, b3Raw) ;
|
|
// eventuale emissione Log
|
|
if ( ExeGetDebugLevel() >= 5) {
|
|
string sBox = " Raw : " + ( b3Raw.IsEmpty() ? string( "Empty Box") : ToString( b3Raw, 1)) ;
|
|
LOG_DBG_INFO( GetEMkLogger(), sBox.c_str()) ;
|
|
}
|
|
// se i box interferiscono
|
|
if ( ! b3Raw.IsEmpty() && b3Head.Overlaps( b3Raw)) {
|
|
// eventuale emissione Log
|
|
if ( ExeGetDebugLevel() >= 5) {
|
|
LOG_DBG_INFO( GetEMkLogger(), " Can collide -->") ;
|
|
}
|
|
// solido del grezzo
|
|
const ISurfTriMesh* pStm = GetSurfTriMesh( m_pGeomDB->GetGeoObj( nRawSolidId)) ;
|
|
// riferimento del grezzo
|
|
Frame3d frSolid ;
|
|
m_pGeomDB->GetGlobFrame( nRawSolidId, frSolid) ;
|
|
// verifico i sottobox
|
|
for ( const auto& biSH : vbiSH) {
|
|
if ( biSH.first.Overlaps( b3Raw)) {
|
|
bool bCutter = ( find( vCutterId.begin(), vCutterId.end(), biSH.second) != vCutterId.end()) ;
|
|
// box del solido parte della testa (se cutter è nascosto)
|
|
BBox3d b3Hsol ;
|
|
int nFlag = ( bCutter ? BBF_STANDARD : BBF_ONLY_VISIBLE) ;
|
|
m_pGeomDB->GetLocalBBox( biSH.second, b3Hsol, nFlag) ;
|
|
// riferimento della parte di testa
|
|
Frame3d frHsol ;
|
|
if ( m_pGeomDB->GetGdbType( biSH.second) == GDB_TY_GEO)
|
|
m_pGeomDB->GetGlobFrame( biSH.second, frHsol) ;
|
|
else
|
|
m_pGeomDB->GetGroupGlobFrame( biSH.second, frHsol) ;
|
|
// se cutter, ruoto riferimento di -90deg attorno a Xlocale (per compensare quanto fatto in LoadTool) quindi box di +90deg
|
|
if ( bCutter) {
|
|
b3Hsol.Rotate( ORIG, X_AX, 0, 1) ;
|
|
frHsol.Rotate( frHsol.Orig(), frHsol.VersX(), 0, -1) ;
|
|
}
|
|
// dati geometrici del box
|
|
Point3d ptMin ;
|
|
Vector3d vtDiag ;
|
|
b3Hsol.GetMinDim( ptMin, vtDiag.x, vtDiag.y, vtDiag.z) ;
|
|
// lo porto nel riferimento del grezzo
|
|
frHsol.ToLoc( frSolid) ;
|
|
// se utensile cilindrico (faccia XY quadra e origine del riferimento nel suo centro)
|
|
if ( abs( vtDiag.x - vtDiag.y) < 100 * EPS_SMALL &&
|
|
abs( vtDiag.x / 2 + ptMin.x) < 100 * EPS_SMALL &&
|
|
abs( vtDiag.y / 2 + ptMin.y) < 100 * EPS_SMALL) {
|
|
// traslo il riferimento per avere il cilindro centrato sull'asse Y in positivo
|
|
frHsol.Translate( frHsol.VersZ() * ptMin.z) ;
|
|
// verifica di collisione tra cilindro e solido
|
|
if ( pStm == nullptr || CDeCylClosedSurfTm( frHsol, max( vtDiag.x, vtDiag.y) / 2, vtDiag.z, *pStm, dSafeDist)) {
|
|
bCollide = true ;
|
|
break ;
|
|
}
|
|
}
|
|
// altrimenti
|
|
else {
|
|
// traslo il riferimento per avere il box nel primo ottante
|
|
frHsol.Translate( frHsol.VersX() * ptMin.x + frHsol.VersY() * ptMin.y + frHsol.VersZ() * ptMin.z) ;
|
|
// verifica di collisione tra box e solido
|
|
if ( pStm == nullptr || CDeBoxClosedSurfTm( frHsol, vtDiag, *pStm, dSafeDist)) {
|
|
bCollide = true ;
|
|
break ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
if ( bCollide)
|
|
break ;
|
|
}
|
|
}
|
|
// eventuale emissione Log
|
|
if ( ExeGetDebugLevel() >= 5) {
|
|
LOG_DBG_INFO( GetEMkLogger(), ( bCollide ? " COLLIDE" : " avoid")) ;
|
|
}
|
|
// Se non trovata collisione, li confronto con oggetti da verificare della tavola
|
|
if ( ! bCollide) {
|
|
for ( const auto nTabCollId : vTabCollId) {
|
|
// verifico i box
|
|
BBox3d b3TabColl ;
|
|
int nFlag = BBF_ONLY_VISIBLE | BBF_IGNORE_TEXT | BBF_IGNORE_DIM ;
|
|
m_pGeomDB->GetGlobalBBox( nTabCollId, b3TabColl, nFlag) ;
|
|
// eventuale emissione Log
|
|
if ( ExeGetDebugLevel() >= 5) {
|
|
string sBox = " TabColl : " + ( b3TabColl.IsEmpty() ? string( "Empty Box") : ToString( b3TabColl, 1)) ;
|
|
LOG_DBG_INFO( GetEMkLogger(), sBox.c_str()) ;
|
|
}
|
|
// se i box interferiscono
|
|
if ( ! b3TabColl.IsEmpty() && b3Head.Overlaps( b3TabColl)) {
|
|
// eventuale emissione Log
|
|
if ( ExeGetDebugLevel() >= 5) {
|
|
LOG_DBG_INFO( GetEMkLogger(), " Can collide -->") ;
|
|
}
|
|
// verifico i sottobox
|
|
for ( const auto& biSH : vbiSH) {
|
|
if ( biSH.first.Overlaps( b3TabColl)) {
|
|
bCollide = true ;
|
|
break ;
|
|
}
|
|
}
|
|
if ( bCollide)
|
|
break ;
|
|
}
|
|
}
|
|
// eventuale emissione Log
|
|
if ( ExeGetDebugLevel() >= 5) {
|
|
LOG_DBG_INFO( GetEMkLogger(), ( bCollide ? " COLLIDE" : " avoid")) ;
|
|
}
|
|
}
|
|
// Se non trovata collisione, li confronto con i bloccaggi
|
|
if ( ! bCollide) {
|
|
for ( const auto nFxtId : vFxtId) {
|
|
// verifico i box
|
|
BBox3d b3Fxt ;
|
|
int nFlag = BBF_ONLY_VISIBLE | BBF_IGNORE_TEXT | BBF_IGNORE_DIM ;
|
|
m_pGeomDB->GetGlobalBBox( nFxtId, b3Fxt, nFlag) ;
|
|
// eventuale emissione Log
|
|
if ( ExeGetDebugLevel() >= 5) {
|
|
string sBox = " Fxt : " + ( b3Fxt.IsEmpty() ? string( "Empty Box") : ToString( b3Fxt, 1)) ;
|
|
LOG_DBG_INFO( GetEMkLogger(), sBox.c_str()) ;
|
|
}
|
|
// se i box interferiscono
|
|
if ( ! b3Fxt.IsEmpty() && b3Head.Overlaps( b3Fxt)) {
|
|
// eventuale emissione Log
|
|
if ( ExeGetDebugLevel() >= 5) {
|
|
LOG_DBG_INFO( GetEMkLogger(), " Can collide -->") ;
|
|
}
|
|
// verifico i sottobox
|
|
for ( const auto& biSH : vbiSH) {
|
|
if ( biSH.first.Overlaps( b3Fxt)) {
|
|
bCollide = true ;
|
|
break ;
|
|
}
|
|
}
|
|
if ( bCollide)
|
|
break ;
|
|
}
|
|
}
|
|
// eventuale emissione Log
|
|
if ( ExeGetDebugLevel() >= 5) {
|
|
LOG_DBG_INFO( GetEMkLogger(), ( bCollide ? " COLLIDE" : " avoid")) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Ripristino visualizzazione utensili veri e propri
|
|
for ( int nCutterId : vCutterId)
|
|
m_pGeomDB->SetStatus( nCutterId, GDB_ST_ON) ;
|
|
|
|
return ( ! bCollide) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
int
|
|
Operation::SpecialTestCollisionAvoid( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd) const
|
|
{
|
|
// recupero la macchina corrente
|
|
Machine* pMch = ( m_pMchMgr != nullptr ? m_pMchMgr->GetCurrMachine() : nullptr) ;
|
|
if ( pMch == nullptr)
|
|
return false ;
|
|
|
|
// assi già verificati
|
|
|
|
// costanti
|
|
static const string EMC_VAR = "EMC" ; // tabella variabili locali per calcolo
|
|
static const string EVAR_ERROR = ".ERR" ; // OUT (int) codice di errore (0=ok)
|
|
static const string EVAR_RES = ".RES" ; // OUT (int) risultato (0=collide, 1=evita, 2=fare test standard)
|
|
static const string ON_SPECIAL_TESTCOLLISIONAVOID = "OnSpecialTestCollisionAvoid" ;
|
|
|
|
// verifico definizione funzione
|
|
if ( ! pMch->LuaExistsFunction( ON_SPECIAL_TESTCOLLISIONAVOID))
|
|
return SCAV_TOTEST ;
|
|
|
|
// avvio
|
|
bool bOk = true ;
|
|
int nErr = 99 ;
|
|
// imposto valori parametri
|
|
bOk = bOk && pMch->LuaCreateGlobTable( EMC_VAR) ;
|
|
bOk = bOk && pMch->LuaSetGlobVar( EMC_VAR + GVAR_TOOL, GetToolName()) ;
|
|
bOk = bOk && pMch->LuaSetGlobVar( EMC_VAR + GVAR_HEAD, GetHeadName()) ;
|
|
bOk = bOk && pMch->LuaSetGlobVar( EMC_VAR + GVAR_EXIT, GetExitNbr()) ;
|
|
bOk = bOk && pMch->LuaSetGlobVar( EMC_VAR + GVAR_TCPOS, GetToolTcPos()) ;
|
|
bOk = bOk && pMch->LuaSetGlobVar( EMC_VAR + GVAR_MCHID, GetOwner()) ;
|
|
// valori degli assi
|
|
bool bIsRobot = m_pMchMgr->GetCurrIsRobot() ;
|
|
for ( int i = 1 ; i <= int( vAxStart.size()) ; ++ i)
|
|
bOk = bOk && pMch->LuaSetGlobVar( GetGlobVarAxisPrev( i, EMC_VAR, bIsRobot), vAxStart[i-1]) ;
|
|
for ( int i = 1 ; i <= int( vAxEnd.size()) ; ++ i)
|
|
bOk = bOk && pMch->LuaSetGlobVar( GetGlobVarAxisValue( i, EMC_VAR, bIsRobot), vAxEnd[i-1]) ;
|
|
// eseguo
|
|
bOk = bOk && pMch->LuaCallFunction( ON_SPECIAL_TESTCOLLISIONAVOID, false) ;
|
|
// recupero valori parametri obbligatori
|
|
bOk = bOk && pMch->LuaGetGlobVar( EMC_VAR + EVAR_ERROR, nErr) ;
|
|
int nRes = SCAV_TOTEST ;
|
|
bOk = bOk && pMch->LuaGetGlobVar( EMC_VAR + EVAR_RES, nRes) ;
|
|
// reset
|
|
bOk = bOk && pMch->LuaResetGlobVar( EMC_VAR) ;
|
|
// segnalo errori
|
|
if ( nErr != 0) {
|
|
bOk = false ;
|
|
string sOut = " Error in " + ON_SPECIAL_TESTCOLLISIONAVOID + " (" + ToString( nErr) + ")" ;
|
|
LOG_ERROR( GetEMkLogger(), sOut.c_str())
|
|
}
|
|
return ( nErr != 0 ? SCAV_ERROR : nRes) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::SpecialMoveZup( DBLVECTOR& vAx, Vector3d& vtTool, int& nFlag, int& nFlag2, bool& bModif)
|
|
{
|
|
// recupero la macchina corrente
|
|
Machine* pMch = ( m_pMchMgr != nullptr ? m_pMchMgr->GetCurrMachine() : nullptr) ;
|
|
if ( pMch == nullptr)
|
|
return false ;
|
|
// costanti
|
|
static const string EMC_VAR = "EMC" ; // tabella variabili locali per calcolo
|
|
static const string EVAR_ERROR = ".ERR" ; // OUT (int) codice di errore ( 0 = ok)
|
|
static const string EVAR_MODIF = ".MODIF" ; // OUT (bool) flag di modifica effettuata
|
|
static const string ON_SPECIAL_MOVEZUP = "OnSpecialMoveZup" ;
|
|
|
|
// eseguo l'azione
|
|
if ( pMch->LuaExistsFunction( ON_SPECIAL_MOVEZUP)) {
|
|
bool bOk = true ;
|
|
int nErr = 99 ;
|
|
// imposto valori parametri
|
|
bOk = bOk && pMch->LuaCreateGlobTable( EMC_VAR) ;
|
|
bOk = bOk && pMch->LuaSetGlobVar( EMC_VAR + GVAR_FLAG, nFlag) ;
|
|
bOk = bOk && pMch->LuaSetGlobVar( EMC_VAR + GVAR_FLAG2, nFlag2) ;
|
|
bOk = bOk && pMch->LuaSetGlobVar( EMC_VAR + GVAR_TOOL, GetToolName()) ;
|
|
bOk = bOk && pMch->LuaSetGlobVar( EMC_VAR + GVAR_HEAD, GetHeadName()) ;
|
|
bOk = bOk && pMch->LuaSetGlobVar( EMC_VAR + GVAR_EXIT, GetExitNbr()) ;
|
|
bOk = bOk && pMch->LuaSetGlobVar( EMC_VAR + GVAR_TCPOS, GetToolTcPos()) ;
|
|
// direzione utensile
|
|
bOk = bOk && pMch->LuaSetGlobVar( EMC_VAR + GVAR_TDIR, vtTool) ;
|
|
// valore degli assi
|
|
bool bIsRobot = m_pMchMgr->GetCurrIsRobot() ;
|
|
int nNumAxes = int( vAx.size()) ;
|
|
for ( int i = 1 ; i <= nNumAxes ; ++ i)
|
|
bOk = bOk && pMch->LuaSetGlobVar( GetGlobVarAxisValue( i, EMC_VAR, bIsRobot), vAx[i-1]) ;
|
|
// eseguo
|
|
bOk = bOk && pMch->LuaCallFunction( ON_SPECIAL_MOVEZUP, false) ;
|
|
// recupero valori parametri obbligatori
|
|
bOk = bOk && pMch->LuaGetGlobVar( EMC_VAR + EVAR_ERROR, nErr) ;
|
|
bOk = bOk && pMch->LuaGetGlobVar( EMC_VAR + EVAR_MODIF, bModif) ;
|
|
bOk = bOk && pMch->LuaGetGlobVar( EMC_VAR + GVAR_FLAG, nFlag) ;
|
|
bOk = bOk && pMch->LuaGetGlobVar( EMC_VAR + GVAR_FLAG2, nFlag2) ;
|
|
bOk = bOk && pMch->LuaGetGlobVar( EMC_VAR + GVAR_TDIR, vtTool) ;
|
|
for ( int i = 1 ; i <= nNumAxes ; ++ i)
|
|
bOk = bOk && pMch->LuaGetGlobVar( GetGlobVarAxisValue( i, EMC_VAR, bIsRobot), vAx[i-1]) ;
|
|
// reset
|
|
bOk = bOk && pMch->LuaResetGlobVar( EMC_VAR) ;
|
|
// segnalo errori
|
|
if ( nErr != 0) {
|
|
bOk = false ;
|
|
string sOut = " Error in " + ON_SPECIAL_MOVEZUP + " (" + ToString( nErr) + ")" ;
|
|
LOG_ERROR( GetEMkLogger(), sOut.c_str())
|
|
}
|
|
return bOk ;
|
|
}
|
|
else {
|
|
bModif = false ;
|
|
return true ;
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::SpecialMoveRapid( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd, DBLVECTOR& vAxNew, bool& bModif)
|
|
{
|
|
// recupero la macchina corrente
|
|
Machine* pMch = ( m_pMchMgr != nullptr ? m_pMchMgr->GetCurrMachine() : nullptr) ;
|
|
if ( pMch == nullptr)
|
|
return false ;
|
|
// costanti
|
|
static const string EMC_VAR = "EMC" ; // tabella variabili locali per calcolo
|
|
static const string EVAR_ERROR = ".ERR" ; // OUT (int) codice di errore ( 0 = ok)
|
|
static const string EVAR_MODIF = ".MODIF" ; // OUT (bool) flag di modifica effettuata
|
|
static const string ON_SPECIAL_MOVERAPID = "OnSpecialMoveRapid" ;
|
|
|
|
// eseguo l'azione
|
|
if ( pMch->LuaExistsFunction( ON_SPECIAL_MOVERAPID)) {
|
|
bool bOk = true ;
|
|
int nErr = 99 ;
|
|
// imposto valori parametri
|
|
bOk = bOk && pMch->LuaCreateGlobTable( EMC_VAR) ;
|
|
bOk = bOk && pMch->LuaSetGlobVar( EMC_VAR + GVAR_TOOL, GetToolName()) ;
|
|
bOk = bOk && pMch->LuaSetGlobVar( EMC_VAR + GVAR_HEAD, GetHeadName()) ;
|
|
bOk = bOk && pMch->LuaSetGlobVar( EMC_VAR + GVAR_EXIT, GetExitNbr()) ;
|
|
bOk = bOk && pMch->LuaSetGlobVar( EMC_VAR + GVAR_TCPOS, GetToolTcPos()) ;
|
|
// valore degli assi
|
|
bool bIsRobot = m_pMchMgr->GetCurrIsRobot() ;
|
|
int nNumAxes = int( vAxStart.size()) ;
|
|
for ( int i = 1 ; i <= nNumAxes ; ++ i)
|
|
bOk = bOk && pMch->LuaSetGlobVar( GetGlobVarAxisPrev( i, EMC_VAR, bIsRobot), vAxStart[i-1]) ;
|
|
for ( int i = 1 ; i <= nNumAxes ; ++ i)
|
|
bOk = bOk && pMch->LuaSetGlobVar( GetGlobVarAxisValue( i, EMC_VAR, bIsRobot), vAxEnd[i-1]) ;
|
|
// eseguo
|
|
bOk = bOk && pMch->LuaCallFunction( ON_SPECIAL_MOVERAPID, false) ;
|
|
// recupero valori parametri obbligatori
|
|
bOk = bOk && pMch->LuaGetGlobVar( EMC_VAR + EVAR_ERROR, nErr) ;
|
|
bOk = bOk && pMch->LuaGetGlobVar( EMC_VAR + EVAR_MODIF, bModif) ;
|
|
vAxNew.resize( nNumAxes) ;
|
|
for ( int i = 1 ; i <= nNumAxes ; ++ i)
|
|
bOk = bOk && pMch->LuaGetGlobVar( GetGlobVarAxisValue( i, EMC_VAR, bIsRobot), vAxNew[i-1]) ;
|
|
// reset
|
|
bOk = bOk && pMch->LuaResetGlobVar( EMC_VAR) ;
|
|
// segnalo errori
|
|
if ( nErr != 0) {
|
|
bOk = false ;
|
|
string sOut = " Error in " + ON_SPECIAL_MOVERAPID + " (" + ToString( nErr) + ")" ;
|
|
LOG_ERROR( GetEMkLogger(), sOut.c_str())
|
|
}
|
|
return bOk ;
|
|
}
|
|
else {
|
|
bModif = false ;
|
|
return true ;
|
|
}
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::GetAggrBottomData( const string& sHead, AggrBottom& agbData) const
|
|
{
|
|
// inizializzo la struttura
|
|
agbData.Clear() ;
|
|
// verifiche
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// recupero identificativo della testa
|
|
int nHeadId = m_pMchMgr->GetHeadId( sHead) ;
|
|
if ( nHeadId == GDB_ID_NULL)
|
|
return false ;
|
|
// leggo i dati
|
|
m_pGeomDB->GetInfo( nHeadId, MCH_AGB_TYPE, agbData.nType) ;
|
|
m_pGeomDB->GetInfo( nHeadId, MCH_AGB_DMAX, agbData.dDMax) ;
|
|
m_pGeomDB->GetInfo( nHeadId, MCH_AGB_ENCH, agbData.dEncH) ;
|
|
m_pGeomDB->GetInfo( nHeadId, MCH_AGB_ENCV, agbData.dEncV) ;
|
|
m_pGeomDB->GetInfo( nHeadId, MCH_AGB_MDIR, agbData.vtMDir) ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Operation::IsAggrBottom( const string& sHead) const
|
|
{
|
|
// verifiche
|
|
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
|
|
return false ;
|
|
// recupero identificativo della testa
|
|
int nHeadId = m_pMchMgr->GetHeadId( sHead) ;
|
|
if ( nHeadId == GDB_ID_NULL)
|
|
return false ;
|
|
// recupero info se aggregato
|
|
int nAggrBott = 0 ;
|
|
return ( m_pGeomDB->GetInfo( nHeadId, MCH_AGB_TYPE, nAggrBott) && nAggrBott != 0) ;
|
|
}
|