Implement Dijkstra's algorithm for finding shortest paths in Graph

core/types/graph.cpp

@@ -3,14 +3,17 @@
 #include "core/script_language.h"
 #include "core/string_names.h"
 
+#include "core/types/templates/priority_queue.h"
 #include "core/types/templates/union_find.h"
 
+#include <limits>
+
 using EdgeKey = GraphData::EdgeKey;
 using EdgeList = LocalVector<GraphEdge *, int>;
 
 void GraphData::remove_vertex(GraphVertex *p_vertex) {
 	if (clearing_all) {
-		return;	
+		return;
 	}
 	EdgeList edges_to_delete;
 
@@ -41,7 +44,7 @@ void GraphData::remove_vertex(GraphVertex *p_vertex) {
 
 void GraphData::remove_edge(GraphEdge *p_edge) {
 	if (clearing_all) {
-		return;	
+		return;
 	}
 	GraphVertex *&a = p_edge->a;
 	GraphVertex *&b = p_edge->b;
@@ -350,6 +353,28 @@ GraphEdge *Graph::find_edge(GraphVertex *p_a, GraphVertex *p_b) const {
 	return nullptr;
 }
 
+GraphEdge *Graph::_find_minimum_edge(GraphVertex *p_a, GraphVertex *p_b) const {
+	ERR_FAIL_NULL_V(p_a, nullptr);
+	ERR_FAIL_NULL_V(p_b, nullptr);
+
+	const EdgeList &list = graph->get_edges(p_a->id, p_b->id);
+	if (list.empty()) {
+		return nullptr;
+	}
+	GraphEdge *min_edge = list[0];
+	real_t min_weight = min_edge->value;
+
+	for (int i = 1; i < list.size(); ++i) {
+		GraphEdge *edge = list[i];
+		real_t weight = edge->value;
+		if (weight < min_weight) {
+			min_weight = weight;
+			min_edge = edge;
+		}
+	}
+	return min_edge;
+}
+
 bool Graph::has_edge(GraphVertex *p_a, GraphVertex *p_b) const {
 	ERR_FAIL_NULL_V(p_a, false);
 	ERR_FAIL_NULL_V(p_b, false);
@@ -428,9 +453,8 @@ struct SortEdgesMST {
 	}
 };
 
+// Kruskal's algorithm.
 Array Graph::minimum_spanning_tree() const {
-	// Kruskal's algorithm.
-
 	// Sort all edges in increasing weight.
 	Vector<GraphEdge *> edges;
 	{
@@ -466,6 +490,95 @@ Array Graph::minimum_spanning_tree() const {
 	return edges_tree;
 }
 
+struct DistanceComparator {
+	HashMap<uint32_t, real_t> distance;
+
+	_FORCE_INLINE_ bool operator()(const GraphVertex *a, const GraphVertex *b) const {
+		return distance[a->get_id()] < distance[b->get_id()];
+	}
+};
+
+// Dijkstra's algorithm.
+Dictionary Graph::shortest_path_tree(GraphVertex *p_root) const {
+	ERR_FAIL_NULL_V(p_root, Dictionary());
+
+	Dictionary tree;
+
+	PriorityQueue<GraphVertex *, DistanceComparator> queue;
+	LocalVector<GraphVertex *> vertices;
+
+	HashMap<uint32_t, real_t> &distance = queue.compare.distance;
+	HashMap<uint32_t, uint32_t> backtrace;
+	{
+		const uint32_t *k = nullptr;
+		while ((k = graph->vertices.next(k))) {
+			GraphVertex *v = graph->vertices[*k];
+			distance[v->id] = v == p_root ? 0.0 : std::numeric_limits<real_t>::infinity();
+			backtrace[v->id] = 0;
+			vertices.push_back(v);
+		}
+	}
+	queue.initialize(vertices);
+
+	// Find shortest path tree.
+	while (!queue.is_empty()) {
+		GraphVertex *u = queue.pop();
+
+		const uint32_t *k = nullptr;
+		while ((k = u->neighbors.next(k))) {
+			GraphVertex *v = graph->vertices[*k];
+			GraphEdge *edge = _find_minimum_edge(u, v);
+			real_t weight = edge->value;
+
+			if (distance[v->id] > distance[u->id] + weight) {
+				distance[v->id] = distance[u->id] + weight;
+				backtrace[v->id] = u->id; // Previous.
+				queue.update(v);
+			}
+		}
+	}
+	// Convert output.
+	Dictionary out_backtrace;
+	{
+		const uint32_t *k = nullptr;
+		while ((k = backtrace.next(k))) {
+			GraphVertex *vc = graph->vertices[*k];
+			GraphVertex *vp = nullptr;
+			if (backtrace[*k] != 0) {
+				vp = graph->vertices[backtrace[*k]];
+			}
+			out_backtrace[vc] = vp;
+		}
+	}
+	Dictionary out_distance;
+	{
+		const uint32_t *k = nullptr;
+		while ((k = distance.next(k))) {
+			GraphVertex *v = graph->vertices[*k];
+			real_t d = distance[*k];
+			out_distance[v] = d;
+		}
+	}
+	Array out_edges;
+	{
+		const uint32_t *k = nullptr;
+		while ((k = backtrace.next(k))) {
+			if (backtrace[*k] == 0) {
+				continue; // Stumbled upon root.
+			}
+			GraphVertex *current = graph->vertices[*k];
+			GraphVertex *previous = graph->vertices[backtrace[*k]];
+			GraphEdge *edge = _find_minimum_edge(previous, current);
+			out_edges.push_back(edge);
+		}
+	}
+	tree["backtrace"] = out_backtrace;
+	tree["distance"] = out_distance;
+	tree["edges"] = out_edges;
+
+	return tree;
+}
+
 Dictionary Graph::get_connected_components() {
 	Dictionary components;
 
@@ -567,6 +680,7 @@ void Graph::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("is_strongly_connected"), &Graph::is_strongly_connected);
 
 	ClassDB::bind_method(D_METHOD("minimum_spanning_tree"), &Graph::minimum_spanning_tree);
+	ClassDB::bind_method(D_METHOD("shortest_path_tree", "root"), &Graph::shortest_path_tree);
 
 	ClassDB::bind_method(D_METHOD("clear"), &Graph::clear);
 	ClassDB::bind_method(D_METHOD("clear_edges"), &Graph::clear_edges);

core/types/graph.h

@@ -74,6 +74,7 @@ class Graph : public Resource {
 	// Utilities
 	GraphVertex *_add_vertex(const Variant &p_value, uint32_t p_id = 0);
 	GraphEdge *_add_edge(const Variant &p_a, const Variant &p_b, const Variant &p_value, bool p_directed);
+	GraphEdge *_find_minimum_edge(GraphVertex *p_vertex_a, GraphVertex *p_vertex_b) const;
 
 public:
 	// Instantiation
@@ -103,6 +104,7 @@ class Graph : public Resource {
 	bool is_strongly_connected();
 
 	Array minimum_spanning_tree() const;
+	Dictionary shortest_path_tree(GraphVertex *p_root) const;
 
 	// Cleanup
 	void clear();
@@ -133,7 +135,7 @@ class GraphVertex : public Object {
 
 	HashMap<uint32_t, GraphVertex *> neighbors;
 	Variant value;
-	uint32_t id;
+	uint32_t id = 0;
 
 protected:
 	void _notification(int p_what);
@@ -151,6 +153,8 @@ class GraphVertex : public Object {
 
 	void set_value(const Variant &p_value) { value = p_value; }
 	Variant get_value() const { return value; }
+
+	uint32_t get_id() const { return id; }
 };
 
 class GraphEdge : public Object {
@@ -166,7 +170,7 @@ class GraphEdge : public Object {
 	bool directed = false;
 
 	Variant value;
-	uint32_t id;
+	uint32_t id = 0;
 
 protected:
 	void _notification(int p_what);

core/types/templates/priority_queue.h

@@ -0,0 +1,101 @@
+#pragma once
+
+template <class T>
+struct PriorityQueueMinHeapComparator {
+	_FORCE_INLINE_ bool operator()(const T &a, const T &b) const { return (a < b); }
+};
+
+template <class T>
+struct PriorityQueueMaxHeapComparator {
+	_FORCE_INLINE_ bool operator()(const T &a, const T &b) const { return (a > b); }
+};
+
+template <typename T, class Comparator=PriorityQueueMinHeapComparator<T>>
+class PriorityQueue {
+	LocalVector<T, int> vector;
+
+	_FORCE_INLINE_ int parent(int i) const {
+		return (i - 1) / 2;
+	}
+	_FORCE_INLINE_ int left(int i) const {
+		return i * 2 + 1;
+	}
+	_FORCE_INLINE_ int right(int i) const {
+		return i * 2 + 2;
+	}
+	_FORCE_INLINE_ void swap(int i, int j) {
+		T tmp = vector[i];
+		vector[i] = vector[j];
+		vector[j] = tmp;
+	}
+	void bubble_up(int i) {
+		while (i > 0 && compare(vector[i], vector[parent(i)])) {
+			swap(i, parent(i));
+			i = parent(i);
+		}
+	}
+	void sift_down(int i) {
+		while (left(i) < vector.size()) {
+			int c;
+			if (right(i) > vector.size() - 1) {
+				c = left(i);
+			} else if (compare(vector[left(i)], vector[right(i)])) {
+				c = left(i);
+			} else {
+				c = right(i);
+			}
+			if (compare(vector[c], vector[i])) {
+				swap(c, i);
+			}
+			i = c;
+		}
+	}
+
+public:
+	Comparator compare;
+
+	void initialize(const LocalVector<T> &p_elements) {
+		vector.clear();
+		if (p_elements.empty()) {
+			return;
+		}
+		vector.resize(p_elements.size());
+		for (int i = 0; i < p_elements.size(); ++i) {
+			vector[i] = p_elements[i];
+		}
+
+		int i = vector.size() / 2;
+		while (i >= 0) {
+			sift_down(i--);
+		}
+	}
+	void insert(const T &p_value) {
+		vector.push_back(p_value);
+		bubble_up(vector.size() - 1);
+	}
+	void update(const T &p_value) {
+		int i = vector.find(p_value);
+		ERR_FAIL_COND(i < 0);
+		bubble_up(i);
+	}
+	T pop() {
+		T root = vector[0];
+		const int n = vector.size() - 1;
+		vector[0] = vector[n];
+		vector.resize(n);
+		sift_down(0);
+		return root;
+	}
+	_FORCE_INLINE_ bool is_empty() const {
+		return vector.empty();
+	}
+	_FORCE_INLINE_ T top() {
+		ERR_FAIL_COND_V(vector.empty(), T());
+		return vector[0];
+	}
+
+	PriorityQueue() {}
+	PriorityQueue(const LocalVector<T> &p_elements) {
+		initialize(p_elements);
+	}
+};

doc/Graph.xml

@@ -184,7 +184,7 @@
 		<method name="minimum_spanning_tree" qualifiers="const">
 			<return type="Array" />
 			<description>
-				Returns a minimum spanning tree (MST) of this graph. An MST is represented as an [Array] of [GraphEdge]s in this graph, from which you can create a new [Graph], if you need to.
+				Returns a minimum spanning tree (MST) of this graph using Kruskal's algorithm. An MST is represented as an [Array] of [GraphEdge]s in this graph, from which you can create a new [Graph], if you need to.
 				The [member GraphEdge.value] is interpreted as a [float] weight, which is up to you to define.
 				In order to obtain a [i]maximum spanning tree[/i], you can inverse the weights, for example:
 				[codeblock]
@@ -222,6 +222,37 @@
 				Use depth-first search iterator (default).
 			</description>
 		</method>
+		<method name="shortest_path_tree" qualifiers="const">
+			<return type="Dictionary" />
+			<argument index="0" name="root" type="GraphVertex" />
+			<description>
+				Returns a shortest path tree starting at the [code]root[/code] vertex using Dijkstra's algorithm. This solves the Single-Source Shortest Path (SSSP) problem, which allows to find the shortest paths between a given vertex to all other vertices in the graph. The algorithm is structurally equivalent to the breadth-first search, except that this uses a priority queue to choose the next vertex based on [member GraphEdge.value] weights interpreted as [float] values.
+				The tree is represented as a [Dictionary] containing the following keys:
+				[code]backtrace:[/code] A [Dictionary] which contains exhaustive information that allows to reconstruct the shortest path. The keys hold current [GraphVertex], and values contain previous [GraphVertex]. Therefore, the shortest path between the source to any other connected vertex can be obtained in the following way:
+				[codeblock]
+				# Find the shortest path tree starting from the root vertex of interest.
+				var root = Random.choice(graph.get_vertex_list())
+				var tree = graph.shortest_path_tree(root)
+
+				# Pick any target vertex.
+				var current = Random.choice(graph.get_vertex_list())
+
+				# Extract shortest path.
+				var shortest_path = []
+				while true:
+				    shortest_path.append(current)
+				    var previous = tree.backtrace[current]
+				    if not previous:
+				        break # Reached source vertex (root).
+				    current = previous
+
+				# Invert the path for source-to-target order.
+				shortest_path.invert()
+				[/codeblock]
+				[code]distance:[/code] A [Dictionary] which contains the total distance (sum of edge weights) between source and target. The key is the [GraphVertex], the value is [float].
+				[code]edges:[/code] An [Array] of all [GraphEdge]s reachable from the [code]root[/code] vertex. Since there may be multiple edges between vertices, the edges with the minimum weight are collected only.
+			</description>
+		</method>
 	</methods>
 	<members>
 		<member name="data" type="Dictionary" setter="_set_data" getter="_get_data" default="{&quot;edges&quot;: [  ],&quot;vertices&quot;: [  ]}">