Revamped A* algorithm
with extended test suite
This commit is contained in:
tamasmeszaros
parent
df552a9226
commit
f9fb7f947d
@ -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 };
|
||||
using Node = TracerNodeT<Tracer>;
|
||||
using QNode = QNode<Tracer>;
|
||||
|
||||
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;
|
||||
|
||||
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 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});
|
||||
|
||||
auto qclosed =
|
||||
make_mutable_priority_queue<size_t, false>([](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
|
||||
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user