3D-printing/OrcaSlicer
1
0
Fork 0
mirror of https://github.com/SoftFever/OrcaSlicer.git synced 2025-07-18 04:08:02 -06:00
Code Issues Projects Releases Packages Wiki Activity Actions 2

Fixes and improvements to MotionPlanner, much smarter now

Browse source
This commit is contained in:
Alessandro Ranellucci 2015-12-21 14:46:35 +01:00
parent f7e97f7e9b
commit 1a286fc906
12 changed files with 198 additions and 202 deletions
Download patch file Download diff file Expand all files Collapse all files

250
xs/src/libslic3r/MotionPlanner.cpp
View file

@ -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)
{