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.
|
# compute the offsetted convex hull for each object and repeat it for each copy.
|
||||||
my @islands_p = ();
|
my @islands_p = ();
|
||||||
foreach my $object (@{$self->objects}) {
|
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)
|
# discard objects only containing thin walls (offset would fail on an empty polygon)
|
||||||
next if !@$convex_hull;
|
my @polygons = map $_->contour, map @{$_->slices}, @{$object->layers};
|
||||||
|
next if !@polygons;
|
||||||
# grow convex hull by the wanted clearance
|
|
||||||
my @obj_islands_p = @{offset([$convex_hull], $distance_from_objects, 1, JT_SQUARE)};
|
|
||||||
|
|
||||||
# translate convex hull for each object copy and append it to the islands array
|
# translate convex hull for each object copy and append it to the islands array
|
||||||
foreach my $copy (@{ $object->_shifted_copies }) {
|
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;
|
$_->translate(@$copy) for @copy_islands_p;
|
||||||
push @islands_p, @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::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_);
|
template void intersection<Slic3r::Lines, Slic3r::Lines>(const Slic3r::Lines &subject, const Slic3r::Polygons &clip, Slic3r::Lines* retval, bool safety_offset_);
|
||||||
|
|
||||||
Slic3r::Polygons
|
template <class SubjectType>
|
||||||
intersection(const Slic3r::Polygons &subject, const Slic3r::Polygons &clip, bool safety_offset_)
|
SubjectType intersection(const SubjectType &subject, const Slic3r::Polygons &clip, bool safety_offset_)
|
||||||
{
|
{
|
||||||
Slic3r::Polygons pp;
|
SubjectType pp;
|
||||||
intersection(subject, clip, &pp, safety_offset_);
|
intersection(subject, clip, &pp, safety_offset_);
|
||||||
return pp;
|
return pp;
|
||||||
}
|
}
|
||||||
|
|
||||||
Slic3r::Polylines
|
template Slic3r::Polygons intersection<Slic3r::Polygons>(const Slic3r::Polygons &subject, const Slic3r::Polygons &clip, bool safety_offset_);
|
||||||
intersection(const Slic3r::Polylines &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::Polylines pp;
|
|
||||||
intersection(subject, clip, &pp, safety_offset_);
|
|
||||||
return pp;
|
|
||||||
}
|
|
||||||
|
|
||||||
Slic3r::ExPolygons
|
Slic3r::ExPolygons
|
||||||
intersection_ex(const Slic3r::Polygons &subject, const Slic3r::Polygons &clip, bool safety_offset_)
|
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>
|
template <class SubjectType, class ResultType>
|
||||||
void intersection(const SubjectType &subject, const Slic3r::Polygons &clip, ResultType* retval, bool safety_offset_ = false);
|
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);
|
template <class SubjectType>
|
||||||
Slic3r::Polylines intersection(const Slic3r::Polylines &subject, const Slic3r::Polygons &clip, bool safety_offset_ = false);
|
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);
|
|
||||||
|
Slic3r::ExPolygons
|
||||||
|
intersection_ex(const Slic3r::Polygons &subject, const Slic3r::Polygons &clip, bool safety_offset_ = false);
|
||||||
|
|
||||||
template <class SubjectType>
|
template <class SubjectType>
|
||||||
bool intersects(const SubjectType &subject, const Slic3r::Polygons &clip, bool safety_offset_ = false);
|
bool intersects(const SubjectType &subject, const Slic3r::Polygons &clip, bool safety_offset_ = false);
|
||||||
|
@ -160,17 +160,14 @@ ExPolygons
|
|||||||
ExPolygon::simplify(double tolerance) const
|
ExPolygon::simplify(double tolerance) const
|
||||||
{
|
{
|
||||||
Polygons pp = this->simplify_p(tolerance);
|
Polygons pp = this->simplify_p(tolerance);
|
||||||
ExPolygons expp;
|
return union_ex(pp);
|
||||||
union_(pp, &expp);
|
|
||||||
return expp;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
ExPolygon::simplify(double tolerance, ExPolygons &expolygons) const
|
ExPolygon::simplify(double tolerance, ExPolygons* expolygons) const
|
||||||
{
|
{
|
||||||
ExPolygons ep = this->simplify(tolerance);
|
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
|
void
|
||||||
|
@ -31,7 +31,7 @@ class ExPolygon
|
|||||||
void simplify_p(double tolerance, Polygons* polygons) const;
|
void simplify_p(double tolerance, Polygons* polygons) const;
|
||||||
Polygons simplify_p(double tolerance) const;
|
Polygons simplify_p(double tolerance) const;
|
||||||
ExPolygons simplify(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 medial_axis(double max_width, double min_width, Polylines* polylines) const;
|
||||||
void get_trapezoids(Polygons* polygons) const;
|
void get_trapezoids(Polygons* polygons) const;
|
||||||
void get_trapezoids(Polygons* polygons, double angle) const;
|
void get_trapezoids(Polygons* polygons, double angle) const;
|
||||||
|
@ -87,7 +87,7 @@ ExPolygonCollection::simplify(double tolerance)
|
|||||||
{
|
{
|
||||||
ExPolygons expp;
|
ExPolygons expp;
|
||||||
for (ExPolygons::const_iterator it = this->expolygons.begin(); it != this->expolygons.end(); ++it) {
|
for (ExPolygons::const_iterator it = this->expolygons.begin(); it != this->expolygons.end(); ++it) {
|
||||||
it->simplify(tolerance, expp);
|
it->simplify(tolerance, &expp);
|
||||||
}
|
}
|
||||||
this->expolygons = expp;
|
this->expolygons = expp;
|
||||||
}
|
}
|
||||||
|
@ -55,7 +55,7 @@ AvoidCrossingPerimeters::travel_to(GCode &gcodegen, Point point)
|
|||||||
|
|
||||||
// calculate path
|
// calculate path
|
||||||
Polyline travel = this->_external_mp->shortest_path(last_pos, point);
|
Polyline travel = this->_external_mp->shortest_path(last_pos, point);
|
||||||
|
//exit(0);
|
||||||
// translate the path back into the shifted coordinate system that gcodegen
|
// translate the path back into the shifted coordinate system that gcodegen
|
||||||
// is currently using for writing coordinates
|
// is currently using for writing coordinates
|
||||||
travel.translate(scaled_origin.negative());
|
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
|
// check again whether the new travel path still needs a retraction
|
||||||
needs_retraction = this->needs_retraction(travel, role);
|
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
|
// Re-allow avoid_crossing_perimeters for the next travel moves
|
||||||
|
@ -10,8 +10,15 @@ using boost::polygon::voronoi_diagram;
|
|||||||
namespace Slic3r {
|
namespace Slic3r {
|
||||||
|
|
||||||
MotionPlanner::MotionPlanner(const ExPolygons &islands)
|
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()
|
MotionPlanner::~MotionPlanner()
|
||||||
{
|
{
|
||||||
@ -31,75 +38,62 @@ MotionPlanner::initialize()
|
|||||||
if (this->initialized) return;
|
if (this->initialized) return;
|
||||||
if (this->islands.empty()) return; // prevent initialization of empty BoundingBox
|
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
|
// loop through islands in order to create inner expolygons and collect their contours
|
||||||
this->inner.reserve(this->islands.size());
|
|
||||||
Polygons outer_holes;
|
Polygons outer_holes;
|
||||||
for (ExPolygons::const_iterator island = this->islands.begin(); island != this->islands.end(); ++island) {
|
for (std::vector<MotionPlannerEnv>::iterator island = this->islands.begin(); island != this->islands.end(); ++island) {
|
||||||
this->inner.push_back(ExPolygonCollection());
|
// generate the internal env boundaries by shrinking the island
|
||||||
offset(*island, &this->inner.back().expolygons, -MP_INNER_MARGIN);
|
// 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
|
// 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)
|
for (Polygons::const_iterator contour = outer_holes.begin(); contour != outer_holes.end(); ++contour)
|
||||||
points.insert(points.end(), contour->points.begin(), contour->points.end());
|
bb.merge(contour->bounding_box());
|
||||||
BoundingBox bb(points);
|
|
||||||
|
|
||||||
// grow outer contour
|
// grow outer contour
|
||||||
Polygons contour;
|
Polygons contour = offset(bb.polygon(), +MP_OUTER_MARGIN*2);
|
||||||
offset(bb.polygon(), &contour, +MP_OUTER_MARGIN);
|
|
||||||
assert(contour.size() == 1);
|
assert(contour.size() == 1);
|
||||||
|
|
||||||
// make expolygon for outer environment
|
// make expolygon for outer environment
|
||||||
ExPolygons outer;
|
ExPolygons outer = diff_ex(contour, outer_holes);
|
||||||
diff(contour, outer_holes, &outer);
|
|
||||||
assert(outer.size() == 1);
|
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->graphs.resize(this->islands.size() + 1, NULL);
|
||||||
this->initialized = true;
|
this->initialized = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
ExPolygonCollection
|
const MotionPlannerEnv&
|
||||||
MotionPlanner::get_env(int island_idx) const
|
MotionPlanner::get_env(int island_idx) const
|
||||||
{
|
{
|
||||||
if (island_idx == -1) {
|
if (island_idx == -1) {
|
||||||
return ExPolygonCollection(this->outer);
|
return this->outer;
|
||||||
} else {
|
} else {
|
||||||
return this->inner[island_idx];
|
return this->islands[island_idx];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
Polyline
|
Polyline
|
||||||
MotionPlanner::shortest_path(const Point &from, const Point &to)
|
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 we have an empty configuration space, return a straight move
|
||||||
if (this->islands.empty())
|
if (this->islands.empty())
|
||||||
return Line(from, to);
|
return Line(from, to);
|
||||||
|
|
||||||
// Are both points in the same island?
|
// Are both points in the same island?
|
||||||
int island_idx = -1;
|
int island_idx = -1;
|
||||||
for (ExPolygons::const_iterator island = this->islands.begin(); island != this->islands.end(); ++island) {
|
for (std::vector<MotionPlannerEnv>::const_iterator island = this->islands.begin(); island != this->islands.end(); ++island) {
|
||||||
if (island->contains(from) && island->contains(to)) {
|
if (island->island.contains(from) && island->island.contains(to)) {
|
||||||
// since both points are in the same island, is a direct move possible?
|
// since both points are in the same island, is a direct move possible?
|
||||||
// if so, we avoid generating the visibility environment
|
// 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);
|
return Line(from, to);
|
||||||
|
|
||||||
island_idx = island - this->islands.begin();
|
island_idx = island - this->islands.begin();
|
||||||
@ -107,9 +101,12 @@ MotionPlanner::shortest_path(const Point &from, const Point &to)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// lazy generation of configuration space
|
||||||
|
this->initialize();
|
||||||
|
|
||||||
// get environment
|
// get environment
|
||||||
ExPolygonCollection env = this->get_env(island_idx);
|
MotionPlannerEnv env = this->get_env(island_idx);
|
||||||
if (env.expolygons.empty()) {
|
if (env.env.expolygons.empty()) {
|
||||||
// if this environment is empty (probably because it's too small), perform straight move
|
// if this environment is empty (probably because it's too small), perform straight move
|
||||||
// and avoid running the algorithms on empty dataset
|
// and avoid running the algorithms on empty dataset
|
||||||
return Line(from, to);
|
return Line(from, to);
|
||||||
@ -119,13 +116,20 @@ MotionPlanner::shortest_path(const Point &from, const Point &to)
|
|||||||
Point inner_from = from;
|
Point inner_from = from;
|
||||||
Point inner_to = to;
|
Point inner_to = to;
|
||||||
|
|
||||||
if (!env.contains(from)) {
|
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.
|
// Find the closest inner point to start from.
|
||||||
inner_from = this->nearest_env_point(env, from, to);
|
inner_from = env.nearest_env_point(from, to);
|
||||||
}
|
}
|
||||||
if (!env.contains(to)) {
|
if (!env.island.contains(inner_to)) {
|
||||||
// Find the closest inner point to start from.
|
// Find the closest inner point to start from.
|
||||||
inner_to = this->nearest_env_point(env, to, inner_from);
|
inner_to = env.nearest_env_point(to, inner_from);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// perform actual path search
|
// 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()
|
// grow our environment slightly in order for simplify_by_visibility()
|
||||||
// to work best by considering moves on boundaries valid as well
|
// to work best by considering moves on boundaries valid as well
|
||||||
ExPolygonCollection grown_env;
|
ExPolygonCollection grown_env(offset_ex(env.env, +SCALED_EPSILON));
|
||||||
offset(env, &grown_env.expolygons, +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
|
// 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 svg("shortest_path.svg");
|
||||||
svg.draw(this->outer);
|
svg.draw(grown_env.expolygons);
|
||||||
svg.arrows = false;
|
svg.arrows = false;
|
||||||
for (MotionPlannerGraph::adjacency_list_t::const_iterator it = graph->adjacency_list.begin(); it != graph->adjacency_list.end(); ++it) {
|
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()];
|
Point a = graph->nodes[it - graph->adjacency_list.begin()];
|
||||||
@ -164,64 +191,11 @@ MotionPlanner::shortest_path(const Point &from, const Point &to)
|
|||||||
svg.draw(polyline, "red");
|
svg.draw(polyline, "red");
|
||||||
svg.Close();
|
svg.Close();
|
||||||
*/
|
*/
|
||||||
|
}
|
||||||
|
|
||||||
return polyline;
|
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*
|
MotionPlannerGraph*
|
||||||
MotionPlanner::init_graph(int island_idx)
|
MotionPlanner::init_graph(int island_idx)
|
||||||
{
|
{
|
||||||
@ -240,8 +214,8 @@ MotionPlanner::init_graph(int island_idx)
|
|||||||
t_vd_vertices vd_vertices;
|
t_vd_vertices vd_vertices;
|
||||||
|
|
||||||
// get boundaries as lines
|
// get boundaries as lines
|
||||||
ExPolygonCollection env = this->get_env(island_idx);
|
MotionPlannerEnv env = this->get_env(island_idx);
|
||||||
Lines lines = env.lines();
|
Lines lines = env.env.lines();
|
||||||
boost::polygon::construct_voronoi(lines.begin(), lines.end(), &vd);
|
boost::polygon::construct_voronoi(lines.begin(), lines.end(), &vd);
|
||||||
|
|
||||||
// traverse the Voronoi diagram and generate graph nodes and edges
|
// 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());
|
Point p1 = Point(v1->x(), v1->y());
|
||||||
|
|
||||||
// skip edge if any of its endpoints is outside our configuration space
|
// 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);
|
t_vd_vertices::const_iterator i_v0 = vd_vertices.find(v0);
|
||||||
size_t v0_idx;
|
size_t v0_idx;
|
||||||
@ -284,6 +258,60 @@ MotionPlanner::init_graph(int island_idx)
|
|||||||
return this->graphs[island_idx + 1];
|
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
|
void
|
||||||
MotionPlannerGraph::add_edge(size_t from, size_t to, double weight)
|
MotionPlannerGraph::add_edge(size_t from, size_t to, double weight)
|
||||||
{
|
{
|
||||||
|
@ -14,27 +14,18 @@
|
|||||||
|
|
||||||
namespace Slic3r {
|
namespace Slic3r {
|
||||||
|
|
||||||
class MotionPlannerGraph;
|
class MotionPlanner;
|
||||||
|
|
||||||
class MotionPlanner
|
class MotionPlannerEnv
|
||||||
{
|
{
|
||||||
|
friend class MotionPlanner;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
MotionPlanner(const ExPolygons &islands);
|
ExPolygon island;
|
||||||
~MotionPlanner();
|
ExPolygonCollection env;
|
||||||
Polyline shortest_path(const Point &from, const Point &to);
|
MotionPlannerEnv() {};
|
||||||
size_t islands_count() const;
|
MotionPlannerEnv(const ExPolygon &island) : island(island) {};
|
||||||
|
Point nearest_env_point(const Point &from, const Point &to) 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;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
class MotionPlannerGraph
|
class MotionPlannerGraph
|
||||||
@ -61,6 +52,25 @@ class MotionPlannerGraph
|
|||||||
Polyline shortest_path(size_t from, size_t to);
|
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
|
#endif
|
||||||
|
@ -138,47 +138,18 @@ Polyline::simplify_by_visibility(const T &area)
|
|||||||
{
|
{
|
||||||
Points &pp = this->points;
|
Points &pp = this->points;
|
||||||
|
|
||||||
// find first point in area
|
|
||||||
size_t s = 0;
|
size_t s = 0;
|
||||||
while (s < pp.size() && !area.contains(pp[s])) {
|
bool did_erase = false;
|
||||||
++s;
|
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);
|
||||||
// find last point in area
|
did_erase = true;
|
||||||
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;
|
|
||||||
} else {
|
} else {
|
||||||
e = mid;
|
++s;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
/*
|
if (did_erase)
|
||||||
// The following implementation is complete but it's not efficient at all:
|
this->simplify_by_visibility(area);
|
||||||
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<ExPolygon>(const ExPolygon &area);
|
template void Polyline::simplify_by_visibility<ExPolygon>(const ExPolygon &area);
|
||||||
template void Polyline::simplify_by_visibility<ExPolygonCollection>(const ExPolygonCollection &area);
|
template void Polyline::simplify_by_visibility<ExPolygonCollection>(const ExPolygonCollection &area);
|
||||||
|
@ -29,7 +29,7 @@ SurfaceCollection::simplify(double tolerance)
|
|||||||
Surfaces ss;
|
Surfaces ss;
|
||||||
for (Surfaces::const_iterator it_s = this->surfaces.begin(); it_s != this->surfaces.end(); ++it_s) {
|
for (Surfaces::const_iterator it_s = this->surfaces.begin(); it_s != this->surfaces.end(); ++it_s) {
|
||||||
ExPolygons expp;
|
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) {
|
for (ExPolygons::const_iterator it_e = expp.begin(); it_e != expp.end(); ++it_e) {
|
||||||
Surface s = *it_s;
|
Surface s = *it_s;
|
||||||
s.expolygon = *it_e;
|
s.expolygon = *it_e;
|
||||||
|
@ -9,7 +9,7 @@ BEGIN {
|
|||||||
}
|
}
|
||||||
|
|
||||||
use Slic3r::XS;
|
use Slic3r::XS;
|
||||||
use Test::More tests => 22;
|
use Test::More tests => 20;
|
||||||
|
|
||||||
my $square = Slic3r::Polygon->new( # ccw
|
my $square = Slic3r::Polygon->new( # ccw
|
||||||
[100, 100],
|
[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';
|
ok $expolygon2->contains_point($to), 'end point is contained in second expolygon';
|
||||||
my $path = $mp->shortest_path($from, $to);
|
my $path = $mp->shortest_path($from, $to);
|
||||||
ok $path->is_valid(), 'return path is valid';
|
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';
|
ok $expolygons->[0]->contains_point($to), 'end point is contained in first expolygon';
|
||||||
my $path = $mp->shortest_path($from, $to);
|
my $path = $mp->shortest_path($from, $to);
|
||||||
ok $path->is_valid(), 'return path is valid';
|
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__
|
__END__
|
||||||
|
Loading…
Reference in New Issue
Block a user