implement bfs multi-target distance calculation to nearest winning state

src/distance.cpp

@@ -0,0 +1,92 @@
+#include "distance.hpp"
+#include "config.hpp"
+
+#include <cstddef>
+#include <iostream>
+#include <queue>
+
+#ifdef TRACY
+#include "tracy.hpp"
+#include <tracy/Tracy.hpp>
+#endif
+
+auto DistanceResult::Clear() -> void {
+  distances.clear();
+  parents.clear();
+  nearest_targets.clear();
+}
+
+auto DistanceResult::Empty() -> bool {
+  return distances.empty() || parents.empty() || nearest_targets.empty();
+}
+
+auto CalculateDistances(
+    std::size_t node_count,
+    const std::vector<std::pair<std::size_t, std::size_t>> &edges,
+    const std::vector<std::size_t> &targets) -> DistanceResult {
+
+  // Build a list of adjacent nodes to speed up BFS
+  std::vector<std::vector<std::size_t>> adjacency(node_count);
+  for (const auto &[from, to] : edges) {
+    adjacency[from].push_back(to);
+    adjacency[to].push_back(from);
+  }
+  // for (size_t i = 0; i < adjacency.size(); ++i) {
+  //   std::cout << "Node " << i << "'s neighbors: ";
+  //   for (const auto &neighbor : adjacency[i]) {
+  //     std::cout << neighbor;
+  //   }
+  //   std::cout << "\n";
+  // }
+  // std::cout << std::endl;
+
+  std::vector<int> distances(node_count, -1);
+  std::vector<std::size_t> parents(node_count, -1);
+  std::vector<std::size_t> nearest_targets(node_count, -1);
+
+  std::queue<std::size_t> queue;
+  for (std::size_t target : targets) {
+    distances[target] = 0;
+    nearest_targets[target] = target;
+    queue.push(target);
+  }
+
+  while (!queue.empty()) {
+    std::size_t current = queue.front();
+    queue.pop();
+
+    for (std::size_t neighbor : adjacency[current]) {
+      // std::cout << "Visiting edge (" << current << "->" << neighbor << ")."
+      //           << std::endl;
+      if (distances[neighbor] == -1) {
+        // If distance is -1 we haven't visited the node yet
+        distances[neighbor] = distances[current] + 1;
+        parents[neighbor] = current;
+        nearest_targets[neighbor] = nearest_targets[current];
+        // std::cout << "- Distance: " << distances[neighbor]
+        //           << ", Parent: " << parents[neighbor]
+        //           << ", Nearest Target: " << nearest_targets[neighbor] << "."
+        //           << std::endl;
+        queue.push(neighbor);
+      }
+    }
+  }
+
+  return {distances, parents, nearest_targets};
+}
+
+auto GetPath(const DistanceResult &result, std::size_t source)
+    -> std::vector<std::size_t> {
+  if (result.distances[source] == -1) {
+    // Unreachable
+    return {};
+  }
+
+  std::vector<std::size_t> path;
+  for (std::size_t n = source; n != static_cast<std::size_t>(-1);
+       n = result.parents[n]) {
+    path.push_back(n);
+  }
+
+  return path;
+}

src/input.cpp

@@ -1,5 +1,6 @@
 #include "input.hpp"
 #include "config.hpp"
+#include "distance.hpp"
 
 #include <algorithm>
 #include <raylib.h>

src/main.cpp

@@ -3,6 +3,7 @@
 #include <raymath.h>
 
 #include "config.hpp"
+#include "distance.hpp"
 #include "input.hpp"
 #include "physics.hpp"
 #include "renderer.hpp"
@@ -13,12 +14,6 @@
 #include <tracy/Tracy.hpp>
 #endif
 
-// TODO: Klotski state file loading
-//       - File should contain a single state per line, multiple lines possible
-//       - If a file is loaded, the presets should be replaced with the states
-//         from the file
-//       - Automatically determine the winning condition based on a configured
-//         board exit
 // TODO: Graph interaction
 //       - Click states to display them in the board
 //       - Find shortest path to any winning state and mark it in the graph
@@ -48,8 +43,7 @@ auto main(int argc, char *argv[]) -> int {
   Renderer renderer(camera, state, input);
 
   unsigned int ups;
-  std::vector<Vector3> masses;                              // Read from physics
-  std::vector<std::pair<std::size_t, std::size_t>> springs; // Read from physics
+  std::vector<Vector3> masses; // Read from physics
 
   // Game loop
   while (!WindowShouldClose()) {
@@ -78,7 +72,6 @@ auto main(int argc, char *argv[]) -> int {
       // Only copy data if any has been produced
       if (physics.state.data_ready) {
         masses = physics.state.masses;
-        springs = physics.state.springs;
 
         physics.state.data_ready = false;
         physics.state.data_consumed = true;
@@ -101,9 +94,9 @@ auto main(int argc, char *argv[]) -> int {
 
     // Rendering
     renderer.UpdateTextureSizes();
-    renderer.DrawMassSprings(masses, springs);
+    renderer.DrawMassSprings(masses);
     renderer.DrawKlotski();
-    renderer.DrawMenu(masses, springs);
+    renderer.DrawMenu(masses);
     renderer.DrawTextures(ups);
 #ifdef TRACY
     FrameMark;

src/physics.cpp

@@ -288,12 +288,6 @@ auto ThreadedPhysics::PhysicsThread(ThreadedPhysics::PhysicsState &state)
         state.masses.emplace_back(mass.position);
       }
 
-      state.springs.clear();
-      state.springs.reserve(mass_springs.springs.size());
-      for (const auto &spring : mass_springs.springs) {
-        state.springs.emplace_back(spring.a, spring.b);
-      }
-
       state.data_ready = true;
       state.data_consumed = false;
     }

src/renderer.cpp

@@ -59,9 +59,7 @@ auto Renderer::ReallocateGraphInstancingIfNecessary(std::size_t size) -> void {
   }
 }
 
-auto Renderer::DrawMassSprings(
-    const std::vector<Vector3> &masses,
-    const std::vector<std::pair<std::size_t, std::size_t>> &springs) -> void {
+auto Renderer::DrawMassSprings(const std::vector<Vector3> &masses) -> void {
 #ifdef TRACY
   ZoneScoped;
 #endif
@@ -96,7 +94,7 @@ auto Renderer::DrawMassSprings(
     ZoneNamedN(draw_springs, "DrawSprings", true);
 #endif
     rlBegin(RL_LINES);
-    for (const auto &[from, to] : springs) {
+    for (const auto &[from, to] : state.springs) {
       if (masses.size() > from && masses.size() > to) {
         const Vector3 &a = masses.at(from);
         const Vector3 &b = masses.at(to);
@@ -289,9 +287,7 @@ auto Renderer::DrawKlotski() -> void {
   EndTextureMode();
 }
 
-auto Renderer::DrawMenu(
-    const std::vector<Vector3> &masses,
-    const std::vector<std::pair<std::size_t, std::size_t>> &springs) -> void {
+auto Renderer::DrawMenu(const std::vector<Vector3> &masses) -> void {
 #ifdef TRACY
   ZoneScoped;
 #endif
@@ -317,7 +313,7 @@ auto Renderer::DrawMenu(
 
   draw_btn(0, 0,
            std::format("States: {}, Transitions: {}, Winning: {}",
-                       masses.size(), springs.size(),
+                       masses.size(), state.springs.size(),
                        state.winning_states.size()),
            DARKGREEN);
   draw_btn(

src/state.cpp

@@ -68,6 +68,8 @@ auto StateManager::NextPreset() -> void {
 }
 
 auto StateManager::FillGraph() -> void {
+  ZoneScoped;
+
   ClearGraph();
 
   std::pair<std::vector<State>,
@@ -78,8 +80,14 @@ auto StateManager::FillGraph() -> void {
   physics.AddMassSpringsCmd(closure.first.size(), closure.second);
   for (const State &state : closure.first) {
     states.insert(std::make_pair(state, states.size()));
+    masses.insert(std::make_pair(masses.size(), state));
+  }
+  for (const auto &[from, to] : closure.second) {
+    springs.emplace_back(from, to);
   }
   FindWinningStates();
+  FindTargetDistances();
+  FindTargetPath();
 }
 
 auto StateManager::UpdateGraph() -> void {
@@ -89,23 +97,34 @@ auto StateManager::UpdateGraph() -> void {
 
   if (!states.contains(current_state)) {
     states.insert(std::make_pair(current_state, states.size()));
+    masses.insert(std::make_pair(masses.size(), current_state));
+    springs.emplace_back(states.at(current_state), states.at(previous_state));
     physics.AddMassCmd();
     physics.AddSpringCmd(states.at(current_state), states.at(previous_state));
+
+    if (current_state.IsWon()) {
+      winning_states.insert(current_state);
+    }
+    FindTargetDistances();
   }
 
   visited_states.insert(current_state);
-  if (current_state.IsWon()) {
-    winning_states.insert(current_state);
-  }
+  FindTargetPath();
 }
 
 auto StateManager::ClearGraph() -> void {
   states.clear();
   winning_states.clear();
   visited_states.clear();
+  masses.clear();
+  winning_path.clear();
+  springs.clear();
+  target_distances.Clear();
   physics.ClearCmd();
 
+  // Re-add the default stuff to the graph
   states.insert(std::make_pair(current_state, states.size()));
+  masses.insert(std::make_pair(masses.size(), current_state));
   visited_states.insert(current_state);
   physics.AddMassCmd();
 
@@ -123,6 +142,37 @@ auto StateManager::FindWinningStates() -> void {
   }
 }
 
+auto StateManager::FindTargetDistances() -> void {
+  ZoneScoped;
+
+  if (springs.size() == 0 || winning_states.size() == 0) {
+    return;
+  }
+
+  // Find target indices
+  std::vector<std::size_t> targets;
+  targets.reserve(winning_states.size());
+  for (const auto &_state : winning_states) {
+    targets.push_back(states.at(_state));
+  }
+
+  target_distances = CalculateDistances(states.size(), springs, targets);
+
+  std::cout << "Calculated " << target_distances.distances.size()
+            << " distances to " << targets.size() << " targets." << std::endl;
+}
+
+auto StateManager::FindTargetPath() -> void {
+  if (target_distances.Empty()) {
+    return;
+  }
+
+  winning_path = GetPath(target_distances, CurrentMassIndex());
+
+  std::cout << "Nearest target is " << winning_path.size() << " moves away."
+            << std::endl;
+}
+
 auto StateManager::CurrentMassIndex() const -> std::size_t {
   return states.at(current_state);
 }