From eef38d2d8065ba7805b2ed34c6054fcb48f70da2 Mon Sep 17 00:00:00 2001 From: Hudd Date: Sun, 7 Apr 2024 14:13:48 +0400 Subject: [PATCH] WIP: astar --- algorithm/include/aw/algorithm/astar.h | 87 ++++++++ .../include/aw/algorithm/shortest_path.h | 65 ++++++ algorithm/tests/CMakeLists.txt | 2 + algorithm/tests/astar.c++ | 191 ++++++++++++++++++ .../tests/{wav_reader.cpp => wav_reader.c++} | 0 .../sound/{wav_reader.cpp => wav_reader.c++} | 0 math/include/aw/math/vector.h | 2 +- types/include/aw/types/containers/queue.h | 26 +-- 8 files changed, 360 insertions(+), 13 deletions(-) create mode 100644 algorithm/include/aw/algorithm/astar.h create mode 100644 algorithm/include/aw/algorithm/shortest_path.h create mode 100644 algorithm/tests/astar.c++ rename fileformat/sound/tests/{wav_reader.cpp => wav_reader.c++} (100%) rename fileformat/sound/{wav_reader.cpp => wav_reader.c++} (100%) diff --git a/algorithm/include/aw/algorithm/astar.h b/algorithm/include/aw/algorithm/astar.h new file mode 100644 index 00000000..b3d06da8 --- /dev/null +++ b/algorithm/include/aw/algorithm/astar.h @@ -0,0 +1,87 @@ +#ifndef aw_algorithm_astar_h +#define aw_algorithm_astar_h + +#include +#include +#include + +namespace aw { +namespace detail { +template +using distance_type = decltype( std::declval().distance( std::declval() ) ); + +template +using cost_type = decltype( + std::declval().cost( + std::declval(), + std::declval() ) +); +} // namespace detail + +template< + typename Grid, + typename Cell = typename Grid::location_type, + typename Distance = detail::cost_type, + typename Strategy, + typename Cost_map, + typename Came_from_map> +auto astar( const Grid& grid, Cell start, Strategy&& strategy, + Cost_map& cost, Came_from_map& came_from ) + -> std::optional +{ + struct node { + Distance priority; + Cell pos; + + bool operator<(const node& b) const + { + const node& a = *this; + // Use > so that the smallest is on top + return a.priority > b.priority; + } + }; + + std::priority_queue queue; + + cost[start] = 0; + came_from[start] = start; + + queue.push({ 0, start }); + + while (!queue.empty()) { + const auto [prev_priority, pos] = queue.top(); + const auto dist = *cost[pos]; + + queue.pop(); + + if (strategy.goal(pos, dist)) + return pos; + + if (strategy.limit(pos, dist)) + continue; + + const auto neighbors = grid.neighbors(pos); + for (auto next : neighbors) { + const auto next_cost = grid.cost( pos, next ); + if (next_cost < 0) + continue; + const auto total_cost = dist + next_cost; + if (cost[next] && *cost[next] <= total_cost) + continue; + + cost[next] = total_cost; + came_from[next] = pos; + + const auto priority = total_cost + strategy.heuristic( next ); +#if AW_ASTAR_DEBUG_HEURISTIC + assert( priority >= prev_priority ); +#endif + queue.push({ priority, next }); + } + } + + return {}; +} +} // namespace aw + +#endif // aw_algorithm_astar_h diff --git a/algorithm/include/aw/algorithm/shortest_path.h b/algorithm/include/aw/algorithm/shortest_path.h new file mode 100644 index 00000000..716a3c71 --- /dev/null +++ b/algorithm/include/aw/algorithm/shortest_path.h @@ -0,0 +1,65 @@ +#ifndef aw_algo_shortest_path_h +#define aw_algo_shortest_path_h + +#include "astar.h" + +#include +#include + +#if AW_ASTAR_DEBUG_CYCLES +#include +#endif + +namespace aw { +template +auto reconstruct_path( Cell start, Cell goal, const Came_from_map& came_from ) + -> std::vector +{ +#if AW_ASTAR_DEBUG_CYCLES + std::unordered_set visited; +#endif + + std::vector path; + + if (!came_from[goal]) + return path; + + Cell current = goal; + while (current != start) { +#if AW_ASTAR_DEBUG_CYCLES + auto [_,cycle] = visited.emplace(current); + if (cycle) + return break; +#endif + + path.push_back(current); + current = *came_from[current]; + } + + std::reverse(path.begin(), path.end()); + return path; +} + +template< + typename Grid, + typename Cell = typename Grid::location_type, + typename Distance = detail::cost_type, + typename Cost_map, + typename Came_from_map, + typename Strategy> +auto find_path( const Grid& grid, Cell start, + Strategy&& strategy, + Cost_map& cost, + Came_from_map& came_from) + -> std::vector +{ + const auto goal = astar( grid, start, strategy, cost, came_from ); + if (!goal) + return {}; + + return reconstruct_path( start, *goal, came_from ); +} + +} // namespace aw + +#endif // aw_algo_shortest_path_h diff --git a/algorithm/tests/CMakeLists.txt b/algorithm/tests/CMakeLists.txt index 0279d442..3fa9950b 100644 --- a/algorithm/tests/CMakeLists.txt +++ b/algorithm/tests/CMakeLists.txt @@ -1,5 +1,6 @@ aw_add_test(test_algo SOURCES + astar.c++ common_prefix.c++ exchange.c++ join.c++ @@ -9,5 +10,6 @@ aw_add_test(test_algo target_link_libraries(test_algo PRIVATE awalgo + awmath awtest awtest_main) diff --git a/algorithm/tests/astar.c++ b/algorithm/tests/astar.c++ new file mode 100644 index 00000000..818d8fc0 --- /dev/null +++ b/algorithm/tests/astar.c++ @@ -0,0 +1,191 @@ +#include + +#include +#include +#include +#include + +TestFile( "algorithm::astar" ); + +namespace aw { +using vec2 = math::vector2d; + +struct test_grid { + using location_type = vec2; + + std::vector> grid; + + double cost(location_type a, location_type b) const + { + auto cost = grid[b.y()][b.x()]; + if (b.y() != a.y() && b.x() != a.x()) + cost *= sqrt(2); + return cost; + } + + bool in_bounds(location_type p) const + { + if (p.x() < 0 || p.y() < 0) + return false; + size_t max_y = grid.size(); + if (p.y() > max_y) + return false; + size_t max_x = grid[0].size(); + if (p.x() > max_x) + return false; + return true; + } + + std::vector neighbors(location_type p) const + { + std::vector neighbors; + std::vector candidates{ + { p.x() - 1, p.y() - 1 }, + { p.x() - 1, p.y() }, + { p.x() - 1, p.y() + 1 }, + { p.x() , p.y() - 1 }, + { p.x() , p.y() + 1 }, + { p.x() + 1, p.y() - 1 }, + { p.x() + 1, p.y() }, + { p.x() + 1, p.y() + 1 }, + }; + for (auto p : candidates) + if (in_bounds(p)) + neighbors.push_back(p); + return neighbors; + } + +}; + +struct test_strategy { + vec2 start; + vec2 end_goal; + + bool goal(vec2 pos, int dist) const + { + return pos == end_goal; + } + + bool limit(vec2 pos, int dist) const + { + return false; + } + + double heuristic(vec2 pos) const + { + return distance(pos, end_goal); + }; +}; + +struct vec2_hash { + std::hash int_hash; + size_t operator()(vec2 v) const + { + return int_hash(v.x()) + int_hash(v.y()); + } +}; + +template +struct map_wrapper { + Container c; + + using key_type = Container::key_type; + using value_type = Container::value_type; + + template + value_type* operator[](K&& key) + { + auto it = c.find(std::forward(key)); + if (it == c.end()) + return {}; + return it->second; + } + + template + const value_type* operator[](K&& key) const + { + auto it = c.find(std::forward(key)); + if (it == c.end()) + return {}; + return it->second; + } + +}; + +struct test_cost_map : std::unordered_map, vec2_hash> { + std::optional& operator[](vec2 p) + { + return std::unordered_map, vec2_hash>::operator[](p); + } + + std::optional operator[](vec2 p) const + { + auto it = find(p); + if (it == end()) + return {}; + return it->second; + } +}; + +struct test_came_from_map : std::unordered_map, vec2_hash> { + + std::optional& operator[](vec2 p) + { + return std::unordered_map, vec2_hash>::operator[](p); + } + + std::optional operator[](vec2 p) const + { + auto it = find(p); + if (it == end()) + return {}; + return it->second; + } +}; + +Test(astar_shortest_path) { + constexpr int X = -1; + test_grid grid{{ + { 1, 1, 9, 1, 1, 1, 1, 1, 1 }, + { 1, 1, 9, 1, 1, 1, 1, 1, 1 }, + { 1, 1, 9, 1, 1, 1, 1, 1, 1 }, + { 1, 1, 9, 1, 1, 1, 1, 1, 1 }, + { 1, 1, 9, 1, 1, 1, 1, 1, 1 }, + { 1, 1, 9, 1, 1, X, X, X, 1 }, + { 1, 1, 1, 1, 1, X, 1, X, 1 }, + { 1, 1, 1, 1, 1, X, X, X, 1 }, + { 1, 1, 1, 1, 1, 1, 1, 1, 1 }, + }}; + + test_cost_map cost; + test_came_from_map came_from; + { + vec2 start{0,0}; + vec2 goal{4,0}; + + auto path = find_path( grid, start, test_strategy{ start, goal }, cost, came_from ); + TestAssert(!path.empty()); + TestEqual(path, std::vector{ {1,0}, {2,0}, {3,0}, {4,0} }); + } + + { + vec2 start{0,2}; + vec2 goal{4,0}; + + auto path = find_path( grid, start, test_strategy{ start, goal }, cost, came_from ); + TestEqual(path, std::vector{ + {0, 3}, {0, 4}, {1, 5}, {2, 6}, {3, 5}, + {3, 4}, {3, 3}, {3, 2}, {3, 1}, {4, 0} }); + } + + { + vec2 start{0,0}; + vec2 goal{6,6}; + + auto path = find_path( grid, start, test_strategy{ start, goal }, cost, came_from ); + TestAssert(path.empty()); + } +} + + +} // namespace aw diff --git a/fileformat/sound/tests/wav_reader.cpp b/fileformat/sound/tests/wav_reader.c++ similarity index 100% rename from fileformat/sound/tests/wav_reader.cpp rename to fileformat/sound/tests/wav_reader.c++ diff --git a/fileformat/sound/wav_reader.cpp b/fileformat/sound/wav_reader.c++ similarity index 100% rename from fileformat/sound/wav_reader.cpp rename to fileformat/sound/wav_reader.c++ diff --git a/math/include/aw/math/vector.h b/math/include/aw/math/vector.h index bce21937..2a1d9d0c 100644 --- a/math/include/aw/math/vector.h +++ b/math/include/aw/math/vector.h @@ -191,7 +191,7 @@ struct vector { return dot(*this); } - T length() const + auto length() const { return math::sqrt(length_sq()); } diff --git a/types/include/aw/types/containers/queue.h b/types/include/aw/types/containers/queue.h index af8f13bb..109040da 100644 --- a/types/include/aw/types/containers/queue.h +++ b/types/include/aw/types/containers/queue.h @@ -465,57 +465,59 @@ class queue : _impl::queue_base { /*! * Insert element at the front of queue */ - void push_front(const_reference val) + reference push_front(const_reference val) { - emplace_front(val); + return emplace_front(val); } /*! * Insert element at the end of queue */ - void push_back(const_reference val) + reference push_back(const_reference val) { - emplace_back(val); + return emplace_back(val); } /*! * Insert element at the front of queue */ - void push_front(value_type&& val) + reference push_front(value_type&& val) { - emplace_front(std::move(val)); + return emplace_front(std::move(val)); } /*! * Insert element at the end of queue */ - void push_back(value_type&& val) + reference push_back(value_type&& val) { - emplace_back(std::move(val)); + return emplace_back(std::move(val)); } /*! * Construct element at the front of queue */ template - void emplace_front(Args&&... args) + reference emplace_front(Args&&... args) { check_capacity(); impl.head = impl.prev_p(impl.head); construct(impl.head, std::forward(args)...); + return *impl.head; } /*! * Construct element at the end of queue */ template - void emplace_back(Args&&... args) + reference emplace_back(Args&&... args) { check_capacity(); - construct(impl.tail, std::forward(args)...); - impl.tail = impl.next_p(impl.tail); + auto elem_ptr = std::exchange(impl.tail, impl.next_p(impl.tail)); + construct(elem_ptr, std::forward(args)...); + return *elem_ptr; } /*! Get element at the head of queue */