WIP: astar

algorithm/include/aw/algorithm/astar.h

@@ -0,0 +1,87 @@
+#ifndef aw_algorithm_astar_h
+#define aw_algorithm_astar_h
+
+#include <optional>
+#include <queue>
+#include <cassert>
+
+namespace aw {
+namespace detail {
+template<typename Cell>
+using distance_type = decltype( std::declval<Cell>().distance( std::declval<Cell>() ) );
+
+template<typename Grid, typename Cell>
+using cost_type = decltype(
+	std::declval<Grid>().cost(
+		std::declval<Cell>(),
+		std::declval<Cell>() )
+);
+} // namespace detail
+
+template<
+	typename Grid,
+	typename Cell = typename Grid::location_type,
+	typename Distance = detail::cost_type<Grid, Cell>,
+	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<Cell>
+{
+	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<node> 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

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 <algorithm>
+#include <unordered_map>
+
+#if AW_ASTAR_DEBUG_CYCLES
+#include <unordered_set>
+#endif
+
+namespace aw {
+template<typename Cell, typename Came_from_map>
+auto reconstruct_path( Cell start, Cell goal, const Came_from_map& came_from )
+	-> std::vector<Cell>
+{
+#if AW_ASTAR_DEBUG_CYCLES
+	std::unordered_set<Cell> visited;
+#endif
+
+	std::vector<Cell> 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<Grid, Cell>,
+	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<Cell>
+{
+	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

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)

algorithm/tests/astar.c++

@@ -0,0 +1,191 @@
+#include <aw/algorithm/shortest_path.h>
+
+#include <aw/math/vector2d.h>
+#include <aw/math/vector_compare.h>
+#include <aw/test/test.h>
+#include <aw/utility/to_string/math/vector.h>
+
+TestFile( "algorithm::astar" );
+
+namespace aw {
+using vec2 = math::vector2d<int>;
+
+struct test_grid {
+	using location_type = vec2;
+
+	std::vector<std::vector<double>> 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<location_type> neighbors(location_type p) const
+	{
+		std::vector<location_type> neighbors;
+		std::vector<location_type> 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> int_hash;
+	size_t operator()(vec2 v) const
+	{
+		return int_hash(v.x()) + int_hash(v.y());
+	}
+};
+
+template<typename Container>
+struct map_wrapper {
+	Container c;
+
+	using key_type = Container::key_type;
+	using value_type = Container::value_type;
+
+	template<typename K>
+	value_type* operator[](K&& key)
+	{
+		auto it = c.find(std::forward<K>(key));
+		if (it == c.end())
+			return {};
+		return it->second;
+	}
+
+	template<typename K>
+	const value_type* operator[](K&& key) const
+	{
+		auto it = c.find(std::forward<K>(key));
+		if (it == c.end())
+			return {};
+		return it->second;
+	}
+
+};
+
+struct test_cost_map : std::unordered_map<vec2, std::optional<double>, vec2_hash> {
+	std::optional<double>& operator[](vec2 p)
+	{
+		return std::unordered_map<vec2, std::optional<double>, vec2_hash>::operator[](p);
+	}
+
+	std::optional<double> operator[](vec2 p) const
+	{
+		auto it = find(p);
+		if (it == end())
+			return {};
+		return it->second;
+	}
+};
+
+struct test_came_from_map : std::unordered_map<vec2, std::optional<vec2>, vec2_hash> {
+
+	std::optional<vec2>& operator[](vec2 p)
+	{
+		return std::unordered_map<vec2, std::optional<vec2>, vec2_hash>::operator[](p);
+	}
+
+	std::optional<vec2> 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<vec2>{ {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<vec2>{
+			{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

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());
 	}