Revamped A* algorithm

with extended test suite

2 files changed, 448 insertions, 69 deletions

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 <unordered_map>
+
 #include "libslic3r/Point.hpp"
 #include "libslic3r/MutablePriorityQueue.hpp"
 
-#include <unordered_map>
-
 namespace Slic3r { namespace astar {
 
 // Input interface for the Astar algorithm. Specialize this struct for a
@@ -34,6 +34,8 @@ template<class T> 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<T> &n)
 
 } // namespace astar_detail
 
+constexpr size_t UNASSIGNED = size_t(-1);
+
+template<class Tracer>
+struct QNode // Queue node. Keeps track of scores g, and h
+{
+    TracerNodeT<Tracer>  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<Tracer> n = {},
+          size_t              p    = UNASSIGNED,
+          float               gval = std::numeric_limits<float>::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<class Tracer, class It>
-bool search_route(const Tracer &tracer, const TracerNodeT<Tracer> &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<class Tracer,
+         class It,
+         class NodeMap = std::unordered_map<size_t, QNode<Tracer>>>
+bool search_route(const Tracer              &tracer,
+                  const TracerNodeT<Tracer> &source,
+                  It                         out,
+                  NodeMap                  &&cached_nodes = {})
 {
     using namespace detail;
 
-    using Node = TracerNodeT<Tracer>;
-    enum  class QueueType { Open, Closed, None };
-
-    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<size_t, QNode>;
-
-       // The traversed nodes are stored here encapsulated in QNodes
-    QMap cached_nodes;
+    using Node  = TracerNodeT<Tracer>;
+    using QNode = QNode<Tracer>;
 
-    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, false>([](size_t, size_t){},
-                                                   LessPred{cached_nodes});
-
-    auto qclosed =
-        make_mutable_priority_queue<size_t, false>([](size_t, size_t){},
-                                                   LessPred{cached_nodes});
+    auto qopen = make_mutable_priority_queue<size_t, true>(
+        [&cached_nodes](size_t el, size_t qidx) {
+            cached_nodes[el].queue_id = qidx;
+        },
+        LessPred{cached_nodes});
 
-    QNode initial{source, QueueType::Open};
-    cached_nodes.insert({unique_id(tracer, source), initial});
-    qopen.push(unique_id(tracer, source));
+    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);
 
-    bool goal_reached = false;
+    size_t goal_id = goal_heuristic(tracer, source) < 0.f ? source_id :
+                                                            UNASSIGNED;
 
-    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];
+
+        // This should absolutely be initialized in the cache already
+        assert(!std::isinf(q.g));
 
-        foreach_reachable(tracer, q.node, [&](const Node &nd) {
-            if (goal_reached) return goal_reached;
+        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];
+
+                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;
 
-                auto it = cached_nodes.find(qnd_id);
+                    // The cache needs to be updated either way
+                    prev_nd = qsucc_nd;
 
-                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);
+                    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, 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];
+        }
 
-           // write the output
-        *out = q.node;
-        ++out;
+        if (std::isinf(q->g)) // Something went wrong
+            goal_id = UNASSIGNED;
     }
 
-    return goal_reached;
+    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<bool(int)>) const {}
+    };
+
+    std::vector<int> 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<float> &grid;
     size_t final;
 
-    PointGridTracer(const PointGrid<float> &g, size_t goal) :
+    PointGridTracer3D(const PointGrid<float> &g, size_t goal) :
         grid{g}, final{goal} {}
 
     template<class Fn>
@@ -49,14 +79,328 @@ struct PointGridTracer {
     size_t unique_id(size_t n) const { return n; }
 };
 
+template<class Node, class Cmp = std::less<Node>>
+bool has_duplicates(const std::vector<Node> &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<Vec3f>{{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<size_t> 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<std::array<CellValue, Cols>, Rows> &grid;
+    Vec2i goal;
+
+    CellGridTracer2D_AllDirs(const std::array<std::array<CellValue, Cols>, Rows> &g,
+                     const Vec2i &goal_)
+        : grid{g}, goal{goal_}
+    {}
+
+    template<class Fn>
+    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<std::array<CellValue, Cols>, Rows> &grid;
+    Vec2i goal;
+
+    CellGridTracer2D_Axis(
+        const std::array<std::array<CellValue, Cols>, Rows> &g,
+        const Vec2i                                         &goal_)
+        : grid{g}, goal{goal_}
+    {}
+
+    template<class Fn>
+    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<std::array<CellValue, 5>, 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<Vec2i> 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<std::array<CellValue, 5>, 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<Vec2i> 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<std::array<CellValue, 5>, 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<Vec2i> 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<GraphTracer>;
+
+        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<Edge> edges;
+
+            ENode(size_t node_id, std::initializer_list<Edge> 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<ENode> 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<float>::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<bool(int)> fn) const
+        {
+            if (n < nodes.size()) {
+                for (const Edge &e : nodes[n].edges)
+                    fn(e.to_id);
+            }
+        }
+    } graph;
+
+    std::vector<size_t> 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<float, 9> 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);
+    }
 }