From d3db1f25372a1df0baa4899b0d2378a18eea3959 Mon Sep 17 00:00:00 2001 From: "wenjie.guo" Date: Mon, 25 Sep 2023 10:40:12 +0800 Subject: [PATCH] NEW: path planning algorithm Astar.hpp, JumpPointSearch.hpp and JumpPointSearch.cpp are from Prusa. Github: 1793 Signed-off-by: wenjie.guo Original Authors: tamasmeszaros , Pavel Mikus Change-Id: I72f68d59bc8bb3b7cc0f37276c10811bca2ac38c (cherry picked from commit 7a14a6a48af68b6a3d7985bdb9c171aabeb70817) --- src/libslic3r/AStar.hpp | 157 +++++++++++ src/libslic3r/CMakeLists.txt | 3 + src/libslic3r/JumpPointSearch.cpp | 349 +++++++++++++++++++++++++ src/libslic3r/JumpPointSearch.hpp | 38 +++ src/libslic3r/MutablePriorityQueue.hpp | 2 +- 5 files changed, 548 insertions(+), 1 deletion(-) create mode 100644 src/libslic3r/AStar.hpp create mode 100644 src/libslic3r/JumpPointSearch.cpp create mode 100644 src/libslic3r/JumpPointSearch.hpp diff --git a/src/libslic3r/AStar.hpp b/src/libslic3r/AStar.hpp new file mode 100644 index 000000000..a7d9e7b8f --- /dev/null +++ b/src/libslic3r/AStar.hpp @@ -0,0 +1,157 @@ +#ifndef ASTAR_HPP +#define ASTAR_HPP + +#include // std::isinf() is here +#include + +#include "libslic3r/MutablePriorityQueue.hpp" + +namespace Slic3r { namespace astar { + +// Borrowed from C++20 +template using remove_cvref_t = std::remove_cv_t>; + +// Input interface for the Astar algorithm. Specialize this struct for a +// particular type and implement all the 4 methods and specify the Node type +// to register the new type for the astar implementation. +template struct TracerTraits_ +{ + // The type of a node used by this tracer. Usually a point in space. + using Node = typename T::Node; + + // Call fn for every new node reachable from node 'src'. fn should have the + // candidate node as its only argument. + template static void foreach_reachable(const T &tracer, const Node &src, Fn &&fn) { tracer.foreach_reachable(src, fn); } + + // Get the distance from node 'a' to node 'b'. This is sometimes referred + // to as the g value of a node in AStar context. + static float distance(const T &tracer, const Node &a, const Node &b) { return tracer.distance(a, b); } + + // Get the estimated distance heuristic from node 'n' to the destination. + // This is referred to as the h value in AStar context. + // If node 'n' is the goal, this function should return a negative value. + // Note that this heuristic should be admissible (never bigger than the real + // cost) in order for Astar to work. + static float goal_heuristic(const T &tracer, const Node &n) { return tracer.goal_heuristic(n); } + + // Return a unique identifier (hash) for node 'n'. + static size_t unique_id(const T &tracer, const Node &n) { return tracer.unique_id(n); } +}; + +// Helper definition to get the node type of a tracer +template using TracerNodeT = typename TracerTraits_>::Node; + +constexpr auto Unassigned = std::numeric_limits::max(); + +template struct QNode // Queue node. Keeps track of scores g, and h +{ + TracerNodeT node; // The actual node itself + size_t queue_id; // Position in the open queue or Unassigned if closed + size_t parent; // unique id of the parent or Unassigned + + float g, h; + float f() const { return g + h; } + + QNode(TracerNodeT n = {}, size_t p = Unassigned, float gval = std::numeric_limits::infinity(), float hval = 0.f) + : node{std::move(n)}, parent{p}, queue_id{InvalidQueueID}, g{gval}, h{hval} + {} +}; + +// Run the AStar algorithm on a tracer implementation. +// The 'tracer' argument encapsulates the domain (grid, point cloud, etc...) +// The 'source' argument is the starting node. +// The 'out' argument is the output iterator into which the output nodes are +// written. For performance reasons, the order is reverse, from the destination +// to the source -- (destination included, source is not). +// The 'cached_nodes' argument is an optional associative container to hold a +// QNode entry for each visited node. Any compatible container can be used +// (like std::map or maps with different allocators, even a sufficiently large +// std::vector). +// +// Note that no destination node is given in the signature. The tracer's +// goal_heuristic() method should return a negative value if a node is a +// destination node. +template>> +bool search_route(const Tracer &tracer, const TracerNodeT &source, It out, NodeMap &&cached_nodes = {}) +{ + using Node = TracerNodeT; + using QNode = QNode; + using TracerTraits = TracerTraits_>; + + struct LessPred + { // Comparison functor needed by the priority queue + NodeMap &m; + bool operator()(size_t node_a, size_t node_b) { return m[node_a].f() < m[node_b].f(); } + }; + + auto qopen = make_mutable_priority_queue([&cached_nodes](size_t el, size_t qidx) { cached_nodes[el].queue_id = qidx; }, LessPred{cached_nodes}); + + QNode initial{source, /*parent = */ Unassigned, /*g = */ 0.f}; + size_t source_id = TracerTraits::unique_id(tracer, source); + cached_nodes[source_id] = initial; + qopen.push(source_id); + + size_t goal_id = TracerTraits::goal_heuristic(tracer, source) < 0.f ? source_id : Unassigned; + + while (goal_id == Unassigned && !qopen.empty()) { + size_t q_id = qopen.top(); + qopen.pop(); + QNode &q = cached_nodes[q_id]; + + // This should absolutely be initialized in the cache already + assert(!std::isinf(q.g)); + + TracerTraits::foreach_reachable(tracer, q.node, [&](const Node &succ_nd) { + if (goal_id != Unassigned) return true; + + float h = TracerTraits::goal_heuristic(tracer, succ_nd); + float dst = TracerTraits::distance(tracer, q.node, succ_nd); + size_t succ_id = TracerTraits::unique_id(tracer, succ_nd); + QNode qsucc_nd{succ_nd, q_id, q.g + dst, h}; + + if (h < 0.f) { + goal_id = succ_id; + cached_nodes[succ_id] = qsucc_nd; + } else { + // If succ_id is not in cache, it gets created with g = infinity + QNode &prev_nd = cached_nodes[succ_id]; + + if (qsucc_nd.g < prev_nd.g) { + // new route is better, apply it: + + // Save the old queue id, it would be lost after the next line + size_t queue_id = prev_nd.queue_id; + + // The cache needs to be updated either way + prev_nd = qsucc_nd; + + if (queue_id == InvalidQueueID) + // was in closed or unqueued, rescheduling + qopen.push(succ_id); + else // was in open, updating + qopen.update(queue_id); + } + } + + return goal_id != Unassigned; + }); + } + + // Write the output, do not reverse. Clients can do so if they need to. + if (goal_id != Unassigned) { + const QNode *q = &cached_nodes[goal_id]; + while (q->parent != Unassigned) { + assert(!std::isinf(q->g)); // Uninitialized nodes are NOT allowed + + *out = q->node; + ++out; + q = &cached_nodes[q->parent]; + } + } + + return goal_id != Unassigned; +} + +}} // namespace Slic3r::astar + +#endif // ASTAR_HPP diff --git a/src/libslic3r/CMakeLists.txt b/src/libslic3r/CMakeLists.txt index 7f3fbf2ed..5fb314e9c 100644 --- a/src/libslic3r/CMakeLists.txt +++ b/src/libslic3r/CMakeLists.txt @@ -27,6 +27,7 @@ set(lisbslic3r_sources AABBMesh.hpp AABBMesh.cpp AnyPtr.hpp + AStar.hpp BoundingBox.cpp BoundingBox.hpp BridgeDetector.cpp @@ -177,6 +178,8 @@ set(lisbslic3r_sources Int128.hpp InternalBridgeDetector.cpp InternalBridgeDetector.hpp + JumpPointSearch.hpp + JumpPointSearch.cpp KDTreeIndirect.hpp Layer.cpp Layer.hpp diff --git a/src/libslic3r/JumpPointSearch.cpp b/src/libslic3r/JumpPointSearch.cpp new file mode 100644 index 000000000..f8ef2ff10 --- /dev/null +++ b/src/libslic3r/JumpPointSearch.cpp @@ -0,0 +1,349 @@ +#include "JumpPointSearch.hpp" +#include "BoundingBox.hpp" +#include "ExPolygon.hpp" +#include "Point.hpp" +#include "libslic3r/AStar.hpp" +#include "libslic3r/KDTreeIndirect.hpp" +#include "libslic3r/Polygon.hpp" +#include "libslic3r/Polyline.hpp" +#include "libslic3r/libslic3r.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +//#define DEBUG_FILES +#ifdef DEBUG_FILES +#include "libslic3r/SVG.hpp" +#endif + +namespace Slic3r { + +// execute fn for each pixel on the line. If fn returns false, terminate the iteration +template void dda(coord_t x0, coord_t y0, coord_t x1, coord_t y1, const PointFn &fn) +{ + coord_t dx = abs(x1 - x0); + coord_t dy = abs(y1 - y0); + coord_t x = x0; + coord_t y = y0; + coord_t n = 1 + dx + dy; + coord_t x_inc = (x1 > x0) ? 1 : -1; + coord_t y_inc = (y1 > y0) ? 1 : -1; + coord_t error = dx - dy; + dx *= 2; + dy *= 2; + + for (; n > 0; --n) { + if (!fn(x, y)) return; + + if (error > 0) { + x += x_inc; + error -= dy; + } else { + y += y_inc; + error += dx; + } + } +} + +// will draw the line twice, second time with and offset of 1 in the direction of normal +// may call the fn on the same coordiantes multiple times! +template void double_dda_with_offset(coord_t x0, coord_t y0, coord_t x1, coord_t y1, const PointFn &fn) +{ + Vec2d normal = Point{y1 - y0, x1 - x0}.cast().normalized(); + normal.x() = ceil(normal.x()); + normal.y() = ceil(normal.y()); + Point start_offset = Point(x0, y0) + (normal).cast(); + Point end_offset = Point(x1, y1) + (normal).cast(); + + dda(x0, y0, x1, y1, fn); + dda(start_offset.x(), start_offset.y(), end_offset.x(), end_offset.y(), fn); +} + +template class JPSTracer +{ +public: + // Use incoming_dir [0,0] for starting points, so that all directions are checked from that point + struct Node + { + CellPositionType position; + CellPositionType incoming_dir; + }; + + JPSTracer(CellPositionType target, CellQueryFn is_passable) : target(target), is_passable(is_passable) {} + +private: + CellPositionType target; + CellQueryFn is_passable; // should return boolean whether the cell is passable or not + + CellPositionType find_jump_point(CellPositionType start, CellPositionType forward_dir) const + { + CellPositionType next = start + forward_dir; + while (next != target && is_passable(next) && !(is_jump_point(next, forward_dir))) { next = next + forward_dir; } + + if (is_passable(next)) { + return next; + } else { + return start; + } + } + + bool is_jump_point(CellPositionType pos, CellPositionType forward_dir) const + { + if (abs(forward_dir.x()) + abs(forward_dir.y()) == 2) { + // diagonal + CellPositionType horizontal_check_dir = CellPositionType{forward_dir.x(), 0}; + CellPositionType vertical_check_dir = CellPositionType{0, forward_dir.y()}; + + if (!is_passable(pos - horizontal_check_dir) && is_passable(pos + forward_dir - 2 * horizontal_check_dir)) { return true; } + + if (!is_passable(pos - vertical_check_dir) && is_passable(pos + forward_dir - 2 * vertical_check_dir)) { return true; } + + if (find_jump_point(pos, horizontal_check_dir) != pos) { return true; } + + if (find_jump_point(pos, vertical_check_dir) != pos) { return true; } + + return false; + } else { // horizontal or vertical + CellPositionType side_dir = CellPositionType(forward_dir.y(), forward_dir.x()); + + if (!is_passable(pos + side_dir) && is_passable(pos + forward_dir + side_dir)) { return true; } + + if (!is_passable(pos - side_dir) && is_passable(pos + forward_dir - side_dir)) { return true; } + + return false; + } + } + +public: + template void foreach_reachable(const Node &from, Fn &&fn) const + { + const CellPositionType & pos = from.position; + const CellPositionType & forward_dir = from.incoming_dir; + std::vector dirs_to_check{}; + + if (abs(forward_dir.x()) + abs(forward_dir.y()) == 0) { // special case for starting point + dirs_to_check = all_directions; + } else if (abs(forward_dir.x()) + abs(forward_dir.y()) == 2) { + // diagonal + CellPositionType horizontal_check_dir = CellPositionType{forward_dir.x(), 0}; + CellPositionType vertical_check_dir = CellPositionType{0, forward_dir.y()}; + + if (!is_passable(pos - horizontal_check_dir) && is_passable(pos + forward_dir - 2 * horizontal_check_dir)) { + dirs_to_check.push_back(forward_dir - 2 * horizontal_check_dir); + } + + if (!is_passable(pos - vertical_check_dir) && is_passable(pos + forward_dir - 2 * vertical_check_dir)) { + dirs_to_check.push_back(forward_dir - 2 * vertical_check_dir); + } + + dirs_to_check.push_back(horizontal_check_dir); + dirs_to_check.push_back(vertical_check_dir); + dirs_to_check.push_back(forward_dir); + + } else { // horizontal or vertical + CellPositionType side_dir = CellPositionType(forward_dir.y(), forward_dir.x()); + + if (!is_passable(pos + side_dir) && is_passable(pos + forward_dir + side_dir)) { dirs_to_check.push_back(forward_dir + side_dir); } + + if (!is_passable(pos - side_dir) && is_passable(pos + forward_dir - side_dir)) { dirs_to_check.push_back(forward_dir - side_dir); } + dirs_to_check.push_back(forward_dir); + } + + for (const CellPositionType &dir : dirs_to_check) { + CellPositionType jp = find_jump_point(pos, dir); + if (jp != pos) fn(Node{jp, dir}); + } + } + + float distance(Node a, Node b) const { return (a.position - b.position).template cast().norm(); } + + float goal_heuristic(Node n) const { return n.position == target ? -1.f : (target - n.position).template cast().norm(); } + + size_t unique_id(Node n) const { return (static_cast(uint16_t(n.position.x())) << 16) + static_cast(uint16_t(n.position.y())); } + + const std::vector all_directions{{1, 0}, {1, 1}, {0, 1}, {-1, 1}, {-1, 0}, {-1, -1}, {0, -1}, {1, -1}}; +}; + +void JPSPathFinder::clear() +{ + inpassable.clear(); + max_search_box.max = Pixel(std::numeric_limits::min(), std::numeric_limits::min()); + max_search_box.min = Pixel(std::numeric_limits::max(), std::numeric_limits::max()); + add_obstacles(bed_shape); +} + +void JPSPathFinder::add_obstacles(const Lines &obstacles) +{ + auto store_obstacle = [&](coord_t x, coord_t y) { + max_search_box.max.x() = std::max(max_search_box.max.x(), x); + max_search_box.max.y() = std::max(max_search_box.max.y(), y); + max_search_box.min.x() = std::min(max_search_box.min.x(), x); + max_search_box.min.y() = std::min(max_search_box.min.y(), y); + inpassable.insert(Pixel{x, y}); + return true; + }; + + for (const Line &l : obstacles) { + Pixel start = pixelize(l.a); + Pixel end = pixelize(l.b); + double_dda_with_offset(start.x(), start.y(), end.x(), end.y(), store_obstacle); + } +} + +Polyline JPSPathFinder::find_path(const Point &p0, const Point &p1) +{ + Pixel start = pixelize(p0); + Pixel end = pixelize(p1); + if (inpassable.empty() || (start - end).cast().norm() < 3.0) { return Polyline{p0, p1}; } + + if (inpassable.find(start) != inpassable.end()) { + dda(start.x(), start.y(), end.x(), end.y(), [&](coord_t x, coord_t y) { + if (inpassable.find(Pixel(x, y)) == inpassable.end() || start == end) { // new start not found yet, and xy passable + start = Pixel(x, y); + return false; + } + return true; + }); + } + + if (inpassable.find(end) != inpassable.end()) { + dda(end.x(), end.y(), start.x(), start.y(), [&](coord_t x, coord_t y) { + if (inpassable.find(Pixel(x, y)) == inpassable.end() || start == end) { // new start not found yet, and xy passable + end = Pixel(x, y); + return false; + } + return true; + }); + } + + BoundingBox search_box = max_search_box; + search_box.max -= Pixel(1, 1); + search_box.min += Pixel(1, 1); + + BoundingBox bounding_square(Points{start, end}); + bounding_square.max += Pixel(5, 5); + bounding_square.min -= Pixel(5, 5); + coord_t bounding_square_size = 2 * std::max(bounding_square.size().x(), bounding_square.size().y()); + bounding_square.max.x() += (bounding_square_size - bounding_square.size().x()) / 2; + bounding_square.min.x() -= (bounding_square_size - bounding_square.size().x()) / 2; + bounding_square.max.y() += (bounding_square_size - bounding_square.size().y()) / 2; + bounding_square.min.y() -= (bounding_square_size - bounding_square.size().y()) / 2; + + // Intersection - limit the search box to a square area around the start and end, to fasten the path searching + search_box.max = search_box.max.cwiseMin(bounding_square.max); + search_box.min = search_box.min.cwiseMax(bounding_square.min); + + auto cell_query = [&](Pixel pixel) { return search_box.contains(pixel) && (pixel == start || pixel == end || inpassable.find(pixel) == inpassable.end()); }; + + JPSTracer tracer(end, cell_query); + using QNode = astar::QNode>; + + std::unordered_map astar_cache{}; + std::vector out_path; + std::vector out_nodes; + + if (!astar::search_route(tracer, {start, {0, 0}}, std::back_inserter(out_nodes), astar_cache)) { + // path not found - just reconstruct the best path from astar cache. + // Note that astar_cache is NOT empty - at least the starting point should always be there + auto coordiante_func = [&astar_cache](size_t idx, size_t dim) { return float(astar_cache[idx].node.position[dim]); }; + std::vector keys; + keys.reserve(astar_cache.size()); + for (const auto &pair : astar_cache) { keys.push_back(pair.first); } + KDTreeIndirect<2, float, decltype(coordiante_func)> kd_tree(coordiante_func, keys); + size_t closest_qnode = find_closest_point(kd_tree, end.cast()); + + out_path.push_back(end); + while (closest_qnode != astar::Unassigned) { + out_path.push_back(astar_cache[closest_qnode].node.position); + closest_qnode = astar_cache[closest_qnode].parent; + } + } else { + for (const auto &node : out_nodes) { out_path.push_back(node.position); } + out_path.push_back(start); + } + +#ifdef DEBUG_FILES + auto scaled_points = [](const Points &ps) { + Points r; + for (const Point &p : ps) { r.push_back(Point::new_scale(p.x(), p.y())); } + return r; + }; + auto scaled_point = [](const Point &p) { return Point::new_scale(p.x(), p.y()); }; + ::Slic3r::SVG svg(debug_out_path(("path_jps" + std::to_string(print_z) + "_" + std::to_string(rand() % 1000)).c_str()).c_str(), + BoundingBox(scaled_point(search_box.min), scaled_point(search_box.max))); + for (const auto &p : inpassable) { svg.draw(scaled_point(p), "black", scale_(0.4)); } + for (const auto &qn : astar_cache) { svg.draw(scaled_point(qn.second.node.position), "blue", scale_(0.3)); } + svg.draw(Polyline(scaled_points(out_path)), "yellow", scale_(0.25)); + svg.draw(scaled_point(end), "purple", scale_(0.4)); + svg.draw(scaled_point(start), "green", scale_(0.4)); +#endif + + std::vector tmp_path; + tmp_path.reserve(out_path.size()); + // Some path found, reverse and remove points that do not change direction + std::reverse(out_path.begin(), out_path.end()); + { + tmp_path.push_back(out_path.front()); // first point + for (size_t i = 1; i < out_path.size() - 1; i++) { + if ((out_path[i] - out_path[i - 1]).cast().normalized() != (out_path[i + 1] - out_path[i]).cast().normalized()) { tmp_path.push_back(out_path[i]); } + } + tmp_path.push_back(out_path.back()); // last_point + out_path = tmp_path; + } + +#ifdef DEBUG_FILES + svg.draw(Polyline(scaled_points(out_path)), "orange", scale_(0.20)); +#endif + + tmp_path.clear(); + // remove redundant jump points - there are points that change direction but are not needed - this inefficiency arises from the + // usage of grid search The removal alg tries to find the longest Px Px+k path without obstacles. If Px Px+k+1 is blocked, it will + // insert the Px+k point to result and continue search from Px+k + { + tmp_path.push_back(out_path.front()); // first point + size_t index_of_last_stored_point = 0; + for (size_t i = 1; i < out_path.size(); i++) { + if (i - index_of_last_stored_point < 2) continue; + bool passable = true; + auto store_obstacle = [&](coord_t x, coord_t y) { + if (Pixel(x, y) != start && Pixel(x, y) != end && inpassable.find(Pixel(x, y)) != inpassable.end()) { + passable = false; + return false; + } + return true; + }; + dda(tmp_path.back().x(), tmp_path.back().y(), out_path[i].x(), out_path[i].y(), store_obstacle); + if (!passable) { + tmp_path.push_back(out_path[i - 1]); + index_of_last_stored_point = i - 1; + } + } + tmp_path.push_back(out_path.back()); // last_point + out_path = tmp_path; + } + +#ifdef DEBUG_FILES + svg.draw(Polyline(scaled_points(out_path)), "red", scale_(0.15)); + svg.Close(); +#endif + + // before returing the path, transform it from pixels back to points. + // Also replace the first and last pixel by input points so that result path patches input params exactly. + for (Pixel &p : out_path) { p = unpixelize(p); } + out_path.front() = p0; + out_path.back() = p1; + + return Polyline(out_path); +} + +} // namespace Slic3r diff --git a/src/libslic3r/JumpPointSearch.hpp b/src/libslic3r/JumpPointSearch.hpp new file mode 100644 index 000000000..86eeb9ce6 --- /dev/null +++ b/src/libslic3r/JumpPointSearch.hpp @@ -0,0 +1,38 @@ +#pragma once +#ifndef SRC_LIBSLIC3R_JUMPPOINTSEARCH_HPP_ +#define SRC_LIBSLIC3R_JUMPPOINTSEARCH_HPP_ + +#include "BoundingBox.hpp" +#include "Polygon.hpp" +#include "libslic3r/Layer.hpp" +#include "libslic3r/Point.hpp" +#include "libslic3r/Polyline.hpp" +#include "libslic3r/libslic3r.h" +#include +#include + +namespace Slic3r { + +class JPSPathFinder +{ + using Pixel = Point; + std::unordered_set inpassable; + coordf_t print_z; + BoundingBox max_search_box; + Lines bed_shape; + + const coord_t resolution = scaled(1.5); + Pixel pixelize(const Point &p) { return p / resolution; } + Point unpixelize(const Pixel &p) { return p * resolution; } + +public: + JPSPathFinder() = default; + void init_bed_shape(const Points &bed_shape) { this->bed_shape = (to_lines(Polygon{bed_shape})); }; + void clear(); + void add_obstacles(const Lines &obstacles); + Polyline find_path(const Point &start, const Point &end); +}; + +} // namespace Slic3r + +#endif /* SRC_LIBSLIC3R_JUMPPOINTSEARCH_HPP_ */ diff --git a/src/libslic3r/MutablePriorityQueue.hpp b/src/libslic3r/MutablePriorityQueue.hpp index cc1cae68c..2565f417f 100644 --- a/src/libslic3r/MutablePriorityQueue.hpp +++ b/src/libslic3r/MutablePriorityQueue.hpp @@ -3,7 +3,7 @@ #include #include - +constexpr auto InvalidQueueID = std::numeric_limits::max(); template class MutablePriorityQueue {