Implemented avoid_crossing_perimeters with VisiLibity

xs/src/BoundingBox.cpp

@@ -58,6 +58,14 @@ BoundingBox::polygon(Polygon* polygon) const
     polygon->points[3].y = this->max.y;
 }
 
+Polygon
+BoundingBox::polygon() const
+{
+    Polygon p;
+    this->polygon(&p);
+    return p;
+}
+
 template <class PointClass> void
 BoundingBoxBase<PointClass>::scale(double factor)
 {

xs/src/BoundingBox.hpp

@@ -46,6 +46,7 @@ class BoundingBox : public BoundingBoxBase<Point>
 {
     public:
     void polygon(Polygon* polygon) const;
+    Polygon polygon() const;
     
     BoundingBox() {};
     BoundingBox(const Points &points) : BoundingBoxBase<Point>(points) {};

xs/src/ExPolygon.cpp

@@ -84,11 +84,14 @@ ExPolygon::is_valid() const
 bool
 ExPolygon::contains_line(const Line &line) const
 {
-    Polylines pl;
-    pl.push_back(line);
-    
+    return this->contains_polyline(line);
+}
+
+bool
+ExPolygon::contains_polyline(const Polyline &polyline) const
+{
     Polylines pl_out;
-    diff(pl, *this, pl_out);
+    diff((Polylines)polyline, *this, pl_out);
     return pl_out.empty();
 }
 

xs/src/ExPolygon.hpp

@@ -2,6 +2,7 @@
 #define slic3r_ExPolygon_hpp_
 
 #include "Polygon.hpp"
+#include "Polyline.hpp"
 #include <vector>
 
 namespace Slic3r {
@@ -22,6 +23,7 @@ class ExPolygon
     double area() const;
     bool is_valid() const;
     bool contains_line(const Line &line) const;
+    bool contains_polyline(const Polyline &polyline) const;
     bool contains_point(const Point &point) const;
     Polygons simplify_p(double tolerance) const;
     ExPolygons simplify(double tolerance) const;

xs/src/ExPolygonCollection.cpp

@@ -3,6 +3,17 @@
 
 namespace Slic3r {
 
+ExPolygonCollection::operator Points() const
+{
+    Points points;
+    Polygons pp = *this;
+    for (Polygons::const_iterator poly = pp.begin(); poly != pp.end(); ++poly) {
+        for (Points::const_iterator point = poly->points.begin(); point != poly->points.end(); ++point)
+            points.push_back(*point);
+    }
+    return points;
+}
+
 ExPolygonCollection::operator Polygons() const
 {
     Polygons polygons;
@@ -15,6 +26,11 @@ ExPolygonCollection::operator Polygons() const
     return polygons;
 }
 
+ExPolygonCollection::operator ExPolygons&()
+{
+    return this->expolygons;
+}
+
 void
 ExPolygonCollection::scale(double factor)
 {

xs/src/ExPolygonCollection.hpp

@@ -6,11 +6,19 @@
 
 namespace Slic3r {
 
+class ExPolygonCollection;
+typedef std::vector<ExPolygonCollection> ExPolygonCollections;
+
 class ExPolygonCollection
 {
     public:
     ExPolygons expolygons;
+    
+    ExPolygonCollection() {};
+    ExPolygonCollection(const ExPolygons &expolygons) : expolygons(expolygons) {};
+    operator Points() const;
     operator Polygons() const;
+    operator ExPolygons&();
     void scale(double factor);
     void translate(double x, double y);
     void rotate(double angle, const Point &center);

xs/src/MotionPlanner.cpp

@@ -0,0 +1,181 @@
+#include "BoundingBox.hpp"
+#include "MotionPlanner.hpp"
+
+namespace Slic3r {
+
+MotionPlanner::MotionPlanner(const ExPolygons &islands)
+    : islands(islands), initialized(false)
+{}
+
+MotionPlanner::~MotionPlanner()
+{
+    for (std::vector<VisiLibity::Environment*>::iterator env = this->envs.begin(); env != this->envs.end(); ++env)
+        delete *env;
+    for (std::vector<VisiLibity::Visibility_Graph*>::iterator graph = this->graphs.begin(); graph != this->graphs.end(); ++graph)
+        delete *graph;
+}
+
+void
+MotionPlanner::initialize()
+{
+    if (this->initialized) return;
+    
+    // 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_ex(*island, this->inner.back(), -MP_INNER_MARGIN);
+        
+        outer_holes.push_back(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;
+    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);
+    
+    // grow outer contour
+    Polygons contour;
+    offset(bb.polygon(), contour, +MP_OUTER_MARGIN);
+    assert(contour.size() == 1);
+    
+    // make expolygon for outer environment
+    ExPolygons outer;
+    diff(contour, outer_holes, outer);
+    assert(outer.size() == 1);
+    this->outer = outer.front();
+    
+    this->envs.resize(this->islands.size() + 1, NULL);
+    this->graphs.resize(this->islands.size() + 1, NULL);
+    this->initialized = true;
+}
+
+void
+MotionPlanner::shortest_path(const Point &from, const Point &to, Polyline* polyline)
+{
+    if (!this->initialized) this->initialize();
+    
+    // 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_point(from) && island->contains_point(to)) {
+            island_idx = island - this->islands.begin();
+            break;
+        }
+    }
+    
+    // Generate environment.
+    this->generate_environment(island_idx);
+    
+    // Now check whether points are inside the environment.
+    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_point(from))) {
+            // Find the closest inner point to start from.
+            from.nearest_point(this->outer, &inner_from);
+        }
+        if (!(to_is_inside = this->outer.contains_point(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_point(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_point(to))) {
+            // Find the closest inner point to start from.
+            to.nearest_point(this->inner[island_idx], &inner_to);
+        }
+    }
+    
+    // perform actual path search
+    *polyline = convert_polyline(this->envs[island_idx + 1]->shortest_path(convert_point(inner_from),
+        convert_point(inner_to), *this->graphs[island_idx + 1], SCALED_EPSILON));
+    
+    // if start point was outside the environment, prepend it
+    if (!from_is_inside) polyline->points.insert(polyline->points.begin(), from);
+    
+    // if end point was outside the environment, append it
+    if (!to_is_inside) polyline->points.push_back(to);
+}
+
+void
+MotionPlanner::generate_environment(int island_idx)
+{
+    if (this->envs[island_idx + 1] != NULL) return;
+    
+    Polygons pp;
+    if (island_idx == -1) {
+        pp = this->outer;
+    } else {
+        pp = this->inner[island_idx];
+    }
+        
+    // populate VisiLibity polygons
+    std::vector<VisiLibity::Polygon> v_polygons;
+    for (Polygons::const_iterator p = pp.begin(); p != pp.end(); ++p)
+        v_polygons.push_back(convert_polygon(*p));
+    
+    // generate graph and environment
+    this->envs[island_idx + 1] = new VisiLibity::Environment(v_polygons);
+    this->graphs[island_idx + 1] = new VisiLibity::Visibility_Graph(*this->envs[island_idx + 1], SCALED_EPSILON);
+}
+
+VisiLibity::Polyline
+MotionPlanner::convert_polyline(const Polyline &polyline)
+{
+    VisiLibity::Polyline v_polyline;
+    for (Points::const_iterator point = polyline.points.begin(); point != polyline.points.end(); ++point) {
+        v_polyline.push_back(convert_point(*point));
+    }
+    return v_polyline;
+}
+
+Polyline
+MotionPlanner::convert_polyline(const VisiLibity::Polyline &v_polyline)
+{
+    Polyline polyline;
+    polyline.points.reserve(v_polyline.size());
+    for (size_t i = 0; i < v_polyline.size(); ++i) {
+        polyline.points.push_back(convert_point(v_polyline[i]));
+    }
+    return polyline;
+}
+
+VisiLibity::Polygon
+MotionPlanner::convert_polygon(const Polygon &polygon)
+{
+    VisiLibity::Polygon v_polygon;
+    for (Points::const_iterator point = polygon.points.begin(); point != polygon.points.end(); ++point) {
+        v_polygon.push_back(convert_point(*point));
+    }
+    return v_polygon;
+}
+
+VisiLibity::Point
+MotionPlanner::convert_point(const Point &point)
+{
+    return VisiLibity::Point(point.x, point.y);
+}
+
+Point
+MotionPlanner::convert_point(const VisiLibity::Point &v_point)
+{
+    return Point((coord_t)v_point.x(), (coord_t)v_point.y());
+}
+
+#ifdef SLIC3RXS
+REGISTER_CLASS(MotionPlanner, "MotionPlanner");
+#endif
+
+}

xs/src/MotionPlanner.hpp

@@ -0,0 +1,42 @@
+#ifndef slic3r_MotionPlanner_hpp_
+#define slic3r_MotionPlanner_hpp_
+
+#include <myinit.h>
+#include "ClipperUtils.hpp"
+#include "ExPolygonCollection.hpp"
+#include "Polyline.hpp"
+#include "visilibity.hpp"
+#include <vector>
+
+#define MP_INNER_MARGIN scale_(1.0)
+#define MP_OUTER_MARGIN scale_(2.0)
+
+namespace Slic3r {
+
+class MotionPlanner
+{
+    public:
+    MotionPlanner(const ExPolygons &islands);
+    ~MotionPlanner();
+    void shortest_path(const Point &from, const Point &to, Polyline* polyline);
+    
+    private:
+    ExPolygons islands;
+    bool initialized;
+    ExPolygon outer;
+    ExPolygonCollections inner;
+    std::vector<VisiLibity::Environment*> envs;
+    std::vector<VisiLibity::Visibility_Graph*> graphs;
+    
+    void initialize();
+    void generate_environment(int island_idx);
+    static VisiLibity::Polyline convert_polyline(const Polyline &polyline);
+    static Polyline convert_polyline(const VisiLibity::Polyline &v_polyline);
+    static VisiLibity::Polygon convert_polygon(const Polygon &polygon);
+    static VisiLibity::Point convert_point(const Point &point);
+    static Point convert_point(const VisiLibity::Point &v_point);
+};
+
+}
+
+#endif

xs/src/Polyline.cpp

@@ -30,9 +30,11 @@ Lines
 Polyline::lines() const
 {
     Lines lines;
-    lines.reserve(this->points.size() - 1);
-    for (Points::const_iterator it = this->points.begin(); it != this->points.end()-1; ++it) {
-        lines.push_back(Line(*it, *(it + 1)));
+    if (this->points.size() >= 2) {
+        lines.reserve(this->points.size() - 1);
+        for (Points::const_iterator it = this->points.begin(); it != this->points.end()-1; ++it) {
+            lines.push_back(Line(*it, *(it + 1)));
+        }
     }
     return lines;
 }