From 6b23e90424dadaaef055d077ca84812b98b8f45a Mon Sep 17 00:00:00 2001 From: tamasmeszaros Date: Wed, 11 May 2022 12:06:07 +0200 Subject: [PATCH] Add astar algorithm Fix windows build --- src/libslic3r/AStar.hpp | 182 +++++++++++++++++++++++++++++++++ src/libslic3r/CMakeLists.txt | 1 + src/libslic3r/PointGrid.hpp | 12 +-- tests/libslic3r/CMakeLists.txt | 1 + tests/libslic3r/test_astar.cpp | 71 +++++++++++++ 5 files changed, 261 insertions(+), 6 deletions(-) create mode 100644 src/libslic3r/AStar.hpp create mode 100644 tests/libslic3r/test_astar.cpp diff --git a/src/libslic3r/AStar.hpp b/src/libslic3r/AStar.hpp new file mode 100644 index 000000000..052d0e814 --- /dev/null +++ b/src/libslic3r/AStar.hpp @@ -0,0 +1,182 @@ +#ifndef ASTAR_HPP +#define ASTAR_HPP + +#include "libslic3r/Point.hpp" +#include "libslic3r/MutablePriorityQueue.hpp" + +#include + +namespace Slic3r { namespace astar { + +// 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. + 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; + +namespace detail { +// Helper functions dispatching calls through the TracerTraits_ interface + +template using TracerTraits = TracerTraits_>; + +template +void foreach_reachable(const T &tracer, const TracerNodeT &from, Fn &&fn) +{ + TracerTraits::foreach_reachable(tracer, from, fn); +} + +template +float trace_distance(const T &tracer, const TracerNodeT &a, const TracerNodeT &b) +{ + return TracerTraits::distance(tracer, a, b); +} + +template +float goal_heuristic(const T &tracer, const TracerNodeT &n) +{ + return TracerTraits::goal_heuristic(tracer, n); +} + +template +size_t unique_id(const T &tracer, const TracerNodeT &n) +{ + return TracerTraits::unique_id(tracer, n); +} + +} // namespace astar_detail + +// 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) +{ + using namespace detail; + + using Node = TracerNodeT; + 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; + + // The traversed nodes are stored here encapsulated in QNodes + QMap cached_nodes; + + struct LessPred { // Comparison functor needed by MutablePriorityQueue + QMap &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(); + } + }; + + auto qopen = + make_mutable_priority_queue([](size_t, size_t){}, + LessPred{cached_nodes}); + + auto qclosed = + make_mutable_priority_queue([](size_t, size_t){}, + LessPred{cached_nodes}); + + QNode initial{source, QueueType::Open}; + cached_nodes.insert({unique_id(tracer, source), initial}); + qopen.push(unique_id(tracer, source)); + + bool goal_reached = false; + + while (!goal_reached && !qopen.empty()) { + size_t q_id = qopen.top(); + qopen.pop(); + QNode q = cached_nodes.at(q_id); + + foreach_reachable(tracer, q.node, [&](const Node &nd) { + if (goal_reached) return goal_reached; + + float h = goal_heuristic(tracer, nd); + if (h < 0.f) { + goal_reached = true; + } 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); + + auto it = cached_nodes.find(qnd_id); + + 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); + } + } + + return goal_reached; + }); + + 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; +} + +}} // namespace Slic3r::astar + +#endif // ASTAR_HPP diff --git a/src/libslic3r/CMakeLists.txt b/src/libslic3r/CMakeLists.txt index 242378d6d..c9d8aa4fa 100644 --- a/src/libslic3r/CMakeLists.txt +++ b/src/libslic3r/CMakeLists.txt @@ -17,6 +17,7 @@ endif() set(SLIC3R_SOURCES pchheader.cpp pchheader.hpp + AStar.hpp BoundingBox.cpp BoundingBox.hpp BridgeDetector.cpp diff --git a/src/libslic3r/PointGrid.hpp b/src/libslic3r/PointGrid.hpp index acffd2b87..259a2950e 100644 --- a/src/libslic3r/PointGrid.hpp +++ b/src/libslic3r/PointGrid.hpp @@ -32,9 +32,9 @@ public: } Vec3i get_coord(size_t idx) const { - size_t iz = idx / XY; - size_t iy = (idx / m_size.x()) % m_size.y(); - size_t ix = idx % m_size.x(); + int iz = idx / XY; + int iy = (idx / m_size.x()) % m_size.y(); + int ix = idx % m_size.x(); return {ix, iy, iz}; } @@ -59,9 +59,9 @@ PointGrid point_grid(Ex policy, size_t XY = numpts[X] * numpts[Y]; execution::for_each(policy, size_t(0), out.size(), [&](size_t i) { - size_t iz = i / XY; - size_t iy = (i / numpts[X]) % numpts[Y]; - size_t ix = i % numpts[X]; + int iz = i / XY; + int iy = (i / numpts[X]) % numpts[Y]; + int ix = i % numpts[X]; out[i] = Vec<3, CoordT>(ix * stride.x(), iy * stride.y(), iz * stride.z()); }); diff --git a/tests/libslic3r/CMakeLists.txt b/tests/libslic3r/CMakeLists.txt index 57e473647..cf89b2246 100644 --- a/tests/libslic3r/CMakeLists.txt +++ b/tests/libslic3r/CMakeLists.txt @@ -26,6 +26,7 @@ add_executable(${_TEST_NAME}_tests test_png_io.cpp test_timeutils.cpp test_indexed_triangle_set.cpp + test_astar.cpp ../libnest2d/printer_parts.cpp ) diff --git a/tests/libslic3r/test_astar.cpp b/tests/libslic3r/test_astar.cpp new file mode 100644 index 000000000..176d7694a --- /dev/null +++ b/tests/libslic3r/test_astar.cpp @@ -0,0 +1,71 @@ +#include + +#include "libslic3r/BoundingBox.hpp" +#include "libslic3r/AStar.hpp" +#include "libslic3r/Execution/ExecutionSeq.hpp" +#include "libslic3r/PointGrid.hpp" + +using namespace Slic3r; + +struct PointGridTracer { + using Node = size_t; + const PointGrid &grid; + size_t final; + + PointGridTracer(const PointGrid &g, size_t goal) : + grid{g}, final{goal} {} + + template + void foreach_reachable(size_t from, Fn &&fn) const + { + Vec3i from_crd = grid.get_coord(from); + REQUIRE(grid.get_idx(from_crd) == from); + + if (size_t i = grid.get_idx(from_crd + Vec3i{ 1, 0, 0}); i < grid.point_count()) fn(i); + if (size_t i = grid.get_idx(from_crd + Vec3i{ 0, 1, 0}); i < grid.point_count()) fn(i); + if (size_t i = grid.get_idx(from_crd + Vec3i{ 0, 0, 1}); i < grid.point_count()) fn(i); + if (size_t i = grid.get_idx(from_crd + Vec3i{ 1, 1, 0}); i < grid.point_count()) fn(i); + if (size_t i = grid.get_idx(from_crd + Vec3i{ 0, 1, 1}); i < grid.point_count()) fn(i); + if (size_t i = grid.get_idx(from_crd + Vec3i{ 1, 1, 1}); i < grid.point_count()) fn(i); + if (size_t i = grid.get_idx(from_crd + Vec3i{-1, 0, 0}); from_crd.x() > 0 && i < grid.point_count()) fn(i); + if (size_t i = grid.get_idx(from_crd + Vec3i{ 0, -1, 0}); from_crd.y() > 0 && i < grid.point_count()) fn(i); + if (size_t i = grid.get_idx(from_crd + Vec3i{ 0, 0, -1}); from_crd.z() > 0 && i < grid.point_count()) fn(i); + if (size_t i = grid.get_idx(from_crd + Vec3i{-1, -1, 0}); from_crd.x() > 0 && from_crd.y() > 0 && i < grid.point_count()) fn(i); + if (size_t i = grid.get_idx(from_crd + Vec3i{ 0, -1, -1}); from_crd.y() > 0 && from_crd.z() && i < grid.point_count()) fn(i); + if (size_t i = grid.get_idx(from_crd + Vec3i{-1, -1, -1}); from_crd.x() > 0 && from_crd.y() > 0 && from_crd.z() && i < grid.point_count()) fn(i); + + } + + float distance(size_t a, size_t b) const + { + return (grid.get(a) - grid.get(b)).squaredNorm(); + } + + float goal_heuristic(size_t n) const + { + return n == final ? -1.f : (grid.get(n) - grid.get(final)).squaredNorm(); + } + + size_t unique_id(size_t n) const { return n; } +}; + +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}); + + size_t target = pgrid.point_count() - 1; + + std::cout << "Tracing route to " << pgrid.get_coord(target).transpose() << std::endl; + PointGridTracer pgt{pgrid, pgrid.point_count() - 1}; + std::vector out; + bool found = astar::search_route(pgt, size_t(0), std::back_inserter(out)); + + std::cout << "Route taken: "; + for (size_t i : out) { + std::cout << "(" << pgrid.get_coord(i).transpose() << ") "; + } + std::cout << std::endl; + + REQUIRE(found); +}