feat: A Star Search (#84)

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

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 <typename V, typename E, graph_type T, typename HEURISTIC_T, typename WEIGHT_T = decltype(get_weight(std::declval<E>()))>
+  requires std::is_invocable_r_v<WEIGHT_T, HEURISTIC_T&, vertex_id_t>
+std::optional<graph_path<WEIGHT_T>> a_star_search(
+    const graph<V, E, T> &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.

include/graaflib/algorithm/shortest_path.h

@@ -93,6 +93,26 @@ std::unordered_map<vertex_id_t, graph_path<WEIGHT_T>>
 bellman_ford_shortest_paths(const graph<V, E, T>& 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 <typename V, typename E, graph_type T, typename HEURISTIC_T,
+          typename WEIGHT_T = decltype(get_weight(std::declval<E>()))>
+  requires std::is_invocable_r_v<WEIGHT_T, HEURISTIC_T&, vertex_id_t>
+std::optional<graph_path<WEIGHT_T>> a_star_search(const graph<V, E, T>& graph,
+                                                  vertex_id_t start_vertex,
+                                                  vertex_id_t target_vertex,
+                                                  const HEURISTIC_T& heuristic);
+
 }  // namespace graaf::algorithm
 
 #include "shortest_path.tpp"

include/graaflib/algorithm/shortest_path.tpp

@@ -3,9 +3,9 @@
 #include <graaflib/algorithm/graph_traversal.h>
 
 #include <algorithm>
-#include <sstream>
 #include <optional>
 #include <queue>
+#include <sstream>
 #include <unordered_map>
 #include <unordered_set>
 
@@ -102,8 +102,9 @@ std::optional<graph_path<WEIGHT_T>> 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<V, E, T>& 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<V, E, T>& graph,
   return shortest_paths;
 }
 
+template <typename V, typename E, graph_type T, typename HEURISTIC_T,
+          typename WEIGHT_T>
+  requires std::is_invocable_r_v<WEIGHT_T, HEURISTIC_T&, vertex_id_t>
+std::optional<graph_path<WEIGHT_T>> a_star_search(
+    const graph<V, E, T>& 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<WEIGHT_T>;
+  // 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<weighted_path_item, std::vector<weighted_path_item>,
+                          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<vertex_id_t, WEIGHT_T> g_score;
+  // Initialize g_score map.
+  g_score[start_vertex] = 0;
+
+  std::unordered_map<vertex_id_t, weighted_path_item> 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