From 8b8ad9af44b2d693b93ff834a8d062bd0567d569 Mon Sep 17 00:00:00 2001 From: Daniele Bariletti Date: Fri, 5 Jun 2026 12:07:38 +0200 Subject: [PATCH] EgtMachKernel : - aggiunta gestione per angoli interni smooth. --- Machine.h | 1 + MachineLua.cpp | 1 + MachineLuaCL.cpp | 431 ++++++++++++++++++++++++++++++++++++++++++++--- 3 files changed, 408 insertions(+), 25 deletions(-) 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..c9019a2 100644 --- a/MachineLuaCL.cpp +++ b/MachineLuaCL.cpp @@ -19,12 +19,15 @@ #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/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 +156,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 +223,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 +596,369 @@ Machine::LuaEmtGetBackAuxDir( lua_State* L) return 1 ; } + +struct Cyl { + Frame3d frCyl ; + double dH ; + double dRad ; + Cyl( void): frCyl( GLOB_FRM), dH( 0.), dRad( 0.) {;} ; + Cyl( const Frame3d& _frCyl, double _dH, double _dRad) : + frCyl( _frCyl), dH( _dH), dRad( _dRad) { ;} +}; + +typedef vector OFFSETCYLVECT ; + +//---------------------------------------------------------------------------- +bool +IsPointInsideCylinder( const Point3d& ptTest, const Cyl& offCyl, double dLinTol = EPS_SMALL) +{ + 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 - dLinTol) * (offCyl.dRad - dLinTol) ; + if ( dDist > dRadSq) + return false ; + return true ; +} + +//---------------------------------------------------------------------------- +bool +IntersLineCyl( const Point3d& ptLineSt, const Vector3d& vtLineDir, + const Cyl& Cyl, double dLinTol, + Point3d& ptInt1, double& dU1,Vector3d& vtN1, Point3d& ptInt2, double& dU2, Vector3d& vtN2) +{ + // Porto la linea nel riferimento del cilindro + Point3d ptP = GetToLoc( ptLineSt, Cyl.frCyl) ; + Vector3d vtV = GetToLoc( vtLineDir, Cyl.frCyl) ; + + // Determino le eventuali intersezioni con le due basi a quota minima e massima (solo se linea non parallela ad esse) + int nBasInt = 0 ; + if ( abs( vtV.z) > EPS_ZERO) { + // le linee tangenti al cilindro non sono considerate intersecanti + double dEpsRad = dLinTol ; + ptInt1 = ptP + ( ( 0 - ptP.z) / vtV.z) * vtV ; + if ( ptInt1.x * ptInt1.x + ptInt1.y * ptInt1.y < ( Cyl.dRad - dEpsRad) * ( Cyl.dRad - dEpsRad)) { + nBasInt += 1 ; + dU1 = ( ( 0 - ptP.z) / vtV.z) ; + vtN1 = - Z_AX ; + } + ptInt2 = ptP + ( ( Cyl.dH - ptP.z) / vtV.z) * vtV ; + if ( ptInt2.x * ptInt2.x + ptInt2.y * ptInt2.y < ( Cyl.dRad - dEpsRad) * ( Cyl.dRad - dEpsRad)) { + nBasInt += 2 ; + dU2 = ( ( Cyl.dH - ptP.z) / vtV.z) ; + vtN2 = Z_AX ; + } + } + + // Se la linea interseca entrambe le basi, si sono trovate le due intersezioni + if ( nBasInt == 3) { + // Porto i punti e i versori nel riferimento globale + ptInt1.ToGlob( Cyl.frCyl) ; + vtN1.ToGlob( Cyl.frCyl) ; + ptInt2.ToGlob( Cyl.frCyl) ; + vtN2.ToGlob( Cyl.frCyl) ; + // Trovate intersezioni + return true ; + } + + // Determino le intersezioni con la superficie laterale del cilindro + DBLVECTOR vdCoeff{ ptP.x * ptP.x + ptP.y * ptP.y - Cyl.dRad * Cyl.dRad, + 2 * ( ptP.x * vtV.x + ptP.y * vtV.y), + vtV.x * vtV.x + vtV.y * vtV.y} ; + DBLVECTOR vdRoots ; + int nRoot = PolynomialRoots( 2, vdCoeff, vdRoots) ; + + // Elimino le soluzioni cha danno intersezioni fuori dai limiti in Z del cilindro + if ( nRoot == 2) { + double dIntZ2 = ptP.z + vdRoots[1] * vtV.z ; + if ( dIntZ2 < 0 + dLinTol || dIntZ2 > Cyl.dH - dLinTol) + -- nRoot ; + else + dU2 = vdRoots[1] ; + } + if ( nRoot >= 1) { + double dIntZ1 = ptP.z + vdRoots[0] * vtV.z ; + if ( dIntZ1 < 0 + dLinTol || dIntZ1 > Cyl.dH - dLinTol) { + if ( nRoot == 2) + vdRoots[0] = vdRoots[1] ; + -- nRoot ; + } + else + dU1 = vdRoots[0] ; + } + + // Due soluzioni: la retta interseca due volte la superficie laterale + if ( nRoot == 2) { + // Punti di intersezione con la superficie del cilindro + ptInt1 = ptP + vdRoots[0] * vtV ; + ptInt2 = ptP + vdRoots[1] * vtV ; + // Determino le normali + vtN1.Set( ptInt1.x, ptInt1.y, 0) ; + vtN1.Normalize() ; + vtN2.Set( ptInt2.x, ptInt2.y, 0) ; + vtN2.Normalize() ; + // Porto i punti e i versori nel riferimento globale + ptInt1.ToGlob( Cyl.frCyl) ; + vtN1.ToGlob( Cyl.frCyl) ; + ptInt2.ToGlob( Cyl.frCyl) ; + vtN2.ToGlob( Cyl.frCyl) ; + // Trovate intersezioni + return true ; + } + + // Una soluzione : la retta interseca la superficie laterale e un piano + else if ( nRoot == 1) { + // Se piano superiore + if ( nBasInt == 2) { + // Punto di intersezione + ptInt1 = ptP + vdRoots[0] * vtV ; + // Normale alla superficie del cilindro + vtN1.Set( ptInt1.x, ptInt1.y, 0) ; + vtN1.Normalize() ; + } + // altrimenti piano inferiore + else if ( nBasInt == 1) { + // Punto di intersezione + ptInt2 = ptP + vdRoots[0] * vtV ; + // Normale alla superficie del cilindro + vtN2.Set( ptInt2.x, ptInt2.y, 0) ; + vtN2.Normalize() ; + } + // altrimenti niente + else + return false ; + // Porto i punti e i versori nel riferimento globale + ptInt1.ToGlob( Cyl.frCyl) ; + vtN1.ToGlob( Cyl.frCyl) ; + ptInt2.ToGlob( Cyl.frCyl) ; + vtN2.ToGlob( Cyl.frCyl) ; + // Trovate intersezioni + return true ; + } + else + return false ; +} + +//---------------------------------------------------------------------------- +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 ; + + ////debug + //int nParent = pGeomDB->GetParentId( nPathId) ; + //int nTempLay = pGeomDB->AddGroup( GDB_ID_NULL, nParent, GLOB_FRM) ; + ////debug + 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) { + //debug + pGeomDB->SetMaterial(vLines[i].first, GREEN) ; + //debug + + 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) { + vCyl.emplace_back() ; + 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.back().frCyl.Set( ptStart, vtHeight) ; + vCyl.back().dH = dHeight ; + vCyl.back().dRad = dRad ; + + ////debug + //if ( dPar > 116) { + // pGeomDB->SetGridFrame( vCyl.back().frCyl) ; + // int nIdCrv = ExeCreateCircle( nTempLay, ORIG, dRad, GDB_ID_GRID) ; + // ExeCreateSurfTmByExtrusion( nTempLay, {nIdCrv}, Z_AX * dHeight, 0.005, GDB_ID_GRID) ; + // pGeomDB->Erase( nIdCrv) ; + // + //} + ////debug + } + } + ////debug + //pGeomDB->SetGridFrame( GLOB_FRM) ; + + // 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], dLinTol) ; + if ( bErasedPrev && ! bToErase) + bToErase = bToErase || IsPointInsideCylinder( ptStart, vCyl[j]) ; + if ( bToErase) { + //// devo eliminare il corrente e anche il successivo + //pGeomDB->Erase( vLines[i].first) ; + //if ( i < ssize( vLines) - 1) + // pGeomDB->Erase( vLines[i+1].first) ; + + //debug + //coloro solo i tratti che cancello + pGeomDB->SetMaterial( vLines[i].first, YELLOW) ; + pGeomDB->SetMaterial( vLines[i+1].first, YELLOW) ; + //debug + + 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], dLinTol, ptInt1, dU1, vtN1, ptInt2, dU2, 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) ; + } + pGeomDB->SetMaterial( vInters[0].first, RED) ; + 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) ; + } + pGeomDB->SetMaterial( vInters[i].first, RED) ; + } + } + } + // 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