Revamped A* algorithm
with extended test suite
This commit is contained in:
tamasmeszaros
parent
df552a9226
commit
f9fb7f947d
@ -1,11 +1,11 @@
|
|||||||
#ifndef ASTAR_HPP
|
#ifndef ASTAR_HPP
|
||||||
#define ASTAR_HPP
|
#define ASTAR_HPP
|
||||||
|
|
||||||
|
#include <unordered_map>
|
||||||
|
|
||||||
#include "libslic3r/Point.hpp"
|
#include "libslic3r/Point.hpp"
|
||||||
#include "libslic3r/MutablePriorityQueue.hpp"
|
#include "libslic3r/MutablePriorityQueue.hpp"
|
||||||
|
|
||||||
#include <unordered_map>
|
|
||||||
|
|
||||||
namespace Slic3r { namespace astar {
|
namespace Slic3r { namespace astar {
|
||||||
|
|
||||||
// Input interface for the Astar algorithm. Specialize this struct for a
|
// 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.
|
// Get the estimated distance heuristic from node 'n' to the destination.
|
||||||
// This is referred to as the h value in AStar context.
|
// This is referred to as the h value in AStar context.
|
||||||
// If node 'n' is the goal, this function should return a negative value.
|
// 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)
|
static float goal_heuristic(const T &tracer, const Node &n)
|
||||||
{
|
{
|
||||||
return tracer.goal_heuristic(n);
|
return tracer.goal_heuristic(n);
|
||||||
@ -81,100 +83,133 @@ size_t unique_id(const T &tracer, const TracerNodeT<T> &n)
|
|||||||
|
|
||||||
} // namespace astar_detail
|
} // 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.
|
// Run the AStar algorithm on a tracer implementation.
|
||||||
// The 'tracer' argument encapsulates the domain (grid, point cloud, etc...)
|
// The 'tracer' argument encapsulates the domain (grid, point cloud, etc...)
|
||||||
// The 'source' argument is the starting node.
|
// The 'source' argument is the starting node.
|
||||||
// The 'out' argument is the output iterator into which the output nodes are
|
// The 'out' argument is the output iterator into which the output nodes are
|
||||||
// written.
|
// written. For performance reasons, the order is reverse, from the destination
|
||||||
// Note that no destination node is given. The tracer's goal_heuristic() method
|
// to the source -- (destination included, source is not).
|
||||||
// should return a negative value if a node is a destination node.
|
// The 'cached_nodes' argument is an optional associative container to hold a
|
||||||
template<class Tracer, class It>
|
// QNode entry for each visited node. Any compatible container can be used
|
||||||
bool search_route(const Tracer &tracer, const TracerNodeT<Tracer> &source, It out)
|
// (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 namespace detail;
|
||||||
|
|
||||||
using Node = TracerNodeT<Tracer>;
|
using Node = TracerNodeT<Tracer>;
|
||||||
enum class QueueType { Open, Closed, None };
|
using QNode = QNode<Tracer>;
|
||||||
|
|
||||||
struct QNode // Queue node. Keeps track of scores g, and h
|
struct LessPred { // Comparison functor needed by the priority queue
|
||||||
{
|
NodeMap &m;
|
||||||
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;
|
|
||||||
bool operator ()(size_t node_a, size_t node_b) {
|
bool operator ()(size_t node_a, size_t node_b) {
|
||||||
auto ait = m.find(node_a);
|
return m[node_a].f() < m[node_b].f();
|
||||||
auto bit = m.find(node_b);
|
|
||||||
assert (ait != m.end() && bit != m.end());
|
|
||||||
|
|
||||||
return ait->second.f() < bit->second.f();
|
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
auto qopen =
|
auto qopen = make_mutable_priority_queue<size_t, true>(
|
||||||
make_mutable_priority_queue<size_t, false>([](size_t, size_t){},
|
[&cached_nodes](size_t el, size_t qidx) {
|
||||||
|
cached_nodes[el].queue_id = qidx;
|
||||||
|
},
|
||||||
LessPred{cached_nodes});
|
LessPred{cached_nodes});
|
||||||
|
|
||||||
auto qclosed =
|
QNode initial{source, /*parent = */ UNASSIGNED, /*g = */0.f};
|
||||||
make_mutable_priority_queue<size_t, false>([](size_t, size_t){},
|
size_t source_id = unique_id(tracer, source);
|
||||||
LessPred{cached_nodes});
|
cached_nodes[source_id] = initial;
|
||||||
|
qopen.push(source_id);
|
||||||
|
|
||||||
QNode initial{source, QueueType::Open};
|
size_t goal_id = goal_heuristic(tracer, source) < 0.f ? source_id :
|
||||||
cached_nodes.insert({unique_id(tracer, source), initial});
|
UNASSIGNED;
|
||||||
qopen.push(unique_id(tracer, source));
|
|
||||||
|
|
||||||
bool goal_reached = false;
|
while (goal_id == UNASSIGNED && !qopen.empty()) {
|
||||||
|
|
||||||
while (!goal_reached && !qopen.empty()) {
|
|
||||||
size_t q_id = qopen.top();
|
size_t q_id = qopen.top();
|
||||||
qopen.pop();
|
qopen.pop();
|
||||||
QNode q = cached_nodes.at(q_id);
|
QNode &q = cached_nodes[q_id];
|
||||||
|
|
||||||
foreach_reachable(tracer, q.node, [&](const Node &nd) {
|
// This should absolutely be initialized in the cache already
|
||||||
if (goal_reached) return goal_reached;
|
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) {
|
if (h < 0.f) {
|
||||||
goal_reached = true;
|
goal_id = succ_id;
|
||||||
|
cached_nodes[succ_id] = qsucc_nd;
|
||||||
} else {
|
} else {
|
||||||
float dst = trace_distance(tracer, q.node, nd);
|
// If succ_id is not in cache, it gets created with g = infinity
|
||||||
QNode qnd{nd, QueueType::None, q.g + dst, h};
|
QNode &prev_nd = cached_nodes[succ_id];
|
||||||
size_t qnd_id = unique_id(tracer, nd);
|
|
||||||
|
|
||||||
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() ||
|
// Save the old queue id, it would be lost after the next line
|
||||||
(it->second.qtype != QueueType::None && qnd.f() < it->second.f())) {
|
size_t queue_id = prev_nd.queue_id;
|
||||||
qnd.qtype = QueueType::Open;
|
|
||||||
cached_nodes.insert_or_assign(qnd_id, qnd);
|
// The cache needs to be updated either way
|
||||||
qopen.push(qnd_id);
|
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
|
}} // namespace Slic3r::astar
|
||||||
|
|||||||
@ -7,12 +7,42 @@
|
|||||||
|
|
||||||
using namespace Slic3r;
|
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;
|
using Node = size_t;
|
||||||
const PointGrid<float> &grid;
|
const PointGrid<float> &grid;
|
||||||
size_t final;
|
size_t final;
|
||||||
|
|
||||||
PointGridTracer(const PointGrid<float> &g, size_t goal) :
|
PointGridTracer3D(const PointGrid<float> &g, size_t goal) :
|
||||||
grid{g}, final{goal} {}
|
grid{g}, final{goal} {}
|
||||||
|
|
||||||
template<class Fn>
|
template<class Fn>
|
||||||
@ -49,14 +79,328 @@ struct PointGridTracer {
|
|||||||
size_t unique_id(size_t n) const { return n; }
|
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]") {
|
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 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});
|
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;
|
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(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