Performance improvements of the MotionPlanner
(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
parent
8a628c451c
commit
60528c5c2a
@ -395,6 +395,7 @@ src/libslic3r/MotionPlanner.cpp
|
||||
src/libslic3r/MotionPlanner.hpp
|
||||
src/libslic3r/MultiPoint.cpp
|
||||
src/libslic3r/MultiPoint.hpp
|
||||
src/libslic3r/MutablePriorityQueue.hpp
|
||||
src/libslic3r/PerimeterGenerator.cpp
|
||||
src/libslic3r/PerimeterGenerator.hpp
|
||||
src/libslic3r/PlaceholderParser.cpp
|
||||
|
||||
@ -25,64 +25,15 @@
|
||||
|
||||
namespace Slic3r {
|
||||
|
||||
AvoidCrossingPerimeters::AvoidCrossingPerimeters()
|
||||
: use_external_mp(false), use_external_mp_once(false), disable_once(true),
|
||||
_external_mp(NULL), _layer_mp(NULL)
|
||||
Polyline AvoidCrossingPerimeters::travel_to(GCode &gcodegen, Point point)
|
||||
{
|
||||
}
|
||||
|
||||
AvoidCrossingPerimeters::~AvoidCrossingPerimeters()
|
||||
{
|
||||
if (this->_external_mp != NULL)
|
||||
delete this->_external_mp;
|
||||
|
||||
if (this->_layer_mp != NULL)
|
||||
delete this->_layer_mp;
|
||||
}
|
||||
|
||||
void
|
||||
AvoidCrossingPerimeters::init_external_mp(const ExPolygons &islands)
|
||||
{
|
||||
if (this->_external_mp != NULL)
|
||||
delete this->_external_mp;
|
||||
|
||||
this->_external_mp = new MotionPlanner(islands);
|
||||
}
|
||||
|
||||
void
|
||||
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);
|
||||
}
|
||||
bool use_external = this->use_external_mp || this->use_external_mp_once;
|
||||
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())->
|
||||
shortest_path(gcodegen.last_pos() + scaled_origin, point + scaled_origin);
|
||||
if (! use_external)
|
||||
result.translate(scaled_origin.negative());
|
||||
return result;
|
||||
}
|
||||
|
||||
std::string OozePrevention::pre_toolchange(GCode &gcodegen)
|
||||
@ -201,12 +152,6 @@ inline void writeln(FILE *file, const std::string &what)
|
||||
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)
|
||||
{
|
||||
// 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);
|
||||
|
||||
// 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) {
|
||||
bool enable = (layer.id() > 0 || print.config.brim_width.value == 0.) && (layer.id() >= print.config.skirt_height.value && ! print.has_infinite_skirt());
|
||||
if (enable) {
|
||||
@ -888,10 +834,8 @@ std::string GCode::change_layer(const Layer &layer)
|
||||
}
|
||||
|
||||
// avoid computing islands and overhangs if they're not needed
|
||||
if (m_config.avoid_crossing_perimeters) {
|
||||
ExPolygons islands = union_ex(layer.slices, true);
|
||||
m_avoid_crossing_perimeters.init_layer_mp(islands);
|
||||
}
|
||||
if (m_config.avoid_crossing_perimeters)
|
||||
m_avoid_crossing_perimeters.init_layer_mp(union_ex(layer.slices, true));
|
||||
|
||||
if (m_layer_count > 0)
|
||||
gcode += m_writer.update_progress(m_layer_index, m_layer_count);
|
||||
|
||||
@ -34,15 +34,17 @@ public:
|
||||
// we enable it by default for the first travel move in print
|
||||
bool disable_once;
|
||||
|
||||
AvoidCrossingPerimeters();
|
||||
~AvoidCrossingPerimeters();
|
||||
void init_external_mp(const ExPolygons &islands);
|
||||
void init_layer_mp(const ExPolygons &islands);
|
||||
AvoidCrossingPerimeters() : use_external_mp(false), use_external_mp_once(false), disable_once(true) {}
|
||||
~AvoidCrossingPerimeters() {}
|
||||
|
||||
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);
|
||||
|
||||
private:
|
||||
MotionPlanner* _external_mp;
|
||||
MotionPlanner* _layer_mp;
|
||||
std::unique_ptr<MotionPlanner> m_external_mp;
|
||||
std::unique_ptr<MotionPlanner> m_layer_mp;
|
||||
};
|
||||
|
||||
class OozePrevention {
|
||||
|
||||
@ -1,5 +1,8 @@
|
||||
#include "BoundingBox.hpp"
|
||||
#include "MotionPlanner.hpp"
|
||||
#include "MutablePriorityQueue.hpp"
|
||||
#include "Utils.hpp"
|
||||
|
||||
#include <limits> // for numeric_limits
|
||||
#include <assert.h>
|
||||
|
||||
@ -9,103 +12,73 @@ using boost::polygon::voronoi_diagram;
|
||||
|
||||
namespace Slic3r {
|
||||
|
||||
MotionPlanner::MotionPlanner(const ExPolygons &islands)
|
||||
: initialized(false)
|
||||
MotionPlanner::MotionPlanner(const ExPolygons &islands) : initialized(false)
|
||||
{
|
||||
ExPolygons expp;
|
||||
for (ExPolygons::const_iterator island = islands.begin(); island != islands.end(); ++island)
|
||||
island->simplify(SCALED_EPSILON, &expp);
|
||||
|
||||
for (ExPolygons::const_iterator island = expp.begin(); island != expp.end(); ++island)
|
||||
this->islands.push_back(MotionPlannerEnv(*island));
|
||||
for (const ExPolygon &island : islands) {
|
||||
island.simplify(SCALED_EPSILON, &expp);
|
||||
for (ExPolygon &island : expp)
|
||||
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)
|
||||
delete *graph;
|
||||
}
|
||||
|
||||
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
|
||||
// prevent initialization of empty BoundingBox
|
||||
if (this->initialized || this->islands.empty())
|
||||
return;
|
||||
|
||||
// loop through islands in order to create inner expolygons and collect their contours
|
||||
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
|
||||
// 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
|
||||
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
|
||||
outer_holes.push_back(island->island.contour);
|
||||
outer_holes.push_back(island.island.contour);
|
||||
}
|
||||
|
||||
// generate outer contour as bounding box of everything
|
||||
BoundingBox bb;
|
||||
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);
|
||||
// Generate a box contour around everyting.
|
||||
Polygons contour = offset(get_extents(outer_holes).polygon(), +MP_OUTER_MARGIN*2);
|
||||
assert(contour.size() == 1);
|
||||
|
||||
// make expolygon for outer environment
|
||||
ExPolygons outer = diff_ex(contour, outer_holes);
|
||||
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.env = ExPolygonCollection(diff_ex(contour, offset(outer_holes, +MP_OUTER_MARGIN)));
|
||||
|
||||
this->graphs.resize(this->islands.size() + 1, NULL);
|
||||
this->graphs.resize(this->islands.size() + 1);
|
||||
this->initialized = true;
|
||||
}
|
||||
|
||||
const MotionPlannerEnv&
|
||||
MotionPlanner::get_env(int island_idx) const
|
||||
Polyline MotionPlanner::shortest_path(const Point &from, const Point &to)
|
||||
{
|
||||
if (island_idx == -1) {
|
||||
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 we have an empty configuration space, return a straight move.
|
||||
if (this->islands.empty())
|
||||
return Line(from, to);
|
||||
|
||||
// Are both points in the same island?
|
||||
int island_idx = -1;
|
||||
for (std::vector<MotionPlannerEnv>::const_iterator island = this->islands.begin(); island != this->islands.end(); ++island) {
|
||||
if (island->island.contains(from) && island->island.contains(to)) {
|
||||
// since both points are in the same island, is a direct move possible?
|
||||
// if so, we avoid generating the visibility environment
|
||||
if (island->island.contains(Line(from, to)))
|
||||
for (MotionPlannerEnv &island : islands) {
|
||||
if (island.island_bbox.contains(from) && island.island_bbox.contains(to) &&
|
||||
island.island.contains(from) && island.island.contains(to)) {
|
||||
// Since both points are in the same island, is a direct move possible?
|
||||
// If so, we avoid generating the visibility environment.
|
||||
if (island.island.contains(Line(from, to)))
|
||||
return Line(from, to);
|
||||
|
||||
island_idx = island - this->islands.begin();
|
||||
// Both points are inside a single island, but the straight line crosses the island boundary.
|
||||
island_idx = &island - this->islands.data();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// lazy generation of configuration space
|
||||
// lazy generation of configuration space.
|
||||
this->initialize();
|
||||
|
||||
// get environment
|
||||
MotionPlannerEnv env = this->get_env(island_idx);
|
||||
const MotionPlannerEnv &env = this->get_env(island_idx);
|
||||
if (env.env.expolygons.empty()) {
|
||||
// if this environment is empty (probably because it's too small), perform straight move
|
||||
// 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
|
||||
// figure out the entire path - this should also replace the call to
|
||||
// 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.
|
||||
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.
|
||||
inner_to = env.nearest_env_point(to, inner_from);
|
||||
}
|
||||
}
|
||||
|
||||
// perform actual path search
|
||||
MotionPlannerGraph* graph = this->init_graph(island_idx);
|
||||
Polyline polyline = graph->shortest_path(graph->find_node(inner_from), graph->find_node(inner_to));
|
||||
const MotionPlannerGraph &graph = this->init_graph(island_idx);
|
||||
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.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,
|
||||
which may not be enough for, say, including a skirt point). So we prune
|
||||
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
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
if (!grown_env.contains(to)) {
|
||||
while (polyline.points.size() > 2 && intersection_ln((Lines)Line(*(polyline.points.end() - 3), to), grown_env).size() == 1) {
|
||||
if (! grown_env.contains(to)) {
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -178,7 +149,7 @@ MotionPlanner::shortest_path(const Point &from, const Point &to)
|
||||
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) {
|
||||
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));
|
||||
}
|
||||
@ -196,12 +167,12 @@ MotionPlanner::shortest_path(const Point &from, const Point &to)
|
||||
return polyline;
|
||||
}
|
||||
|
||||
MotionPlannerGraph*
|
||||
MotionPlanner::init_graph(int island_idx)
|
||||
const MotionPlannerGraph& 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
|
||||
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
|
||||
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;
|
||||
|
||||
// get boundaries as lines
|
||||
MotionPlannerEnv env = this->get_env(island_idx);
|
||||
const MotionPlannerEnv &env = this->get_env(island_idx);
|
||||
Lines lines = env.env.lines();
|
||||
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());
|
||||
|
||||
// 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;
|
||||
|
||||
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]);
|
||||
graph->add_edge(v0_idx, v1_idx, dist);
|
||||
}
|
||||
|
||||
return graph;
|
||||
}
|
||||
return this->graphs[island_idx + 1];
|
||||
|
||||
return *this->graphs[island_idx + 1].get();
|
||||
}
|
||||
|
||||
Point
|
||||
MotionPlannerEnv::nearest_env_point(const Point &from, const Point &to) const
|
||||
Point 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
|
||||
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
|
||||
Points pp;
|
||||
for (ExPolygons::const_iterator ex = this->env.expolygons.begin(); ex != this->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;
|
||||
for (const ExPolygon &ex : this->env.expolygons) {
|
||||
for (const Polygon &hole : ex.holes)
|
||||
if (hole.contains(from))
|
||||
pp = hole;
|
||||
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 = this->env.expolygons.begin(); ex != this->env.expolygons.end(); ++ex) {
|
||||
Points contour_pp = ex->contour;
|
||||
pp.insert(pp.end(), contour_pp.begin(), contour_pp.end());
|
||||
}
|
||||
}
|
||||
if (pp.empty())
|
||||
for (const ExPolygon &ex : this->env.expolygons)
|
||||
append(pp, ex.contour.points);
|
||||
|
||||
/* Find the candidate result and check that it doesn't cross too many boundaries. */
|
||||
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) {
|
||||
// discard result
|
||||
pp.erase(pp.begin() + result);
|
||||
} else {
|
||||
} else
|
||||
return pp[result];
|
||||
}
|
||||
}
|
||||
|
||||
// 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
|
||||
// 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
|
||||
MotionPlannerGraph::add_edge(size_t from, size_t to, double weight)
|
||||
// Add a new directed edge to the adjacency graph.
|
||||
void MotionPlannerGraph::add_edge(size_t from, size_t to, double weight)
|
||||
{
|
||||
// extend adjacency list until this start node
|
||||
if (this->adjacency_list.size() < from+1)
|
||||
this->adjacency_list.resize(from+1);
|
||||
|
||||
this->adjacency_list[from].push_back(neighbor(to, weight));
|
||||
}
|
||||
|
||||
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();
|
||||
// Extend adjacency list until this start node.
|
||||
if (this->adjacency_list.size() < 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)));
|
||||
// Allocate new empty adjacency vectors.
|
||||
this->adjacency_list.resize(from + 1);
|
||||
}
|
||||
*/
|
||||
return point.nearest_point_index(this->nodes);
|
||||
this->adjacency_list[from].emplace_back(Neighbor(node_t(to), weight));
|
||||
}
|
||||
|
||||
Polyline
|
||||
MotionPlannerGraph::shortest_path(size_t from, size_t to)
|
||||
// Dijkstra's shortest path in a weighted graph from node_start to node_end.
|
||||
// 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
|
||||
if (this->adjacency_list.empty()) return Polyline();
|
||||
// This prevents a crash in case for some reason we got here with an empty adjacency list.
|
||||
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<node_t> previous(this->adjacency_list.size(), -1);
|
||||
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));
|
||||
distance[node_start] = 0.;
|
||||
|
||||
std::vector<weight_t> dist;
|
||||
std::vector<node_t> previous;
|
||||
{
|
||||
// number of nodes
|
||||
size_t n = this->adjacency_list.size();
|
||||
auto queue = make_mutable_priority_queue<node_t>(
|
||||
[&map_node_to_queue_id](const node_t &node, size_t idx) { map_node_to_queue_id[node] = idx; },
|
||||
[&distance](const node_t &node1, const node_t &node2) { return distance[node1] < distance[node2]; });
|
||||
queue.reserve(this->adjacency_list.size());
|
||||
for (size_t i = 0; i < this->adjacency_list.size(); ++ i)
|
||||
queue.push(node_t(i));
|
||||
|
||||
// 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);
|
||||
|
||||
// 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())
|
||||
{
|
||||
// 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];
|
||||
}
|
||||
while (! queue.empty()) {
|
||||
// Get the next node with the lowest distance to node_start.
|
||||
node_t u = node_t(queue.top());
|
||||
queue.pop();
|
||||
map_node_to_queue_id[u] = size_t(-1);
|
||||
// Stop searching if we reached our destination.
|
||||
if (u == node_end)
|
||||
break;
|
||||
// Visit each edge starting at node u.
|
||||
for (const Neighbor& neighbor : this->adjacency_list[u])
|
||||
if (map_node_to_queue_id[neighbor.target] != size_t(-1)) {
|
||||
weight_t alt = distance[u] + neighbor.weight;
|
||||
// If total distance through u is shorter than the previous
|
||||
// distance (if any) between node_start and neighbor.target, replace it.
|
||||
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;
|
||||
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[from]);
|
||||
polyline.points.push_back(this->nodes[node_start]);
|
||||
polyline.reverse();
|
||||
return polyline;
|
||||
}
|
||||
|
||||
@ -2,11 +2,13 @@
|
||||
#define slic3r_MotionPlanner_hpp_
|
||||
|
||||
#include "libslic3r.h"
|
||||
#include "BoundingBox.hpp"
|
||||
#include "ClipperUtils.hpp"
|
||||
#include "ExPolygonCollection.hpp"
|
||||
#include "Polyline.hpp"
|
||||
#include <map>
|
||||
#include <utility>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#define MP_INNER_MARGIN scale_(1.0)
|
||||
@ -20,11 +22,12 @@ class MotionPlannerEnv
|
||||
{
|
||||
friend class MotionPlanner;
|
||||
|
||||
public:
|
||||
ExPolygon island;
|
||||
public:
|
||||
ExPolygon island;
|
||||
BoundingBox island_bbox;
|
||||
ExPolygonCollection env;
|
||||
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;
|
||||
};
|
||||
|
||||
@ -32,43 +35,43 @@ class MotionPlannerGraph
|
||||
{
|
||||
friend class MotionPlanner;
|
||||
|
||||
private:
|
||||
typedef int node_t;
|
||||
typedef double weight_t;
|
||||
struct neighbor {
|
||||
node_t target;
|
||||
private:
|
||||
typedef int node_t;
|
||||
typedef double weight_t;
|
||||
struct Neighbor {
|
||||
node_t target;
|
||||
weight_t weight;
|
||||
neighbor(node_t arg_target, weight_t arg_weight)
|
||||
: target(arg_target), weight(arg_weight) { }
|
||||
Neighbor(node_t arg_target, weight_t 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;
|
||||
|
||||
public:
|
||||
Points nodes;
|
||||
//std::map<std::pair<size_t,size_t>, double> edges;
|
||||
void add_edge(size_t from, size_t to, double weight);
|
||||
size_t find_node(const Point &point) const;
|
||||
Polyline shortest_path(size_t from, size_t to);
|
||||
public:
|
||||
Points nodes;
|
||||
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); }
|
||||
Polyline shortest_path(size_t from, size_t to) const;
|
||||
};
|
||||
|
||||
class MotionPlanner
|
||||
{
|
||||
public:
|
||||
public:
|
||||
MotionPlanner(const ExPolygons &islands);
|
||||
~MotionPlanner();
|
||||
Polyline shortest_path(const Point &from, const Point &to);
|
||||
size_t islands_count() const;
|
||||
~MotionPlanner() {}
|
||||
|
||||
private:
|
||||
bool initialized;
|
||||
std::vector<MotionPlannerEnv> islands;
|
||||
MotionPlannerEnv outer;
|
||||
std::vector<MotionPlannerGraph*> graphs;
|
||||
Polyline shortest_path(const Point &from, const Point &to);
|
||||
size_t islands_count() const { return this->islands.size(); }
|
||||
|
||||
void initialize();
|
||||
MotionPlannerGraph* init_graph(int island_idx);
|
||||
const MotionPlannerEnv& get_env(int island_idx) const;
|
||||
private:
|
||||
bool initialized;
|
||||
std::vector<MotionPlannerEnv> islands;
|
||||
MotionPlannerEnv outer;
|
||||
std::vector<std::unique_ptr<MotionPlannerGraph>> graphs;
|
||||
|
||||
void initialize();
|
||||
const MotionPlannerGraph& init_graph(int island_idx);
|
||||
const MotionPlannerEnv& get_env(int island_idx) const
|
||||
{ return (island_idx == -1) ? this->outer : this->islands[island_idx]; }
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
147
xs/src/libslic3r/MutablePriorityQueue.hpp
Normal file
147
xs/src/libslic3r/MutablePriorityQueue.hpp
Normal 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_ */
|
||||
@ -94,8 +94,7 @@ inline T sqr(const T x)
|
||||
return x * x;
|
||||
}
|
||||
|
||||
int
|
||||
Point::nearest_point_index(const PointConstPtrs &points) const
|
||||
int Point::nearest_point_index(const PointConstPtrs &points) const
|
||||
{
|
||||
int idx = -1;
|
||||
double distance = -1; // double because long is limited to 2147483647 on some platforms and it's not enough
|
||||
@ -121,26 +120,23 @@ Point::nearest_point_index(const PointConstPtrs &points) const
|
||||
}
|
||||
|
||||
/* 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 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
|
||||
size_t idx = size_t(-1);
|
||||
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) {
|
||||
// distance from this to candidate
|
||||
double d = sqr<double>(this->x - p->x) + sqr<double>(this->y - p->y);
|
||||
|
||||
// distance from candidate to dest
|
||||
d += sqr<double>(p->x - dest.x) + sqr<double>(p->y - dest.y);
|
||||
|
||||
// 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;
|
||||
for (const Point &p : points) {
|
||||
double d2 =
|
||||
// distance from this to candidate
|
||||
sqr<double>(this->x - p.x) + sqr<double>(this->y - p.y) +
|
||||
// distance from candidate to dest
|
||||
sqr<double>(p.x - dest.x) + sqr<double>(p.y - dest.y);
|
||||
if (d2 < d2min) {
|
||||
idx = &p - points.data();
|
||||
d2min = d2;
|
||||
if (d2min < EPSILON)
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return idx;
|
||||
|
||||
@ -127,6 +127,12 @@ void remove_nulls(std::vector<T*> &vec)
|
||||
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
|
||||
|
||||
#endif
|
||||
|
||||
Loading…
Reference in New Issue
Block a user