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
@ -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);
|
||||
|
||||
|
@ -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++;
|
||||
/* 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. */
|
||||
|
||||
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;
|
||||
|
||||
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;
|
||||
}
|
||||
polygon->lines(&lines);
|
||||
}
|
||||
|
||||
// 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;
|
||||
|
||||
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);
|
||||
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());
|
||||
// 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);
|
||||
|
||||
// Visit each edge exiting 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…
Reference in New Issue
Block a user