diff --git a/lib/Slic3r/GCode.pm b/lib/Slic3r/GCode.pm
index 70d0330c2..f39ded634 100644
--- a/lib/Slic3r/GCode.pm
+++ b/lib/Slic3r/GCode.pm
@@ -620,12 +620,11 @@ sub travel_to {
         
         # represent last_pos in absolute G-code coordinates
         my $last_pos = $gcodegen->last_pos->clone;
-        $last_pos->translate(@{$gcodegen->origin});
+        $last_pos->translate(@$scaled_origin);
         
         # represent $point in absolute G-code coordinates
         $point = $point->clone;
         $point->translate(@$scaled_origin);
-        
         # calculate path
         my $travel = $self->_external_mp->shortest_path($last_pos, $point);
         
diff --git a/xs/src/libslic3r/ExPolygon.cpp b/xs/src/libslic3r/ExPolygon.cpp
index 34b75940b..2c5246aad 100644
--- a/xs/src/libslic3r/ExPolygon.cpp
+++ b/xs/src/libslic3r/ExPolygon.cpp
@@ -105,6 +105,23 @@ ExPolygon::contains(const Point &point) const
     return true;
 }
 
+// inclusive version of contains() that also checks whether point is on boundaries
+bool
+ExPolygon::contains_b(const Point &point) const
+{
+    return this->contains(point) || this->has_boundary_point(point);
+}
+
+bool
+ExPolygon::has_boundary_point(const Point &point) const
+{
+    if (this->contour.has_boundary_point(point)) return true;
+    for (Polygons::const_iterator h = this->holes.begin(); h != this->holes.end(); ++h) {
+        if (h->has_boundary_point(point)) return true;
+    }
+    return false;
+}
+
 Polygons
 ExPolygon::simplify_p(double tolerance) const
 {
@@ -364,6 +381,16 @@ ExPolygon::triangulate_p2t(Polygons* polygons) const
     }
 }
 
+Lines
+ExPolygon::lines() const
+{
+    Lines lines;
+    this->contour.lines(&lines);
+    for (Polygons::const_iterator h = this->holes.begin(); h != this->holes.end(); ++h)
+        h->lines(&lines);
+    return lines;
+}
+
 #ifdef SLIC3RXS
 
 REGISTER_CLASS(ExPolygon, "ExPolygon");
diff --git a/xs/src/libslic3r/ExPolygon.hpp b/xs/src/libslic3r/ExPolygon.hpp
index 9d65fcb2a..3d7cd3540 100644
--- a/xs/src/libslic3r/ExPolygon.hpp
+++ b/xs/src/libslic3r/ExPolygon.hpp
@@ -25,6 +25,8 @@ class ExPolygon
     bool contains(const Line &line) const;
     bool contains(const Polyline &polyline) const;
     bool contains(const Point &point) const;
+    bool contains_b(const Point &point) const;
+    bool has_boundary_point(const Point &point) const;
     Polygons simplify_p(double tolerance) const;
     ExPolygons simplify(double tolerance) const;
     void simplify(double tolerance, ExPolygons &expolygons) const;
@@ -36,6 +38,7 @@ class ExPolygon
     void triangulate(Polygons* polygons) const;
     void triangulate_pp(Polygons* polygons) const;
     void triangulate_p2t(Polygons* polygons) const;
+    Lines lines() const;
     
     #ifdef SLIC3RXS
     void from_SV(SV* poly_sv);
diff --git a/xs/src/libslic3r/ExPolygonCollection.cpp b/xs/src/libslic3r/ExPolygonCollection.cpp
index 6f29dbb0d..aaa747d8d 100644
--- a/xs/src/libslic3r/ExPolygonCollection.cpp
+++ b/xs/src/libslic3r/ExPolygonCollection.cpp
@@ -3,6 +3,11 @@
 
 namespace Slic3r {
 
+ExPolygonCollection::ExPolygonCollection(const ExPolygon &expolygon)
+{
+    this->expolygons.push_back(expolygon);
+}
+
 ExPolygonCollection::operator Points() const
 {
     Points points;
@@ -68,6 +73,15 @@ template bool ExPolygonCollection::contains<Point>(const Point &item) const;
 template bool ExPolygonCollection::contains<Line>(const Line &item) const;
 template bool ExPolygonCollection::contains<Polyline>(const Polyline &item) const;
 
+bool
+ExPolygonCollection::contains_b(const Point &point) const
+{
+    for (ExPolygons::const_iterator it = this->expolygons.begin(); it != this->expolygons.end(); ++it) {
+        if (it->contains_b(point)) return true;
+    }
+    return false;
+}
+
 void
 ExPolygonCollection::simplify(double tolerance)
 {
@@ -87,6 +101,17 @@ ExPolygonCollection::convex_hull(Polygon* hull) const
     Slic3r::Geometry::convex_hull(pp, hull);
 }
 
+Lines
+ExPolygonCollection::lines() const
+{
+    Lines lines;
+    for (ExPolygons::const_iterator it = this->expolygons.begin(); it != this->expolygons.end(); ++it) {
+        Lines ex_lines = it->lines();
+        lines.insert(lines.end(), ex_lines.begin(), ex_lines.end());
+    }
+    return lines;
+}
+
 #ifdef SLIC3RXS
 REGISTER_CLASS(ExPolygonCollection, "ExPolygon::Collection");
 #endif
diff --git a/xs/src/libslic3r/ExPolygonCollection.hpp b/xs/src/libslic3r/ExPolygonCollection.hpp
index 6aa5b0a56..cca6064d5 100644
--- a/xs/src/libslic3r/ExPolygonCollection.hpp
+++ b/xs/src/libslic3r/ExPolygonCollection.hpp
@@ -17,6 +17,7 @@ class ExPolygonCollection
     ExPolygons expolygons;
     
     ExPolygonCollection() {};
+    ExPolygonCollection(const ExPolygon &expolygon);
     ExPolygonCollection(const ExPolygons &expolygons) : expolygons(expolygons) {};
     operator Points() const;
     operator Polygons() const;
@@ -25,8 +26,10 @@ class ExPolygonCollection
     void translate(double x, double y);
     void rotate(double angle, const Point &center);
     template <class T> bool contains(const T &item) const;
+    bool contains_b(const Point &point) const;
     void simplify(double tolerance);
     void convex_hull(Polygon* hull) const;
+    Lines lines() const;
 };
 
 }
diff --git a/xs/src/libslic3r/Line.cpp b/xs/src/libslic3r/Line.cpp
index ef8598ada..3cce6c971 100644
--- a/xs/src/libslic3r/Line.cpp
+++ b/xs/src/libslic3r/Line.cpp
@@ -16,6 +16,13 @@ Line::wkt() const
     return ss.str();
 }
 
+Line::operator Lines() const
+{
+    Lines lines;
+    lines.push_back(*this);
+    return lines;
+}
+
 Line::operator Polyline() const
 {
     Polyline pl;
diff --git a/xs/src/libslic3r/Line.hpp b/xs/src/libslic3r/Line.hpp
index 52f3ef77f..76c385ce6 100644
--- a/xs/src/libslic3r/Line.hpp
+++ b/xs/src/libslic3r/Line.hpp
@@ -9,6 +9,7 @@ namespace Slic3r {
 class Line;
 class Linef3;
 class Polyline;
+typedef std::vector<Line> Lines;
 
 class Line
 {
@@ -18,6 +19,7 @@ class Line
     Line() {};
     explicit Line(Point _a, Point _b): a(_a), b(_b) {};
     std::string wkt() const;
+    operator Lines() const;
     operator Polyline() const;
     void scale(double factor);
     void translate(double x, double y);
@@ -45,8 +47,6 @@ class Line
     #endif
 };
 
-typedef std::vector<Line> Lines;
-
 class Linef3
 {
     public:
diff --git a/xs/src/libslic3r/MotionPlanner.cpp b/xs/src/libslic3r/MotionPlanner.cpp
index 88b69ef82..9c76a3cd5 100644
--- a/xs/src/libslic3r/MotionPlanner.cpp
+++ b/xs/src/libslic3r/MotionPlanner.cpp
@@ -72,11 +72,23 @@ MotionPlanner::initialize()
     this->initialized = true;
 }
 
+ExPolygonCollection
+MotionPlanner::get_env(size_t island_idx) const
+{
+    if (island_idx == -1) {
+        return ExPolygonCollection(this->outer);
+    } else {
+        return this->inner[island_idx];
+    }
+}
+
 void
 MotionPlanner::shortest_path(const Point &from, const Point &to, Polyline* polyline)
 {
+    // lazy generation of configuration space
     if (!this->initialized) this->initialize();
     
+    // if we have an empty configuration space, return a straight move
     if (this->islands.empty()) {
         polyline->points.push_back(from);
         polyline->points.push_back(to);
@@ -103,111 +115,163 @@ MotionPlanner::shortest_path(const Point &from, const Point &to, Polyline* polyl
     Point inner_from    = from;
     Point inner_to      = to;
     bool from_is_inside, to_is_inside;
-    if (island_idx == -1) {
-        if (!(from_is_inside = this->outer.contains(from))) {
-            // Find the closest inner point to start from.
-            from.nearest_point(this->outer, &inner_from);
-        }
-        if (!(to_is_inside = this->outer.contains(to))) {
-            // Find the closest inner point to start from.
-            to.nearest_point(this->outer, &inner_to);
-        }
-    } else {
-        if (!(from_is_inside = this->inner[island_idx].contains(from))) {
-            // Find the closest inner point to start from.
-            from.nearest_point(this->inner[island_idx], &inner_from);
-        }
-        if (!(to_is_inside = this->inner[island_idx].contains(to))) {
-            // Find the closest inner point to start from.
-            to.nearest_point(this->inner[island_idx], &inner_to);
-        }
+    
+    ExPolygonCollection env = this->get_env(island_idx);
+    if (!(from_is_inside = env.contains(from))) {
+        // Find the closest inner point to start from.
+        inner_from = this->nearest_env_point(env, from, to);
+    }
+    if (!(to_is_inside = env.contains(to))) {
+        // Find the closest inner point to start from.
+        inner_to = this->nearest_env_point(env, to, inner_from);
     }
     
     // perform actual path search
     MotionPlannerGraph* graph = this->init_graph(island_idx);
     graph->shortest_path(graph->find_node(inner_from), graph->find_node(inner_to), polyline);
     
-    polyline->points.insert(polyline->points.begin(), from);
-    polyline->points.push_back(to);
+    if (!from_is_inside)
+        polyline->points.insert(polyline->points.begin(), from);
+    
+    if (!to_is_inside)
+        polyline->points.push_back(to);
+    
+    {
+        // grow our environment slightly in order for simplify_by_visibility()
+        // to work best by considering moves on boundaries valid as well
+        ExPolygonCollection grown_env;
+        offset(env, &grown_env.expolygons, +SCALED_EPSILON);
+        
+        // remove unnecessary vertices
+        polyline->simplify_by_visibility(grown_env);
+    }
+    
+    /*
+        SVG svg("shortest_path.svg");
+        svg.draw(this->outer);
+        svg.arrows = false;
+        for (MotionPlannerGraph::adjacency_list_t::const_iterator it = graph->adjacency_list.begin(); it != graph->adjacency_list.end(); ++it) {
+            Point a = graph->nodes[it - graph->adjacency_list.begin()];
+            for (std::vector<MotionPlannerGraph::neighbor>::const_iterator n = it->begin(); n != it->end(); ++n) {
+                Point b = graph->nodes[n->target];
+                svg.draw(Line(a, b));
+            }
+        }
+        svg.arrows = true;
+        svg.draw(from);
+        svg.draw(inner_from, "red");
+        svg.draw(to);
+        svg.draw(inner_to, "red");
+        svg.draw(*polyline, "red");
+        svg.Close();
+    */
+}
+
+Point
+MotionPlanner::nearest_env_point(const ExPolygonCollection &env, const Point &from, const Point &to) const
+{
+    /*  In order to ensure that the move between 'from' and the initial env point does
+        not violate any of the configuration space boundaries, we limit our search to
+        the points that satisfy this condition. */
+    
+    /*  Assume that this method is never called when 'env' contains 'from';
+        so 'from' is either inside a hole or outside all contours */
+    
+    // get the points of the hole containing 'from', if any
+    Points pp;
+    for (ExPolygons::const_iterator ex = env.expolygons.begin(); ex != env.expolygons.end(); ++ex) {
+        for (Polygons::const_iterator h = ex->holes.begin(); h != ex->holes.end(); ++h) {
+            if (h->contains(from)) {
+                pp = *h;
+            }
+        }
+        if (!pp.empty()) break;
+    }
+    
+    /*  If 'from' is not inside a hole, it's outside of all contours, so take all
+        contours' points */
+    if (pp.empty()) {
+        for (ExPolygons::const_iterator ex = env.expolygons.begin(); ex != env.expolygons.end(); ++ex) {
+            Points contour_pp = ex->contour;
+            pp.insert(pp.end(), contour_pp.begin(), contour_pp.end());
+        }
+    }
+    
+    /*  Find the candidate result and check that it doesn't cross any boundary.
+        (We could skip all of the above polygon finding logic and directly test all points
+        in env, but this way we probably reduce complexity). */
+    Polygons env_pp = env;
+    while (pp.size() >= 2) {
+        // find the point in pp that is closest to both 'from' and 'to'
+        size_t result = from.nearest_waypoint_index(pp, to);
+        
+        if (intersects((Lines)Line(from, pp[result]), env_pp)) {
+            // discard result
+            pp.erase(pp.begin() + result);
+        } else {
+            return pp[result];
+        }
+    }
+    
+    // if we're here, return last point (better than nothing)
+    return pp.front();
 }
 
 MotionPlannerGraph*
 MotionPlanner::init_graph(int island_idx)
 {
     if (this->graphs[island_idx + 1] == NULL) {
-        Polygons pp;
-        if (island_idx == -1) {
-            pp = this->outer;
-        } else {
-            pp = this->inner[island_idx];
-        }
-        
+        // if this graph doesn't exist, initialize it
         MotionPlannerGraph* graph = this->graphs[island_idx + 1] = new MotionPlannerGraph();
         
-        // add polygon boundaries as edges
-        size_t node_idx = 0;
-        Lines lines;
-        for (Polygons::const_iterator polygon = pp.begin(); polygon != pp.end(); ++polygon) {
-            graph->nodes.push_back(polygon->points.back());
-            node_idx++;
-            for (Points::const_iterator p = polygon->points.begin(); p != polygon->points.end(); ++p) {
-                graph->nodes.push_back(*p);
-                double dist = graph->nodes[node_idx-1].distance_to(*p);
-                graph->add_edge(node_idx-1, node_idx, dist);
-                graph->add_edge(node_idx, node_idx-1, dist);
-                node_idx++;
-            }
-            polygon->lines(&lines);
-        }
+        /*  We don't add polygon boundaries as graph edges, because we'd need to connect
+            them to the Voronoi-generated edges by recognizing coinciding nodes. */
         
-        // add Voronoi edges as internal edges
-        {
-            typedef voronoi_diagram<double> VD;
-            typedef std::map<const VD::vertex_type*,size_t> t_vd_vertices;
-            VD vd;
-            t_vd_vertices vd_vertices;
+        typedef voronoi_diagram<double> VD;
+        VD vd;
+        
+        // mapping between Voronoi vertices and graph nodes
+        typedef std::map<const VD::vertex_type*,size_t> t_vd_vertices;
+        t_vd_vertices vd_vertices;
+        
+        // get boundaries as lines
+        ExPolygonCollection env = this->get_env(island_idx);
+        Lines lines = env.lines();
+        boost::polygon::construct_voronoi(lines.begin(), lines.end(), &vd);
+        
+        // traverse the Voronoi diagram and generate graph nodes and edges
+        for (VD::const_edge_iterator edge = vd.edges().begin(); edge != vd.edges().end(); ++edge) {
+            if (edge->is_infinite()) continue;
             
-            boost::polygon::construct_voronoi(lines.begin(), lines.end(), &vd);
-            for (VD::const_edge_iterator edge = vd.edges().begin(); edge != vd.edges().end(); ++edge) {
-                if (edge->is_infinite()) continue;
-                
-                const VD::vertex_type* v0 = edge->vertex0();
-                const VD::vertex_type* v1 = edge->vertex1();
-                Point p0 = Point(v0->x(), v0->y());
-                Point p1 = Point(v1->x(), v1->y());
-                // contains() should probably be faster than contains(),
-                // and should it fail on any boundary points it's not a big problem
-                if (island_idx == -1) {
-                    if (!this->outer.contains(p0) || !this->outer.contains(p1)) continue;
-                } else {
-                    if (!this->inner[island_idx].contains(p0) || !this->inner[island_idx].contains(p1)) continue;
-                }
-                
-                t_vd_vertices::const_iterator i_v0 = vd_vertices.find(v0);
-                size_t v0_idx;
-                if (i_v0 == vd_vertices.end()) {
-                    graph->nodes.push_back(p0);
-                    v0_idx = node_idx;
-                    vd_vertices[v0] = node_idx;
-                    node_idx++;
-                } else {
-                    v0_idx = i_v0->second;
-                }
-                
-                t_vd_vertices::const_iterator i_v1 = vd_vertices.find(v1);
-                size_t v1_idx;
-                if (i_v1 == vd_vertices.end()) {
-                    graph->nodes.push_back(p1);
-                    v1_idx = node_idx;
-                    vd_vertices[v1] = node_idx;
-                    node_idx++;
-                } else {
-                    v1_idx = i_v1->second;
-                }
-                
-                double dist = graph->nodes[v0_idx].distance_to(graph->nodes[v1_idx]);
-                graph->add_edge(v0_idx, v1_idx, dist);
+            const VD::vertex_type* v0 = edge->vertex0();
+            const VD::vertex_type* v1 = edge->vertex1();
+            Point p0 = Point(v0->x(), v0->y());
+            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;
+            
+            t_vd_vertices::const_iterator i_v0 = vd_vertices.find(v0);
+            size_t v0_idx;
+            if (i_v0 == vd_vertices.end()) {
+                graph->nodes.push_back(p0);
+                vd_vertices[v0] = v0_idx = graph->nodes.size()-1;
+            } else {
+                v0_idx = i_v0->second;
             }
+            
+            t_vd_vertices::const_iterator i_v1 = vd_vertices.find(v1);
+            size_t v1_idx;
+            if (i_v1 == vd_vertices.end()) {
+                graph->nodes.push_back(p1);
+                vd_vertices[v1] = v1_idx = graph->nodes.size()-1;
+            } else {
+                v1_idx = i_v1->second;
+            }
+            
+            // Euclidean distance is used as weight for the graph edge
+            double dist = graph->nodes[v0_idx].distance_to(graph->nodes[v1_idx]);
+            graph->add_edge(v0_idx, v1_idx, dist);
         }
         
         return graph;
@@ -244,38 +308,61 @@ MotionPlannerGraph::shortest_path(size_t from, size_t to, Polyline* polyline)
     
     const weight_t max_weight = std::numeric_limits<weight_t>::infinity();
     
-    std::vector<weight_t> min_distance;
+    std::vector<weight_t> dist;
     std::vector<node_t> previous;
     {
-        int n = this->adjacency_list.size();
-        min_distance.clear();
-        min_distance.resize(n, max_weight);
-        min_distance[from] = 0;
+        // number of nodes
+        size_t n = this->adjacency_list.size();
+        
+        // initialize dist and previous
+        dist.clear();
+        dist.resize(n, max_weight);
+        dist[from] = 0;  // distance from 'from' to itself
         previous.clear();
         previous.resize(n, -1);
-        std::set<std::pair<weight_t, node_t> > vertex_queue;
-        vertex_queue.insert(std::make_pair(min_distance[from], from));
- 
-        while (!vertex_queue.empty()) 
+        
+        // initialize the Q with all nodes
+        std::set<node_t> Q;
+        for (node_t i = 0; i < n; ++i) Q.insert(i);
+        
+        while (!Q.empty()) 
         {
-            weight_t dist = vertex_queue.begin()->first;
-            node_t u = vertex_queue.begin()->second;
-            vertex_queue.erase(vertex_queue.begin());
- 
-            // Visit each edge exiting u
+            // get node in Q having the minimum dist ('from' in the first loop)
+            node_t u;
+            {
+                double min_dist = -1;
+                for (std::set<node_t>::const_iterator n = Q.begin(); n != Q.end(); ++n) {
+                    if (dist[*n] < min_dist || min_dist == -1) {
+                        u = *n;
+                        min_dist = dist[*n];
+                    }
+                }
+            }
+            Q.erase(u);
+            
+            // stop searching if we reached our destination
+            if (u == to) break;
+            
+            // Visit each edge starting from node u
             const std::vector<neighbor> &neighbors = this->adjacency_list[u];
             for (std::vector<neighbor>::const_iterator neighbor_iter = neighbors.begin();
                  neighbor_iter != neighbors.end();
                  neighbor_iter++)
             {
+                // neighbor node is v
                 node_t v = neighbor_iter->target;
-                weight_t weight = neighbor_iter->weight;
-                weight_t distance_through_u = dist + weight;
-                if (distance_through_u < min_distance[v]) {
-                    vertex_queue.erase(std::make_pair(min_distance[v], v));
-                    min_distance[v] = distance_through_u;
+                
+                // skip if we already visited this
+                if (Q.find(v) == Q.end()) continue;
+                
+                // calculate total distance
+                weight_t alt = dist[u] + neighbor_iter->weight;
+                
+                // if total distance through u is shorter than the previous
+                // distance (if any) between 'from' and 'v', replace it
+                if (alt < dist[v]) {
+                    dist[v]     = alt;
                     previous[v] = u;
-                    vertex_queue.insert(std::make_pair(min_distance[v], v));
                 }
 
             }
@@ -284,6 +371,7 @@ MotionPlannerGraph::shortest_path(size_t from, size_t to, Polyline* polyline)
     
     for (node_t vertex = to; vertex != -1; vertex = previous[vertex])
         polyline->points.push_back(this->nodes[vertex]);
+    polyline->points.push_back(this->nodes[from]);
     polyline->reverse();
 }
 
diff --git a/xs/src/libslic3r/MotionPlanner.hpp b/xs/src/libslic3r/MotionPlanner.hpp
index b40ed2ef7..c1617e184 100644
--- a/xs/src/libslic3r/MotionPlanner.hpp
+++ b/xs/src/libslic3r/MotionPlanner.hpp
@@ -33,10 +33,14 @@ class MotionPlanner
     
     void initialize();
     MotionPlannerGraph* init_graph(int island_idx);
+    ExPolygonCollection get_env(size_t island_idx) const;
+    Point nearest_env_point(const ExPolygonCollection &env, const Point &from, const Point &to) const;
 };
 
 class MotionPlannerGraph
 {
+    friend class MotionPlanner;
+    
     private:
     typedef size_t node_t;
     typedef double weight_t;
diff --git a/xs/src/libslic3r/MultiPoint.cpp b/xs/src/libslic3r/MultiPoint.cpp
index e4944edca..6ed430cf7 100644
--- a/xs/src/libslic3r/MultiPoint.cpp
+++ b/xs/src/libslic3r/MultiPoint.cpp
@@ -76,6 +76,13 @@ MultiPoint::find_point(const Point &point) const
     return -1;  // not found
 }
 
+bool
+MultiPoint::has_boundary_point(const Point &point) const
+{
+    double dist = point.distance_to(point.projection_onto(*this));
+    return dist < SCALED_EPSILON;
+}
+
 void
 MultiPoint::bounding_box(BoundingBox* bb) const
 {
diff --git a/xs/src/libslic3r/MultiPoint.hpp b/xs/src/libslic3r/MultiPoint.hpp
index eaca9b8eb..96c2876f5 100644
--- a/xs/src/libslic3r/MultiPoint.hpp
+++ b/xs/src/libslic3r/MultiPoint.hpp
@@ -30,6 +30,7 @@ class MultiPoint
     double length() const;
     bool is_valid() const;
     int find_point(const Point &point) const;
+    bool has_boundary_point(const Point &point) const;
     void bounding_box(BoundingBox* bb) const;
     
     static Points _douglas_peucker(const Points &points, const double tolerance);
diff --git a/xs/src/libslic3r/Point.cpp b/xs/src/libslic3r/Point.cpp
index d40a8efc7..9a564f879 100644
--- a/xs/src/libslic3r/Point.cpp
+++ b/xs/src/libslic3r/Point.cpp
@@ -104,6 +104,32 @@ Point::nearest_point_index(const PointConstPtrs &points) const
     return idx;
 }
 
+/* This method finds the point that is closest to both this point and the supplied one */
+size_t
+Point::nearest_waypoint_index(const Points &points, const Point &dest) const
+{
+    size_t idx = -1;
+    double distance = -1;  // double because long is limited to 2147483647 on some platforms and it's not enough
+    
+    for (Points::const_iterator p = points.begin(); p != points.end(); ++p) {
+        // distance from this to candidate
+        double d = pow(this->x - p->x, 2) + pow(this->y - p->y, 2);
+        
+        // distance from candidate to dest
+        d += pow(p->x - dest.x, 2) + pow(p->y - dest.y, 2);
+        
+        // if the total distance is greater than current min distance, ignore it
+        if (distance != -1 && d > distance) continue;
+        
+        idx = p - points.begin();
+        distance = d;
+        
+        if (distance < EPSILON) break;
+    }
+    
+    return idx;
+}
+
 int
 Point::nearest_point_index(const PointPtrs &points) const
 {
@@ -123,6 +149,15 @@ Point::nearest_point(const Points &points, Point* point) const
     return true;
 }
 
+bool
+Point::nearest_waypoint(const Points &points, const Point &dest, Point* point) const
+{
+    int idx = this->nearest_waypoint_index(points, dest);
+    if (idx == -1) return false;
+    *point = points.at(idx);
+    return true;
+}
+
 double
 Point::distance_to(const Point &point) const
 {
diff --git a/xs/src/libslic3r/Point.hpp b/xs/src/libslic3r/Point.hpp
index 6fa216405..a3283c260 100644
--- a/xs/src/libslic3r/Point.hpp
+++ b/xs/src/libslic3r/Point.hpp
@@ -45,7 +45,9 @@ class Point
     int nearest_point_index(const Points &points) const;
     int nearest_point_index(const PointConstPtrs &points) const;
     int nearest_point_index(const PointPtrs &points) const;
+    size_t nearest_waypoint_index(const Points &points, const Point &point) const;
     bool nearest_point(const Points &points, Point* point) const;
+    bool nearest_waypoint(const Points &points, const Point &dest, Point* point) const;
     double distance_to(const Point &point) const;
     double distance_to(const Line &line) const;
     double perp_distance_to(const Line &line) const;
diff --git a/xs/src/libslic3r/Polyline.cpp b/xs/src/libslic3r/Polyline.cpp
index 7f27adf58..ba41893f8 100644
--- a/xs/src/libslic3r/Polyline.cpp
+++ b/xs/src/libslic3r/Polyline.cpp
@@ -1,4 +1,6 @@
 #include "Polyline.hpp"
+#include "ExPolygon.hpp"
+#include "ExPolygonCollection.hpp"
 #include "Line.hpp"
 #include "Polygon.hpp"
 #include <iostream>
@@ -127,6 +129,36 @@ Polyline::simplify(double tolerance)
     this->points = MultiPoint::_douglas_peucker(this->points, tolerance);
 }
 
+/* This method simplifies all *lines* contained in the supplied area */
+template <class T>
+void
+Polyline::simplify_by_visibility(const T &area)
+{
+    Points &pp = this->points;
+    
+    // find first point in area
+    size_t start = 0;
+    while (start < pp.size() && !area.contains(pp[start])) {
+        start++;
+    }
+    
+    for (size_t s = start; s < pp.size() && !pp.empty(); ++s) {
+        // find the farthest point to which we can build
+        // a line that is contained in the supplied area
+        // a binary search would be more efficient for this
+        for (size_t e = pp.size()-1; e > (s + 1); --e) {
+            if (area.contains(Line(pp[s], pp[e]))) {
+                // we can suppress points between s and e
+                pp.erase(pp.begin() + s + 1, pp.begin() + e);
+                
+                // repeat recursively until no further simplification is possible
+                return this->simplify_by_visibility(area);
+            }
+        }
+    }
+}
+template void Polyline::simplify_by_visibility<ExPolygonCollection>(const ExPolygonCollection &area);
+
 void
 Polyline::split_at(const Point &point, Polyline* p1, Polyline* p2) const
 {
diff --git a/xs/src/libslic3r/Polyline.hpp b/xs/src/libslic3r/Polyline.hpp
index 2a9a79032..3fe89f26e 100644
--- a/xs/src/libslic3r/Polyline.hpp
+++ b/xs/src/libslic3r/Polyline.hpp
@@ -7,6 +7,7 @@
 
 namespace Slic3r {
 
+class ExPolygon;
 class Polyline;
 typedef std::vector<Polyline> Polylines;
 
@@ -23,6 +24,7 @@ class Polyline : public MultiPoint {
     void extend_start(double distance);
     void equally_spaced_points(double distance, Points* points) const;
     void simplify(double tolerance);
+    template <class T> void simplify_by_visibility(const T &area);
     void split_at(const Point &point, Polyline* p1, Polyline* p2) const;
     bool is_straight() const;
     std::string wkt() const;
diff --git a/xs/t/09_polyline.t b/xs/t/09_polyline.t
index 824f049e9..fa5a3c48b 100644
--- a/xs/t/09_polyline.t
+++ b/xs/t/09_polyline.t
@@ -4,7 +4,7 @@ use strict;
 use warnings;
 
 use Slic3r::XS;
-use Test::More tests => 18;
+use Test::More tests => 21;
 
 my $points = [
     [100, 100],
@@ -88,4 +88,40 @@ is_deeply $polyline->pp, [ @$points, @$points ], 'append_polyline';
     is scalar(@$p2), 4, 'split_at';
 }
 
+{
+    my $polyline = Slic3r::Polyline->new(
+        map [$_,10], (0,10,20,30,40,50,60)
+    );
+    {
+        my $expolygon = Slic3r::ExPolygon->new(Slic3r::Polygon->new(
+            [25,0], [55,0], [55,30], [25,30],
+        ));
+        my $p = $polyline->clone;
+        $p->simplify_by_visibility($expolygon);
+        is_deeply $p->pp, [
+            map [$_,10], (0,10,20,30,50,60)
+        ], 'simplify_by_visibility()';
+    }
+    {
+        my $expolygon = Slic3r::ExPolygon->new(Slic3r::Polygon->new(
+            [-15,0], [75,0], [75,30], [-15,30],
+        ));
+        my $p = $polyline->clone;
+        $p->simplify_by_visibility($expolygon);
+        is_deeply $p->pp, [
+            map [$_,10], (0,60)
+        ], 'simplify_by_visibility()';
+    }
+    {
+        my $expolygon = Slic3r::ExPolygon->new(Slic3r::Polygon->new(
+            [-15,0], [25,0], [25,30], [-15,30],
+        ));
+        my $p = $polyline->clone;
+        $p->simplify_by_visibility($expolygon);
+        is_deeply $p->pp, [
+            map [$_,10], (0,20,30,40,50,60)
+        ], 'simplify_by_visibility()';
+    }
+}
+
 __END__
diff --git a/xs/xsp/Polyline.xsp b/xs/xsp/Polyline.xsp
index 2df0d17c1..a8eb527d5 100644
--- a/xs/xsp/Polyline.xsp
+++ b/xs/xsp/Polyline.xsp
@@ -32,6 +32,8 @@
     void extend_end(double distance);
     void extend_start(double distance);
     void simplify(double tolerance);
+    void simplify_by_visibility(ExPolygon* expolygon)
+        %code{% THIS->simplify_by_visibility(*expolygon); %};
     void split_at(Point* point, Polyline* p1, Polyline* p2)
         %code{% THIS->split_at(*point, p1, p2); %};
     bool is_straight();