kindred/create
1
1
Fork 0
Code Issues 30 Pull Requests Actions Packages Projects 9 Releases 2 Wiki Activity

Merge pull request #2565 from mlampert/feature/Path-BoundBox

Browse Source 
Path: add BoundBox property to Path object
This commit is contained in:
sliptonic
2019-10-02 08:17:26 -05:00
committed by GitHub
parent d2be4f115d e3232169ab
commit cee9f4ff40
8 changed files with 667 additions and 291 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/Mod/Path/App/CMakeLists.txt
View File

@@ -88,6 +88,8 @@ SET(Path_SRCS
ParamsHelper.h
FeatureArea.cpp
FeatureArea.h
PathSegmentWalker.h
PathSegmentWalker.cpp
${Mod_SRCS}
${Python_SRCS}
)

73
src/Mod/Path/App/Path.cpp
View File

@@ -39,6 +39,7 @@
//#include "Mod/Robot/App/kdl_cp/utilities/error.h"
#include "Path.h"
#include <Mod/Path/App/PathSegmentWalker.h>
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<Base::Vector3d> &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<Base::Vector3d> &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<Base::Vector3d> &pts, const Base::Vector3d &center)
{
(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<Base::Vector3d> &pts,
const std::deque<Base::Vector3d> &p, const std::deque<Base::Vector3d> &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<Base::Vector3d> &pts) {
for (std::deque<Base::Vector3d>::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<Command*> &commands, bool &inches)
{
Command *cmd = new Command();

2
src/Mod/Path/App/Path.h
View File

@@ -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 <Base/BoundBox.h>
#include <Base/Persistence.h>
#include <Base/Vector3D.h>
@@ -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(); }

6
src/Mod/Path/App/PathPy.xml
View File

@@ -40,6 +40,12 @@ commands (optional) is a list of Path commands</UserDocu>
</Documentation>
<Parameter Name="Center" Type="Object"/>
</Attribute>
<Attribute Name="BoundBox" ReadOnly="true">
<Documentation>
<UserDocu>the extent of this path</UserDocu>
</Documentation>
<Parameter Name="BoundBox" Type="Object"/>
</Attribute>
<Methode Name="addCommands">
<Documentation>
<UserDocu>adds a command or a list of commands at the end of the path</UserDocu>

6
src/Mod/Path/App/PathPyImp.cpp
View File

@@ -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)

359
src/Mod/Path/App/PathSegmentWalker.cpp Normal file
View File

@@ -0,0 +1,359 @@
/*
***************************************************************************
* *
* Copyright (c) 2019 sliptonic <shopinthewoods@gmail.com> *
* *
* 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 <App/Application.h>
#include <Base/Parameter.h>
#include <vector>
#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 &center)
{
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<Base::Vector3d> &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<Base::Vector3d> &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<Base::Vector3d> &pts, const Base::Vector3d &center)
{
(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<Base::Vector3d> &pts,
const std::deque<Base::Vector3d> &p, const std::deque<Base::Vector3d> &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<Base::Vector3d> 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<Base::Vector3d> plist;
std::deque<Base::Vector3d> 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;
}
}
}
}

69
src/Mod/Path/App/PathSegmentWalker.h Normal file
View File

@@ -0,0 +1,69 @@
/*
***************************************************************************
* *
* Copyright (c) 2019 sliptonic <shopinthewoods@gmail.com> *
* *
* 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 <Mod/Path/App/Path.h>
#include <deque>
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<Base::Vector3d> &pts);
virtual void g1(int id, const Base::Vector3d &last, const Base::Vector3d &next, const std::deque<Base::Vector3d> &pts);
virtual void g23(int id, const Base::Vector3d &last, const Base::Vector3d &next, const std::deque<Base::Vector3d> &pts, const Base::Vector3d &center);
virtual void g8x(int id, const Base::Vector3d &last, const Base::Vector3d &next, const std::deque<Base::Vector3d> &pts,
const std::deque<Base::Vector3d> &p, const std::deque<Base::Vector3d> &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;
};
}

441
src/Mod/Path/Gui/ViewProviderPath.cpp
View File

@@ -49,6 +49,7 @@
#include <Mod/Path/App/FeaturePath.h>
#include <Mod/Path/App/Path.h>
#include <Mod/Path/App/PathSegmentWalker.h>
#include <App/Application.h>
#include <App/Document.h>
#include <Base/FileInfo.h>
@@ -61,18 +62,6 @@
#include <Gui/SoAxisCrossKit.h>
#include <Gui/SoFCUnifiedSelection.h>
#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 &center)
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<int> &command2Edge_,
std::deque<int> &edge2Command_,
std::deque<int> &edgeIndices_,
std::vector<int> &colorindex_,
std::deque<Base::Vector3d> &points_,
std::deque<Base::Vector3d> &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<Base::Vector3d> &pts)
{
(void)last;
gx(id, &next, pts, 0);
}
virtual void g1(int id, const Base::Vector3d &last, const Base::Vector3d &next, const std::deque<Base::Vector3d> &pts)
{
(void)last;
gx(id, &next, pts, 1);
}
virtual void g23(int id, const Base::Vector3d &last, const Base::Vector3d &next, const std::deque<Base::Vector3d> &pts, const Base::Vector3d &center)
{
(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<Base::Vector3d> &pts,
const std::deque<Base::Vector3d> &p, const std::deque<Base::Vector3d> &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<Base::Vector3d>::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<int> &command2Edge;
std::deque<int> &edge2Command;
std::deque<int> &edgeIndices;
std::vector<int> &colorindex;
std::deque<Base::Vector3d> &points;
std::deque<Base::Vector3d> &markers;
virtual void gx(int id, const Base::Vector3d *next, const std::deque<Base::Vector3d> &pts, int color)
{
for (std::deque<Base::Vector3d>::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<Path::Feature*>(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<Base::Vector3d> points;
std::deque<Base::Vector3d> 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());