3DPrinting/PrusaSlicer-NonPlainar
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Lots of improvements to MotionPlanner/avoid_crossing_perimeters. Smoother paths and several edge cases now handled better

Browse Source
This commit is contained in:
Alessandro Ranellucci 2015-01-06 20:52:36 +01:00
parent 5e100abe25
commit 8f4cbefd0d
17 changed files with 386 additions and 113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
lib/Slic3r/GCode.pm
View File

@ -620,12 +620,11 @@ sub travel_to {
# represent last_pos in absolute G-code coordinates
my $last_pos = $gcodegen->last_pos->clone;
$last_pos->translate(@{$gcodegen->origin});
$last_pos->translate(@$scaled_origin);
# represent $point in absolute G-code coordinates
$point = $point->clone;
$point->translate(@$scaled_origin);
# calculate path
my $travel = $self->_external_mp->shortest_path($last_pos, $point);

27
xs/src/libslic3r/ExPolygon.cpp
View File

@ -105,6 +105,23 @@ ExPolygon::contains(const Point &point) const
return true;
}
// inclusive version of contains() that also checks whether point is on boundaries
bool
ExPolygon::contains_b(const Point &point) const
{
return this->contains(point) || this->has_boundary_point(point);
}
bool
ExPolygon::has_boundary_point(const Point &point) const
{
if (this->contour.has_boundary_point(point)) return true;
for (Polygons::const_iterator h = this->holes.begin(); h != this->holes.end(); ++h) {
if (h->has_boundary_point(point)) return true;
}
return false;
}
Polygons
ExPolygon::simplify_p(double tolerance) const
{
@ -364,6 +381,16 @@ ExPolygon::triangulate_p2t(Polygons* polygons) const
}
}
Lines
ExPolygon::lines() const
{
Lines lines;
this->contour.lines(&lines);
for (Polygons::const_iterator h = this->holes.begin(); h != this->holes.end(); ++h)
h->lines(&lines);
return lines;
}
#ifdef SLIC3RXS
REGISTER_CLASS(ExPolygon, "ExPolygon");

3
xs/src/libslic3r/ExPolygon.hpp
View File

@ -25,6 +25,8 @@ class ExPolygon
bool contains(const Line &line) const;
bool contains(const Polyline &polyline) const;
bool contains(const Point &point) const;
bool contains_b(const Point &point) const;
bool has_boundary_point(const Point &point) const;
Polygons simplify_p(double tolerance) const;
ExPolygons simplify(double tolerance) const;
void simplify(double tolerance, ExPolygons &expolygons) const;
@ -36,6 +38,7 @@ class ExPolygon
void triangulate(Polygons* polygons) const;
void triangulate_pp(Polygons* polygons) const;
void triangulate_p2t(Polygons* polygons) const;
Lines lines() const;
#ifdef SLIC3RXS
void from_SV(SV* poly_sv);

25
xs/src/libslic3r/ExPolygonCollection.cpp
View File

@ -3,6 +3,11 @@
namespace Slic3r {
ExPolygonCollection::ExPolygonCollection(const ExPolygon &expolygon)
{
this->expolygons.push_back(expolygon);
}
ExPolygonCollection::operator Points() const
{
Points points;
@ -68,6 +73,15 @@ template bool ExPolygonCollection::contains<Point>(const Point &item) const;
template bool ExPolygonCollection::contains<Line>(const Line &item) const;
template bool ExPolygonCollection::contains<Polyline>(const Polyline &item) const;
bool
ExPolygonCollection::contains_b(const Point &point) const
{
for (ExPolygons::const_iterator it = this->expolygons.begin(); it != this->expolygons.end(); ++it) {
if (it->contains_b(point)) return true;
}
return false;
}
void
ExPolygonCollection::simplify(double tolerance)
{
@ -87,6 +101,17 @@ ExPolygonCollection::convex_hull(Polygon* hull) const
Slic3r::Geometry::convex_hull(pp, hull);
}
Lines
ExPolygonCollection::lines() const
{
Lines lines;
for (ExPolygons::const_iterator it = this->expolygons.begin(); it != this->expolygons.end(); ++it) {
Lines ex_lines = it->lines();
lines.insert(lines.end(), ex_lines.begin(), ex_lines.end());
}
return lines;
}
#ifdef SLIC3RXS
REGISTER_CLASS(ExPolygonCollection, "ExPolygon::Collection");
#endif

3
xs/src/libslic3r/ExPolygonCollection.hpp
View File

@ -17,6 +17,7 @@ class ExPolygonCollection
ExPolygons expolygons;
ExPolygonCollection() {};
ExPolygonCollection(const ExPolygon &expolygon);
ExPolygonCollection(const ExPolygons &expolygons) : expolygons(expolygons) {};
operator Points() const;
operator Polygons() const;
@ -25,8 +26,10 @@ class ExPolygonCollection
void translate(double x, double y);
void rotate(double angle, const Point &center);
template <class T> bool contains(const T &item) const;
bool contains_b(const Point &point) const;
void simplify(double tolerance);
void convex_hull(Polygon* hull) const;
Lines lines() const;
};
}

7
xs/src/libslic3r/Line.cpp
View File

@ -16,6 +16,13 @@ Line::wkt() const
return ss.str();
}
Line::operator Lines() const
{
Lines lines;
lines.push_back(*this);
return lines;
}
Line::operator Polyline() const
{
Polyline pl;

4
xs/src/libslic3r/Line.hpp
View File

@ -9,6 +9,7 @@ namespace Slic3r {
class Line;
class Linef3;
class Polyline;
typedef std::vector<Line> Lines;
class Line
{
@ -18,6 +19,7 @@ class Line
Line() {};
explicit Line(Point _a, Point _b): a(_a), b(_b) {};
std::string wkt() const;
operator Lines() const;
operator Polyline() const;
void scale(double factor);
void translate(double x, double y);
@ -45,8 +47,6 @@ class Line
#endif
};
typedef std::vector<Line> Lines;
class Linef3
{
public:

232
xs/src/libslic3r/MotionPlanner.cpp
View File

@ -72,11 +72,23 @@ MotionPlanner::initialize()
this->initialized = true;
}
ExPolygonCollection
MotionPlanner::get_env(size_t island_idx) const
{
if (island_idx == -1) {
return ExPolygonCollection(this->outer);
} else {
return this->inner[island_idx];
}
}
void
MotionPlanner::shortest_path(const Point &from, const Point &to, Polyline* polyline)
{
// lazy generation of configuration space
if (!this->initialized) this->initialize();
// if we have an empty configuration space, return a straight move
if (this->islands.empty()) {
polyline->points.push_back(from);
polyline->points.push_back(to);
@ -103,71 +115,131 @@ MotionPlanner::shortest_path(const Point &from, const Point &to, Polyline* polyl
Point inner_from = from;
Point inner_to = to;
bool from_is_inside, to_is_inside;
if (island_idx == -1) {
if (!(from_is_inside = this->outer.contains(from))) {
ExPolygonCollection env = this->get_env(island_idx);
if (!(from_is_inside = env.contains(from))) {
// Find the closest inner point to start from.
from.nearest_point(this->outer, &inner_from);
inner_from = this->nearest_env_point(env, from, to);
}
if (!(to_is_inside = this->outer.contains(to))) {
if (!(to_is_inside = env.contains(to))) {
// Find the closest inner point to start from.
to.nearest_point(this->outer, &inner_to);
}
} else {
if (!(from_is_inside = this->inner[island_idx].contains(from))) {
// Find the closest inner point to start from.
from.nearest_point(this->inner[island_idx], &inner_from);
}
if (!(to_is_inside = this->inner[island_idx].contains(to))) {
// Find the closest inner point to start from.
to.nearest_point(this->inner[island_idx], &inner_to);
}
inner_to = this->nearest_env_point(env, to, inner_from);
}
// perform actual path search
MotionPlannerGraph* graph = this->init_graph(island_idx);
graph->shortest_path(graph->find_node(inner_from), graph->find_node(inner_to), polyline);
if (!from_is_inside)
polyline->points.insert(polyline->points.begin(), from);
if (!to_is_inside)
polyline->points.push_back(to);
{
// grow our environment slightly in order for simplify_by_visibility()
// to work best by considering moves on boundaries valid as well
ExPolygonCollection grown_env;
offset(env, &grown_env.expolygons, +SCALED_EPSILON);
// remove unnecessary vertices
polyline->simplify_by_visibility(grown_env);
}
/*
SVG svg("shortest_path.svg");
svg.draw(this->outer);
svg.arrows = false;
for (MotionPlannerGraph::adjacency_list_t::const_iterator it = graph->adjacency_list.begin(); it != graph->adjacency_list.end(); ++it) {
Point a = graph->nodes[it - graph->adjacency_list.begin()];
for (std::vector<MotionPlannerGraph::neighbor>::const_iterator n = it->begin(); n != it->end(); ++n) {
Point b = graph->nodes[n->target];
svg.draw(Line(a, b));
}
}
svg.arrows = true;
svg.draw(from);
svg.draw(inner_from, "red");
svg.draw(to);
svg.draw(inner_to, "red");
svg.draw(*polyline, "red");
svg.Close();
*/
}
Point
MotionPlanner::nearest_env_point(const ExPolygonCollection &env, const Point &from, const Point &to) const
{
/* In order to ensure that the move between 'from' and the initial env point does
not violate any of the configuration space boundaries, we limit our search to
the points that satisfy this condition. */
/* Assume that this method is never called when 'env' contains 'from';
so 'from' is either inside a hole or outside all contours */
// get the points of the hole containing 'from', if any
Points pp;
for (ExPolygons::const_iterator ex = env.expolygons.begin(); ex != env.expolygons.end(); ++ex) {
for (Polygons::const_iterator h = ex->holes.begin(); h != ex->holes.end(); ++h) {
if (h->contains(from)) {
pp = *h;
}
}
if (!pp.empty()) break;
}
/* If 'from' is not inside a hole, it's outside of all contours, so take all
contours' points */
if (pp.empty()) {
for (ExPolygons::const_iterator ex = env.expolygons.begin(); ex != env.expolygons.end(); ++ex) {
Points contour_pp = ex->contour;
pp.insert(pp.end(), contour_pp.begin(), contour_pp.end());
}
}
/* Find the candidate result and check that it doesn't cross any boundary.
(We could skip all of the above polygon finding logic and directly test all points
in env, but this way we probably reduce complexity). */
Polygons env_pp = env;
while (pp.size() >= 2) {
// find the point in pp that is closest to both 'from' and 'to'
size_t result = from.nearest_waypoint_index(pp, to);
if (intersects((Lines)Line(from, pp[result]), env_pp)) {
// discard result
pp.erase(pp.begin() + result);
} else {
return pp[result];
}
}
// if we're here, return last point (better than nothing)
return pp.front();
}
MotionPlannerGraph*
MotionPlanner::init_graph(int island_idx)
{
if (this->graphs[island_idx + 1] == NULL) {
Polygons pp;
if (island_idx == -1) {
pp = this->outer;
} else {
pp = this->inner[island_idx];
}
// if this graph doesn't exist, initialize it
MotionPlannerGraph* graph = this->graphs[island_idx + 1] = new MotionPlannerGraph();
// add polygon boundaries as edges
size_t node_idx = 0;
Lines lines;
for (Polygons::const_iterator polygon = pp.begin(); polygon != pp.end(); ++polygon) {
graph->nodes.push_back(polygon->points.back());
node_idx++;
for (Points::const_iterator p = polygon->points.begin(); p != polygon->points.end(); ++p) {
graph->nodes.push_back(*p);
double dist = graph->nodes[node_idx-1].distance_to(*p);
graph->add_edge(node_idx-1, node_idx, dist);
graph->add_edge(node_idx, node_idx-1, dist);
node_idx++;
}
polygon->lines(&lines);
}
/* We don't add polygon boundaries as graph edges, because we'd need to connect
them to the Voronoi-generated edges by recognizing coinciding nodes. */
// add Voronoi edges as internal edges
{
typedef voronoi_diagram<double> VD;
typedef std::map<const VD::vertex_type*,size_t> t_vd_vertices;
VD vd;
// mapping between Voronoi vertices and graph nodes
typedef std::map<const VD::vertex_type*,size_t> t_vd_vertices;
t_vd_vertices vd_vertices;
// get boundaries as lines
ExPolygonCollection env = this->get_env(island_idx);
Lines lines = env.lines();
boost::polygon::construct_voronoi(lines.begin(), lines.end(), &vd);
// traverse the Voronoi diagram and generate graph nodes and edges
for (VD::const_edge_iterator edge = vd.edges().begin(); edge != vd.edges().end(); ++edge) {
if (edge->is_infinite()) continue;
@ -175,21 +247,15 @@ MotionPlanner::init_graph(int island_idx)
const VD::vertex_type* v1 = edge->vertex1();
Point p0 = Point(v0->x(), v0->y());
Point p1 = Point(v1->x(), v1->y());
// contains() should probably be faster than contains(),
// and should it fail on any boundary points it's not a big problem
if (island_idx == -1) {
if (!this->outer.contains(p0) || !this->outer.contains(p1)) continue;
} else {
if (!this->inner[island_idx].contains(p0) || !this->inner[island_idx].contains(p1)) continue;
}
// skip edge if any of its endpoints is outside our configuration space
if (!env.contains_b(p0) || !env.contains_b(p1)) continue;
t_vd_vertices::const_iterator i_v0 = vd_vertices.find(v0);
size_t v0_idx;
if (i_v0 == vd_vertices.end()) {
graph->nodes.push_back(p0);
v0_idx = node_idx;
vd_vertices[v0] = node_idx;
node_idx++;
vd_vertices[v0] = v0_idx = graph->nodes.size()-1;
} else {
v0_idx = i_v0->second;
}
@ -198,17 +264,15 @@ MotionPlanner::init_graph(int island_idx)
size_t v1_idx;
if (i_v1 == vd_vertices.end()) {
graph->nodes.push_back(p1);
v1_idx = node_idx;
vd_vertices[v1] = node_idx;
node_idx++;
vd_vertices[v1] = v1_idx = graph->nodes.size()-1;
} else {
v1_idx = i_v1->second;
}
// Euclidean distance is used as weight for the graph edge
double dist = graph->nodes[v0_idx].distance_to(graph->nodes[v1_idx]);
graph->add_edge(v0_idx, v1_idx, dist);
}
}
return graph;
}
@ -244,38 +308,61 @@ MotionPlannerGraph::shortest_path(size_t from, size_t to, Polyline* polyline)
const weight_t max_weight = std::numeric_limits<weight_t>::infinity();
std::vector<weight_t> min_distance;
std::vector<weight_t> dist;
std::vector<node_t> previous;
{
int n = this->adjacency_list.size();
min_distance.clear();
min_distance.resize(n, max_weight);
min_distance[from] = 0;
// number of nodes
size_t n = this->adjacency_list.size();
// initialize dist and previous
dist.clear();
dist.resize(n, max_weight);
dist[from] = 0; // distance from 'from' to itself
previous.clear();
previous.resize(n, -1);
std::set<std::pair<weight_t, node_t> > vertex_queue;
vertex_queue.insert(std::make_pair(min_distance[from], from));
while (!vertex_queue.empty())
// initialize the Q with all nodes
std::set<node_t> Q;
for (node_t i = 0; i < n; ++i) Q.insert(i);
while (!Q.empty())
{
weight_t dist = vertex_queue.begin()->first;
node_t u = vertex_queue.begin()->second;
vertex_queue.erase(vertex_queue.begin());
// get node in Q having the minimum dist ('from' in the first loop)
node_t u;
{
double min_dist = -1;
for (std::set<node_t>::const_iterator n = Q.begin(); n != Q.end(); ++n) {
if (dist[*n] < min_dist || min_dist == -1) {
u = *n;
min_dist = dist[*n];
}
}
}
Q.erase(u);
// Visit each edge exiting u
// stop searching if we reached our destination
if (u == to) break;
// Visit each edge starting from node u
const std::vector<neighbor> &neighbors = this->adjacency_list[u];
for (std::vector<neighbor>::const_iterator neighbor_iter = neighbors.begin();
neighbor_iter != neighbors.end();
neighbor_iter++)
{
// neighbor node is v
node_t v = neighbor_iter->target;
weight_t weight = neighbor_iter->weight;
weight_t distance_through_u = dist + weight;
if (distance_through_u < min_distance[v]) {
vertex_queue.erase(std::make_pair(min_distance[v], v));
min_distance[v] = distance_through_u;
// skip if we already visited this
if (Q.find(v) == Q.end()) continue;
// calculate total distance
weight_t alt = dist[u] + neighbor_iter->weight;
// if total distance through u is shorter than the previous
// distance (if any) between 'from' and 'v', replace it
if (alt < dist[v]) {
dist[v] = alt;
previous[v] = u;
vertex_queue.insert(std::make_pair(min_distance[v], v));
}
}
@ -284,6 +371,7 @@ MotionPlannerGraph::shortest_path(size_t from, size_t to, Polyline* polyline)
for (node_t vertex = to; vertex != -1; vertex = previous[vertex])
polyline->points.push_back(this->nodes[vertex]);
polyline->points.push_back(this->nodes[from]);
polyline->reverse();
}

4
xs/src/libslic3r/MotionPlanner.hpp
View File

@ -33,10 +33,14 @@ class MotionPlanner
void initialize();
MotionPlannerGraph* init_graph(int island_idx);
ExPolygonCollection get_env(size_t island_idx) const;
Point nearest_env_point(const ExPolygonCollection &env, const Point &from, const Point &to) const;
};
class MotionPlannerGraph
{
friend class MotionPlanner;
private:
typedef size_t node_t;
typedef double weight_t;

7
xs/src/libslic3r/MultiPoint.cpp
View File

@ -76,6 +76,13 @@ MultiPoint::find_point(const Point &point) const
return -1; // not found
}
bool
MultiPoint::has_boundary_point(const Point &point) const
{
double dist = point.distance_to(point.projection_onto(*this));
return dist < SCALED_EPSILON;
}
void
MultiPoint::bounding_box(BoundingBox* bb) const
{

1
xs/src/libslic3r/MultiPoint.hpp
View File

@ -30,6 +30,7 @@ class MultiPoint
double length() const;
bool is_valid() const;
int find_point(const Point &point) const;
bool has_boundary_point(const Point &point) const;
void bounding_box(BoundingBox* bb) const;
static Points _douglas_peucker(const Points &points, const double tolerance);

35
xs/src/libslic3r/Point.cpp
View File

@ -104,6 +104,32 @@ Point::nearest_point_index(const PointConstPtrs &points) const
return idx;
}
/* This method finds the point that is closest to both this point and the supplied one */
size_t
Point::nearest_waypoint_index(const Points &points, const Point &dest) const
{
size_t idx = -1;
double distance = -1; // double because long is limited to 2147483647 on some platforms and it's not enough
for (Points::const_iterator p = points.begin(); p != points.end(); ++p) {
// distance from this to candidate
double d = pow(this->x - p->x, 2) + pow(this->y - p->y, 2);
// distance from candidate to dest
d += pow(p->x - dest.x, 2) + pow(p->y - dest.y, 2);
// if the total distance is greater than current min distance, ignore it
if (distance != -1 && d > distance) continue;
idx = p - points.begin();
distance = d;
if (distance < EPSILON) break;
}
return idx;
}
int
Point::nearest_point_index(const PointPtrs &points) const
{
@ -123,6 +149,15 @@ Point::nearest_point(const Points &points, Point* point) const
return true;
}
bool
Point::nearest_waypoint(const Points &points, const Point &dest, Point* point) const
{
int idx = this->nearest_waypoint_index(points, dest);
if (idx == -1) return false;
*point = points.at(idx);
return true;
}
double
Point::distance_to(const Point &point) const
{

2
xs/src/libslic3r/Point.hpp
View File

@ -45,7 +45,9 @@ class Point
int nearest_point_index(const Points &points) const;
int nearest_point_index(const PointConstPtrs &points) const;
int nearest_point_index(const PointPtrs &points) const;
size_t nearest_waypoint_index(const Points &points, const Point &point) const;
bool nearest_point(const Points &points, Point* point) const;
bool nearest_waypoint(const Points &points, const Point &dest, Point* point) const;
double distance_to(const Point &point) const;
double distance_to(const Line &line) const;
double perp_distance_to(const Line &line) const;

32
xs/src/libslic3r/Polyline.cpp
View File

@ -1,4 +1,6 @@
#include "Polyline.hpp"
#include "ExPolygon.hpp"
#include "ExPolygonCollection.hpp"
#include "Line.hpp"
#include "Polygon.hpp"
#include <iostream>
@ -127,6 +129,36 @@ Polyline::simplify(double tolerance)
this->points = MultiPoint::_douglas_peucker(this->points, tolerance);
}
/* This method simplifies all *lines* contained in the supplied area */
template <class T>
void
Polyline::simplify_by_visibility(const T &area)
{
Points &pp = this->points;
// find first point in area
size_t start = 0;
while (start < pp.size() && !area.contains(pp[start])) {
start++;
}
for (size_t s = start; s < pp.size() && !pp.empty(); ++s) {
// find the farthest point to which we can build
// a line that is contained in the supplied area
// a binary search would be more efficient for this
for (size_t e = pp.size()-1; e > (s + 1); --e) {
if (area.contains(Line(pp[s], pp[e]))) {
// we can suppress points between s and e
pp.erase(pp.begin() + s + 1, pp.begin() + e);
// repeat recursively until no further simplification is possible
return this->simplify_by_visibility(area);
}
}
}
}
template void Polyline::simplify_by_visibility<ExPolygonCollection>(const ExPolygonCollection &area);
void
Polyline::split_at(const Point &point, Polyline* p1, Polyline* p2) const
{

2
xs/src/libslic3r/Polyline.hpp
View File

@ -7,6 +7,7 @@
namespace Slic3r {
class ExPolygon;
class Polyline;
typedef std::vector<Polyline> Polylines;
@ -23,6 +24,7 @@ class Polyline : public MultiPoint {
void extend_start(double distance);
void equally_spaced_points(double distance, Points* points) const;
void simplify(double tolerance);
template <class T> void simplify_by_visibility(const T &area);
void split_at(const Point &point, Polyline* p1, Polyline* p2) const;
bool is_straight() const;
std::string wkt() const;

38
xs/t/09_polyline.t
View File

@ -4,7 +4,7 @@ use strict;
use warnings;
use Slic3r::XS;
use Test::More tests => 18;
use Test::More tests => 21;
my $points = [
[100, 100],
@ -88,4 +88,40 @@ is_deeply $polyline->pp, [ @$points, @$points ], 'append_polyline';
is scalar(@$p2), 4, 'split_at';
}
{
my $polyline = Slic3r::Polyline->new(
map [$_,10], (0,10,20,30,40,50,60)
);
{
my $expolygon = Slic3r::ExPolygon->new(Slic3r::Polygon->new(
[25,0], [55,0], [55,30], [25,30],
));
my $p = $polyline->clone;
$p->simplify_by_visibility($expolygon);
is_deeply $p->pp, [
map [$_,10], (0,10,20,30,50,60)
], 'simplify_by_visibility()';
}
{
my $expolygon = Slic3r::ExPolygon->new(Slic3r::Polygon->new(
[-15,0], [75,0], [75,30], [-15,30],
));
my $p = $polyline->clone;
$p->simplify_by_visibility($expolygon);
is_deeply $p->pp, [
map [$_,10], (0,60)
], 'simplify_by_visibility()';
}
{
my $expolygon = Slic3r::ExPolygon->new(Slic3r::Polygon->new(
[-15,0], [25,0], [25,30], [-15,30],
));
my $p = $polyline->clone;
$p->simplify_by_visibility($expolygon);
is_deeply $p->pp, [
map [$_,10], (0,20,30,40,50,60)
], 'simplify_by_visibility()';
}
}
__END__

2
xs/xsp/Polyline.xsp
View File

@ -32,6 +32,8 @@
void extend_end(double distance);
void extend_start(double distance);
void simplify(double tolerance);
void simplify_by_visibility(ExPolygon* expolygon)
%code{% THIS->simplify_by_visibility(*expolygon); %};
void split_at(Point* point, Polyline* p1, Polyline* p2)
%code{% THIS->split_at(*point, p1, p2); %};
bool is_straight();