Fixes and improvements to MotionPlanner, much smarter now

1 files changed, 139 insertions, 111 deletions

diff --git a/xs/src/libslic3r/MotionPlanner.cpp b/xs/src/libslic3r/MotionPlanner.cpp
index ee8378588..21afb37c4 100644
--- a/xs/src/libslic3r/MotionPlanner.cpp
+++ b/xs/src/libslic3r/MotionPlanner.cpp
@@ -10,8 +10,15 @@ using boost::polygon::voronoi_diagram;
 namespace Slic3r {
 
 MotionPlanner::MotionPlanner(const ExPolygons &islands)
-    : islands(islands), initialized(false)
-{}
+    : 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));
+}
 
 MotionPlanner::~MotionPlanner()
 {
@@ -31,75 +38,62 @@ MotionPlanner::initialize()
     if (this->initialized) return;
     if (this->islands.empty()) return;  // prevent initialization of empty BoundingBox
     
-    ExPolygons expp;
-    for (ExPolygons::const_iterator island = this->islands.begin(); island != this->islands.end(); ++island) {
-        island->simplify(SCALED_EPSILON, expp);
-    }
-    this->islands = expp;
-    
     // loop through islands in order to create inner expolygons and collect their contours
-    this->inner.reserve(this->islands.size());
     Polygons outer_holes;
-    for (ExPolygons::const_iterator island = this->islands.begin(); island != this->islands.end(); ++island) {
-        this->inner.push_back(ExPolygonCollection());
-        offset(*island, &this->inner.back().expolygons, -MP_INNER_MARGIN);
+    for (std::vector<MotionPlannerEnv>::iterator island = this->islands.begin(); island != this->islands.end(); ++island) {
+        // 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);
         
-        outer_holes.push_back(island->contour);
+        // island contours are holes of our external environment
+        outer_holes.push_back(island->island.contour);
     }
     
-    // grow island contours in order to prepare holes of the outer environment
-    // This is actually wrong because it might merge contours that are close,
-    // thus confusing the island check in shortest_path() below
-    //offset(outer_holes, &outer_holes, +MP_OUTER_MARGIN);
-    
     // generate outer contour as bounding box of everything
-    Points points;
+    BoundingBox bb;
     for (Polygons::const_iterator contour = outer_holes.begin(); contour != outer_holes.end(); ++contour)
-        points.insert(points.end(), contour->points.begin(), contour->points.end());
-    BoundingBox bb(points);
+        bb.merge(contour->bounding_box());
     
     // grow outer contour
-    Polygons contour;
-    offset(bb.polygon(), &contour, +MP_OUTER_MARGIN);
+    Polygons contour = offset(bb.polygon(), +MP_OUTER_MARGIN*2);
     assert(contour.size() == 1);
     
     // make expolygon for outer environment
-    ExPolygons outer;
-    diff(contour, outer_holes, &outer);
+    ExPolygons outer = diff_ex(contour, outer_holes);
     assert(outer.size() == 1);
-    this->outer = outer.front();
+    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->initialized = true;
 }
 
-ExPolygonCollection
+const MotionPlannerEnv&
 MotionPlanner::get_env(int island_idx) const
 {
     if (island_idx == -1) {
-        return ExPolygonCollection(this->outer);
+        return this->outer;
     } else {
-        return this->inner[island_idx];
+        return this->islands[island_idx];
     }
 }
 
 Polyline
 MotionPlanner::shortest_path(const Point &from, const Point &to)
 {
-    // lazy generation of configuration space
-    if (!this->initialized) this->initialize();
-    
     // if we have an empty configuration space, return a straight move
     if (this->islands.empty())
         return Line(from, to);
     
     // Are both points in the same island?
     int island_idx = -1;
-    for (ExPolygons::const_iterator island = this->islands.begin(); island != this->islands.end(); ++island) {
-        if (island->contains(from) && island->contains(to)) {
+    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->contains(Line(from, to)))
+            if (island->island.contains(Line(from, to)))
                 return Line(from, to);
             
             island_idx = island - this->islands.begin();
@@ -107,25 +101,35 @@ MotionPlanner::shortest_path(const Point &from, const Point &to)
         }
     }
     
+    // lazy generation of configuration space
+    this->initialize();
+    
     // get environment
-    ExPolygonCollection env = this->get_env(island_idx);
-    if (env.expolygons.empty()) {
+    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
         return Line(from, to);
     }
     
     // Now check whether points are inside the environment.
-    Point inner_from    = from;
-    Point inner_to      = to;
-
-    if (!env.contains(from)) {
-        // Find the closest inner point to start from.
-        inner_from = this->nearest_env_point(env, from, to);
-    }
-    if (!env.contains(to)) {
-        // Find the closest inner point to start from.
-        inner_to = this->nearest_env_point(env, to, inner_from);
+    Point inner_from = from;
+    Point inner_to   = to;
+    
+    if (island_idx == -1) {
+        // TODO: instead of using the nearest_env_point() logic, we should
+        // create a temporary graph where we connect 'from' and 'to' to the
+        // 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)) {
+            // Find the closest inner point to start from.
+            inner_from = env.nearest_env_point(from, to);
+        }
+        if (!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
@@ -138,16 +142,39 @@ MotionPlanner::shortest_path(const Point &from, const Point &to)
     {
         // grow our environment slightly in order for simplify_by_visibility()
         // to work best by considering moves on boundaries valid as well
-        ExPolygonCollection grown_env;
-        offset(env, &grown_env.expolygons, +SCALED_EPSILON);
+        ExPolygonCollection grown_env(offset_ex(env.env, +SCALED_EPSILON));
+        
+        if (island_idx == -1) {
+            /*  If 'from' or 'to' are not inside our env, they were connected using the 
+                nearest_env_point() search which maybe produce ugly paths since it does not
+                include the endpoint in the Dijkstra search; the simplify_by_visibility() 
+                call below will not work in many cases where the endpoint is not contained in
+                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)) {
+                // 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((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((Lines)Line(*(polyline.points.end() - 3), to), grown_env).size() == 1) {
+                    polyline.points.erase(polyline.points.end() - 2);
+                }
+            }
+        }
         
         // remove unnecessary vertices
-        polyline.simplify_by_visibility(grown_env);
-    }
+        // Note: this is computationally intensive and does not look very necessary
+        // now that we prune the endpoints with the logic above,
+        // so we comment it for now until a good test case arises
+        //polyline.simplify_by_visibility(grown_env);
     
-    /*
+        /*
         SVG svg("shortest_path.svg");
-        svg.draw(this->outer);
+        svg.draw(grown_env.expolygons);
         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()];
@@ -163,63 +190,10 @@ MotionPlanner::shortest_path(const Point &from, const Point &to)
         svg.draw(inner_to, "red");
         svg.draw(polyline, "red");
         svg.Close();
-    */
-    
-    return polyline;
-}
-
-Point
-MotionPlanner::nearest_env_point(const ExPolygonCollection &env, const Point &from, const Point &to) const
-{
-    /*  In order to ensure that the move between 'from' and the initial env point does
-        not violate any of the configuration space boundaries, we limit our search to
-        the points that satisfy this condition. */
-    
-    /*  Assume that this method is never called when 'env' contains 'from';
-        so 'from' is either inside a hole or outside all contours */
-    
-    // get the points of the hole containing 'from', if any
-    Points pp;
-    for (ExPolygons::const_iterator ex = env.expolygons.begin(); ex != env.expolygons.end(); ++ex) {
-        for (Polygons::const_iterator h = ex->holes.begin(); h != ex->holes.end(); ++h) {
-            if (h->contains(from)) {
-                pp = *h;
-            }
-        }
-        if (!pp.empty()) break;
-    }
-    
-    /*  If 'from' is not inside a hole, it's outside of all contours, so take all
-        contours' points */
-    if (pp.empty()) {
-        for (ExPolygons::const_iterator ex = env.expolygons.begin(); ex != env.expolygons.end(); ++ex) {
-            Points contour_pp = ex->contour;
-            pp.insert(pp.end(), contour_pp.begin(), contour_pp.end());
-        }
-    }
-    
-    /*  Find the candidate result and check that it doesn't cross any boundary.
-        (We could skip all of the above polygon finding logic and directly test all points
-        in env, but this way we probably reduce complexity). */
-    Polygons env_pp = env;
-    while (pp.size() >= 2) {
-        // find the point in pp that is closest to both 'from' and 'to'
-        size_t result = from.nearest_waypoint_index(pp, to);
-        
-        if (intersects((Lines)Line(from, pp[result]), env_pp)) {
-            // discard result
-            pp.erase(pp.begin() + result);
-        } else {
-            return pp[result];
-        }
+        */
     }
     
-    // if we're here, return last point 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 polyline;
 }
 
 MotionPlannerGraph*
@@ -240,8 +214,8 @@ MotionPlanner::init_graph(int island_idx)
         t_vd_vertices vd_vertices;
         
         // get boundaries as lines
-        ExPolygonCollection env = this->get_env(island_idx);
-        Lines lines = env.lines();
+        MotionPlannerEnv env = this->get_env(island_idx);
+        Lines lines = env.env.lines();
         boost::polygon::construct_voronoi(lines.begin(), lines.end(), &vd);
         
         // traverse the Voronoi diagram and generate graph nodes and edges
@@ -254,7 +228,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
-            if (!env.contains_b(p0) || !env.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);
             size_t v0_idx;
@@ -284,6 +258,60 @@ MotionPlanner::init_graph(int island_idx)
     return this->graphs[island_idx + 1];
 }
 
+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
+        the points that satisfy this condition. */
+    
+    /*  Assume that this method is never called when 'env' contains 'from';
+        so 'from' is either inside a hole or outside all contours */
+    
+    // get the points of the hole containing 'from', if any
+    Points pp;
+    for (ExPolygons::const_iterator ex = 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;
+    }
+    
+    /*  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());
+        }
+    }
+    
+    /*  Find the candidate result and check that it doesn't cross too many boundaries. */
+    while (pp.size() >= 2) {
+        // find the point in pp that is closest to both 'from' and 'to'
+        size_t result = from.nearest_waypoint_index(pp, to);
+        
+        // as we assume 'from' is outside env, any node will require at least one crossing
+        if (intersection((Lines)Line(from, pp[result]), this->island).size() > 1) {
+            // discard result
+            pp.erase(pp.begin() + result);
+        } 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;
+}
+
 void
 MotionPlannerGraph::add_edge(size_t from, size_t to, double weight)
 {