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

xs/src/libslic3r/MotionPlanner.cpp

@@ -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();
-    
-    const weight_t max_weight = std::numeric_limits<weight_t>::infinity();
-    
-    std::vector<weight_t> dist;
-    std::vector<node_t> previous;
-    {
-        // 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);
-        
-        // 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];
-                    }
+    // This prevents a crash in case for some reason we got here with an empty adjacency list.
+    if (this->adjacency_list.empty())
+        return Polyline();
+
+    // 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.;
+
+    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));
+
+    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;
 }