mirror of
				https://github.com/SoftFever/OrcaSlicer.git
				synced 2025-11-02 20:51:23 -07:00 
			
		
		
		
	Lots of improvements to MotionPlanner/avoid_crossing_perimeters. Smoother paths and several edge cases now handled better
This commit is contained in:
		
							parent
							
								
									5e100abe25
								
							
						
					
					
						commit
						8f4cbefd0d
					
				
					 17 changed files with 386 additions and 113 deletions
				
			
		| 
						 | 
				
			
			@ -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");
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -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);
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -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
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -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 ¢er);
 | 
			
		||||
    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;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
}
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -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;
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -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:
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -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();
 | 
			
		||||
}
 | 
			
		||||
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -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;
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -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
 | 
			
		||||
{
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -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);
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -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
 | 
			
		||||
{
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -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;
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -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
 | 
			
		||||
{
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -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;
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -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__
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -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();
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
		Loading…
	
	Add table
		Add a link
		
	
		Reference in a new issue