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

Performance improvements of the MotionPlanner

Browse Source 
(rewrote the Dijkstra shortest path algorithm to use a binary priority
heap instead of a dumb O(n^2) algorithm, added some bounding box tests
to avoid expensive in-polygon tests if possible).
This commit is contained in:
bubnikv 2017-05-05 09:59:56 +02:00
parent 8a628c451c
commit 60528c5c2a
8 changed files with 340 additions and 313 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
xs/MANIFEST
View File

@ -395,6 +395,7 @@ src/libslic3r/MotionPlanner.cpp
src/libslic3r/MotionPlanner.hpp src/libslic3r/MotionPlanner.hpp
src/libslic3r/MultiPoint.cpp src/libslic3r/MultiPoint.cpp
src/libslic3r/MultiPoint.hpp src/libslic3r/MultiPoint.hpp
src/libslic3r/MutablePriorityQueue.hpp
src/libslic3r/PerimeterGenerator.cpp src/libslic3r/PerimeterGenerator.cpp
src/libslic3r/PerimeterGenerator.hpp src/libslic3r/PerimeterGenerator.hpp
src/libslic3r/PlaceholderParser.cpp src/libslic3r/PlaceholderParser.cpp

80
xs/src/libslic3r/GCode.cpp
View File

@ -24,65 +24,16 @@
#include <assert.h> #include <assert.h>
namespace Slic3r { namespace Slic3r {
AvoidCrossingPerimeters::AvoidCrossingPerimeters()
: use_external_mp(false), use_external_mp_once(false), disable_once(true),
_external_mp(NULL), _layer_mp(NULL)
{
}
AvoidCrossingPerimeters::~AvoidCrossingPerimeters()
{
if (this->_external_mp != NULL)
delete this->_external_mp;
if (this->_layer_mp != NULL) Polyline AvoidCrossingPerimeters::travel_to(GCode &gcodegen, Point point)
delete this->_layer_mp;
}
void
AvoidCrossingPerimeters::init_external_mp(const ExPolygons &islands)
{ {
if (this->_external_mp != NULL) bool use_external = this->use_external_mp || this->use_external_mp_once;
delete this->_external_mp; Point scaled_origin = use_external ? Point(0, 0) : Point::new_scale(gcodegen.origin().x, gcodegen.origin().y);
Polyline result = (use_external ? m_external_mp.get() : m_layer_mp.get())->
this->_external_mp = new MotionPlanner(islands); shortest_path(gcodegen.last_pos() + scaled_origin, point + scaled_origin);
} if (! use_external)
result.translate(scaled_origin.negative());
void return result;
AvoidCrossingPerimeters::init_layer_mp(const ExPolygons &islands)
{
if (this->_layer_mp != NULL)
delete this->_layer_mp;
this->_layer_mp = new MotionPlanner(islands);
}
Polyline
AvoidCrossingPerimeters::travel_to(GCode &gcodegen, Point point)
{
if (this->use_external_mp || this->use_external_mp_once) {
// get current origin set in gcodegen
// (the one that will be used to translate the G-code coordinates by)
Point scaled_origin = Point::new_scale(gcodegen.origin().x, gcodegen.origin().y);
// represent last_pos in absolute G-code coordinates
Point last_pos = gcodegen.last_pos();
last_pos.translate(scaled_origin);
// represent point in absolute G-code coordinates
point.translate(scaled_origin);
// calculate path
Polyline travel = this->_external_mp->shortest_path(last_pos, point);
//exit(0);
// translate the path back into the shifted coordinate system that gcodegen
// is currently using for writing coordinates
travel.translate(scaled_origin.negative());
return travel;
} else {
return this->_layer_mp->shortest_path(gcodegen.last_pos(), point);
}
} }
std::string OozePrevention::pre_toolchange(GCode &gcodegen) std::string OozePrevention::pre_toolchange(GCode &gcodegen)
@ -201,12 +152,6 @@ inline void writeln(FILE *file, const std::string &what)
fprintf(file, "\n"); fprintf(file, "\n");
} }
// Older compilers do not provide a std::make_unique template. Provide a simple one.
template<typename T, typename... Args>
std::unique_ptr<T> make_unique(Args&&... args) {
return std::unique_ptr<T>(new T(std::forward<Args>(args)...));
}
bool GCode::do_export(FILE *file, Print &print) bool GCode::do_export(FILE *file, Print &print)
{ {
// How many times will be change_layer() called? // How many times will be change_layer() called?
@ -544,6 +489,7 @@ void GCode::process_layer(FILE *file, const Print &print, const Layer &layer, co
const SupportLayer *support_layer = dynamic_cast<const SupportLayer*>(&layer); const SupportLayer *support_layer = dynamic_cast<const SupportLayer*>(&layer);
// Check whether it is possible to apply the spiral vase logic for this layer. // Check whether it is possible to apply the spiral vase logic for this layer.
// Just a reminder: A spiral vase mode is allowed for a single object, single material print only.
if (m_spiral_vase) { if (m_spiral_vase) {
bool enable = (layer.id() > 0 || print.config.brim_width.value == 0.) && (layer.id() >= print.config.skirt_height.value && ! print.has_infinite_skirt()); bool enable = (layer.id() > 0 || print.config.brim_width.value == 0.) && (layer.id() >= print.config.skirt_height.value && ! print.has_infinite_skirt());
if (enable) { if (enable) {
@ -888,11 +834,9 @@ std::string GCode::change_layer(const Layer &layer)
} }
// avoid computing islands and overhangs if they're not needed // avoid computing islands and overhangs if they're not needed
if (m_config.avoid_crossing_perimeters) { if (m_config.avoid_crossing_perimeters)
ExPolygons islands = union_ex(layer.slices, true); m_avoid_crossing_perimeters.init_layer_mp(union_ex(layer.slices, true));
m_avoid_crossing_perimeters.init_layer_mp(islands);
}
if (m_layer_count > 0) if (m_layer_count > 0)
gcode += m_writer.update_progress(m_layer_index, m_layer_count); gcode += m_writer.update_progress(m_layer_index, m_layer_count);

16
xs/src/libslic3r/GCode.hpp
View File

@ -34,15 +34,17 @@ public:
// we enable it by default for the first travel move in print // we enable it by default for the first travel move in print
bool disable_once; bool disable_once;
AvoidCrossingPerimeters(); AvoidCrossingPerimeters() : use_external_mp(false), use_external_mp_once(false), disable_once(true) {}
~AvoidCrossingPerimeters(); ~AvoidCrossingPerimeters() {}
void init_external_mp(const ExPolygons &islands);
void init_layer_mp(const ExPolygons &islands); void init_external_mp(const ExPolygons &islands) { m_external_mp = Slic3r::make_unique<MotionPlanner>(islands); }
void init_layer_mp(const ExPolygons &islands) { m_layer_mp = Slic3r::make_unique<MotionPlanner>(islands); }
Polyline travel_to(GCode &gcodegen, Point point); Polyline travel_to(GCode &gcodegen, Point point);
private: private:
MotionPlanner* _external_mp; std::unique_ptr<MotionPlanner> m_external_mp;
MotionPlanner* _layer_mp; std::unique_ptr<MotionPlanner> m_layer_mp;
}; };
class OozePrevention { class OozePrevention {

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

@ -1,5 +1,8 @@
#include "BoundingBox.hpp" #include "BoundingBox.hpp"
#include "MotionPlanner.hpp" #include "MotionPlanner.hpp"
#include "MutablePriorityQueue.hpp"
#include "Utils.hpp"
#include <limits> // for numeric_limits #include <limits> // for numeric_limits
#include <assert.h> #include <assert.h>
@ -9,103 +12,73 @@ using boost::polygon::voronoi_diagram;
namespace Slic3r { namespace Slic3r {
MotionPlanner::MotionPlanner(const ExPolygons &islands) MotionPlanner::MotionPlanner(const ExPolygons &islands) : initialized(false)
: initialized(false)
{ {
ExPolygons expp; ExPolygons expp;
for (ExPolygons::const_iterator island = islands.begin(); island != islands.end(); ++island) for (const ExPolygon &island : islands) {
island->simplify(SCALED_EPSILON, &expp); island.simplify(SCALED_EPSILON, &expp);
for (ExPolygon &island : expp)
for (ExPolygons::const_iterator island = expp.begin(); island != expp.end(); ++island) this->islands.push_back(MotionPlannerEnv(island));
this->islands.push_back(MotionPlannerEnv(*island)); expp.clear();
}
} }
MotionPlanner::~MotionPlanner() void MotionPlanner::initialize()
{ {
for (std::vector<MotionPlannerGraph*>::iterator graph = this->graphs.begin(); graph != this->graphs.end(); ++graph) // prevent initialization of empty BoundingBox
delete *graph; if (this->initialized || this->islands.empty())
} return;
size_t
MotionPlanner::islands_count() const
{
return this->islands.size();
}
void
MotionPlanner::initialize()
{
if (this->initialized) return;
if (this->islands.empty()) return; // prevent initialization of empty BoundingBox
// loop through islands in order to create inner expolygons and collect their contours // loop through islands in order to create inner expolygons and collect their contours
Polygons outer_holes; Polygons outer_holes;
for (std::vector<MotionPlannerEnv>::iterator island = this->islands.begin(); island != this->islands.end(); ++island) { for (MotionPlannerEnv &island : this->islands) {
// generate the internal env boundaries by shrinking the island // generate the internal env boundaries by shrinking the island
// we'll use these inner rings for motion planning (endpoints of the Voronoi-based // we'll use these inner rings for motion planning (endpoints of the Voronoi-based
// graph, visibility check) in order to avoid moving too close to the boundaries // graph, visibility check) in order to avoid moving too close to the boundaries
island->env = offset_ex(island->island, -MP_INNER_MARGIN); island.env = ExPolygonCollection(offset_ex(island.island, -MP_INNER_MARGIN));
// island contours are holes of our external environment // island contours are holes of our external environment
outer_holes.push_back(island->island.contour); outer_holes.push_back(island.island.contour);
} }
// generate outer contour as bounding box of everything // Generate a box contour around everyting.
BoundingBox bb; Polygons contour = offset(get_extents(outer_holes).polygon(), +MP_OUTER_MARGIN*2);
for (Polygons::const_iterator contour = outer_holes.begin(); contour != outer_holes.end(); ++contour)
bb.merge(contour->bounding_box());
// grow outer contour
Polygons contour = offset(bb.polygon(), +MP_OUTER_MARGIN*2);
assert(contour.size() == 1); assert(contour.size() == 1);
// make expolygon for outer environment // make expolygon for outer environment
ExPolygons outer = diff_ex(contour, outer_holes); ExPolygons outer = diff_ex(contour, outer_holes);
assert(outer.size() == 1); assert(outer.size() == 1);
//FIXME What if some of the islands are nested? Then the front contour may not be the outmost contour!
this->outer.island = outer.front(); this->outer.island = outer.front();
this->outer.env = ExPolygonCollection(diff_ex(contour, offset(outer_holes, +MP_OUTER_MARGIN))); this->outer.env = ExPolygonCollection(diff_ex(contour, offset(outer_holes, +MP_OUTER_MARGIN)));
this->graphs.resize(this->islands.size() + 1);
this->graphs.resize(this->islands.size() + 1, NULL);
this->initialized = true; this->initialized = true;
} }
const MotionPlannerEnv& Polyline MotionPlanner::shortest_path(const Point &from, const Point &to)
MotionPlanner::get_env(int island_idx) const
{ {
if (island_idx == -1) { // If we have an empty configuration space, return a straight move.
return this->outer;
} else {
return this->islands[island_idx];
}
}
Polyline
MotionPlanner::shortest_path(const Point &from, const Point &to)
{
// if we have an empty configuration space, return a straight move
if (this->islands.empty()) if (this->islands.empty())
return Line(from, to); return Line(from, to);
// Are both points in the same island? // Are both points in the same island?
int island_idx = -1; int island_idx = -1;
for (std::vector<MotionPlannerEnv>::const_iterator island = this->islands.begin(); island != this->islands.end(); ++island) { for (MotionPlannerEnv &island : islands) {
if (island->island.contains(from) && island->island.contains(to)) { if (island.island_bbox.contains(from) && island.island_bbox.contains(to) &&
// since both points are in the same island, is a direct move possible? island.island.contains(from) && island.island.contains(to)) {
// if so, we avoid generating the visibility environment // Since both points are in the same island, is a direct move possible?
if (island->island.contains(Line(from, to))) // If so, we avoid generating the visibility environment.
if (island.island.contains(Line(from, to)))
return Line(from, to); return Line(from, to);
// Both points are inside a single island, but the straight line crosses the island boundary.
island_idx = island - this->islands.begin(); island_idx = &island - this->islands.data();
break; break;
} }
} }
// lazy generation of configuration space // lazy generation of configuration space.
this->initialize(); this->initialize();
// get environment // get environment
MotionPlannerEnv env = this->get_env(island_idx); const MotionPlannerEnv &env = this->get_env(island_idx);
if (env.env.expolygons.empty()) { if (env.env.expolygons.empty()) {
// if this environment is empty (probably because it's too small), perform straight move // if this environment is empty (probably because it's too small), perform straight move
// and avoid running the algorithms on empty dataset // and avoid running the algorithms on empty dataset
@ -122,19 +95,19 @@ MotionPlanner::shortest_path(const Point &from, const Point &to)
// nodes which don't require more than one crossing, and let Dijkstra // nodes which don't require more than one crossing, and let Dijkstra
// figure out the entire path - this should also replace the call to // figure out the entire path - this should also replace the call to
// find_node() below // find_node() below
if (!env.island.contains(inner_from)) { if (! env.island_bbox.contains(inner_from) || ! env.island.contains(inner_from)) {
// Find the closest inner point to start from. // Find the closest inner point to start from.
inner_from = env.nearest_env_point(from, to); inner_from = env.nearest_env_point(from, to);
} }
if (!env.island.contains(inner_to)) { if (! env.island_bbox.contains(inner_to) || ! env.island.contains(inner_to)) {
// Find the closest inner point to start from. // Find the closest inner point to start from.
inner_to = env.nearest_env_point(to, inner_from); inner_to = env.nearest_env_point(to, inner_from);
} }
} }
// perform actual path search // perform actual path search
MotionPlannerGraph* graph = this->init_graph(island_idx); const MotionPlannerGraph &graph = this->init_graph(island_idx);
Polyline polyline = graph->shortest_path(graph->find_node(inner_from), graph->find_node(inner_to)); Polyline polyline = graph.shortest_path(graph.find_closest_node(inner_from), graph.find_closest_node(inner_to));
polyline.points.insert(polyline.points.begin(), from); polyline.points.insert(polyline.points.begin(), from);
polyline.points.push_back(to); polyline.points.push_back(to);
@ -152,17 +125,15 @@ MotionPlanner::shortest_path(const Point &from, const Point &to)
grown_env (whose contour was arbitrarily constructed with MP_OUTER_MARGIN, grown_env (whose contour was arbitrarily constructed with MP_OUTER_MARGIN,
which may not be enough for, say, including a skirt point). So we prune which may not be enough for, say, including a skirt point). So we prune
the extra points manually. */ the extra points manually. */
if (!grown_env.contains(from)) { if (! grown_env.contains(from)) {
// delete second point while the line connecting first to third crosses the // delete second point while the line connecting first to third crosses the
// boundaries as many times as the current first to second // boundaries as many times as the current first to second
while (polyline.points.size() > 2 && intersection_ln((Lines)Line(from, polyline.points[2]), grown_env).size() == 1) { while (polyline.points.size() > 2 && intersection_ln((Lines)Line(from, polyline.points[2]), grown_env).size() == 1)
polyline.points.erase(polyline.points.begin() + 1); polyline.points.erase(polyline.points.begin() + 1);
}
} }
if (!grown_env.contains(to)) { if (! grown_env.contains(to)) {
while (polyline.points.size() > 2 && intersection_ln((Lines)Line(*(polyline.points.end() - 3), to), grown_env).size() == 1) { while (polyline.points.size() > 2 && intersection_ln((Lines)Line(*(polyline.points.end() - 3), to), grown_env).size() == 1)
polyline.points.erase(polyline.points.end() - 2); polyline.points.erase(polyline.points.end() - 2);
}
} }
} }
@ -178,7 +149,7 @@ MotionPlanner::shortest_path(const Point &from, const Point &to)
svg.arrows = false; svg.arrows = false;
for (MotionPlannerGraph::adjacency_list_t::const_iterator it = graph->adjacency_list.begin(); it != graph->adjacency_list.end(); ++it) { 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()]; Point a = graph->nodes[it - graph->adjacency_list.begin()];
for (std::vector<MotionPlannerGraph::neighbor>::const_iterator n = it->begin(); n != it->end(); ++n) { for (std::vector<MotionPlannerGraph::Neighbor>::const_iterator n = it->begin(); n != it->end(); ++n) {
Point b = graph->nodes[n->target]; Point b = graph->nodes[n->target];
svg.draw(Line(a, b)); svg.draw(Line(a, b));
} }
@ -196,12 +167,12 @@ MotionPlanner::shortest_path(const Point &from, const Point &to)
return polyline; return polyline;
} }
MotionPlannerGraph* const MotionPlannerGraph& MotionPlanner::init_graph(int island_idx)
MotionPlanner::init_graph(int island_idx)
{ {
if (this->graphs[island_idx + 1] == NULL) { if (! this->graphs[island_idx + 1]) {
// if this graph doesn't exist, initialize it // if this graph doesn't exist, initialize it
MotionPlannerGraph* graph = this->graphs[island_idx + 1] = new MotionPlannerGraph(); this->graphs[island_idx + 1] = make_unique<MotionPlannerGraph>();
MotionPlannerGraph* graph = this->graphs[island_idx + 1].get();
/* We don't add polygon boundaries as graph edges, because we'd need to connect /* 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. */ them to the Voronoi-generated edges by recognizing coinciding nodes. */
@ -214,7 +185,7 @@ MotionPlanner::init_graph(int island_idx)
t_vd_vertices vd_vertices; t_vd_vertices vd_vertices;
// get boundaries as lines // get boundaries as lines
MotionPlannerEnv env = this->get_env(island_idx); const MotionPlannerEnv &env = this->get_env(island_idx);
Lines lines = env.env.lines(); Lines lines = env.env.lines();
boost::polygon::construct_voronoi(lines.begin(), lines.end(), &vd); boost::polygon::construct_voronoi(lines.begin(), lines.end(), &vd);
@ -228,6 +199,7 @@ MotionPlanner::init_graph(int island_idx)
Point p1 = Point(v1->x(), v1->y()); Point p1 = Point(v1->x(), v1->y());
// skip edge if any of its endpoints is outside our configuration space // skip edge if any of its endpoints is outside our configuration space
//FIXME This test has a terrible O(n^2) time complexity.
if (!env.island.contains_b(p0) || !env.island.contains_b(p1)) continue; if (!env.island.contains_b(p0) || !env.island.contains_b(p1)) continue;
t_vd_vertices::const_iterator i_v0 = vd_vertices.find(v0); t_vd_vertices::const_iterator i_v0 = vd_vertices.find(v0);
@ -252,14 +224,12 @@ MotionPlanner::init_graph(int island_idx)
double dist = graph->nodes[v0_idx].distance_to(graph->nodes[v1_idx]); double dist = graph->nodes[v0_idx].distance_to(graph->nodes[v1_idx]);
graph->add_edge(v0_idx, v1_idx, dist); graph->add_edge(v0_idx, v1_idx, dist);
} }
return graph;
} }
return this->graphs[island_idx + 1];
return *this->graphs[island_idx + 1].get();
} }
Point Point MotionPlannerEnv::nearest_env_point(const Point &from, const Point &to) const
MotionPlannerEnv::nearest_env_point(const Point &from, const Point &to) const
{ {
/* In order to ensure that the move between 'from' and the initial env point does /* 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 not violate any of the configuration space boundaries, we limit our search to
@ -270,23 +240,19 @@ MotionPlannerEnv::nearest_env_point(const Point &from, const Point &to) const
// get the points of the hole containing 'from', if any // get the points of the hole containing 'from', if any
Points pp; Points pp;
for (ExPolygons::const_iterator ex = this->env.expolygons.begin(); ex != this->env.expolygons.end(); ++ex) { for (const ExPolygon &ex : this->env.expolygons) {
for (Polygons::const_iterator h = ex->holes.begin(); h != ex->holes.end(); ++h) { for (const Polygon &hole : ex.holes)
if (h->contains(from)) { if (hole.contains(from))
pp = *h; pp = hole;
} if (! pp.empty())
} break;
if (!pp.empty()) break;
} }
/* If 'from' is not inside a hole, it's outside of all contours, so take all /* If 'from' is not inside a hole, it's outside of all contours, so take all
contours' points */ contours' points */
if (pp.empty()) { if (pp.empty())
for (ExPolygons::const_iterator ex = this->env.expolygons.begin(); ex != this->env.expolygons.end(); ++ex) { for (const ExPolygon &ex : this->env.expolygons)
Points contour_pp = ex->contour; append(pp, ex.contour.points);
pp.insert(pp.end(), contour_pp.begin(), contour_pp.end());
}
}
/* Find the candidate result and check that it doesn't cross too many boundaries. */ /* Find the candidate result and check that it doesn't cross too many boundaries. */
while (pp.size() >= 2) { while (pp.size() >= 2) {
@ -297,115 +263,77 @@ MotionPlannerEnv::nearest_env_point(const Point &from, const Point &to) const
if (intersection_ln((Lines)Line(from, pp[result]), this->island).size() > 1) { if (intersection_ln((Lines)Line(from, pp[result]), this->island).size() > 1) {
// discard result // discard result
pp.erase(pp.begin() + result); pp.erase(pp.begin() + result);
} else { } else
return pp[result]; return pp[result];
}
} }
// if we're here, return last point if any (better than nothing) // if we're here, return last point if any (better than nothing)
if (!pp.empty()) {
return pp.front();
}
// if we have no points at all, then we have an empty environment and we // if we have no points at all, then we have an empty environment and we
// make this method behave as a no-op (we shouldn't get here by the way) // make this method behave as a no-op (we shouldn't get here by the way)
return from; return pp.empty() ? from : pp.front();
} }
void // Add a new directed edge to the adjacency graph.
MotionPlannerGraph::add_edge(size_t from, size_t to, double weight) void MotionPlannerGraph::add_edge(size_t from, size_t to, double weight)
{ {
// extend adjacency list until this start node // Extend adjacency list until this start node.
if (this->adjacency_list.size() < from+1) if (this->adjacency_list.size() < from + 1) {
this->adjacency_list.resize(from+1); // Reserve in powers of two to avoid repeated reallocation.
this->adjacency_list.reserve(std::max<size_t>(8, next_highest_power_of_2(from + 1)));
this->adjacency_list[from].push_back(neighbor(to, weight)); // Allocate new empty adjacency vectors.
} this->adjacency_list.resize(from + 1);
size_t
MotionPlannerGraph::find_node(const Point &point) const
{
/*
for (Points::const_iterator p = this->nodes.begin(); p != this->nodes.end(); ++p) {
if (p->coincides_with(point)) return p - this->nodes.begin();
} }
*/ this->adjacency_list[from].emplace_back(Neighbor(node_t(to), weight));
return point.nearest_point_index(this->nodes);
} }
Polyline // Dijkstra's shortest path in a weighted graph from node_start to node_end.
MotionPlannerGraph::shortest_path(size_t from, size_t to) // The returned path contains the end points.
Polyline MotionPlannerGraph::shortest_path(size_t node_start, size_t node_end) const
{ {
// this prevents a crash in case for some reason we got here with an empty adjacency list // This prevents a crash in case for some reason we got here with an empty adjacency list.
if (this->adjacency_list.empty()) return Polyline(); if (this->adjacency_list.empty())
return Polyline();
const weight_t max_weight = std::numeric_limits<weight_t>::infinity();
// Dijkstra algorithm, previous node of the current node 'u' in the shortest path towards node_start.
std::vector<weight_t> dist; std::vector<node_t> previous(this->adjacency_list.size(), -1);
std::vector<node_t> previous; std::vector<weight_t> distance(this->adjacency_list.size(), std::numeric_limits<weight_t>::infinity());
{ std::vector<size_t> map_node_to_queue_id(this->adjacency_list.size(), size_t(-1));
// number of nodes distance[node_start] = 0.;
size_t n = this->adjacency_list.size();
auto queue = make_mutable_priority_queue<node_t>(
// initialize dist and previous [&map_node_to_queue_id](const node_t &node, size_t idx) { map_node_to_queue_id[node] = idx; },
dist.clear(); [&distance](const node_t &node1, const node_t &node2) { return distance[node1] < distance[node2]; });
dist.resize(n, max_weight); queue.reserve(this->adjacency_list.size());
dist[from] = 0; // distance from 'from' to itself for (size_t i = 0; i < this->adjacency_list.size(); ++ i)
previous.clear(); queue.push(node_t(i));
previous.resize(n, -1);
while (! queue.empty()) {
// initialize the Q with all nodes // Get the next node with the lowest distance to node_start.
std::set<node_t> Q; node_t u = node_t(queue.top());
for (node_t i = 0; i < n; ++i) Q.insert(i); queue.pop();
map_node_to_queue_id[u] = size_t(-1);
while (!Q.empty()) // Stop searching if we reached our destination.
{ if (u == node_end)
// get node in Q having the minimum dist ('from' in the first loop) break;
node_t u; // Visit each edge starting at node u.
{ for (const Neighbor& neighbor : this->adjacency_list[u])
double min_dist = -1; if (map_node_to_queue_id[neighbor.target] != size_t(-1)) {
for (std::set<node_t>::const_iterator n = Q.begin(); n != Q.end(); ++n) { weight_t alt = distance[u] + neighbor.weight;
if (dist[*n] < min_dist || min_dist == -1) { // If total distance through u is shorter than the previous
u = *n; // distance (if any) between node_start and neighbor.target, replace it.
min_dist = dist[*n]; if (alt < distance[neighbor.target]) {
} distance[neighbor.target] = alt;
previous[neighbor.target] = u;
queue.update(map_node_to_queue_id[neighbor.target]);
} }
} }
Q.erase(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;
// 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;
}
}
}
} }
Polyline polyline; Polyline polyline;
for (node_t vertex = to; vertex != -1; vertex = previous[vertex]) polyline.points.reserve(previous.size());
for (node_t vertex = node_t(node_end); vertex != -1; vertex = previous[vertex])
polyline.points.push_back(this->nodes[vertex]); polyline.points.push_back(this->nodes[vertex]);
polyline.points.push_back(this->nodes[from]); polyline.points.push_back(this->nodes[node_start]);
polyline.reverse(); polyline.reverse();
return polyline; return polyline;
} }

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

@ -2,11 +2,13 @@
#define slic3r_MotionPlanner_hpp_ #define slic3r_MotionPlanner_hpp_
#include "libslic3r.h" #include "libslic3r.h"
#include "BoundingBox.hpp"
#include "ClipperUtils.hpp" #include "ClipperUtils.hpp"
#include "ExPolygonCollection.hpp" #include "ExPolygonCollection.hpp"
#include "Polyline.hpp" #include "Polyline.hpp"
#include <map> #include <map>
#include <utility> #include <utility>
#include <memory>
#include <vector> #include <vector>
#define MP_INNER_MARGIN scale_(1.0) #define MP_INNER_MARGIN scale_(1.0)
@ -20,11 +22,12 @@ class MotionPlannerEnv
{ {
friend class MotionPlanner; friend class MotionPlanner;
public: public:
ExPolygon island; ExPolygon island;
BoundingBox island_bbox;
ExPolygonCollection env; ExPolygonCollection env;
MotionPlannerEnv() {}; MotionPlannerEnv() {};
MotionPlannerEnv(const ExPolygon &island) : island(island) {}; MotionPlannerEnv(const ExPolygon &island) : island(island), island_bbox(get_extents(island)) {};
Point nearest_env_point(const Point &from, const Point &to) const; Point nearest_env_point(const Point &from, const Point &to) const;
}; };
@ -32,43 +35,43 @@ class MotionPlannerGraph
{ {
friend class MotionPlanner; friend class MotionPlanner;
private: private:
typedef int node_t; typedef int node_t;
typedef double weight_t; typedef double weight_t;
struct neighbor { struct Neighbor {
node_t target; node_t target;
weight_t weight; weight_t weight;
neighbor(node_t arg_target, weight_t arg_weight) Neighbor(node_t arg_target, weight_t arg_weight) : target(arg_target), weight(arg_weight) {}
: target(arg_target), weight(arg_weight) { }
}; };
typedef std::vector< std::vector<neighbor> > adjacency_list_t; typedef std::vector<std::vector<Neighbor>> adjacency_list_t;
adjacency_list_t adjacency_list; adjacency_list_t adjacency_list;
public: public:
Points nodes; Points nodes;
//std::map<std::pair<size_t,size_t>, double> edges; void add_edge(size_t from, size_t to, double weight);
void add_edge(size_t from, size_t to, double weight); size_t find_closest_node(const Point &point) const { return point.nearest_point_index(this->nodes); }
size_t find_node(const Point &point) const; Polyline shortest_path(size_t from, size_t to) const;
Polyline shortest_path(size_t from, size_t to);
}; };
class MotionPlanner class MotionPlanner
{ {
public: public:
MotionPlanner(const ExPolygons &islands); MotionPlanner(const ExPolygons &islands);
~MotionPlanner(); ~MotionPlanner() {}
Polyline shortest_path(const Point &from, const Point &to);
size_t islands_count() const; Polyline shortest_path(const Point &from, const Point &to);
size_t islands_count() const { return this->islands.size(); }
private:
bool initialized;
std::vector<MotionPlannerEnv> islands;
MotionPlannerEnv outer;
std::vector<std::unique_ptr<MotionPlannerGraph>> graphs;
private: void initialize();
bool initialized; const MotionPlannerGraph& init_graph(int island_idx);
std::vector<MotionPlannerEnv> islands; const MotionPlannerEnv& get_env(int island_idx) const
MotionPlannerEnv outer; { return (island_idx == -1) ? this->outer : this->islands[island_idx]; }
std::vector<MotionPlannerGraph*> graphs;
void initialize();
MotionPlannerGraph* init_graph(int island_idx);
const MotionPlannerEnv& get_env(int island_idx) const;
}; };
} }

147
xs/src/libslic3r/MutablePriorityQueue.hpp Normal file
View File

@ -0,0 +1,147 @@
#ifndef slic3r_MutablePriorityQueue_hpp_
#define slic3r_MutablePriorityQueue_hpp_
#include <assert.h>
template<typename T, typename IndexSetter, typename LessPredicate>
class MutablePriorityQueue
{
public:
MutablePriorityQueue(IndexSetter &&index_setter, LessPredicate &&less_predicate) :
m_index_setter(std::forward<IndexSetter>(index_setter)),
m_less_predicate(std::forward<LessPredicate>(less_predicate))
{}
~MutablePriorityQueue() { clear(); }
inline void clear() { m_heap.clear(); }
inline void reserve(size_t cnt) { m_heap.reserve(cnt); }
inline void push(const T &item);
inline void push(T &&item);
inline void pop();
inline T& top() { return m_heap.front(); }
inline void remove(size_t idx);
inline void update(size_t idx) { T item = m_heap[idx]; remove(idx); push(item); }
inline size_t size() const { return m_heap.size(); }
inline bool empty() const { return m_heap.empty(); }
protected:
inline void update_heap_up(size_t top, size_t bottom);
inline void update_heap_down(size_t top, size_t bottom);
private:
std::vector<T> m_heap;
IndexSetter m_index_setter;
LessPredicate m_less_predicate;
};
template<typename T, typename IndexSetter, typename LessPredicate>
MutablePriorityQueue<T, IndexSetter, LessPredicate> make_mutable_priority_queue(IndexSetter &&index_setter, LessPredicate &&less_predicate)
{
return MutablePriorityQueue<T, IndexSetter, LessPredicate>(
std::forward<IndexSetter>(index_setter), std::forward<LessPredicate>(less_predicate));
}
template<class T, class LessPredicate, class IndexSetter>
inline void MutablePriorityQueue<T, LessPredicate, IndexSetter>::push(const T &item)
{
size_t idx = m_heap.size();
m_heap.emplace_back(item);
m_index_setter(m_heap.back(), idx);
update_heap_up(0, idx);
}
template<class T, class LessPredicate, class IndexSetter>
inline void MutablePriorityQueue<T, LessPredicate, IndexSetter>::push(T &&item)
{
size_t idx = m_heap.size();
m_heap.emplace_back(std::move(item));
m_index_setter(m_heap.back(), idx);
update_heap_up(0, idx);
}
template<class T, class LessPredicate, class IndexSetter>
inline void MutablePriorityQueue<T, LessPredicate, IndexSetter>::pop()
{
assert(! m_heap.empty());
if (m_heap.size() > 1) {
m_heap.front() = m_heap.back();
m_heap.pop_back();
m_index_setter(m_heap.front(), 0);
update_heap_down(0, m_heap.size() - 1);
} else
m_heap.clear();
}
template<class T, class LessPredicate, class IndexSetter>
inline void MutablePriorityQueue<T, LessPredicate, IndexSetter>::remove(size_t idx)
{
assert(idx < m_heap.size());
if (idx + 1 == m_heap.size()) {
m_heap.pop_back();
return;
}
m_heap[idx] = m_heap.back();
m_index_setter(m_heap[idx], idx);
m_heap.pop_back();
update_heap_down(idx, m_heap.size() - 1);
update_heap_up(0, idx);
}
template<class T, class LessPredicate, class IndexSetter>
inline void MutablePriorityQueue<T, LessPredicate, IndexSetter>::update_heap_up(size_t top, size_t bottom)
{
size_t childIdx = bottom;
T *child = &m_heap[childIdx];
for (;;) {
size_t parentIdx = (childIdx - 1) >> 1;
if (childIdx == 0 || parentIdx < top)
break;
T *parent = &m_heap[parentIdx];
// switch nodes
if (! m_less_predicate(*parent, *child)) {
T tmp = *parent;
m_index_setter(*parent, childIdx);
m_index_setter(*child, parentIdx);
m_heap[parentIdx] = *child;
m_heap[childIdx] = tmp;
}
// shift up
childIdx = parentIdx;
child = parent;
}
}
template<class T, class LessPredicate, class IndexSetter>
inline void MutablePriorityQueue<T, LessPredicate, IndexSetter>::update_heap_down(size_t top, size_t bottom)
{
size_t parentIdx = top;
T *parent = &m_heap[parentIdx];
for (;;) {
size_t childIdx = (parentIdx << 1) + 1;
if (childIdx > bottom)
break;
T *child = &m_heap[childIdx];
size_t child2Idx = childIdx + 1;
if (child2Idx <= bottom) {
T *child2 = &m_heap[child2Idx];
if (! m_less_predicate(*child, *child2)) {
child = child2;
childIdx = child2Idx;
}
}
if (m_less_predicate(*parent, *child))
return;
// switch nodes
m_index_setter(*parent, childIdx);
m_index_setter(*child, parentIdx);
T tmp = *parent;
m_heap[parentIdx] = *child;
m_heap[childIdx] = tmp;
// shift down
parentIdx = childIdx;
parent = child;
}
}
#endif /* slic3r_MutablePriorityQueue_hpp_ */

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

@ -94,8 +94,7 @@ inline T sqr(const T x)
return x * x; return x * x;
} }
int int Point::nearest_point_index(const PointConstPtrs &points) const
Point::nearest_point_index(const PointConstPtrs &points) const
{ {
int idx = -1; int idx = -1;
double distance = -1; // double because long is limited to 2147483647 on some platforms and it's not enough double distance = -1; // double because long is limited to 2147483647 on some platforms and it's not enough
@ -121,28 +120,25 @@ Point::nearest_point_index(const PointConstPtrs &points) const
} }
/* This method finds the point that is closest to both this point and the supplied one */ /* This method finds the point that is closest to both this point and the supplied one */
size_t size_t Point::nearest_waypoint_index(const Points &points, const Point &dest) const
Point::nearest_waypoint_index(const Points &points, const Point &dest) const
{ {
size_t idx = -1; size_t idx = size_t(-1);
double distance = -1; // double because long is limited to 2147483647 on some platforms and it's not enough double d2min = std::numeric_limits<double>::infinity(); // 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) { for (const Point &p : points) {
// distance from this to candidate double d2 =
double d = sqr<double>(this->x - p->x) + sqr<double>(this->y - p->y); // distance from this to candidate
sqr<double>(this->x - p.x) + sqr<double>(this->y - p.y) +
// distance from candidate to dest // distance from candidate to dest
d += sqr<double>(p->x - dest.x) + sqr<double>(p->y - dest.y); sqr<double>(p.x - dest.x) + sqr<double>(p.y - dest.y);
if (d2 < d2min) {
// if the total distance is greater than current min distance, ignore it idx = &p - points.data();
if (distance != -1 && d > distance) continue; d2min = d2;
if (d2min < EPSILON)
idx = p - points.begin(); break;
distance = d; }
if (distance < EPSILON) break;
} }
return idx; return idx;
} }

6
xs/src/libslic3r/libslic3r.h
View File

@ -127,6 +127,12 @@ void remove_nulls(std::vector<T*> &vec)
vec.end()); vec.end());
} }
// Older compilers do not provide a std::make_unique template. Provide a simple one.
template<typename T, typename... Args>
inline std::unique_ptr<T> make_unique(Args&&... args) {
return std::unique_ptr<T>(new T(std::forward<Args>(args)...));
}
} // namespace Slic3r } // namespace Slic3r
#endif #endif