4 files changed, 83 insertions, 0 deletions
diff --git a/game/network/network.cpp b/game/network/network.cpp
index fd61764..7349ff0 100644
--- a/game/network/network.cpp
+++ b/game/network/network.cpp
@@ -1,9 +1,11 @@
 #include "network.h"
+#include "routeWalker.h"
 #include <array>
 #include <cache.h>
 #include <game/network/link.h>
 #include <gfx/models/texture.h>
 #include <initializer_list>
+#include <stdexcept>
 #include <utility>
 
 Network::Network(const std::string & tn) : texture {Texture::cachedTexture.get(tn)} { }
@@ -44,3 +46,19 @@ Network::joinLinks(const LinkPtr & l, const LinkPtr & ol)
 		}
 	}
 }
+
+std::vector<LinkWPtr>
+Network::routeFromTo(const Link::End & start, glm::vec3 dest) const
+{
+	auto destNode {findNodeAt(dest)};
+	if (!destNode) {
+		throw std::out_of_range("Node does not exist in network");
+	}
+	return routeFromTo(start, destNode);
+}
+
+std::vector<LinkWPtr>
+Network::routeFromTo(const Link::End & end, const NodePtr & dest) const
+{
+	return RouteWalker().findRouteTo(end, dest);
+}
diff --git a/game/network/network.h b/game/network/network.h
index c6d4dfa..f84c90c 100644
--- a/game/network/network.h
+++ b/game/network/network.h
@@ -10,6 +10,7 @@
 #include <sorting.hpp>
 #include <string>
 #include <utility>
+#include <vector>
 
 class Texture;
 class Shader;
@@ -22,6 +23,9 @@ public:
 	[[nodiscard]] NodePtr nodeAt(glm::vec3);
 	[[nodiscard]] std::pair<NodePtr, bool> newNodeAt(glm::vec3);
 
+	[[nodiscard]] std::vector<LinkWPtr> routeFromTo(const Link::End &, glm::vec3) const;
+	[[nodiscard]] std::vector<LinkWPtr> routeFromTo(const Link::End &, const NodePtr &) const;
+
 protected:
 	static void joinLinks(const LinkPtr & l, const LinkPtr & ol);
 
diff --git a/game/network/routeWalker.cpp b/game/network/routeWalker.cpp
new file mode 100644
index 0000000..496f9f3
--- /dev/null
+++ b/game/network/routeWalker.cpp
@@ -0,0 +1,37 @@
+#include "routeWalker.h"
+#include <array>
+#include <game/network/link.h>
+#include <limits>
+#include <memory>
+#include <utility>
+
+RouteWalker::RouteWalker() : solutionLength {std::numeric_limits<float>::max()} { }
+
+RouteWalker::Solution
+RouteWalker::findRouteTo(const Link::End & currentEnd, const NodePtr & dest)
+{
+	findRouteTo(currentEnd, dest, 0);
+	return bestSolution;
+}
+
+void
+// NOLINTNEXTLINE(misc-no-recursion)
+RouteWalker::findRouteTo(const Link::End & currentEnd, const NodePtr & dest, float length)
+{
+	if (currentEnd.node == dest && length < solutionLength) {
+		bestSolution = currentSolution;
+		solutionLength = length;
+		return;
+	}
+	if (visited.contains(&currentEnd)) { // We've been here before
+		return;
+	}
+	visited.insert(&currentEnd);
+	for (const auto & nexts : currentEnd.nexts) {
+		const auto link = nexts.first.lock();
+		currentSolution.push_back(link);
+		findRouteTo(link->ends[!nexts.second], dest, length + link->length);
+		currentSolution.pop_back();
+	}
+	visited.erase(&currentEnd);
+}
diff --git a/game/network/routeWalker.h b/game/network/routeWalker.h
new file mode 100644
index 0000000..7b1e1c8
--- /dev/null
+++ b/game/network/routeWalker.h
@@ -0,0 +1,24 @@
+#ifndef ROUTEWALKER_H
+#define ROUTEWALKER_H
+
+#include "link.h"
+#include <set>
+#include <vector>
+
+class RouteWalker {
+public:
+	using Solution = std::vector<LinkWPtr>;
+
+	RouteWalker();
+
+	Solution findRouteTo(const Link::End & currentEnd, const NodePtr & dest);
+
+private:
+	void findRouteTo(const Link::End & currentEnd, const NodePtr & dest, float length);
+
+	std::set<const Link::End *> visited;
+	Solution bestSolution, currentSolution;
+	float solutionLength;
+};
+
+#endif