diff --git a/src/libslic3r/AStar.hpp b/src/libslic3r/AStar.hpp index 052d0e814..b3c8b9c5f 100644 --- a/src/libslic3r/AStar.hpp +++ b/src/libslic3r/AStar.hpp @@ -1,11 +1,11 @@ #ifndef ASTAR_HPP #define ASTAR_HPP +#include + #include "libslic3r/Point.hpp" #include "libslic3r/MutablePriorityQueue.hpp" -#include - namespace Slic3r { namespace astar { // Input interface for the Astar algorithm. Specialize this struct for a @@ -34,6 +34,8 @@ template struct TracerTraits_ // 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); @@ -81,100 +83,133 @@ size_t unique_id(const T &tracer, const TracerNodeT &n) } // namespace astar_detail +constexpr size_t UNASSIGNED = size_t(-1); + +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{UNASSIGNED}, 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. -// Note that no destination node is given. 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) +// 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 namespace detail; - using Node = TracerNodeT; - enum class QueueType { Open, Closed, None }; + using Node = TracerNodeT; + using QNode = QNode; - struct QNode // Queue node. Keeps track of scores g, and h - { - Node node; // The actual node itself - QueueType qtype = QueueType::None; // Which queue holds this node - - float g = 0.f, h = 0.f; - float f() const { return g + h; } - }; - - // TODO: apply a linear memory allocator - using QMap = std::unordered_map; - - // The traversed nodes are stored here encapsulated in QNodes - QMap cached_nodes; - - struct LessPred { // Comparison functor needed by MutablePriorityQueue - QMap &m; + struct LessPred { // Comparison functor needed by the priority queue + NodeMap &m; bool operator ()(size_t node_a, size_t node_b) { - auto ait = m.find(node_a); - auto bit = m.find(node_b); - assert (ait != m.end() && bit != m.end()); - - return ait->second.f() < bit->second.f(); + return m[node_a].f() < m[node_b].f(); } }; - auto qopen = - make_mutable_priority_queue([](size_t, size_t){}, - LessPred{cached_nodes}); + auto qopen = make_mutable_priority_queue( + [&cached_nodes](size_t el, size_t qidx) { + cached_nodes[el].queue_id = qidx; + }, + LessPred{cached_nodes}); - auto qclosed = - make_mutable_priority_queue([](size_t, size_t){}, - LessPred{cached_nodes}); + QNode initial{source, /*parent = */ UNASSIGNED, /*g = */0.f}; + size_t source_id = unique_id(tracer, source); + cached_nodes[source_id] = initial; + qopen.push(source_id); - QNode initial{source, QueueType::Open}; - cached_nodes.insert({unique_id(tracer, source), initial}); - qopen.push(unique_id(tracer, source)); + size_t goal_id = goal_heuristic(tracer, source) < 0.f ? source_id : + UNASSIGNED; - bool goal_reached = false; - - while (!goal_reached && !qopen.empty()) { + while (goal_id == UNASSIGNED && !qopen.empty()) { size_t q_id = qopen.top(); qopen.pop(); - QNode q = cached_nodes.at(q_id); + QNode &q = cached_nodes[q_id]; - foreach_reachable(tracer, q.node, [&](const Node &nd) { - if (goal_reached) return goal_reached; + // This should absolutely be initialized in the cache already + assert(!std::isinf(q.g)); + + foreach_reachable(tracer, q.node, [&](const Node &succ_nd) { + if (goal_id != UNASSIGNED) + return true; + + float h = goal_heuristic(tracer, succ_nd); + float dst = trace_distance(tracer, q.node, succ_nd); + size_t succ_id = unique_id(tracer, succ_nd); + QNode qsucc_nd{succ_nd, q_id, q.g + dst, h}; - float h = goal_heuristic(tracer, nd); if (h < 0.f) { - goal_reached = true; + goal_id = succ_id; + cached_nodes[succ_id] = qsucc_nd; } else { - float dst = trace_distance(tracer, q.node, nd); - QNode qnd{nd, QueueType::None, q.g + dst, h}; - size_t qnd_id = unique_id(tracer, nd); + // If succ_id is not in cache, it gets created with g = infinity + QNode &prev_nd = cached_nodes[succ_id]; - auto it = cached_nodes.find(qnd_id); + if (qsucc_nd.g < prev_nd.g) { + // new route is better, apply it: - if (it == cached_nodes.end() || - (it->second.qtype != QueueType::None && qnd.f() < it->second.f())) { - qnd.qtype = QueueType::Open; - cached_nodes.insert_or_assign(qnd_id, qnd); - qopen.push(qnd_id); + // 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 == UNASSIGNED) + // was in closed or unqueued, rescheduling + qopen.push(succ_id); + else // was in open, updating + qopen.update(queue_id); } } - return goal_reached; + return goal_id != UNASSIGNED; }); - - q.qtype = QueueType::Closed; - cached_nodes.insert_or_assign(q_id, q); - qclosed.push(q_id); - - // write the output - *out = q.node; - ++out; } - return goal_reached; + // 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 (!std::isinf(q->g) && q->parent != UNASSIGNED) { + *out = q->node; + ++out; + q = &cached_nodes[q->parent]; + } + + if (std::isinf(q->g)) // Something went wrong + goal_id = UNASSIGNED; + } + + return goal_id != UNASSIGNED; } }} // namespace Slic3r::astar diff --git a/tests/libslic3r/test_astar.cpp b/tests/libslic3r/test_astar.cpp index f673ad9fe..6c0f7ab42 100644 --- a/tests/libslic3r/test_astar.cpp +++ b/tests/libslic3r/test_astar.cpp @@ -7,12 +7,42 @@ using namespace Slic3r; -struct PointGridTracer { +TEST_CASE("Testing basic invariants of AStar", "[AStar]") { + struct DummyTracer { + using Node = int; + + int goal = 0; + + float distance(int a, int b) const { return a - b; } + + float goal_heuristic(int n) const { return n == goal ? -1.f : 0.f; } + + size_t unique_id(int n) const { return n; } + + void foreach_reachable(int, std::function) const {} + }; + + std::vector out; + + SECTION("Output is empty when source is also the destination") { + bool found = astar::search_route(DummyTracer{}, 0, std::back_inserter(out)); + REQUIRE(out.empty()); + REQUIRE(found); + } + + SECTION("Return false when there is no route to destination") { + bool found = astar::search_route(DummyTracer{}, 1, std::back_inserter(out)); + REQUIRE(!found); + REQUIRE(out.empty()); + } +} + +struct PointGridTracer3D { using Node = size_t; const PointGrid &grid; size_t final; - PointGridTracer(const PointGrid &g, size_t goal) : + PointGridTracer3D(const PointGrid &g, size_t goal) : grid{g}, final{goal} {} template @@ -49,14 +79,328 @@ struct PointGridTracer { size_t unique_id(size_t n) const { return n; } }; +template> +bool has_duplicates(const std::vector &res, Cmp cmp = {}) +{ + auto cpy = res; + std::sort(cpy.begin(), cpy.end(), cmp); + auto it = std::unique(cpy.begin(), cpy.end()); + return it != cpy.end(); +} + TEST_CASE("astar algorithm test over 3D point grid", "[AStar]") { auto vol = BoundingBox3Base{{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}}; auto pgrid = point_grid(ex_seq, vol, {0.1f, 0.1f, 0.1f}); - PointGridTracer pgt{pgrid, pgrid.point_count() - 1}; + size_t target = pgrid.point_count() - 1; + + PointGridTracer3D pgt{pgrid, target}; std::vector out; - bool found = astar::search_route(pgt, size_t(0), std::back_inserter(out)); + bool found = astar::search_route(pgt, 0, std::back_inserter(out)); REQUIRE(found); + REQUIRE(!out.empty()); + REQUIRE(out.front() == target); + +#ifndef NDEBUG + std::cout << "Route taken: "; + for (auto it = out.rbegin(); it != out.rend(); ++it) { + std::cout << "(" << pgrid.get_coord(*it).transpose() << ") "; + } + std::cout << std::endl; +#endif + + REQUIRE(!has_duplicates(out)); // No duplicates in output +} + +enum CellValue {ON, OFF}; + +struct CellGridTracer2D_AllDirs { + using Node = Vec2i; + + static constexpr auto Cols = size_t(5); + static constexpr auto Rows = size_t(8); + static constexpr size_t GridSize = Cols * Rows; + + const std::array, Rows> &grid; + Vec2i goal; + + CellGridTracer2D_AllDirs(const std::array, Rows> &g, + const Vec2i &goal_) + : grid{g}, goal{goal_} + {} + + template + void foreach_reachable(const Vec2i &src, Fn &&fn) const + { + auto is_inside = [](const Vec2i& v) { return v.x() >= 0 && v.x() < Cols && v.y() >= 0 && v.y() < Rows; }; + if (Vec2i crd = src + Vec2i{0, 1}; is_inside(crd) && grid[crd.y()] [crd.x()] == ON) fn(crd); + if (Vec2i crd = src + Vec2i{1, 0}; is_inside(crd) && grid[crd.y()] [crd.x()] == ON) fn(crd); + if (Vec2i crd = src + Vec2i{1, 1}; is_inside(crd) && grid[crd.y()] [crd.x()] == ON) fn(crd); + if (Vec2i crd = src + Vec2i{0, -1}; is_inside(crd) && grid[crd.y()] [crd.x()] == ON) fn(crd); + if (Vec2i crd = src + Vec2i{-1, 0}; is_inside(crd) && grid[crd.y()] [crd.x()] == ON) fn(crd); + if (Vec2i crd = src + Vec2i{-1, -1}; is_inside(crd) && grid[crd.y()] [crd.x()] == ON) fn(crd); + if (Vec2i crd = src + Vec2i{1, -1}; is_inside(crd) && grid[crd.y()] [crd.x()] == ON) fn(crd); + if (Vec2i crd = src + Vec2i{-1, 1}; is_inside(crd) && grid[crd.y()] [crd.x()] == ON) fn(crd); + } + + float distance(const Vec2i & a, const Vec2i & b) const { return (a - b).squaredNorm(); } + + float goal_heuristic(const Vec2i & n) const { return n == goal ? -1.f : (n - goal).squaredNorm(); } + + size_t unique_id(const Vec2i & n) const { return n.y() * Cols + n.x(); } +}; + +struct CellGridTracer2D_Axis { + using Node = Vec2i; + + static constexpr auto Cols = size_t(5); + static constexpr auto Rows = size_t(8); + static constexpr size_t GridSize = Cols * Rows; + + const std::array, Rows> &grid; + Vec2i goal; + + CellGridTracer2D_Axis( + const std::array, Rows> &g, + const Vec2i &goal_) + : grid{g}, goal{goal_} + {} + + template + void foreach_reachable(const Vec2i &src, Fn &&fn) const + { + auto is_inside = [](const Vec2i& v) { return v.x() >= 0 && v.x() < Cols && v.y() >= 0 && v.y() < Rows; }; + if (Vec2i crd = src + Vec2i{0, 1}; is_inside(crd) && grid[crd.y()] [crd.x()] == ON) fn(crd); + if (Vec2i crd = src + Vec2i{0, -1}; is_inside(crd) && grid[crd.y()] [crd.x()] == ON) fn(crd); + if (Vec2i crd = src + Vec2i{1, 0}; is_inside(crd) && grid[crd.y()] [crd.x()] == ON) fn(crd); + if (Vec2i crd = src + Vec2i{-1, 0}; is_inside(crd) && grid[crd.y()] [crd.x()] == ON) fn(crd); + } + + float distance(const Vec2i & a, const Vec2i & b) const { return (a - b).squaredNorm(); } + + float goal_heuristic(const Vec2i &n) const + { + int manhattan_dst = std::abs(n.x() - goal.x()) + + std::abs(n.y() - goal.y()); + + return n == goal ? -1.f : manhattan_dst; + } + + size_t unique_id(const Vec2i & n) const { return n.y() * Cols + n.x(); } +}; + +using TestClasses = std::tuple< CellGridTracer2D_AllDirs, CellGridTracer2D_Axis >; + +TEMPLATE_LIST_TEST_CASE("Astar should avoid simple barrier", "[AStar]", TestClasses) { + + std::array, 8> grid = {{ + {ON , ON , ON , ON , ON}, + {ON , ON , ON , ON , ON}, + {ON , ON , ON , ON , ON}, + {ON , ON , ON , ON , ON}, + {ON , ON , ON , ON , ON}, + {ON , OFF, OFF, OFF, ON}, + {ON , ON , ON , ON , ON}, + {ON , ON , ON , ON , ON} + }}; + + Vec2i dst = {2, 0}; + TestType cgt{grid, dst}; + + std::vector out; + bool found = astar::search_route(cgt, {2, 7}, std::back_inserter(out)); + + REQUIRE(found); + REQUIRE(!out.empty()); + REQUIRE(out.front() == dst); + REQUIRE(!has_duplicates(out, [](const Vec2i &a, const Vec2i &b) { + return a.x() == b.x() ? a.y() < b.y() : a.x() < b.x(); + })); + +#ifndef NDEBUG + std::cout << "Route taken: "; + for (auto it = out.rbegin(); it != out.rend(); ++it) { + std::cout << "(" << it->transpose() << ") "; + } + std::cout << std::endl; +#endif +} + +TEMPLATE_LIST_TEST_CASE("Astar should manage to avoid arbitrary barriers", "[AStar]", TestClasses) { + + std::array, 8> grid = {{ + {ON , ON , ON , ON , ON}, + {ON , ON , ON , OFF, ON}, + {OFF, OFF, ON , OFF, ON}, + {ON , ON , ON , OFF, ON}, + {ON , OFF, ON , OFF, ON}, + {ON , OFF, ON , ON , ON}, + {ON , OFF, ON , OFF, ON}, + {ON , ON , ON , ON , ON} + }}; + + Vec2i dst = {0, 0}; + TestType cgt{grid, dst}; + + std::vector out; + bool found = astar::search_route(cgt, {0, 7}, std::back_inserter(out)); + + REQUIRE(found); + REQUIRE(!out.empty()); + REQUIRE(out.front() == dst); + REQUIRE(!has_duplicates(out, [](const Vec2i &a, const Vec2i &b) { + return a.x() == b.x() ? a.y() < b.y() : a.x() < b.x(); + })); + +#ifndef NDEBUG + std::cout << "Route taken: "; + for (auto it = out.rbegin(); it != out.rend(); ++it) { + std::cout << "(" << it->transpose() << ") "; + } + std::cout << std::endl; +#endif +} + +TEMPLATE_LIST_TEST_CASE("Astar should find the way out of a labyrinth", "[AStar]", TestClasses) { + + std::array, 8> grid = {{ + {ON , ON , ON , ON , ON }, + {ON , OFF, OFF, OFF, OFF}, + {ON , ON , ON , ON , ON }, + {OFF, OFF, OFF, OFF, ON }, + {ON , ON , ON , ON , ON }, + {ON , OFF, OFF, OFF, OFF}, + {ON , ON , ON , ON , ON }, + {OFF, OFF, OFF, OFF, ON } + }}; + + Vec2i dst = {4, 0}; + TestType cgt{grid, dst}; + + std::vector out; + bool found = astar::search_route(cgt, {4, 7}, std::back_inserter(out)); + + REQUIRE(found); + REQUIRE(!out.empty()); + REQUIRE(out.front() == dst); + REQUIRE(!has_duplicates(out, [](const Vec2i &a, const Vec2i &b) { + return a.x() == b.x() ? a.y() < b.y() : a.x() < b.x(); + })); + +#ifndef NDEBUG + std::cout << "Route taken: "; + for (auto it = out.rbegin(); it != out.rend(); ++it) { + std::cout << "(" << it->transpose() << ") "; + } + std::cout << std::endl; +#endif +} + +TEST_CASE("Zero heuristic function should result in dijsktra's algo", "[AStar]") +{ + struct GraphTracer { + using Node = size_t; + using QNode = astar::QNode; + + struct Edge + { + size_t to_id = size_t(-1); + float cost = 0.f; + bool operator <(const Edge &e) const { return to_id < e.to_id; } + }; + + struct ENode: public QNode { + std::vector edges; + + ENode(size_t node_id, std::initializer_list edgelist) + : QNode{node_id}, edges(edgelist) + {} + + ENode &operator=(const QNode &q) + { + assert(node == q.node); + g = q.g; + h = q.h; + parent = q.parent; + queue_id = q.queue_id; + + return *this; + } + }; + + // Example graph from + // https://www.geeksforgeeks.org/dijkstras-shortest-path-algorithm-greedy-algo-7/?ref=lbp + std::vector nodes = { + {0, {{1, 4.f}, {7, 8.f}}}, + {1, {{0, 4.f}, {2, 8.f}, {7, 11.f}}}, + {2, {{1, 8.f}, {3, 7.f}, {5, 4.f}, {8, 2.f}}}, + {3, {{2, 7.f}, {4, 9.f}, {5, 14.f}}}, + {4, {{3, 9.f}, {5, 10.f}}}, + {5, {{2, 4.f}, {3, 14.f}, {4, 10.f}, {6, 2.f}}}, + {6, {{5, 2.f}, {7, 1.f}, {8, 6.f}}}, + {7, {{0, 8.f}, {1, 11.f}, {6, 1.f}, {8, 7.f}}}, + {8, {{2, 2.f}, {6, 6.f}, {7, 7.f}}} + }; + + float distance(size_t a, size_t b) const { + float ret = std::numeric_limits::infinity(); + if (a < nodes.size()) { + auto it = std::lower_bound(nodes[a].edges.begin(), + nodes[a].edges.end(), + Edge{b, 0.f}); + + if (it != nodes[a].edges.end()) { + ret = it->cost; + } + } + + return ret; + } + + float goal_heuristic(size_t) const { return 0.f; } + + size_t unique_id(size_t n) const { return n; } + + void foreach_reachable(size_t n, std::function fn) const + { + if (n < nodes.size()) { + for (const Edge &e : nodes[n].edges) + fn(e.to_id); + } + } + } graph; + + std::vector out; + + // 'graph.nodes' is able to be a node cache (it simulates an associative container) + bool found = astar::search_route(graph, size_t(0), std::back_inserter(out), graph.nodes); + + // But should not crash or loop infinitely. + REQUIRE(!found); + + // Without a destination, there is no output. But the algorithm should halt. + REQUIRE(out.empty()); + + // Source node should have it's parent unset + REQUIRE(graph.nodes[0].parent == astar::UNASSIGNED); + + // All other nodes should have their parents set + for (size_t i = 1; i < graph.nodes.size(); ++i) + REQUIRE(graph.nodes[i].parent != astar::UNASSIGNED); + + std::array ref_distances = {0.f, 4.f, 12.f, 19.f, 21.f, + 11.f, 9.f, 8.f, 14.f}; + + // Try to trace each node back to the source node. Each of them should + // arrive to the source within less hops than the full number of nodes. + for (size_t i = 0, k = 0; i < graph.nodes.size(); ++i, k = 0) { + GraphTracer::QNode *q = &graph.nodes[i]; + REQUIRE(q->g == Approx(ref_distances[i])); + while (k++ < graph.nodes.size() && q->parent != astar::UNASSIGNED) + q = &graph.nodes[q->parent]; + + REQUIRE(q->parent == astar::UNASSIGNED); + } }