diff --git a/src/Mod/Path/App/CMakeLists.txt b/src/Mod/Path/App/CMakeLists.txt index 32fc5b19ae..2cee9a3a3a 100644 --- a/src/Mod/Path/App/CMakeLists.txt +++ b/src/Mod/Path/App/CMakeLists.txt @@ -88,6 +88,8 @@ SET(Path_SRCS ParamsHelper.h FeatureArea.cpp FeatureArea.h + PathSegmentWalker.h + PathSegmentWalker.cpp ${Mod_SRCS} ${Python_SRCS} ) diff --git a/src/Mod/Path/App/Path.cpp b/src/Mod/Path/App/Path.cpp index b67013e754..a5e51efcf9 100644 --- a/src/Mod/Path/App/Path.cpp +++ b/src/Mod/Path/App/Path.cpp @@ -39,6 +39,7 @@ //#include "Mod/Robot/App/kdl_cp/utilities/error.h" #include "Path.h" +#include using namespace Path; using namespace Base; @@ -145,6 +146,78 @@ double Toolpath::getLength() return l; } +class BoundBoxSegmentVisitor : public PathSegmentVisitor +{ +public: + BoundBoxSegmentVisitor() + { } + + virtual void g0(int id, const Base::Vector3d &last, const Base::Vector3d &next, const std::deque &pts) + { + (void)id; + processPt(last); + processPts(pts); + processPt(next); + } + virtual void g1(int id, const Base::Vector3d &last, const Base::Vector3d &next, const std::deque &pts) + { + (void)id; + processPt(last); + processPts(pts); + processPt(next); + } + virtual void g23(int id, const Base::Vector3d &last, const Base::Vector3d &next, const std::deque &pts, const Base::Vector3d ¢er) + { + (void)id; + (void)center; + processPt(last); + processPts(pts); + processPt(next); + } + virtual void g8x(int id, const Base::Vector3d &last, const Base::Vector3d &next, const std::deque &pts, + const std::deque &p, const std::deque &q) + { + (void)id; + (void)q; // always within the bounds of p + processPt(last); + processPts(pts); + processPts(p); + processPt(next); + } + virtual void g38(int id, const Base::Vector3d &last, const Base::Vector3d &next) + { + (void)id; + processPt(last); + processPt(next); + } + + Base::BoundBox3d bb; + +private: + void processPts(const std::deque &pts) { + for (std::deque::const_iterator it=pts.begin(); pts.end() != it; ++it) { + processPt(*it); + } + } + void processPt(const Base::Vector3d &pt) { + bb.MaxX = std::max(bb.MaxX, pt.x); + bb.MinX = std::min(bb.MinX, pt.x); + bb.MaxY = std::max(bb.MaxY, pt.y); + bb.MinY = std::min(bb.MinY, pt.y); + bb.MaxZ = std::max(bb.MaxZ, pt.z); + bb.MinZ = std::min(bb.MinZ, pt.z); + } +}; + +Base::BoundBox3d Toolpath::getBoundBox() const +{ + BoundBoxSegmentVisitor visitor; + PathSegmentWalker walker(*this); + walker.walk(visitor, Vector3d(0, 0, 0)); + + return visitor.bb; +} + static void bulkAddCommand(const std::string &gcodestr, std::vector &commands, bool &inches) { Command *cmd = new Command(); diff --git a/src/Mod/Path/App/Path.h b/src/Mod/Path/App/Path.h index 16319627b5..24e1023312 100644 --- a/src/Mod/Path/App/Path.h +++ b/src/Mod/Path/App/Path.h @@ -27,6 +27,7 @@ #include "Command.h" //#include "Mod/Robot/App/kdl_cp/path_composite.hpp" //#include "Mod/Robot/App/kdl_cp/frames_io.hpp" +#include #include #include @@ -62,6 +63,7 @@ namespace Path void recalculate(void); // recalculates the points void setFromGCode(const std::string); // sets the path from the contents of the given GCode string std::string toGCode(void) const; // gets a gcode string representation from the Path + Base::BoundBox3d getBoundBox(void) const; // shortcut functions unsigned int getSize(void) const { return vpcCommands.size(); } diff --git a/src/Mod/Path/App/PathPy.xml b/src/Mod/Path/App/PathPy.xml index 22c6722255..4440ea2379 100644 --- a/src/Mod/Path/App/PathPy.xml +++ b/src/Mod/Path/App/PathPy.xml @@ -40,6 +40,12 @@ commands (optional) is a list of Path commands + + + the extent of this path + + + adds a command or a list of commands at the end of the path diff --git a/src/Mod/Path/App/PathPyImp.cpp b/src/Mod/Path/App/PathPyImp.cpp index 2757d96913..5df6b892ba 100644 --- a/src/Mod/Path/App/PathPyImp.cpp +++ b/src/Mod/Path/App/PathPyImp.cpp @@ -29,6 +29,7 @@ #include "PathPy.h" #include "PathPy.cpp" +#include "Base/BoundBoxPy.h" #include "Base/GeometryPyCXX.h" #include "CommandPy.h" @@ -128,6 +129,11 @@ Py::Long PathPy::getSize(void) const return Py::Long((long)getToolpathPtr()->getSize()); } +Py::Object PathPy::getBoundBox(void) const +{ + return Py::BoundingBox(getToolpathPtr()->getBoundBox()); +} + // specific methods PyObject* PathPy::copy(PyObject * args) diff --git a/src/Mod/Path/App/PathSegmentWalker.cpp b/src/Mod/Path/App/PathSegmentWalker.cpp new file mode 100644 index 0000000000..e9ac092e5a --- /dev/null +++ b/src/Mod/Path/App/PathSegmentWalker.cpp @@ -0,0 +1,359 @@ +/* + *************************************************************************** + * * + * Copyright (c) 2019 sliptonic * + * * + * This program is free software; you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License (LGPL) * + * as published by the Free Software Foundation; either version 2 of * + * the License, or (at your option) any later version. * + * for detail see the LICENCE text file. * + * * + * This program is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Library General Public License for more details. * + * * + * You should have received a copy of the GNU Library General Public * + * License along with this program; if not, write to the Free Software * + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 * + * USA * + * * + *************************************************************************** + */ +#include "PathSegmentWalker.h" + +#include +#include + +#include + +#define ARC_MIN_SEGMENTS 20.0 // minimum # segments to interpolate an arc + +#ifndef M_PI + #define M_PI 3.14159265358979323846 + #define M_PI 3.14159265358979323846 /* pi */ +#endif + +#ifndef M_PI_2 + #define M_PI_2 1.57079632679489661923 /* pi/2 */ +#endif + + +namespace Path +{ + +Base::Vector3d compensateRotation(const Base::Vector3d &pt, const Base::Rotation &rot, const Base::Vector3d ¢er) +{ + Base::Vector3d ptRotated; + rot.multVec(pt - center, ptRotated); + return ptRotated + center; +} + +Base::Rotation yawPitchRoll(double a, double b, double c) +{ + Base::Rotation rot; + rot.setYawPitchRoll(-c, -b, -a); + return rot; +} + +PathSegmentVisitor::~PathSegmentVisitor() +{ +} + +void PathSegmentVisitor::setup(const Base::Vector3d &last) +{ + (void)last; +} + +void PathSegmentVisitor::g0(int id, const Base::Vector3d &last, const Base::Vector3d &next, const std::deque &pts) +{ + (void)id; + (void)last; + (void)next; + (void)pts; +} + +void PathSegmentVisitor::g1(int id, const Base::Vector3d &last, const Base::Vector3d &next, const std::deque &pts) +{ + (void)id; + (void)last; + (void)next; + (void)pts; +} + +void PathSegmentVisitor::g23(int id, const Base::Vector3d &last, const Base::Vector3d &next, const std::deque &pts, const Base::Vector3d ¢er) +{ + (void)id; + (void)last; + (void)next; + (void)pts; + (void)center; +} + +void PathSegmentVisitor::g8x(int id, const Base::Vector3d &last, const Base::Vector3d &next, const std::deque &pts, + const std::deque &p, const std::deque &q) +{ + (void)id; + (void)last; + (void)next; + (void)pts; + (void)p; + (void)q; +} + +void PathSegmentVisitor::g38(int id, const Base::Vector3d &last, const Base::Vector3d &next) +{ + (void)id; + (void)last; + (void)next; +} + +PathSegmentWalker::PathSegmentWalker(const Toolpath &tp_) + :tp(tp_) +{} + + +void PathSegmentWalker::walk(PathSegmentVisitor &cb, const Base::Vector3d &startPosition) +{ + if(tp.getSize()==0) { + return; + } + + ParameterGrp::handle hGrp = App::GetApplication().GetParameterGroupByPath("User parameter:BaseApp/Preferences/Mod/Part"); + float deviation = hGrp->GetFloat("MeshDeviation",0.2); + + Base::Vector3d rotCenter = tp.getCenter(); + Base::Vector3d last(startPosition); + Base::Rotation lrot; + double A = 0.0; + double B = 0.0; + double C = 0.0; + + bool absolute = true; + bool absolutecenter = false; + + // for mapping the coordinates to XY plane + double Base::Vector3d::*pz = &Base::Vector3d::z; + + cb.setup(last); + + for (unsigned int i = 0; i < tp.getSize(); i++) { + std::deque points; + + const Path::Command &cmd = tp.getCommand(i); + const std::string &name = cmd.Name; + Base::Vector3d next = cmd.getPlacement().getPosition(); + double a = A; + double b = B; + double c = C; + + if (!absolute) + next = last + next; + if (!cmd.has("X")) next.x = last.x; + if (!cmd.has("Y")) next.y = last.y; + if (!cmd.has("Z")) next.z = last.z; + if ( cmd.has("A")) a = cmd.getValue("A"); + if ( cmd.has("B")) b = cmd.getValue("B"); + if ( cmd.has("C")) c = cmd.getValue("C"); + + Base::Rotation nrot = yawPitchRoll(a, b, c); + + Base::Vector3d rnext = compensateRotation(next, nrot, rotCenter); + + if ( (name == "G0") || (name == "G00") || (name == "G1") || (name == "G01") ) { + // straight line + if (nrot != lrot) { + double amax = std::max(fmod(fabs(a - A), 360), std::max(fmod(fabs(b - B), 360), fmod(fabs(c - C), 360))); + double angle = amax / 180 * M_PI; + int segments = std::max(ARC_MIN_SEGMENTS, 3.0/(deviation/angle)); + + double da = (a - A) / segments; + double db = (b - B) / segments; + double dc = (c - C) / segments; + + Base::Vector3d dnext = (next - last) / segments; + + for (int j = 1; j < segments; j++) { + Base::Vector3d inter = last + dnext * j; + + Base::Rotation rot = yawPitchRoll(A + da*j, B + db*j, C + dc*j); + Base::Vector3d rinter = compensateRotation(inter, rot, rotCenter); + + points.push_back(rinter); + } + } + + if ("G0" == name || "G00" == name) { + cb.g0(i, last, rnext, points); + } else { + cb.g1(i, last, rnext, points); + } + + last = next; + A = a; + B = b; + C = c; + lrot = nrot; + + } else if ( (name == "G2") || (name == "G02") || (name == "G3") || (name == "G03") ) { + // arc + Base::Vector3d norm; + Base::Vector3d center; + + if ( (name == "G2") || (name == "G02") ) + norm.*pz = -1.0; + else + norm.*pz = 1.0; + + if (absolutecenter) + center = cmd.getCenter(); + else + center = (last + cmd.getCenter()); + Base::Vector3d next0(next); + next0.*pz = 0.0; + Base::Vector3d last0(last); + last0.*pz = 0.0; + Base::Vector3d center0(center); + center0.*pz = 0.0; + //double radius = (last - center).Length(); + double angle = (next0 - center0).GetAngle(last0 - center0); + // GetAngle will always return the minor angle. Switch if needed + Base::Vector3d anorm = (last0 - center0) % (next0 - center0); + if (anorm.*pz < 0) { + if(name == "G3" || name == "G03") + angle = M_PI * 2 - angle; + } else if(anorm.*pz > 0) { + if(name == "G2" || name == "G02") + angle = M_PI * 2 - angle; + } else if (angle == 0) + angle = M_PI * 2; + + double amax = std::max(fmod(fabs(a - A), 360), std::max(fmod(fabs(b - B), 360), fmod(fabs(c - C), 360))); + + int segments = std::max(ARC_MIN_SEGMENTS, 3.0/(deviation/std::max(angle, amax))); //we use a rather simple rule here, provisorily + double dZ = (next.*pz - last.*pz)/segments; //How far each segment will helix in Z + + double dangle = angle/segments; + double da = (a - A) / segments; + double db = (b - B) / segments; + double dc = (c - C) / segments; + + for (int j = 1; j < segments; j++) { + Base::Vector3d inter; + Base::Rotation rot(norm, dangle*j); + rot.multVec((last0 - center0), inter); + inter.*pz = last.*pz + dZ * j; //Enable displaying helices + + Base::Rotation arot = yawPitchRoll(A + da*j, B + db*j, C + dc*j); + Base::Vector3d rinter = compensateRotation(center0 + inter, arot, rotCenter); + + points.push_back(rinter); + } + + cb.g23(i, last, rnext, points, center); + + last = next; + A = a; + B = b; + C = c; + lrot = nrot; + + } else if (name == "G90") { + // absolute mode + absolute = true; + + } else if (name == "G91") { + // relative mode + absolute = false; + + } else if (name == "G90.1") { + // absolute mode + absolutecenter = true; + + } else if (name == "G91.1") { + // relative mode + absolutecenter = false; + + } else if ((name=="G81")||(name=="G82")||(name=="G83")||(name=="G84")||(name=="G85")||(name=="G86")||(name=="G89")){ + // drill,tap,bore + double r = 0; + if (cmd.has("R")) + r = cmd.getValue("R"); + + std::deque plist; + std::deque qlist; + + Base::Vector3d p1(next); + p1.*pz = last.*pz; + + if (nrot != lrot) { + double amax = std::max(fmod(fabs(a - A), 360), std::max(fmod(fabs(b - B), 360), fmod(fabs(c - C), 360))); + double angle = amax / 180 * M_PI; + int segments = std::max(ARC_MIN_SEGMENTS, 3.0/(deviation/angle)); + + double da = (a - A) / segments; + double db = (b - B) / segments; + double dc = (c - C) / segments; + + Base::Vector3d dnext = (p1 - last) / segments; + + for (int j = 1; j < segments; j++) { + Base::Vector3d inter = last + dnext * j; + + Base::Rotation rot = yawPitchRoll(A + da*j, B + db*j, C + dc*j); + Base::Vector3d rinter = compensateRotation(inter, rot, rotCenter); + + points.push_back(rinter); + } + } + + Base::Vector3d p1r = compensateRotation(p1, nrot, rotCenter); + Base::Vector3d p2(next); + p2.*pz = r; + Base::Vector3d p2r = compensateRotation(p2, nrot, rotCenter); + + double q; + if (cmd.has("Q")) { + q = cmd.getValue("Q"); + if (q>0) { + Base::Vector3d temp(next); + for(temp.*pz=r;temp.*pz>next.*pz;temp.*pz-=q) { + Base::Vector3d pr = compensateRotation(temp, nrot, rotCenter); + qlist.push_back(pr); + } + } + } + + Base::Vector3d p3(next); + p3.*pz = last.*pz; + Base::Vector3d p3r = compensateRotation(p3, nrot, rotCenter); + + plist.push_back(p1r); + plist.push_back(p2r); + plist.push_back(p3r); + + cb.g8x(i, last, next, points, plist, qlist); + + last = p3; + A = a; + B = b; + C = c; + lrot = nrot; + + + } else if ((name=="G38.2")||(name=="38.3")||(name=="G38.4")||(name=="G38.5")){ + // Straight probe + cb.g38(i, last, next); + } else if(name=="G17") { + pz = &Base::Vector3d::z; + } else if(name=="G18") { + pz = &Base::Vector3d::y; + } else if(name=="G19") { + pz = &Base::Vector3d::x; + } + } +} + + +} diff --git a/src/Mod/Path/App/PathSegmentWalker.h b/src/Mod/Path/App/PathSegmentWalker.h new file mode 100644 index 0000000000..50299f14fc --- /dev/null +++ b/src/Mod/Path/App/PathSegmentWalker.h @@ -0,0 +1,69 @@ +/* + *************************************************************************** + * * + * Copyright (c) 2019 sliptonic * + * * + * This program is free software; you can redistribute it and/or modify * + * it under the terms of the GNU Lesser General Public License (LGPL) * + * as published by the Free Software Foundation; either version 2 of * + * the License, or (at your option) any later version. * + * for detail see the LICENCE text file. * + * * + * This program is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Library General Public License for more details. * + * * + * You should have received a copy of the GNU Library General Public * + * License along with this program; if not, write to the Free Software * + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 * + * USA * + * * + *************************************************************************** + */ +#include "PreCompiled.h" +#include + +#include + +namespace Path +{ + +/** + * PathSegmentVisitor is the companion class to PathSegmentWalker. Its members are called + * with the segmented points of each command. + */ +class PathSegmentVisitor +{ + public: + virtual ~PathSegmentVisitor(); + + virtual void setup(const Base::Vector3d &last); + + virtual void g0(int id, const Base::Vector3d &last, const Base::Vector3d &next, const std::deque &pts); + virtual void g1(int id, const Base::Vector3d &last, const Base::Vector3d &next, const std::deque &pts); + virtual void g23(int id, const Base::Vector3d &last, const Base::Vector3d &next, const std::deque &pts, const Base::Vector3d ¢er); + virtual void g8x(int id, const Base::Vector3d &last, const Base::Vector3d &next, const std::deque &pts, + const std::deque &p, const std::deque &q); + virtual void g38(int id, const Base::Vector3d &last, const Base::Vector3d &next); +}; + +/** + * PathSegmentWalker processes a path a splits all movement commands into straight segments and calls the + * appropriate member of the provided PathSegmentVisitor. + * All non-movement commands are processed accordingly if they affect the movement commands. + */ +class PathSegmentWalker +{ +public: + PathSegmentWalker(const Toolpath &tp_); + + + void walk(PathSegmentVisitor &cb, const Base::Vector3d &startPosition); + +private: + const Toolpath &tp; +}; + + +} diff --git a/src/Mod/Path/Gui/ViewProviderPath.cpp b/src/Mod/Path/Gui/ViewProviderPath.cpp index e7b14e9a10..abcb92a99a 100644 --- a/src/Mod/Path/Gui/ViewProviderPath.cpp +++ b/src/Mod/Path/Gui/ViewProviderPath.cpp @@ -49,6 +49,7 @@ #include #include +#include #include #include #include @@ -61,18 +62,6 @@ #include #include - -#define ARC_MIN_SEGMENTS 20.0 // minimum # segments to interpolate an arc - -#ifndef M_PI - #define M_PI 3.14159265358979323846 - #define M_PI 3.14159265358979323846 /* pi */ -#endif - -#ifndef M_PI_2 - #define M_PI_2 1.57079632679489661923 /* pi/2 */ -#endif - using namespace Gui; using namespace PathGui; using namespace Path; @@ -463,19 +452,146 @@ void ViewProviderPath::hideSelection() { pcArrowSwitch->whichChild = -1; } -Base::Vector3d compensateRotation(const Base::Vector3d &pt, const Base::Rotation &rot, const Base::Vector3d ¢er) +class VisualPathSegmentVisitor +: public PathSegmentVisitor { - Base::Vector3d ptRotated; - rot.multVec(pt - center, ptRotated); - return ptRotated + center; -} +public: + VisualPathSegmentVisitor( + const Toolpath &tp, + SoCoordinate3 *pcLineCoords_, + SoCoordinate3 *pcMarkerCoords_, + std::vector &command2Edge_, + std::deque &edge2Command_, + std::deque &edgeIndices_, + std::vector &colorindex_, + std::deque &points_, + std::deque &markers_) + : pcLineCoords(pcLineCoords_) + , pcMarkerCoords(pcMarkerCoords_) + , command2Edge(command2Edge_) + , edge2Command(edge2Command_) + , edgeIndices(edgeIndices_) + , colorindex(colorindex_) + , points(points_) + , markers(markers_) + { + pcLineCoords->point.deleteValues(0); + pcMarkerCoords->point.deleteValues(0); -Base::Rotation yawPitchRoll(double a, double b, double c) -{ - Base::Rotation rot; - rot.setYawPitchRoll(-c, -b, -a); - return rot; -} + command2Edge.clear(); + edge2Command.clear(); + edgeIndices.clear(); + + colorindex.clear(); + + command2Edge.resize(tp.getSize(),-1); + } + + virtual void setup(const Base::Vector3d &last) + { + points.push_back(last); + markers.push_back(last); + } + + virtual void g0(int id, const Base::Vector3d &last, const Base::Vector3d &next, const std::deque &pts) + { + (void)last; + gx(id, &next, pts, 0); + } + + virtual void g1(int id, const Base::Vector3d &last, const Base::Vector3d &next, const std::deque &pts) + { + (void)last; + gx(id, &next, pts, 1); + } + + virtual void g23(int id, const Base::Vector3d &last, const Base::Vector3d &next, const std::deque &pts, const Base::Vector3d ¢er) + { + (void)last; + gx(id, &next, pts, 1); + markers.push_back(center); + } + + virtual void g8x(int id, const Base::Vector3d &last, const Base::Vector3d &next, const std::deque &pts, + const std::deque &p, const std::deque &q) + { + (void)last; + + gx(id, NULL, pts, 0); + + points.push_back(p[0]); + markers.push_back(p[0]); + colorindex.push_back(0); + + points.push_back(p[1]); + markers.push_back(p[1]); + colorindex.push_back(0); + + points.push_back(next); + markers.push_back(next); + colorindex.push_back(1); + + for (std::deque::const_iterator it=q.begin(); q.end() != it; ++it) { + markers.push_back(*it); + } + + points.push_back(p[2]); + markers.push_back(p[2]); + colorindex.push_back(0); + + pushCommand(id); + } + + virtual void g38(int id, const Base::Vector3d &last, const Base::Vector3d &next) + { + Base::Vector3d p1(next.x,next.y,last.z); + points.push_back(p1); + colorindex.push_back(0); + + points.push_back(next); + colorindex.push_back(2); + + Base::Vector3d p3(next.x,next.y,last.z); + points.push_back(p3); + colorindex.push_back(0); + + pushCommand(id); + } + +private: + SoCoordinate3 *pcLineCoords; + SoCoordinate3 *pcMarkerCoords; + + std::vector &command2Edge; + std::deque &edge2Command; + std::deque &edgeIndices; + + std::vector &colorindex; + std::deque &points; + std::deque &markers; + + virtual void gx(int id, const Base::Vector3d *next, const std::deque &pts, int color) + { + for (std::deque::const_iterator it=pts.begin(); pts.end() != it; ++it) { + points.push_back(*it); + colorindex.push_back(color); + } + + if (next != NULL) { + points.push_back(*next); + markers.push_back(*next); + colorindex.push_back(color); + + pushCommand(id); + } + } + + void pushCommand(int id) { + command2Edge[id] = edgeIndices.size(); + edgeIndices.push_back(points.size()); + edge2Command.push_back(id); + } +}; void ViewProviderPath::updateVisual(bool rebuild) { @@ -486,282 +602,25 @@ void ViewProviderPath::updateVisual(bool rebuild) { pcLines->coordIndex.deleteValues(0); if(rebuild) { - pcLineCoords->point.deleteValues(0); - pcMarkerCoords->point.deleteValues(0); - - command2Edge.clear(); - edge2Command.clear(); - edgeIndices.clear(); - Path::Feature* pcPathObj = static_cast(pcObject); const Toolpath &tp = pcPathObj->Path.getValue(); - if(tp.getSize()==0) { - return; - } - ParameterGrp::handle hGrp = App::GetApplication().GetParameterGroupByPath("User parameter:BaseApp/Preferences/Mod/Part"); - float deviation = hGrp->GetFloat("MeshDeviation",0.2); std::deque points; std::deque markers; - Base::Vector3d rotCenter = tp.getCenter(); - Base::Vector3d last(StartPosition.getValue()); - Base::Rotation lrot; - double A = 0.0; - double B = 0.0; - double C = 0.0; + VisualPathSegmentVisitor collect(tp, + pcLineCoords, + pcMarkerCoords, + command2Edge, + edge2Command, + edgeIndices, + colorindex, + points, + markers); - colorindex.clear(); - bool absolute = true; - bool absolutecenter = false; + PathSegmentWalker segments(tp); + segments.walk(collect, StartPosition.getValue()); - // for mapping the coordinates to XY plane - double Base::Vector3d::*pz = &Base::Vector3d::z; - - command2Edge.resize(tp.getSize(),-1); - - points.push_back(last); - markers.push_back(last); // startpoint of path - - for (unsigned int i = 0; i < tp.getSize(); i++) { - const Path::Command &cmd = tp.getCommand(i); - const std::string &name = cmd.Name; - Base::Vector3d next = cmd.getPlacement().getPosition(); - double a = A; - double b = B; - double c = C; - - if (!absolute) - next = last + next; - if (!cmd.has("X")) next.x = last.x; - if (!cmd.has("Y")) next.y = last.y; - if (!cmd.has("Z")) next.z = last.z; - if ( cmd.has("A")) a = cmd.getValue("A"); - if ( cmd.has("B")) b = cmd.getValue("B"); - if ( cmd.has("C")) c = cmd.getValue("C"); - - Base::Rotation nrot = yawPitchRoll(a, b, c); - - Base::Vector3d rnext = compensateRotation(next, nrot, rotCenter); - - if ( (name == "G0") || (name == "G00") || (name == "G1") || (name == "G01") ) { - // straight line - int color = ((name == "G0") || (name == "G00")) ? 0 : 1; - if (nrot != lrot) { - double amax = std::max(fmod(fabs(a - A), 360), std::max(fmod(fabs(b - B), 360), fmod(fabs(c - C), 360))); - double angle = amax / 180 * M_PI; - int segments = std::max(ARC_MIN_SEGMENTS, 3.0/(deviation/angle)); - - double da = (a - A) / segments; - double db = (b - B) / segments; - double dc = (c - C) / segments; - - Base::Vector3d dnext = (next - last) / segments; - - for (int j = 1; j < segments; j++) { - Base::Vector3d inter = last + dnext * j; - - Base::Rotation rot = yawPitchRoll(A + da*j, B + db*j, C + dc*j); - Base::Vector3d rinter = compensateRotation(inter, rot, rotCenter); - - points.push_back(rinter); - colorindex.push_back(color); - } - } - points.push_back(rnext); - markers.push_back(rnext); // endpoint - colorindex.push_back(color); // std color - - command2Edge[i] = edgeIndices.size(); - edgeIndices.push_back(points.size()); - edge2Command.push_back(i); - - last = next; - A = a; - B = b; - C = c; - lrot = nrot; - - } else if ( (name == "G2") || (name == "G02") || (name == "G3") || (name == "G03") ) { - // arc - Base::Vector3d norm; - Base::Vector3d center; - - if ( (name == "G2") || (name == "G02") ) - norm.*pz = -1.0; - else - norm.*pz = 1.0; - - if (absolutecenter) - center = cmd.getCenter(); - else - center = (last + cmd.getCenter()); - Base::Vector3d next0(next); - next0.*pz = 0.0; - Base::Vector3d last0(last); - last0.*pz = 0.0; - Base::Vector3d center0(center); - center0.*pz = 0.0; - //double radius = (last - center).Length(); - double angle = (next0 - center0).GetAngle(last0 - center0); - // GetAngle will always return the minor angle. Switch if needed - Base::Vector3d anorm = (last0 - center0) % (next0 - center0); - if (anorm.*pz < 0) { - if(name == "G3" || name == "G03") - angle = M_PI * 2 - angle; - } else if(anorm.*pz > 0) { - if(name == "G2" || name == "G02") - angle = M_PI * 2 - angle; - } else if (angle == 0) - angle = M_PI * 2; - - double amax = std::max(fmod(fabs(a - A), 360), std::max(fmod(fabs(b - B), 360), fmod(fabs(c - C), 360))); - - int segments = std::max(ARC_MIN_SEGMENTS, 3.0/(deviation/std::max(angle, amax))); //we use a rather simple rule here, provisorily - double dZ = (next.*pz - last.*pz)/segments; //How far each segment will helix in Z - - double dangle = angle/segments; - double da = (a - A) / segments; - double db = (b - B) / segments; - double dc = (c - C) / segments; - - for (int j = 1; j < segments; j++) { - Base::Vector3d inter; - Base::Rotation rot(norm, dangle*j); - rot.multVec((last0 - center0), inter); - inter.*pz = last.*pz + dZ * j; //Enable displaying helices - - Base::Rotation arot = yawPitchRoll(A + da*j, B + db*j, C + dc*j); - Base::Vector3d rinter = compensateRotation(center0 + inter, arot, rotCenter); - - points.push_back(rinter); - colorindex.push_back(1); - } - - points.push_back(rnext); - markers.push_back(rnext); // endpoint - markers.push_back(center); // add a marker at center too - - colorindex.push_back(1); - command2Edge[i] = edgeIndices.size(); - edgeIndices.push_back(points.size()); - edge2Command.push_back(i); - - last = next; - A = a; - B = b; - C = c; - lrot = nrot; - - } else if (name == "G90") { - // absolute mode - absolute = true; - - } else if (name == "G91") { - // relative mode - absolute = false; - - } else if (name == "G90.1") { - // absolute mode - absolutecenter = true; - - } else if (name == "G91.1") { - // relative mode - absolutecenter = false; - - } else if ((name=="G81")||(name=="G82")||(name=="G83")||(name=="G84")||(name=="G85")||(name=="G86")||(name=="G89")){ - // drill,tap,bore - double r = 0; - if (cmd.has("R")) - r = cmd.getValue("R"); - - Base::Vector3d p1(next); - p1.*pz = last.*pz; - - if (nrot != lrot) { - double amax = std::max(fmod(fabs(a - A), 360), std::max(fmod(fabs(b - B), 360), fmod(fabs(c - C), 360))); - double angle = amax / 180 * M_PI; - int segments = std::max(ARC_MIN_SEGMENTS, 3.0/(deviation/angle)); - - double da = (a - A) / segments; - double db = (b - B) / segments; - double dc = (c - C) / segments; - - Base::Vector3d dnext = (p1 - last) / segments; - - for (int j = 1; j < segments; j++) { - Base::Vector3d inter = last + dnext * j; - - Base::Rotation rot = yawPitchRoll(A + da*j, B + db*j, C + dc*j); - Base::Vector3d rinter = compensateRotation(inter, rot, rotCenter); - - points.push_back(rinter); - colorindex.push_back(0); - } - } - - Base::Vector3d p1r = compensateRotation(p1, nrot, rotCenter); - points.push_back(p1r); - markers.push_back(p1r); - colorindex.push_back(0); - Base::Vector3d p2(next); - p2.*pz = r; - Base::Vector3d p2r = compensateRotation(p2, nrot, rotCenter); - points.push_back(p2r); - markers.push_back(p2r); - colorindex.push_back(0); - points.push_back(rnext); - markers.push_back(rnext); - colorindex.push_back(1); - double q; - if (cmd.has("Q")) { - q = cmd.getValue("Q"); - if (q>0) { - Base::Vector3d temp(next); - for(temp.*pz=r;temp.*pz>next.*pz;temp.*pz-=q) { - Base::Vector3d pr = compensateRotation(temp, nrot, rotCenter); - markers.push_back(pr); - } - } - } - Base::Vector3d p3(next); - p3.*pz = last.*pz; - Base::Vector3d p3r = compensateRotation(p3, nrot, rotCenter); - points.push_back(p3r); - markers.push_back(p2r); - colorindex.push_back(0); - command2Edge[i] = edgeIndices.size(); - edgeIndices.push_back(points.size()); - edge2Command.push_back(i); - - last = p3; - A = a; - B = b; - C = c; - lrot = nrot; - - - } else if ((name=="G38.2")||(name=="38.3")||(name=="G38.4")||(name=="G38.5")){ - // Straight probe - Base::Vector3d p1(next.x,next.y,last.z); - points.push_back(p1); - colorindex.push_back(0); - points.push_back(next); - colorindex.push_back(2); - Base::Vector3d p3(next.x,next.y,last.z); - points.push_back(p3); - colorindex.push_back(0); - command2Edge[i] = edgeIndices.size(); - edgeIndices.push_back(points.size()); - edge2Command.push_back(i); - } else if(name=="G17") { - pz = &Base::Vector3d::z; - } else if(name=="G18") { - pz = &Base::Vector3d::y; - } else if(name=="G19") { - pz = &Base::Vector3d::x; - } - } if (!edgeIndices.empty()) { pcLineCoords->point.setNum(points.size());