From 1ce6db5fa7a9048c881dc0864e3912b05f77dddf Mon Sep 17 00:00:00 2001 From: ndcroos Date: Thu, 31 Aug 2023 21:18:40 +0200 Subject: [PATCH] feat: A Star Search (#84) --- README.md | 1 + docs/docs/algorithms/shortest-path/a-star.md | 56 +++++ .../{bellman_ford.md => bellman-ford.md} | 0 include/graaflib/algorithm/shortest_path.h | 20 ++ include/graaflib/algorithm/shortest_path.tpp | 103 +++++++- .../graaflib/algorithm/shortest_path_test.cpp | 234 ++++++++++++++++++ 6 files changed, 409 insertions(+), 5 deletions(-) create mode 100644 docs/docs/algorithms/shortest-path/a-star.md rename docs/docs/algorithms/shortest-path/{bellman_ford.md => bellman-ford.md} (100%) diff --git a/README.md b/README.md index aae2e938..88b70236 100644 --- a/README.md +++ b/README.md @@ -78,6 +78,7 @@ Algorithms implemented in the Graaf library include the following. For more info - Breadth-First Search (BFS) - Depth-First Search (DFS) 2. [**Shortest Path Algorithms**](https://bobluppes.github.io/graaf/docs/category/shortest-path-algorithms): + - A\* search - BFS-Based Shortest Path - Dijkstra - Bellman-Ford diff --git a/docs/docs/algorithms/shortest-path/a-star.md b/docs/docs/algorithms/shortest-path/a-star.md new file mode 100644 index 00000000..f8d02436 --- /dev/null +++ b/docs/docs/algorithms/shortest-path/a-star.md @@ -0,0 +1,56 @@ +--- +sidebar_position: 3 +--- + +# A* Search Algorithm + +A\* computes the shortest path between a starting vertex and a target vertex in weighted and unweighted graphs. +It can be seen as an extension of Dijkstra's classical shortest paths algorithm. The implementation of A\* also tries to follow `dijkstra_shortest_path` closely where appropriate. Compared to Dijkstra's algorithm, A\* only finds the shortest path from a start vertex to a target vertex, and not the shortest path to all possible target vertices. Another difference is that A\* uses a heuristic function to achieve better performance. + +At each iteration of its main loop, A\* needs to determine which of its paths to extend. It does so by minimizing the so-called `f_score`. + +In A\*, the `f_score` represents the estimated total cost of the path from the start vertex to the goal vertex through the current vertex. It's a combination of two components: + +1. `g_score`: The actual cost of the path from the start vertex to the current vertex. +2. `h_score` (heuristic score): An estimate of the cost required from the current vertex to the goal vertex. + +A\* tries to minimize the `f_score` for each vertex as it explores the graph. The idea is to prioritize exploring vertices that have lower `f_score` values, as they are expected to lead to potentially better paths. + +Mathematically, `f_score` is often defined as: +``` +f_score = g_score + h_score +``` + +Where: +- `g_score` is the cost of the path from the start vertex to the current vertex. +- `h_score` is the heuristic estimate of the cost from the current vertex to the goal vertex. + +In the implementation, the heuristic function `heuristic` provides an estimate of `h_score` for each vertex, and the actual cost of the path from the start vertex to the current vertex is stored in the `g_score` unordered map, as the algorithm progresses. + +In the implementation, `dist_from_start` from path_vertex represents the `f_score` of the path. + +The time complexity of A\* depends on the provided heuristic function. In the worst case of an unbounded search space, the number of nodes expanded is exponential in the depth of the solution (the shortest path) `d`. This can be expressed as `O(b^d)`, where `b` is the branching factor (the average number of successors per state) per stage. + +In weighted graphs, edge weights should be non-negative. Like in the implementation of Dijkstra's algorithm, A\* is implemented with the priority queue provided by C++, to perform the repeated selection of minimum (estimated) cost nodes to expand. This is the `open_set`. If the shortest path is not unique, one of the shortest paths is returned. + +* [wikipedia](https://en.wikipedia.org/wiki/A*_search_algorithm) +* [Red Blob Games](https://www.redblobgames.com/pathfinding/a-star/introduction.html) + +## Syntax + +calculates the shortest path between on start_vertex and one end_vertex using A\* search. +Works on both weighted as well as unweighted graphs. For unweighted graphs, a unit weight is used for each edge. + +```cpp +template ()))> + requires std::is_invocable_r_v +std::optional> a_star_search( + const graph &graph, vertex_id_t start_vertex, vertex_id_t target_vertex, + const HEURISTIC_T &heuristic); +``` + +- **graph** The graph to extract shortest path from. +- **start_vertex** The vertex id where the shortest path should should start. +- **target_vertex** The vertex id where the shortest path should end. +- **heuristic** A heuristic function estimating the cost from a vertex to the target. +- **return** An optional containing the shortest path (a list of vertices) if found, or std::nullopt if no such path exists. \ No newline at end of file diff --git a/docs/docs/algorithms/shortest-path/bellman_ford.md b/docs/docs/algorithms/shortest-path/bellman-ford.md similarity index 100% rename from docs/docs/algorithms/shortest-path/bellman_ford.md rename to docs/docs/algorithms/shortest-path/bellman-ford.md diff --git a/include/graaflib/algorithm/shortest_path.h b/include/graaflib/algorithm/shortest_path.h index cd2584ca..4cf4cb0b 100644 --- a/include/graaflib/algorithm/shortest_path.h +++ b/include/graaflib/algorithm/shortest_path.h @@ -93,6 +93,26 @@ std::unordered_map> bellman_ford_shortest_paths(const graph& graph, vertex_id_t start_vertex); +/** + * @brief Finds the shortest path between a start_vertex and target_vertex + * using the A* search algorithm. + * + * @param graph The graph to search in. + * @param start_vertex The starting vertex for the search. + * @param target_vertex The target vertex to reach. + * @param heuristic A heuristic function estimating the cost from a vertex to + * the target. + * @return An optional containing the shortest path if found, or std::nullopt if + * no path exists. + */ +template ()))> + requires std::is_invocable_r_v +std::optional> a_star_search(const graph& graph, + vertex_id_t start_vertex, + vertex_id_t target_vertex, + const HEURISTIC_T& heuristic); + } // namespace graaf::algorithm #include "shortest_path.tpp" \ No newline at end of file diff --git a/include/graaflib/algorithm/shortest_path.tpp b/include/graaflib/algorithm/shortest_path.tpp index 892f6e3b..26f0ed16 100644 --- a/include/graaflib/algorithm/shortest_path.tpp +++ b/include/graaflib/algorithm/shortest_path.tpp @@ -3,9 +3,9 @@ #include #include -#include #include #include +#include #include #include @@ -102,8 +102,9 @@ std::optional> dijkstra_shortest_path( if (edge_weight < 0) { std::ostringstream error_msg; - error_msg << "Negative edge weight [" << edge_weight << "] between vertices [" - << current.id << "] -> [" << neighbor << "]."; + error_msg << "Negative edge weight [" << edge_weight + << "] between vertices [" << current.id << "] -> [" + << neighbor << "]."; throw std::invalid_argument{error_msg.str()}; } @@ -150,8 +151,9 @@ dijkstra_shortest_paths(const graph& graph, if (edge_weight < 0) { std::ostringstream error_msg; - error_msg << "Negative edge weight [" << edge_weight << "] between vertices [" - << current.id << "] -> [" << neighbor << "]."; + error_msg << "Negative edge weight [" << edge_weight + << "] between vertices [" << current.id << "] -> [" + << neighbor << "]."; throw std::invalid_argument{error_msg.str()}; } @@ -209,4 +211,95 @@ bellman_ford_shortest_paths(const graph& graph, return shortest_paths; } +template + requires std::is_invocable_r_v +std::optional> a_star_search( + const graph& graph, vertex_id_t start_vertex, + vertex_id_t target_vertex, const HEURISTIC_T& heuristic) { + // Define a priority queue for open set of vertices to explore. + // This part is similar to dijkstra_shortest_path + using weighted_path_item = detail::path_vertex; + // The set of discovered vertices that may need to be (re-)expanded. + // f_score represents the estimated total cost of the path from the start + // vertex to the goal vertex through the current vertex. + // It's a combination of g_score and h_score: + // f_score[n] = g_score[n] + h_score[n] + // For vertex n, prev_id in path_vertex is the vertex immediately preceding it + // on the cheapest path from the start to n currently known. + // The priority queue uses internally a binary heap. + // To get the minimum element, we use the std::greater comparator. + using a_star_queue_t = + std::priority_queue, + std::greater<>>; + a_star_queue_t open_set{}; + + // For vertex n, g_score[n] is the cost of the cheapest path from start to n + // currently known. It tracks the cost of reaching each vertex + std::unordered_map g_score; + // Initialize g_score map. + g_score[start_vertex] = 0; + + std::unordered_map vertex_info; + vertex_info[start_vertex] = { + start_vertex, + heuristic(start_vertex), // f_score[n] = g_score[n] + h(n), and + // g_score[n] is 0 if n is start_vertex. + start_vertex}; + + // Initialize start vertex in open set queue + open_set.push(vertex_info[start_vertex]); + + while (!open_set.empty()) { + // Get the vertex with the lowest f_score + auto current{open_set.top()}; + open_set.pop(); + + // Check if current vertex is the target + if (current.id == target_vertex) { + return reconstruct_path(start_vertex, target_vertex, vertex_info); + } + + // Iterate through neighboring vertices + for (const auto& neighbor : graph.get_neighbors(current.id)) { + WEIGHT_T edge_weight = get_weight(graph.get_edge(current.id, neighbor)); + + // A* search does not work on negative edge weights. + if (edge_weight < 0) { + throw std::invalid_argument{fmt::format( + "Negative edge weight [{}] between vertices [{}] -> [{}].", + edge_weight, current.id, neighbor)}; + } + + // tentative_g_score is the distance from start to the neighbor through + // current_vertex + WEIGHT_T tentative_g_score = g_score[current.id] + edge_weight; + + // Checks if vertex_info doesn't contain neighbor yet. + // But if it contains it, and the tentative_g_score is smaller, + // we need to update vertex_info and add it to the open set. + if (!vertex_info.contains(neighbor) || + tentative_g_score < g_score[neighbor]) { + // This path to neighbor is better than any previous one, so we need to + // update our data. + // Update neighbor's g_score, f_score and previous vertex on the path + g_score[neighbor] = tentative_g_score; + auto f_score = tentative_g_score + heuristic(neighbor); + + // always update vertex_info[neighbor] + vertex_info[neighbor] = { + neighbor, // vertex id + f_score, // f_score = tentantive_g_score + h(neighbor) + current.id // neighbor vertex came from current vertex + }; + + open_set.push(vertex_info[neighbor]); + } + } + } + + // No path found + return std::nullopt; +} + } // namespace graaf::algorithm \ No newline at end of file diff --git a/test/graaflib/algorithm/shortest_path_test.cpp b/test/graaflib/algorithm/shortest_path_test.cpp index cfc801cf..b3a5ec10 100644 --- a/test/graaflib/algorithm/shortest_path_test.cpp +++ b/test/graaflib/algorithm/shortest_path_test.cpp @@ -627,4 +627,238 @@ TYPED_TEST(BellmanFordShortestPathsTest, ASSERT_EQ(path_map, expected_path_map); } +template +struct AStarShortestPathTest : public testing::Test { + using graph_t = typename T::first_type; + using edge_t = typename T::second_type; +}; + +TYPED_TEST_SUITE(AStarShortestPathTest, weighted_graph_types); + +// Graph with only one vertex. +TYPED_TEST(AStarShortestPathTest, AStarMinimalShortestPath) { + // GIVEN + using graph_t = typename TestFixture::graph_t; + using edge_t = typename TestFixture::edge_t; + using weight_t = decltype(get_weight(std::declval())); + + graph_t graph{}; + + const auto vertex_id_1{graph.add_vertex(10)}; + + // WHEN + // The weight_t type annotation and static_cast are needed to avoid compiler + // warnings. + const auto heuristic = [](vertex_id_t vertex) -> weight_t { + return static_cast(0); + }; + const auto path = a_star_search(graph, vertex_id_1, vertex_id_1, heuristic); + + // THEN + const graph_path expected_path{{vertex_id_1}, 0}; + ASSERT_EQ(path, expected_path); +} + +// Find the shortest path between the only two vertices in a graph. +TYPED_TEST(AStarShortestPathTest, AStarSimpleShortestPath) { + // GIVEN + using graph_t = typename TestFixture::graph_t; + using edge_t = typename TestFixture::edge_t; + using weight_t = decltype(get_weight(std::declval())); + + graph_t graph{}; + + const auto vertex_id_1{graph.add_vertex(10)}; + const auto vertex_id_2{graph.add_vertex(20)}; + graph.add_edge(vertex_id_1, vertex_id_2, edge_t{static_cast(1)}); + + const auto heuristic = [](vertex_id_t vertex) -> weight_t { + return static_cast(0); + }; + + // WHEN + const auto path = a_star_search(graph, vertex_id_1, vertex_id_2, heuristic); + + // THEN + const graph_path expected_path{{vertex_id_1, vertex_id_2}, 1}; + ASSERT_EQ(path, expected_path); +} + +// Graph where there's no path between the start and target vertices. +TYPED_TEST(AStarShortestPathTest, NoPathExistence) { + // GIVEN + using graph_t = typename TestFixture::graph_t; + using edge_t = typename TestFixture::edge_t; + using weight_t = decltype(get_weight(std::declval())); + + graph_t graph{}; + + // Define start and target vertices + vertex_id_t start_vertex = 0; + vertex_id_t target_vertex = 5; + + // Define a heuristic function that always returns 0 + auto zero_heuristic = [](vertex_id_t vertex) -> weight_t { + return static_cast(0); + }; + + // WHEN + auto result = + a_star_search(graph, start_vertex, target_vertex, zero_heuristic); + + // THEN + // Check that the result is an empty optional + ASSERT_FALSE(result.has_value()); +} + +// Find the shortest path between multiple possible paths in a graph. +TYPED_TEST(AStarShortestPathTest, MultiplePathsTest) { + // GIVEN + using graph_t = typename TestFixture::graph_t; + using edge_t = typename TestFixture::edge_t; + using weight_t = decltype(get_weight(std::declval())); + + graph_t graph{}; + + const auto vertex_id_1{graph.add_vertex(10)}; + const auto vertex_id_2{graph.add_vertex(20)}; + const auto vertex_id_3{graph.add_vertex(30)}; + graph.add_edge(vertex_id_1, vertex_id_2, edge_t{static_cast(1)}); + graph.add_edge(vertex_id_1, vertex_id_3, edge_t{static_cast(2)}); + graph.add_edge(vertex_id_2, vertex_id_3, edge_t{static_cast(2)}); + + // WHEN + const auto heuristic = [](vertex_id_t vertex) -> weight_t { + return static_cast(0); + }; + const auto path = a_star_search(graph, vertex_id_1, vertex_id_3, heuristic); + + // THEN + const graph_path expected_path{{vertex_id_1, vertex_id_3}, 2}; + ASSERT_EQ(path, expected_path); +} + +// Suboptimal Path Test +TYPED_TEST(AStarShortestPathTest, AStarSuboptimalPath) { + // GIVEN + using graph_t = typename TestFixture::graph_t; + using edge_t = typename TestFixture::edge_t; + using weight_t = decltype(get_weight(std::declval())); + + graph_t graph{}; + + const auto vertex_id_1{graph.add_vertex(10)}; + const auto vertex_id_2{graph.add_vertex(20)}; + const auto vertex_id_3{graph.add_vertex(30)}; + graph.add_edge(vertex_id_1, vertex_id_2, edge_t{static_cast(2)}); + graph.add_edge(vertex_id_1, vertex_id_3, edge_t{static_cast(4)}); + graph.add_edge(vertex_id_2, vertex_id_3, edge_t{static_cast(3)}); + + // WHEN + const auto heuristic = [](vertex_id_t vertex) -> weight_t { + return static_cast(10); // Overestimate the remaining cost + }; + const auto path = a_star_search(graph, vertex_id_1, vertex_id_3, heuristic); + + // THEN + ASSERT_TRUE(path.has_value()); // Check if optional has a value + // Note: The path might not be the shortest, but it should still be valid +} + +template +struct AStarShortestPathSignedTypesTest : public testing::Test { + using graph_t = typename T::first_type; + using edge_t = typename T::second_type; +}; + +TYPED_TEST_SUITE(AStarShortestPathSignedTypesTest, weighted_graph_signed_types); + +// Negative Weight Test +TYPED_TEST(AStarShortestPathSignedTypesTest, AStarNegativeWeight) { + // GIVEN + using graph_t = typename TestFixture::graph_t; + using edge_t = typename TestFixture::edge_t; + using weight_t = decltype(get_weight(std::declval())); + + graph_t graph{}; + + const auto vertex_id_1{graph.add_vertex(10)}; + const auto vertex_id_2{graph.add_vertex(20)}; + graph.add_edge(vertex_id_1, vertex_id_2, + edge_t{static_cast(-1)}); // Negative weight edge + + const auto heuristic = [](vertex_id_t vertex) -> weight_t { + return static_cast(0); + }; + + // THEN + // Taken from DijkstraNegativeWeight + ASSERT_THROW( + { + try { + // Call the get_edge function for non-existing vertices + [[maybe_unused]] const auto path{ + a_star_search(graph, vertex_id_1, vertex_id_2, heuristic)}; + // If the above line doesn't throw an exception, fail the test + FAIL() + << "Expected std::invalid_argument exception, but no exception " + "was thrown."; + } catch (const std::invalid_argument &ex) { + // Verify that the exception message contains the expected error + // message + EXPECT_EQ( + ex.what(), + fmt::format( + "Negative edge weight [{}] between vertices [{}] -> [{}].", + -1, vertex_id_1, vertex_id_2)); + throw; + } + }, + std::invalid_argument); +} + +// Heuristic Impact Test +TYPED_TEST(AStarShortestPathTest, AStarHeuristicImpact) { + // GIVEN + using graph_t = typename TestFixture::graph_t; + using edge_t = typename TestFixture::edge_t; + using weight_t = decltype(get_weight(std::declval())); + + graph_t graph{}; + + const auto vertex_id_1{graph.add_vertex(10)}; + const auto vertex_id_2{graph.add_vertex(20)}; + const auto vertex_id_3{graph.add_vertex(30)}; + const auto vertex_id_4{graph.add_vertex(40)}; + graph.add_edge(vertex_id_1, vertex_id_2, edge_t{static_cast(1)}); + graph.add_edge(vertex_id_2, vertex_id_3, edge_t{static_cast(2)}); + graph.add_edge(vertex_id_1, vertex_id_3, edge_t{static_cast(3)}); + graph.add_edge(vertex_id_1, vertex_id_4, edge_t{static_cast(2)}); + graph.add_edge(vertex_id_3, vertex_id_4, edge_t{static_cast(1)}); + + const auto start_vertex = vertex_id_1; + const auto target_vertex = vertex_id_3; + + // Define two different heuristic functions + const auto heuristic1 = [](vertex_id_t vertex) -> weight_t { + return static_cast(0); + }; // Underestimating heuristic + const auto heuristic2 = [](vertex_id_t vertex) -> weight_t { + return static_cast(10); + }; // Overestimating heuristic + + // WHEN + const auto path_with_underestimating_heuristic = + a_star_search(graph, start_vertex, target_vertex, heuristic1); + const auto path_with_overestimating_heuristic = + a_star_search(graph, start_vertex, target_vertex, heuristic2); + + // THEN + // Verify that the path with the underestimating heuristic is shorter + ASSERT_TRUE(path_with_underestimating_heuristic.has_value()); + ASSERT_TRUE(path_with_overestimating_heuristic.has_value()); + ASSERT_LT(path_with_underestimating_heuristic->total_weight, + path_with_overestimating_heuristic->total_weight); +} + } // namespace graaf::algorithm \ No newline at end of file