Fixes and improvements to MotionPlanner, much smarter now
This commit is contained in:
parent
f7e97f7e9b
commit
1a286fc906
@ -178,20 +178,13 @@ sub export {
|
||||
# compute the offsetted convex hull for each object and repeat it for each copy.
|
||||
my @islands_p = ();
|
||||
foreach my $object (@{$self->objects}) {
|
||||
# compute the convex hull of the entire object
|
||||
my $convex_hull = convex_hull([
|
||||
map @{$_->contour}, map @{$_->slices}, @{$object->layers},
|
||||
]);
|
||||
|
||||
# discard objects only containing thin walls (offset would fail on an empty polygon)
|
||||
next if !@$convex_hull;
|
||||
|
||||
# grow convex hull by the wanted clearance
|
||||
my @obj_islands_p = @{offset([$convex_hull], $distance_from_objects, 1, JT_SQUARE)};
|
||||
my @polygons = map $_->contour, map @{$_->slices}, @{$object->layers};
|
||||
next if !@polygons;
|
||||
|
||||
# translate convex hull for each object copy and append it to the islands array
|
||||
foreach my $copy (@{ $object->_shifted_copies }) {
|
||||
my @copy_islands_p = map $_->clone, @obj_islands_p;
|
||||
my @copy_islands_p = map $_->clone, @polygons;
|
||||
$_->translate(@$copy) for @copy_islands_p;
|
||||
push @islands_p, @copy_islands_p;
|
||||
}
|
||||
|
@ -504,21 +504,17 @@ template void intersection<Slic3r::Polygons, Slic3r::Polylines>(const Slic3r::Po
|
||||
template void intersection<Slic3r::Polylines, Slic3r::Polylines>(const Slic3r::Polylines &subject, const Slic3r::Polygons &clip, Slic3r::Polylines* retval, bool safety_offset_);
|
||||
template void intersection<Slic3r::Lines, Slic3r::Lines>(const Slic3r::Lines &subject, const Slic3r::Polygons &clip, Slic3r::Lines* retval, bool safety_offset_);
|
||||
|
||||
Slic3r::Polygons
|
||||
intersection(const Slic3r::Polygons &subject, const Slic3r::Polygons &clip, bool safety_offset_)
|
||||
template <class SubjectType>
|
||||
SubjectType intersection(const SubjectType &subject, const Slic3r::Polygons &clip, bool safety_offset_)
|
||||
{
|
||||
Slic3r::Polygons pp;
|
||||
SubjectType pp;
|
||||
intersection(subject, clip, &pp, safety_offset_);
|
||||
return pp;
|
||||
}
|
||||
|
||||
Slic3r::Polylines
|
||||
intersection(const Slic3r::Polylines &subject, const Slic3r::Polygons &clip, bool safety_offset_)
|
||||
{
|
||||
Slic3r::Polylines pp;
|
||||
intersection(subject, clip, &pp, safety_offset_);
|
||||
return pp;
|
||||
}
|
||||
template Slic3r::Polygons intersection<Slic3r::Polygons>(const Slic3r::Polygons &subject, const Slic3r::Polygons &clip, bool safety_offset_);
|
||||
template Slic3r::Polylines intersection<Slic3r::Polylines>(const Slic3r::Polylines &subject, const Slic3r::Polygons &clip, bool safety_offset_);
|
||||
template Slic3r::Lines intersection<Slic3r::Lines>(const Slic3r::Lines &subject, const Slic3r::Polygons &clip, bool safety_offset_);
|
||||
|
||||
Slic3r::ExPolygons
|
||||
intersection_ex(const Slic3r::Polygons &subject, const Slic3r::Polygons &clip, bool safety_offset_)
|
||||
|
@ -106,9 +106,11 @@ Slic3r::ExPolygons diff_ex(const SubjectType &subject, const ClipType &clip, boo
|
||||
template <class SubjectType, class ResultType>
|
||||
void intersection(const SubjectType &subject, const Slic3r::Polygons &clip, ResultType* retval, bool safety_offset_ = false);
|
||||
|
||||
Slic3r::Polygons intersection(const Slic3r::Polygons &subject, const Slic3r::Polygons &clip, bool safety_offset_ = false);
|
||||
Slic3r::Polylines intersection(const Slic3r::Polylines &subject, const Slic3r::Polygons &clip, bool safety_offset_ = false);
|
||||
Slic3r::ExPolygons intersection_ex(const Slic3r::Polygons &subject, const Slic3r::Polygons &clip, bool safety_offset_ = false);
|
||||
template <class SubjectType>
|
||||
SubjectType intersection(const SubjectType &subject, const Slic3r::Polygons &clip, bool safety_offset_ = false);
|
||||
|
||||
Slic3r::ExPolygons
|
||||
intersection_ex(const Slic3r::Polygons &subject, const Slic3r::Polygons &clip, bool safety_offset_ = false);
|
||||
|
||||
template <class SubjectType>
|
||||
bool intersects(const SubjectType &subject, const Slic3r::Polygons &clip, bool safety_offset_ = false);
|
||||
|
@ -160,17 +160,14 @@ ExPolygons
|
||||
ExPolygon::simplify(double tolerance) const
|
||||
{
|
||||
Polygons pp = this->simplify_p(tolerance);
|
||||
ExPolygons expp;
|
||||
union_(pp, &expp);
|
||||
return expp;
|
||||
return union_ex(pp);
|
||||
}
|
||||
|
||||
void
|
||||
ExPolygon::simplify(double tolerance, ExPolygons &expolygons) const
|
||||
ExPolygon::simplify(double tolerance, ExPolygons* expolygons) const
|
||||
{
|
||||
ExPolygons ep = this->simplify(tolerance);
|
||||
expolygons.reserve(expolygons.size() + ep.size());
|
||||
expolygons.insert(expolygons.end(), ep.begin(), ep.end());
|
||||
expolygons->insert(expolygons->end(), ep.begin(), ep.end());
|
||||
}
|
||||
|
||||
void
|
||||
|
@ -31,7 +31,7 @@ class ExPolygon
|
||||
void simplify_p(double tolerance, Polygons* polygons) const;
|
||||
Polygons simplify_p(double tolerance) const;
|
||||
ExPolygons simplify(double tolerance) const;
|
||||
void simplify(double tolerance, ExPolygons &expolygons) const;
|
||||
void simplify(double tolerance, ExPolygons* expolygons) const;
|
||||
void medial_axis(double max_width, double min_width, Polylines* polylines) const;
|
||||
void get_trapezoids(Polygons* polygons) const;
|
||||
void get_trapezoids(Polygons* polygons, double angle) const;
|
||||
|
@ -87,7 +87,7 @@ ExPolygonCollection::simplify(double tolerance)
|
||||
{
|
||||
ExPolygons expp;
|
||||
for (ExPolygons::const_iterator it = this->expolygons.begin(); it != this->expolygons.end(); ++it) {
|
||||
it->simplify(tolerance, expp);
|
||||
it->simplify(tolerance, &expp);
|
||||
}
|
||||
this->expolygons = expp;
|
||||
}
|
||||
|
@ -55,7 +55,7 @@ AvoidCrossingPerimeters::travel_to(GCode &gcodegen, Point point)
|
||||
|
||||
// calculate path
|
||||
Polyline travel = this->_external_mp->shortest_path(last_pos, point);
|
||||
|
||||
//exit(0);
|
||||
// translate the path back into the shifted coordinate system that gcodegen
|
||||
// is currently using for writing coordinates
|
||||
travel.translate(scaled_origin.negative());
|
||||
@ -613,6 +613,7 @@ GCode::travel_to(const Point &point, ExtrusionRole role, std::string comment)
|
||||
|
||||
// check again whether the new travel path still needs a retraction
|
||||
needs_retraction = this->needs_retraction(travel, role);
|
||||
//if (needs_retraction && this->layer_index > 1) exit(0);
|
||||
}
|
||||
|
||||
// Re-allow avoid_crossing_perimeters for the next travel moves
|
||||
|
@ -10,8 +10,15 @@ using boost::polygon::voronoi_diagram;
|
||||
namespace Slic3r {
|
||||
|
||||
MotionPlanner::MotionPlanner(const ExPolygons &islands)
|
||||
: islands(islands), initialized(false)
|
||||
{}
|
||||
: initialized(false)
|
||||
{
|
||||
ExPolygons expp;
|
||||
for (ExPolygons::const_iterator island = islands.begin(); island != islands.end(); ++island)
|
||||
island->simplify(SCALED_EPSILON, &expp);
|
||||
|
||||
for (ExPolygons::const_iterator island = expp.begin(); island != expp.end(); ++island)
|
||||
this->islands.push_back(MotionPlannerEnv(*island));
|
||||
}
|
||||
|
||||
MotionPlanner::~MotionPlanner()
|
||||
{
|
||||
@ -31,75 +38,62 @@ MotionPlanner::initialize()
|
||||
if (this->initialized) return;
|
||||
if (this->islands.empty()) return; // prevent initialization of empty BoundingBox
|
||||
|
||||
ExPolygons expp;
|
||||
for (ExPolygons::const_iterator island = this->islands.begin(); island != this->islands.end(); ++island) {
|
||||
island->simplify(SCALED_EPSILON, expp);
|
||||
}
|
||||
this->islands = expp;
|
||||
|
||||
// loop through islands in order to create inner expolygons and collect their contours
|
||||
this->inner.reserve(this->islands.size());
|
||||
Polygons outer_holes;
|
||||
for (ExPolygons::const_iterator island = this->islands.begin(); island != this->islands.end(); ++island) {
|
||||
this->inner.push_back(ExPolygonCollection());
|
||||
offset(*island, &this->inner.back().expolygons, -MP_INNER_MARGIN);
|
||||
for (std::vector<MotionPlannerEnv>::iterator island = this->islands.begin(); island != this->islands.end(); ++island) {
|
||||
// generate the internal env boundaries by shrinking the island
|
||||
// we'll use these inner rings for motion planning (endpoints of the Voronoi-based
|
||||
// graph, visibility check) in order to avoid moving too close to the boundaries
|
||||
island->env = offset_ex(island->island, -MP_INNER_MARGIN);
|
||||
|
||||
outer_holes.push_back(island->contour);
|
||||
// island contours are holes of our external environment
|
||||
outer_holes.push_back(island->island.contour);
|
||||
}
|
||||
|
||||
// grow island contours in order to prepare holes of the outer environment
|
||||
// This is actually wrong because it might merge contours that are close,
|
||||
// thus confusing the island check in shortest_path() below
|
||||
//offset(outer_holes, &outer_holes, +MP_OUTER_MARGIN);
|
||||
|
||||
// generate outer contour as bounding box of everything
|
||||
Points points;
|
||||
BoundingBox bb;
|
||||
for (Polygons::const_iterator contour = outer_holes.begin(); contour != outer_holes.end(); ++contour)
|
||||
points.insert(points.end(), contour->points.begin(), contour->points.end());
|
||||
BoundingBox bb(points);
|
||||
bb.merge(contour->bounding_box());
|
||||
|
||||
// grow outer contour
|
||||
Polygons contour;
|
||||
offset(bb.polygon(), &contour, +MP_OUTER_MARGIN);
|
||||
Polygons contour = offset(bb.polygon(), +MP_OUTER_MARGIN*2);
|
||||
assert(contour.size() == 1);
|
||||
|
||||
// make expolygon for outer environment
|
||||
ExPolygons outer;
|
||||
diff(contour, outer_holes, &outer);
|
||||
ExPolygons outer = diff_ex(contour, outer_holes);
|
||||
assert(outer.size() == 1);
|
||||
this->outer = outer.front();
|
||||
this->outer.island = outer.front();
|
||||
|
||||
this->outer.env = ExPolygonCollection(diff_ex(contour, offset(outer_holes, +MP_OUTER_MARGIN)));
|
||||
|
||||
this->graphs.resize(this->islands.size() + 1, NULL);
|
||||
this->initialized = true;
|
||||
}
|
||||
|
||||
ExPolygonCollection
|
||||
const MotionPlannerEnv&
|
||||
MotionPlanner::get_env(int island_idx) const
|
||||
{
|
||||
if (island_idx == -1) {
|
||||
return ExPolygonCollection(this->outer);
|
||||
return this->outer;
|
||||
} else {
|
||||
return this->inner[island_idx];
|
||||
return this->islands[island_idx];
|
||||
}
|
||||
}
|
||||
|
||||
Polyline
|
||||
MotionPlanner::shortest_path(const Point &from, const Point &to)
|
||||
{
|
||||
// lazy generation of configuration space
|
||||
if (!this->initialized) this->initialize();
|
||||
|
||||
// if we have an empty configuration space, return a straight move
|
||||
if (this->islands.empty())
|
||||
return Line(from, to);
|
||||
|
||||
// Are both points in the same island?
|
||||
int island_idx = -1;
|
||||
for (ExPolygons::const_iterator island = this->islands.begin(); island != this->islands.end(); ++island) {
|
||||
if (island->contains(from) && island->contains(to)) {
|
||||
for (std::vector<MotionPlannerEnv>::const_iterator island = this->islands.begin(); island != this->islands.end(); ++island) {
|
||||
if (island->island.contains(from) && island->island.contains(to)) {
|
||||
// since both points are in the same island, is a direct move possible?
|
||||
// if so, we avoid generating the visibility environment
|
||||
if (island->contains(Line(from, to)))
|
||||
if (island->island.contains(Line(from, to)))
|
||||
return Line(from, to);
|
||||
|
||||
island_idx = island - this->islands.begin();
|
||||
@ -107,25 +101,35 @@ MotionPlanner::shortest_path(const Point &from, const Point &to)
|
||||
}
|
||||
}
|
||||
|
||||
// lazy generation of configuration space
|
||||
this->initialize();
|
||||
|
||||
// get environment
|
||||
ExPolygonCollection env = this->get_env(island_idx);
|
||||
if (env.expolygons.empty()) {
|
||||
MotionPlannerEnv env = this->get_env(island_idx);
|
||||
if (env.env.expolygons.empty()) {
|
||||
// if this environment is empty (probably because it's too small), perform straight move
|
||||
// and avoid running the algorithms on empty dataset
|
||||
return Line(from, to);
|
||||
}
|
||||
|
||||
// Now check whether points are inside the environment.
|
||||
Point inner_from = from;
|
||||
Point inner_to = to;
|
||||
|
||||
if (!env.contains(from)) {
|
||||
// Find the closest inner point to start from.
|
||||
inner_from = this->nearest_env_point(env, from, to);
|
||||
}
|
||||
if (!env.contains(to)) {
|
||||
// Find the closest inner point to start from.
|
||||
inner_to = this->nearest_env_point(env, to, inner_from);
|
||||
Point inner_from = from;
|
||||
Point inner_to = to;
|
||||
|
||||
if (island_idx == -1) {
|
||||
// TODO: instead of using the nearest_env_point() logic, we should
|
||||
// create a temporary graph where we connect 'from' and 'to' to the
|
||||
// nodes which don't require more than one crossing, and let Dijkstra
|
||||
// figure out the entire path - this should also replace the call to
|
||||
// find_node() below
|
||||
if (!env.island.contains(inner_from)) {
|
||||
// Find the closest inner point to start from.
|
||||
inner_from = env.nearest_env_point(from, to);
|
||||
}
|
||||
if (!env.island.contains(inner_to)) {
|
||||
// Find the closest inner point to start from.
|
||||
inner_to = env.nearest_env_point(to, inner_from);
|
||||
}
|
||||
}
|
||||
|
||||
// perform actual path search
|
||||
@ -138,16 +142,39 @@ MotionPlanner::shortest_path(const Point &from, const Point &to)
|
||||
{
|
||||
// grow our environment slightly in order for simplify_by_visibility()
|
||||
// to work best by considering moves on boundaries valid as well
|
||||
ExPolygonCollection grown_env;
|
||||
offset(env, &grown_env.expolygons, +SCALED_EPSILON);
|
||||
ExPolygonCollection grown_env(offset_ex(env.env, +SCALED_EPSILON));
|
||||
|
||||
if (island_idx == -1) {
|
||||
/* If 'from' or 'to' are not inside our env, they were connected using the
|
||||
nearest_env_point() search which maybe produce ugly paths since it does not
|
||||
include the endpoint in the Dijkstra search; the simplify_by_visibility()
|
||||
call below will not work in many cases where the endpoint is not contained in
|
||||
grown_env (whose contour was arbitrarily constructed with MP_OUTER_MARGIN,
|
||||
which may not be enough for, say, including a skirt point). So we prune
|
||||
the extra points manually. */
|
||||
if (!grown_env.contains(from)) {
|
||||
// delete second point while the line connecting first to third crosses the
|
||||
// boundaries as many times as the current first to second
|
||||
while (polyline.points.size() > 2 && intersection((Lines)Line(from, polyline.points[2]), grown_env).size() == 1) {
|
||||
polyline.points.erase(polyline.points.begin() + 1);
|
||||
}
|
||||
}
|
||||
if (!grown_env.contains(to)) {
|
||||
while (polyline.points.size() > 2 && intersection((Lines)Line(*(polyline.points.end() - 3), to), grown_env).size() == 1) {
|
||||
polyline.points.erase(polyline.points.end() - 2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// remove unnecessary vertices
|
||||
polyline.simplify_by_visibility(grown_env);
|
||||
}
|
||||
// Note: this is computationally intensive and does not look very necessary
|
||||
// now that we prune the endpoints with the logic above,
|
||||
// so we comment it for now until a good test case arises
|
||||
//polyline.simplify_by_visibility(grown_env);
|
||||
|
||||
/*
|
||||
/*
|
||||
SVG svg("shortest_path.svg");
|
||||
svg.draw(this->outer);
|
||||
svg.draw(grown_env.expolygons);
|
||||
svg.arrows = false;
|
||||
for (MotionPlannerGraph::adjacency_list_t::const_iterator it = graph->adjacency_list.begin(); it != graph->adjacency_list.end(); ++it) {
|
||||
Point a = graph->nodes[it - graph->adjacency_list.begin()];
|
||||
@ -163,65 +190,12 @@ MotionPlanner::shortest_path(const Point &from, const Point &to)
|
||||
svg.draw(inner_to, "red");
|
||||
svg.draw(polyline, "red");
|
||||
svg.Close();
|
||||
*/
|
||||
*/
|
||||
}
|
||||
|
||||
return polyline;
|
||||
}
|
||||
|
||||
Point
|
||||
MotionPlanner::nearest_env_point(const ExPolygonCollection &env, const Point &from, const Point &to) const
|
||||
{
|
||||
/* In order to ensure that the move between 'from' and the initial env point does
|
||||
not violate any of the configuration space boundaries, we limit our search to
|
||||
the points that satisfy this condition. */
|
||||
|
||||
/* Assume that this method is never called when 'env' contains 'from';
|
||||
so 'from' is either inside a hole or outside all contours */
|
||||
|
||||
// get the points of the hole containing 'from', if any
|
||||
Points pp;
|
||||
for (ExPolygons::const_iterator ex = env.expolygons.begin(); ex != env.expolygons.end(); ++ex) {
|
||||
for (Polygons::const_iterator h = ex->holes.begin(); h != ex->holes.end(); ++h) {
|
||||
if (h->contains(from)) {
|
||||
pp = *h;
|
||||
}
|
||||
}
|
||||
if (!pp.empty()) break;
|
||||
}
|
||||
|
||||
/* If 'from' is not inside a hole, it's outside of all contours, so take all
|
||||
contours' points */
|
||||
if (pp.empty()) {
|
||||
for (ExPolygons::const_iterator ex = env.expolygons.begin(); ex != env.expolygons.end(); ++ex) {
|
||||
Points contour_pp = ex->contour;
|
||||
pp.insert(pp.end(), contour_pp.begin(), contour_pp.end());
|
||||
}
|
||||
}
|
||||
|
||||
/* Find the candidate result and check that it doesn't cross any boundary.
|
||||
(We could skip all of the above polygon finding logic and directly test all points
|
||||
in env, but this way we probably reduce complexity). */
|
||||
Polygons env_pp = env;
|
||||
while (pp.size() >= 2) {
|
||||
// find the point in pp that is closest to both 'from' and 'to'
|
||||
size_t result = from.nearest_waypoint_index(pp, to);
|
||||
|
||||
if (intersects((Lines)Line(from, pp[result]), env_pp)) {
|
||||
// discard result
|
||||
pp.erase(pp.begin() + result);
|
||||
} else {
|
||||
return pp[result];
|
||||
}
|
||||
}
|
||||
|
||||
// if we're here, return last point if any (better than nothing)
|
||||
if (!pp.empty()) return pp.front();
|
||||
|
||||
// if we have no points at all, then we have an empty environment and we
|
||||
// make this method behave as a no-op (we shouldn't get here by the way)
|
||||
return from;
|
||||
}
|
||||
|
||||
MotionPlannerGraph*
|
||||
MotionPlanner::init_graph(int island_idx)
|
||||
{
|
||||
@ -240,8 +214,8 @@ MotionPlanner::init_graph(int island_idx)
|
||||
t_vd_vertices vd_vertices;
|
||||
|
||||
// get boundaries as lines
|
||||
ExPolygonCollection env = this->get_env(island_idx);
|
||||
Lines lines = env.lines();
|
||||
MotionPlannerEnv env = this->get_env(island_idx);
|
||||
Lines lines = env.env.lines();
|
||||
boost::polygon::construct_voronoi(lines.begin(), lines.end(), &vd);
|
||||
|
||||
// traverse the Voronoi diagram and generate graph nodes and edges
|
||||
@ -254,7 +228,7 @@ MotionPlanner::init_graph(int island_idx)
|
||||
Point p1 = Point(v1->x(), v1->y());
|
||||
|
||||
// skip edge if any of its endpoints is outside our configuration space
|
||||
if (!env.contains_b(p0) || !env.contains_b(p1)) continue;
|
||||
if (!env.island.contains_b(p0) || !env.island.contains_b(p1)) continue;
|
||||
|
||||
t_vd_vertices::const_iterator i_v0 = vd_vertices.find(v0);
|
||||
size_t v0_idx;
|
||||
@ -284,6 +258,60 @@ MotionPlanner::init_graph(int island_idx)
|
||||
return this->graphs[island_idx + 1];
|
||||
}
|
||||
|
||||
Point
|
||||
MotionPlannerEnv::nearest_env_point(const Point &from, const Point &to) const
|
||||
{
|
||||
/* In order to ensure that the move between 'from' and the initial env point does
|
||||
not violate any of the configuration space boundaries, we limit our search to
|
||||
the points that satisfy this condition. */
|
||||
|
||||
/* Assume that this method is never called when 'env' contains 'from';
|
||||
so 'from' is either inside a hole or outside all contours */
|
||||
|
||||
// get the points of the hole containing 'from', if any
|
||||
Points pp;
|
||||
for (ExPolygons::const_iterator ex = this->env.expolygons.begin(); ex != this->env.expolygons.end(); ++ex) {
|
||||
for (Polygons::const_iterator h = ex->holes.begin(); h != ex->holes.end(); ++h) {
|
||||
if (h->contains(from)) {
|
||||
pp = *h;
|
||||
}
|
||||
}
|
||||
if (!pp.empty()) break;
|
||||
}
|
||||
|
||||
/* If 'from' is not inside a hole, it's outside of all contours, so take all
|
||||
contours' points */
|
||||
if (pp.empty()) {
|
||||
for (ExPolygons::const_iterator ex = this->env.expolygons.begin(); ex != this->env.expolygons.end(); ++ex) {
|
||||
Points contour_pp = ex->contour;
|
||||
pp.insert(pp.end(), contour_pp.begin(), contour_pp.end());
|
||||
}
|
||||
}
|
||||
|
||||
/* Find the candidate result and check that it doesn't cross too many boundaries. */
|
||||
while (pp.size() >= 2) {
|
||||
// find the point in pp that is closest to both 'from' and 'to'
|
||||
size_t result = from.nearest_waypoint_index(pp, to);
|
||||
|
||||
// as we assume 'from' is outside env, any node will require at least one crossing
|
||||
if (intersection((Lines)Line(from, pp[result]), this->island).size() > 1) {
|
||||
// discard result
|
||||
pp.erase(pp.begin() + result);
|
||||
} else {
|
||||
return pp[result];
|
||||
}
|
||||
}
|
||||
|
||||
// if we're here, return last point if any (better than nothing)
|
||||
if (!pp.empty()) {
|
||||
return pp.front();
|
||||
}
|
||||
|
||||
// if we have no points at all, then we have an empty environment and we
|
||||
// make this method behave as a no-op (we shouldn't get here by the way)
|
||||
return from;
|
||||
}
|
||||
|
||||
void
|
||||
MotionPlannerGraph::add_edge(size_t from, size_t to, double weight)
|
||||
{
|
||||
|
@ -14,27 +14,18 @@
|
||||
|
||||
namespace Slic3r {
|
||||
|
||||
class MotionPlannerGraph;
|
||||
class MotionPlanner;
|
||||
|
||||
class MotionPlanner
|
||||
class MotionPlannerEnv
|
||||
{
|
||||
friend class MotionPlanner;
|
||||
|
||||
public:
|
||||
MotionPlanner(const ExPolygons &islands);
|
||||
~MotionPlanner();
|
||||
Polyline shortest_path(const Point &from, const Point &to);
|
||||
size_t islands_count() const;
|
||||
|
||||
private:
|
||||
ExPolygons islands;
|
||||
bool initialized;
|
||||
ExPolygon outer;
|
||||
ExPolygonCollections inner;
|
||||
std::vector<MotionPlannerGraph*> graphs;
|
||||
|
||||
void initialize();
|
||||
MotionPlannerGraph* init_graph(int island_idx);
|
||||
ExPolygonCollection get_env(int island_idx) const;
|
||||
Point nearest_env_point(const ExPolygonCollection &env, const Point &from, const Point &to) const;
|
||||
ExPolygon island;
|
||||
ExPolygonCollection env;
|
||||
MotionPlannerEnv() {};
|
||||
MotionPlannerEnv(const ExPolygon &island) : island(island) {};
|
||||
Point nearest_env_point(const Point &from, const Point &to) const;
|
||||
};
|
||||
|
||||
class MotionPlannerGraph
|
||||
@ -61,6 +52,25 @@ class MotionPlannerGraph
|
||||
Polyline shortest_path(size_t from, size_t to);
|
||||
};
|
||||
|
||||
class MotionPlanner
|
||||
{
|
||||
public:
|
||||
MotionPlanner(const ExPolygons &islands);
|
||||
~MotionPlanner();
|
||||
Polyline shortest_path(const Point &from, const Point &to);
|
||||
size_t islands_count() const;
|
||||
|
||||
private:
|
||||
bool initialized;
|
||||
std::vector<MotionPlannerEnv> islands;
|
||||
MotionPlannerEnv outer;
|
||||
std::vector<MotionPlannerGraph*> graphs;
|
||||
|
||||
void initialize();
|
||||
MotionPlannerGraph* init_graph(int island_idx);
|
||||
const MotionPlannerEnv& get_env(int island_idx) const;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
@ -138,47 +138,18 @@ Polyline::simplify_by_visibility(const T &area)
|
||||
{
|
||||
Points &pp = this->points;
|
||||
|
||||
// find first point in area
|
||||
size_t s = 0;
|
||||
while (s < pp.size() && !area.contains(pp[s])) {
|
||||
++s;
|
||||
}
|
||||
|
||||
// find last point in area
|
||||
size_t e = pp.size()-1;
|
||||
while (e > 0 && !area.contains(pp[e])) {
|
||||
--e;
|
||||
}
|
||||
|
||||
// this ugly algorithm resembles a binary search
|
||||
while (e > s + 1) {
|
||||
size_t mid = (s + e) / 2;
|
||||
if (area.contains(Line(pp[s], pp[mid]))) {
|
||||
pp.erase(pp.begin() + s + 1, pp.begin() + mid);
|
||||
// repeat recursively until no further simplification is possible
|
||||
++s;
|
||||
e = pp.size()-1;
|
||||
bool did_erase = false;
|
||||
for (size_t i = s+2; i < pp.size(); i = s + 2) {
|
||||
if (area.contains(Line(pp[s], pp[i]))) {
|
||||
pp.erase(pp.begin() + s + 1, pp.begin() + i);
|
||||
did_erase = true;
|
||||
} else {
|
||||
e = mid;
|
||||
++s;
|
||||
}
|
||||
}
|
||||
/*
|
||||
// The following implementation is complete but it's not efficient at all:
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
*/
|
||||
if (did_erase)
|
||||
this->simplify_by_visibility(area);
|
||||
}
|
||||
template void Polyline::simplify_by_visibility<ExPolygon>(const ExPolygon &area);
|
||||
template void Polyline::simplify_by_visibility<ExPolygonCollection>(const ExPolygonCollection &area);
|
||||
|
@ -29,7 +29,7 @@ SurfaceCollection::simplify(double tolerance)
|
||||
Surfaces ss;
|
||||
for (Surfaces::const_iterator it_s = this->surfaces.begin(); it_s != this->surfaces.end(); ++it_s) {
|
||||
ExPolygons expp;
|
||||
it_s->expolygon.simplify(tolerance, expp);
|
||||
it_s->expolygon.simplify(tolerance, &expp);
|
||||
for (ExPolygons::const_iterator it_e = expp.begin(); it_e != expp.end(); ++it_e) {
|
||||
Surface s = *it_s;
|
||||
s.expolygon = *it_e;
|
||||
|
@ -9,7 +9,7 @@ BEGIN {
|
||||
}
|
||||
|
||||
use Slic3r::XS;
|
||||
use Test::More tests => 22;
|
||||
use Test::More tests => 20;
|
||||
|
||||
my $square = Slic3r::Polygon->new( # ccw
|
||||
[100, 100],
|
||||
@ -71,7 +71,6 @@ my $expolygon = Slic3r::ExPolygon->new($square, $hole_in_square);
|
||||
ok $expolygon2->contains_point($to), 'end point is contained in second expolygon';
|
||||
my $path = $mp->shortest_path($from, $to);
|
||||
ok $path->is_valid(), 'return path is valid';
|
||||
ok $path->length > Slic3r::Line->new($from, $to)->length, 'path length is greater than straight line';
|
||||
}
|
||||
|
||||
{
|
||||
@ -88,7 +87,6 @@ my $expolygon = Slic3r::ExPolygon->new($square, $hole_in_square);
|
||||
ok $expolygons->[0]->contains_point($to), 'end point is contained in first expolygon';
|
||||
my $path = $mp->shortest_path($from, $to);
|
||||
ok $path->is_valid(), 'return path is valid';
|
||||
ok $path->length > Slic3r::Line->new($from, $to)->length, 'path length is greater than straight line';
|
||||
}
|
||||
|
||||
__END__
|
||||
|
Loading…
Reference in New Issue
Block a user