diff --git a/EgtMachKernel.rc b/EgtMachKernel.rc index e249b96..99c6cda 100644 Binary files a/EgtMachKernel.rc and b/EgtMachKernel.rc differ diff --git a/Machine.h b/Machine.h index 7648861..b510b64 100644 --- a/Machine.h +++ b/Machine.h @@ -385,6 +385,7 @@ class Machine static int LuaEmtRemoveClimb( lua_State* L) ; static int LuaEmtAddRise( lua_State* L) ; static int LuaEmtRemoveRise( lua_State* L) ; + static int LuaEmtAdjustConcavePartsInPath( lua_State* L) ; static int LuaEmtGetMoveType( lua_State* L) ; static int LuaEmtGetAxesPos( lua_State* L) ; static int LuaEmtGetBackAuxDir( lua_State* L) ; diff --git a/MachineLua.cpp b/MachineLua.cpp index ba6c000..4d48808 100644 --- a/MachineLua.cpp +++ b/MachineLua.cpp @@ -128,6 +128,7 @@ Machine::LuaInit( const string& sMachineName) m_LuaMgr.RegisterFunction( "EmtGetInitialAxesPos", Machine::LuaEmtGetInitialAxesPos) ; m_LuaMgr.RegisterFunction( "EmtGetFinalAxesPos", Machine::LuaEmtGetFinalAxesPos) ; m_LuaMgr.RegisterFunction( "EmtGetCurrAxesHomePos", Machine::LuaEmtGetCurrAxesHomePos) ; + m_LuaMgr.RegisterFunction( "EmtAdjustConcavePartsInPath", Machine::LuaEmtAdjustConcavePartsInPath) ; // registro le funzioni di lettura entità CL per lua m_LuaMgr.RegisterFunction( "EmtGetMoveType", Machine::LuaEmtGetMoveType) ; m_LuaMgr.RegisterFunction( "EmtGetAxesPos", Machine::LuaEmtGetAxesPos) ; diff --git a/MachineLuaCL.cpp b/MachineLuaCL.cpp index 2605dd6..5142d93 100644 --- a/MachineLuaCL.cpp +++ b/MachineLuaCL.cpp @@ -19,12 +19,16 @@ #include "Operation.h" #include "/EgtDev/Include/EXeExecutor.h" #include "/EgtDev/Include/EGkGeoPoint3d.h" +#include "/EgtDev/Include/EGkGeoVector3d.h" #include "/EgtDev/Include/EGkCurveLine.h" #include "/EgtDev/Include/EGkCurveArc.h" +#include "/EgtDev/Include/EGkIntersLineCylinder.h" #include "/EgtDev/Include/EGkGeomDB.h" #include "/EgtDev/Include/EGkLuaAux.h" +#include "/EgtDev/Include/EGkStringUtils3d.h" #include "/EgtDev/Include/EGnStringUtils.h" #include "/EgtDev/Include/EGnFileUtils.h" +#include "/EgtDev/Include/ENkPolynomialRoots.h" #include "/EgtDev/Include/EgtPointerOwner.h" using namespace std ; @@ -153,6 +157,41 @@ Machine::LuaEmtAddRapidMove( lua_State* L) return 1 ; } +//---------------------------------------------------------------------------- +int +AddLinearMove( const Point3d& ptIni, const Point3d& ptFin, IGeomDB* pGeomDB , int nPathId, const Vector3d& vtTool, + const Vector3d& vtCorr, const Vector3d& vtAux, double dFeed, int nFlag, int nFlag2, bool bToolShow, int nRefId = GDB_ID_NULL) +{ + // creo oggetto linea per DB geometrico + PtrOwner pLine( CreateCurveLine()) ; + bool bOk = ! IsNull( pLine) ; + // assegno gli estremi della linea + bOk = bOk && pLine->Set( ptIni, ptFin) ; + // inserisco l'oggetto nel DB geometrico + int nId = ( bOk ? pGeomDB->AddGeoObj( GDB_ID_NULL, nPathId, Release( pLine)) : GDB_ID_NULL) ; + bOk = bOk && ( nId != GDB_ID_NULL) ; + if ( bOk && nRefId != GDB_ID_NULL) + pGeomDB->RelocateGlob( nId, nRefId, GDB_AFTER) ; + // creo oggetto dati Cam + PtrOwner pCam( new( nothrow) CamData) ; + bOk = bOk && ! IsNull( pCam) ; + if ( bOk) { + // assegno valori + pCam->SetMoveType( 1) ; + pCam->SetToolDir( vtTool) ; + pCam->SetCorrDir( vtCorr) ; + pCam->SetAuxDir( vtAux) ; + pCam->SetEndPoint( ptFin) ; + pCam->SetFeed( dFeed) ; + pCam->SetFlag( nFlag) ; + pCam->SetFlag2( nFlag2) ; + pCam->SetToolShow( bToolShow) ; + // associo questo oggetto a quello geometrico + pGeomDB->SetUserObj( nId, Release( pCam)) ; + } + return nId ; +} + //---------------------------------------------------------------------------- int Machine::LuaEmtAddLinearMove( lua_State* L) @@ -185,31 +224,8 @@ Machine::LuaEmtAddLinearMove( lua_State* L) if ( m_pMchLua == nullptr || m_pMchLua->m_pMchMgr == nullptr || m_pMchLua->m_pGeomDB == nullptr) return luaL_error( L, " Unknown Machine") ; - // creo oggetto linea per DB geometrico - PtrOwner pLine( CreateCurveLine()) ; - bool bOk = ! IsNull( pLine) ; - // assegno gli estremi della linea - bOk = bOk && pLine->Set( ptIni, ptFin) ; - // inserisco l'oggetto nel DB geometrico - int nId = ( bOk ? m_pMchLua->m_pGeomDB->AddGeoObj( GDB_ID_NULL, nPathId, Release( pLine)) : GDB_ID_NULL) ; - bOk = bOk && ( nId != GDB_ID_NULL) ; - // creo oggetto dati Cam - PtrOwner pCam( new( nothrow) CamData) ; - bOk = bOk && ! IsNull( pCam) ; - if ( bOk) { - // assegno valori - pCam->SetMoveType( 1) ; - pCam->SetToolDir( vtTool) ; - pCam->SetCorrDir( vtCorr) ; - pCam->SetAuxDir( vtAux) ; - pCam->SetEndPoint( ptFin) ; - pCam->SetFeed( dFeed) ; - pCam->SetFlag( nFlag) ; - pCam->SetFlag2( nFlag2) ; - pCam->SetToolShow( bToolShow) ; - // associo questo oggetto a quello geometrico - m_pMchLua->m_pGeomDB->SetUserObj( nId, Release( pCam)) ; - } + int nId = AddLinearMove( ptIni, ptFin, m_pMchLua->m_pGeomDB, nPathId, vtTool, vtCorr, vtAux, dFeed, nFlag, nFlag2, bToolShow) ; + bool bOk = nId != GDB_ID_NULL ; // assegno risultato if ( bOk) LuaSetParam( L, nId) ; @@ -581,3 +597,221 @@ Machine::LuaEmtGetBackAuxDir( lua_State* L) return 1 ; } + +struct Cyl { + Cyl( void): frCyl( GLOB_FRM), dH( 0.), dRad( 0.) {;} ; + Cyl( const Frame3d& _frCyl, double _dH, double _dRad, double _dLinTol) : + frCyl( _frCyl), dH( _dH), dRad( _dRad) { ;} + Cyl( const Point3d& _ptBase, const Vector3d& vtZ, double _dH, double _dRad, double _dLinTol) : + dH( _dH), dRad( _dRad){ + frCyl.Set( _ptBase, vtZ); } +public : + Frame3d frCyl ; +public: + double dH ; + double dRad ; +}; + +typedef vector OFFSETCYLVECT ; + +//---------------------------------------------------------------------------- +bool +IsPointInsideCylinder( const Point3d& ptTest, const Cyl& offCyl) +{ + Point3d ptTestLoc = ptTest ; ptTestLoc.ToLoc( offCyl.frCyl) ; + if ( ptTestLoc.z > offCyl.dH || ptTestLoc.z < 0) + return false ; + double dDist = ptTestLoc.x * ptTestLoc.x + ptTestLoc.y * ptTestLoc.y ; + double dRadSq = offCyl.dRad * offCyl.dRad ; + if ( dDist > dRadSq) + return false ; + return true ; +} + +//---------------------------------------------------------------------------- +int +Machine::LuaEmtAdjustConcavePartsInPath( lua_State* L) +{ + bool bOk = true ; + // 3 parametri : nPathId, nAuxPathId, dRad + int nPathId ; + LuaCheckParam( L, 1, nPathId) + int nAuxPathId ; + LuaCheckParam( L, 2, nAuxPathId) + double dRad ; + LuaCheckParam( L, 3, dRad) + LuaClearStack( L) ; + IGeomDB* pGeomDB = m_pMchLua->m_pGeomDB ; + int nId = pGeomDB->GetFirstInGroup( nPathId) ; + int nIdCrvAux = pGeomDB->GetFirstInGroup( nAuxPathId) ; + const ICurve* pCrvAux = GetCurve( pGeomDB->GetGeoObj(nIdCrvAux)) ; + const double dLinTol = 5 * EPS_SMALL ; + + while ( nId != GDB_ID_NULL) { + if ( pGeomDB->GetGeoType( nId) == CRV_LINE) { + int nFlag = 0 ; pGeomDB->GetInfo( nId, "Flg2", nFlag) ; + if ( nFlag == 1) { + // scorro i prossimi finchè trovo la fine della zona concava + INTINTVECTOR vLines ; + while ( nFlag == 1 && nId != GDB_ID_NULL) { + nId = pGeomDB->GetNext( nId) ; + if ( nId != GDB_ID_NULL && pGeomDB->GetGeoType( nId) == CRV_LINE) { + int nDerivedFromId = GDB_ID_NULL ; + if ( pGeomDB->GetInfo( nId, "DerivFrom", nDerivedFromId)) { + pGeomDB->GetInfo( nId, "Flg2", nFlag) ; + vLines.emplace_back( nId, nDerivedFromId) ; + } + } + } + OFFSETCYLVECT vCyl ; + // creo un cilindro della dimensione del raggio + for ( int i = 0 ; i < ssize( vLines) ; ++i) { + + const IGeoVector3d* pGV = GetGeoVector3d( pGeomDB->GetGeoObj( vLines[i].second)) ; + if ( pGV == nullptr) + return false ; + Point3d ptBase = pGV->GetBase() ; + double dPar = 0 ; + if ( ! pCrvAux->GetParamAtPoint( ptBase, dPar)) + return false ; + + dPar = round( dPar) ; + if ( dPar > 0) { + Point3d ptStart, ptEnd ; + Vector3d vtHeight ; + pCrvAux->GetPointD1D2( dPar, ICurve::FROM_MINUS, ptEnd, &vtHeight) ; + pCrvAux->GetPointD1D2( dPar - 1, ICurve::FROM_MINUS, ptStart) ; + vtHeight = ptEnd - ptStart ; + double dHeight = vtHeight.Len() ; + // questa altezza dovrebbe coincidere con quella precedentemente calcolata come direzione della linea + vtHeight.Normalize() ; + vCyl.emplace_back( ptStart, vtHeight, dHeight, dRad, dLinTol) ; + } + } + + // controllo l'end di ogni linea per verificare se sta nel cilindro definito da uno degli altri tratti + // controllo tutto i punti + bool bErasedSomePart = false ; + bool bErasedPrev = false ; + INTINTVECTOR vInters ; + for ( int i = 0 ; i < ssize( vLines) ; ++i) { + Point3d ptStart, ptEnd ; + const ICurveLine* pCL = GetCurveLine( pGeomDB->GetGeoObj( vLines[i].first)) ; + if ( pCL == nullptr) + return false ; + pCL->GetEndPoint( ptEnd) ; + pCL->GetStartPoint( ptStart) ; + // se stanno in uno dei cilindri degli altri tratti della zona concava + for ( int j = 0 ; j < ssize( vLines) ; ++j) { + if ( i == j) + continue ; + bool bToErase = IsPointInsideCylinder( ptEnd, vCyl[j]) ; + if ( bErasedPrev && ! bToErase) + bToErase = bToErase || IsPointInsideCylinder( ptStart, vCyl[j]) ; + if ( bToErase) { + bErasedSomePart = true ; + bErasedPrev = true ; + vInters.emplace_back(vLines[i].first,i) ; + vInters.emplace_back(vLines[i+1].first,i+1) ; + ++i ; + break ; + } + else + bErasedPrev = false ; + } + } + if ( bErasedSomePart) { + // calcolo le intersezioni effettive del primo e ultimo tratto cancellati con i cilindri che li hanno cancellati + // controllo che effettivamente tutti i tratti cancellati siano consecutivi + for ( int i = 1 ; i < ssize( vInters) ; ++i) { + if ( vInters[i].first != vInters[i-1].first + 1) + return false ; + } + for ( int i = 0 ; i < ssize( vInters) ; ++i) { + // cancello i tratti intermedi + if ( i > 0 && i < ssize( vInters) - 1) { + pGeomDB->Erase( vInters[i].first) ; + continue ; + } + // per il primo e ultimo controllo le intersezioni con tutti i cilindri + ICurveLine* pCL = GetCurveLine( pGeomDB->GetGeoObj( vInters[i].first)) ; + Point3d ptStart = pCL->GetStart() ; + Vector3d vtStart ; pCL->GetStartDir( vtStart) ; + double dLen ; pCL->GetLength( dLen) ; + double dUTrim = ( i == 0 ? INFINITO : 0) ; + Point3d ptTrim = P_INVALID ; + for ( int j = 0 ; j < ssize( vCyl) ; ++j) { + if ( vInters[i].second == j) + continue ; + Point3d ptInt1 = P_INVALID, ptInt2 = P_INVALID ; + double dU1, dU2 ; + Vector3d vtN1, vtN2 ; + if ( IntersLineCyl( ptStart, vtStart * dLen, vCyl[j].frCyl, vCyl[j].dH, vCyl[j].dRad, dU1, ptInt1, vtN1, dU2, ptInt2, vtN2)) { + bool bUpdate = ( i == 0 ? dU1 < dUTrim : dU1 > dUTrim) ; + bUpdate = bUpdate && ptInt1.IsValid() && dU1 > 0 && dU1 < 1 ; + bUpdate = bUpdate && vtN1 * vtStart < 0 ; + if ( bUpdate) { + dUTrim = dU1 ; + ptTrim = ptInt1 ; + } + bUpdate = ( i == 0 ? dU2 < dUTrim : dU2 > dUTrim) ; + bUpdate = bUpdate && ptInt2.IsValid() && dU2 > 0 && dU2 < 1 ; + bUpdate = bUpdate && vtN2 * vtStart > 0 ; + if ( bUpdate) { + dUTrim = dU2 ; + ptTrim = ptInt2 ; + } + } + } + if ( ptTrim.IsValid()) { + if ( i == 0) { + pCL->ModifyEnd( ptTrim) ; + double dNewLen ; pCL->GetLength( dNewLen) ; + if ( dNewLen < 0.1) { + int nPrev = pGeomDB->GetPrev( vInters[0].first) ; + pGeomDB->Erase( vInters[0].first) ; + vInters[0].first = nPrev ; + ICurveLine* pCLPrev = GetCurveLine( pGeomDB->GetGeoObj( nPrev)) ; + pCLPrev->ModifyEnd( ptTrim) ; + } + CamData* camData = GetCamData( pGeomDB->GetUserObj( vInters[0].first)) ; + camData->SetEndPoint( ptTrim) ; + } + else { + pCL->ModifyStart( ptTrim) ; + double dNewLen ; pCL->GetLength( dNewLen) ; + if ( dNewLen < 0.1) { + int nNext = pGeomDB->GetNext( vInters[i].first) ; + pGeomDB->Erase( vInters[i].first) ; + vInters[i].first = nNext ; + ICurveLine* pCLNext = GetCurveLine( pGeomDB->GetGeoObj( nNext)) ; + pCLNext->ModifyStart( ptTrim) ; + } + } + } + } + // giunto questi due punti + ICurveLine* pCLprev = GetCurveLine( pGeomDB->GetGeoObj( vInters[0].first)) ; + Point3d ptIni = pCLprev->GetEnd() ; + ICurveLine* pCLnext = GetCurveLine( pGeomDB->GetGeoObj( vInters.back().first)) ; + Point3d ptFin = pCLnext->GetStart() ; + const CamData* camDataPrev = GetCamData( pGeomDB->GetUserObj( vInters[0].first)) ; + const CamData* camDataNext = GetCamData( pGeomDB->GetUserObj( vInters.back().first)) ; + Vector3d vtTool = Media( camDataPrev->GetToolDir(), camDataNext->GetToolDir()) ; + Vector3d vtCorr = Media( camDataPrev->GetCorrDir(), camDataNext->GetCorrDir()) ; + Vector3d vtAux = Media( camDataPrev->GetAuxDir(), camDataNext->GetAuxDir()) ; + double dFeed = camDataPrev->GetFeed() ; + int nFlag = camDataPrev->GetFlag() ; + int nFlag2 = camDataPrev->GetFlag2() ; + bool bToolShow = camDataPrev->GetToolShow() ; + AddLinearMove( ptIni, ptFin, pGeomDB, nPathId, vtTool, vtCorr, vtAux, dFeed, nFlag, nFlag2, bToolShow, vInters[0].first) ; + } + nId = vLines.back().first ; + } + } + nId = m_pMchLua->m_pGeomDB->GetNext( nId) ; + } + + LuaSetParam( L, bOk) ; + return 1 ; +} \ No newline at end of file