Dijkstra Algorithm

gdscript-dijkstra.gd

extends Node2D

func _ready():
	var graph = {
		"a": {"b":4,"d":1},
		"b": {"c":1},
		"c": {"f":1},
		"d": {"b":2, "e":2},
		"e": {"c":3, "f":3},
		"f": {}
	}
	var start = "a"
	var target = "f"
	var result = dijkstra(start, target, graph)
	var path = [target]
	var node = target
	while node != start:
		var prev = result.prev[node]
		path.append(prev)
		node = prev
	path.reverse()
	print("Shortest distance = ", result.dist)
	print("Path3D:\n", path)


func dijkstra(start: String, target: String, graph: Dictionary) -> Dictionary:
	var dist = {}
	var prev = {}
	var priority_queue = []
	for node in graph:
		dist[node] = INF
		if node != start:
			priority_queue.append(node)
	priority_queue.append(start)
	dist[start] = 0
	
	var current_node = ""
	while priority_queue.size() > 0:
		var d = INF
		current_node = priority_queue.pop_back()
		for node in graph[current_node]:
			d = graph[current_node][node] + dist[current_node]
			if dist[node] > d:
				dist[node] = d
				prev[node] = current_node
				for i in priority_queue.size()-1:
					if priority_queue[i] == node:
						if dist[priority_queue[i+1]] > d:
							priority_queue[i] = priority_queue[i+1]
							priority_queue[i+1] = node
						else:
							break
	return {"dist": dist[target], "prev": prev}