perf: use vector-backed bookkeeping for A* search

docs/src/algorithms/shortest-path/a-star.md

@@ -30,8 +30,10 @@ Where:
 - `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.
+actual cost of the path from the start vertex to the current vertex is stored in `vertex_info`, alongside its
+predecessor. This vector is indexed by vertex id and grows as vertices are discovered; empty slots represent
+undiscovered vertices, including gaps from removed vertices. Storage is proportional to the largest discovered id
+plus one, rather than just the number of discovered vertices. This matches the graph's own vector storage.
 
 In the implementation, `dist_from_start` from `path_vertex` represents the `g_score` (the true accumulated cost from
 the start vertex), which is what is reported as `total_weight` on the resulting path. The `f_score` used to order the
@@ -66,4 +68,4 @@ std::optional<graph_path<WEIGHT_T>> a_star_search(
 - **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.
+  exists.

include/graaflib/algorithm/shortest_path/a_star.tpp

@@ -1,5 +1,6 @@
 #pragma once
 #include <queue>
+#include <vector>
 
 #include "a_star.h"
 
@@ -9,7 +10,7 @@ namespace detail {
 
 // Open set entries are ordered by f_score (g_score + heuristic), which is
 // only used to pick the next vertex to expand. This is intentionally kept
-// separate from path_vertex::dist_from_start, which reconstruct_path reports
+// separate from path_vertex::dist_from_start, which path reconstruction reports
 // as the path's total_weight and therefore must hold the true accumulated
 // cost (g_score), not the heuristic-inflated f_score.
 template <typename WEIGHT_T>
@@ -40,22 +41,36 @@ std::optional<graph_path<WEIGHT_T>> a_star_search(
                           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;
+  // Preserve the existing heuristic call and trivial-path behavior even for
+  // an absent start vertex, without allocating storage indexed by an invalid
+  // (potentially very large) id.
+  const auto initial_f_score{heuristic(start_vertex)};
+  if (start_vertex == target_vertex) {
+    return graph_path<WEIGHT_T>{{start_vertex}, 0};
+  }
+  if (!graph.has_vertex(start_vertex)) {
+    return std::nullopt;
+  }
 
-  // vertex_info tracks, for each discovered vertex, the true accumulated
-  // cost from start (dist_from_start = g_score) and its predecessor on the
-  // cheapest known path. This is what reconstruct_path uses to build the
-  // resulting graph_path, including its total_weight.
+  // An empty slot denotes an undiscovered vertex. Grow lazily by vertex id,
+  // including holes left by removed vertices. dist_from_start stores g_score,
+  // so a second container for the same accumulated cost is unnecessary.
   using weighted_path_item = detail::path_vertex<WEIGHT_T>;
-  std::unordered_map<vertex_id_t, weighted_path_item> vertex_info;
-  vertex_info[start_vertex] = {start_vertex, 0, start_vertex};
+  std::vector<std::optional<weighted_path_item>> vertex_info{};
+  const auto has_info{[&](vertex_id_t id) {
+    return id < vertex_info.size() && vertex_info[id].has_value();
+  }};
+  const auto set_info{
+      [&](vertex_id_t id, WEIGHT_T distance, vertex_id_t predecessor) {
+        if (id >= vertex_info.size()) {
+          vertex_info.resize(id + 1);
+        }
+        vertex_info[id] = weighted_path_item{id, distance, predecessor};
+      }};
+  set_info(start_vertex, 0, start_vertex);
 
   // Initialize start vertex in open set queue
-  open_set.push(open_set_item{start_vertex, heuristic(start_vertex)});
+  open_set.push(open_set_item{start_vertex, initial_f_score});
 
   while (!open_set.empty()) {
     // Get the vertex with the lowest f_score
@@ -64,7 +79,15 @@ std::optional<graph_path<WEIGHT_T>> a_star_search(
 
     // Check if current vertex is the target
     if (current.id == target_vertex) {
-      return reconstruct_path(start_vertex, target_vertex, vertex_info);
+      graph_path<WEIGHT_T> path;
+      auto vertex{target_vertex};
+      while (vertex != start_vertex) {
+        path.vertices.push_front(vertex);
+        vertex = vertex_info[vertex]->prev_id;
+      }
+      path.vertices.push_front(start_vertex);
+      path.total_weight = vertex_info[target_vertex]->dist_from_start;
+      return path;
     }
 
     // Iterate through neighboring vertices
@@ -81,26 +104,22 @@ std::optional<graph_path<WEIGHT_T>> a_star_search(
 
       // 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;
+      WEIGHT_T tentative_g_score =
+          vertex_info[current.id]->dist_from_start + 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]) {
+      if (!has_info(neighbor) ||
+          tentative_g_score < vertex_info[neighbor]->dist_from_start) {
         // 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] with the true accumulated cost
-        // (g_score), so reconstruct_path reports the correct total_weight.
-        vertex_info[neighbor] = {
-            neighbor,           // vertex id
-            tentative_g_score,  // true accumulated cost from start
-            current.id          // neighbor vertex came from current vertex
-        };
+        // (g_score), so the resulting path reports the correct total_weight.
+        set_info(neighbor, tentative_g_score, current.id);
 
         open_set.push(open_set_item{neighbor, f_score});
       }

perf/graaflib/a_star_benchmark.cpp

@@ -0,0 +1,46 @@
+#include <benchmark/benchmark.h>
+#include <graaflib/algorithm/shortest_path/a_star.h>
+
+namespace {
+
+void bm_a_star_grid(benchmark::State& state) {
+  const auto side{static_cast<graaf::vertex_id_t>(state.range(0))};
+  const bool use_heuristic{state.range(1) != 0};
+  graaf::graph<int, int, graaf::graph_type::UNDIRECTED> graph;
+  for (graaf::vertex_id_t id{0}; id < side * side; ++id) {
+    [[maybe_unused]] const auto vertex{graph.add_vertex(0)};
+  }
+  for (graaf::vertex_id_t row{0}; row < side; ++row) {
+    for (graaf::vertex_id_t col{0}; col < side; ++col) {
+      const auto id{row * side + col};
+      if (col + 1 < side) {
+        graph.add_edge(id, id + 1, 1);
+      }
+      if (row + 1 < side) {
+        graph.add_edge(id, id + side, 1);
+      }
+    }
+  }
+  const auto heuristic{[side, use_heuristic](graaf::vertex_id_t id) {
+    return use_heuristic
+               ? static_cast<int>(2 * (side - 1) - id / side - id % side)
+               : 0;
+  }};
+  for (auto _ : state) {
+    auto result{
+        graaf::algorithm::a_star_search(graph, 0, side * side - 1, heuristic)};
+    if (!result || result->total_weight != static_cast<int>(2 * (side - 1))) {
+      state.SkipWithError("Unexpected grid shortest path");
+      break;
+    }
+    benchmark::DoNotOptimize(result);
+  }
+}
+
+}  // namespace
+
+BENCHMARK(bm_a_star_grid)
+    ->Args({32, 0})
+    ->Args({32, 1})
+    ->Args({128, 0})
+    ->Args({128, 1});

test/graaflib/algorithm/shortest_path/a_star_test.cpp

@@ -3,6 +3,9 @@
 #include <gtest/gtest.h>
 #include <utils/fixtures/fixtures.h>
 
+#include <limits>
+#include <random>
+
 namespace graaf::algorithm {
 
 namespace {
@@ -254,4 +257,133 @@ TYPED_TEST(AStarShortestPathTest, AStarHeuristicImpact) {
             expected_total_weight);
 }
 
-}  // namespace graaf::algorithm
+TEST(AStarDenseStorageTest, GrowsAcrossHolesAndUpdatesDiscoveredVertices) {
+  graph<int, int, graph_type::DIRECTED> graph;
+  std::vector<vertex_id_t> ids;
+  for (int i{0}; i < 257; ++i) {
+    ids.push_back(graph.add_vertex(0));
+  }
+  const auto start{ids[7]};
+  const auto via{ids[65]};
+  const auto target{ids[256]};
+  for (const auto id : ids) {
+    if (id != start && id != via && id != target) {
+      graph.remove_vertex(id);
+    }
+  }
+  graph.add_edge(start, target, 20);
+  graph.add_edge(start, via, 2);
+  graph.add_edge(via, target, 3);
+
+  const auto heuristic{
+      [target](vertex_id_t id) { return id == target ? 0 : 1; }};
+  const auto path{a_star_search(graph, start, target, heuristic)};
+  const graph_path<int> expected{{start, via, target}, 5};
+  ASSERT_EQ(path, expected);
+}
+
+TEST(AStarDenseStorageTest, HandlesLargeAbsentIdsWithoutIndexedAllocation) {
+  graph<int, int, graph_type::DIRECTED> graph;
+  const auto start{graph.add_vertex(0)};
+  const auto absent{std::numeric_limits<vertex_id_t>::max()};
+  int heuristic_calls{0};
+  const auto heuristic{[&](vertex_id_t) {
+    ++heuristic_calls;
+    return 0;
+  }};
+
+  EXPECT_FALSE(a_star_search(graph, absent, start, heuristic));
+  EXPECT_FALSE(a_star_search(graph, start, absent, heuristic));
+  const graph_path<int> trivial{{absent}, 0};
+  EXPECT_EQ(a_star_search(graph, absent, absent, heuristic), trivial);
+  EXPECT_EQ(heuristic_calls, 3);
+}
+
+TEST(AStarDenseStorageTest, DisconnectedTargetDoesNotAppearDiscovered) {
+  graph<int, int, graph_type::UNDIRECTED> graph;
+  const auto start{graph.add_vertex(0)};
+  const auto target{graph.add_vertex(0)};
+  vertex_id_t neighbor{};
+  std::vector<vertex_id_t> removed;
+  for (int i{0}; i < 63; ++i) {
+    neighbor = graph.add_vertex(0);
+    removed.push_back(neighbor);
+  }
+  removed.pop_back();
+  for (const auto id : removed) {
+    graph.remove_vertex(id);
+  }
+  graph.add_edge(start, neighbor, 0);
+  graph.add_edge(neighbor, neighbor, 0);
+
+  EXPECT_FALSE(
+      a_star_search(graph, start, target, [](vertex_id_t) { return 0; }));
+}
+
+TEST(AStarDenseStorageTest, MatchesIndependentAllPairsDistances) {
+  // Floyd-Warshall supplies an independent oracle. Sparse ids exercise lazy
+  // growth, and zero weights and cycles exercise repeated discovery.
+  constexpr std::size_t vertex_count{12};
+  constexpr int infinity{100000};
+  std::mt19937 generator{463};
+  for (int sample{0}; sample < 24; ++sample) {
+    graph<int, int, graph_type::DIRECTED> graph;
+    std::vector<vertex_id_t> ids;
+    std::vector<std::vector<int>> distances(
+        vertex_count, std::vector<int>(vertex_count, infinity));
+    for (std::size_t i{0}; i < vertex_count; ++i) {
+      [[maybe_unused]] const auto gap_1{graph.add_vertex(0)};
+      [[maybe_unused]] const auto gap_2{graph.add_vertex(0)};
+      ids.push_back(graph.add_vertex(0));
+      distances[i][i] = 0;
+    }
+    for (vertex_id_t id{0}; id < 3 * vertex_count; ++id) {
+      if (id % 3 != 2) {
+        graph.remove_vertex(id);
+      }
+    }
+    for (std::size_t from{0}; from < vertex_count; ++from) {
+      for (std::size_t to{0}; to < vertex_count; ++to) {
+        if (from != to && generator() % 5 == 0) {
+          const auto weight{static_cast<int>(generator() % 10)};
+          graph.add_edge(ids[from], ids[to], weight);
+          distances[from][to] = weight;
+        }
+      }
+    }
+    for (std::size_t via{0}; via < vertex_count; ++via) {
+      for (std::size_t from{0}; from < vertex_count; ++from) {
+        for (std::size_t to{0}; to < vertex_count; ++to) {
+          distances[from][to] = std::min(
+              distances[from][to], distances[from][via] + distances[via][to]);
+        }
+      }
+    }
+    for (std::size_t from{0}; from < vertex_count; ++from) {
+      for (std::size_t to{0}; to < vertex_count; ++to) {
+        SCOPED_TRACE(testing::Message() << "sample=" << sample
+                                        << " from=" << from << " to=" << to);
+        const auto path{a_star_search(graph, ids[from], ids[to],
+                                      [](vertex_id_t) { return 0; })};
+        if (distances[from][to] == infinity) {
+          EXPECT_FALSE(path);
+          continue;
+        }
+        ASSERT_TRUE(path);
+        EXPECT_EQ(path->total_weight, distances[from][to]);
+        EXPECT_EQ(path->vertices.front(), ids[from]);
+        EXPECT_EQ(path->vertices.back(), ids[to]);
+        int total{0};
+        auto current{path->vertices.begin()};
+        for (auto next{std::next(current)}; next != path->vertices.end();
+             ++current, ++next) {
+          ASSERT_TRUE(graph.has_edge(*current, *next));
+          total += graph.get_edge(*current, *next);
+        }
+        EXPECT_EQ(total, path->total_weight);
+      }
+    }
+  }
+}
+
+}  // namespace graaf::algorithm