gai-godot-games/pathfinding-algorithms/scenes/custom-solver/NavigationGraph.gd

class_name NavigationGraph
extends Node2D

# godot does not support types on dictionaries, actual type is Dictionary[NavigationNode, Array[NavigationNode]]
var navigation_nodes: Dictionary = {}
# type is Dictionary[CNavigationPolygon, Array[NavigationNode]]
var polygon_nodes: Dictionary                   = {}

var latest_navigation_result: PathfindingResult = null
var draw_polygons: bool                         = true
var draw_nodes: bool                            = false
var draw_edges: bool                            = false


func all_nodes() -> Array[NavigationNode]:
	# i've had a problem where godot would not allow me to directly return navigation_nodes.keys()
	# because it wasn't able to cast the keys to Array[NavigationNode] directly because the type is not explicit
	# on the dictionary, so i had to do this workaround.
	var keys: Array                  = navigation_nodes.keys()
	var nodes: Array[NavigationNode] = []
	for key in keys:
		if key is NavigationNode:
			nodes.append(key)
		else:
			push_error("Key is not a NavigationNode: %s" % key)
	return nodes


func get_connections(from: NavigationNode) -> Array[NavigationNode]:
	# the same problem as the all_nodes() function
	var connections: Array           = navigation_nodes[from]
	var nodes: Array[NavigationNode] = []
	for connection in connections:
		if connection is NavigationNode:
			nodes.append(connection)
		else:
			push_error("Connection is not a NavigationNode: %s" % connection)
	return nodes


func add_connection(from: NavigationNode, to: NavigationNode) -> void:
	if all_nodes().has(from):
		navigation_nodes[from].append(to)
	else:
		navigation_nodes[from] = [to]


func add_node(x: float, y: float, merge_threshold: float = -1.0) -> NavigationNode:
	if merge_threshold > 0:
		var closest_node: NavigationNode = find_closest_node_with_threshold(Vector2(x, y), merge_threshold)
		if closest_node:
			closest_node.was_merged = true
			return closest_node
	var node: NavigationNode = NavigationNode.new()
	node.set_position(Vector2(x, y))
	navigation_nodes[node] = []
	return node


func find_closest_node_with_threshold(position: Vector2, threshold: float) -> NavigationNode:
	var closest_node: NavigationNode = null
	var closest_distance: float      = threshold
	for node in all_nodes():
		var distance: float = position.distance_to(node.position)
		if distance < closest_distance:
			closest_node    = node
			closest_distance = distance
	return closest_node


func remove_connection(from: NavigationNode, to: NavigationNode) -> void:
	if all_nodes().has(from):
		navigation_nodes[from].erase(to)


func remove_node(node: NavigationNode) -> void:
	navigation_nodes.erase(node)
	for other_node in all_nodes():
		if other_node != node:
			remove_connection(other_node, node)
	for poly in all_polygons():
		if node in polygon_nodes[poly]:
			polygon_nodes[poly].erase(node)


func all_polygons() -> Array[CNavigationPolygon]:
	var keys: Array                         = polygon_nodes.keys()
	var polygons: Array[CNavigationPolygon] = []
	for key in keys:
		if key is CNavigationPolygon:
			polygons.append(key)
		else:
			push_error("Key is not a NavigationPolygon: %s" % key)
	return polygons


func get_nodes_in_polygon(poly: CNavigationPolygon) -> Array[NavigationNode]:
	var relevant_nodes: Array        = polygon_nodes[poly]
	var nodes: Array[NavigationNode] = []
	for node in relevant_nodes:
		if node is NavigationNode:
			nodes.append(node)
		else:
			push_error("Node is not a NavigationNode: %s" % node)
	return nodes


func erase_and_create_nodes_from_polygons(new_polys: Array[PackedVector2Array]) -> void:
	navigation_nodes.clear()
	polygon_nodes.clear()

	for poly in new_polys:
		if poly.size() == 0:
			continue

		var navpoly: CNavigationPolygon = CNavigationPolygon.new()
		poly = navpoly.set_polygon(poly)
		polygon_nodes[navpoly] = []

		# create one in the center of each polygon that is kept no matter what
		# var poly_center: Vector2        = navpoly.center()
		# var center_node: NavigationNode = add_node(poly_center.x, poly_center.y, -1)
		# center_node.was_merged = true
		# polygon_nodes[navpoly].append(center_node)
		# navpoly.center_node = center_node

		for i in range(len(poly) - 1):
			var center: Vector2 = (poly[i] + poly[i + 1]) / 2
			polygon_nodes[navpoly].append(add_node(center.x, center.y, 10))
			var quater: Vector2 = (poly[i] + center) / 2
			polygon_nodes[navpoly].append(add_node(quater.x, quater.y, 10))
			var three_quater: Vector2 = (center + poly[i + 1]) / 2
			polygon_nodes[navpoly].append(add_node(three_quater.x, three_quater.y, 10))

	# clear any that were not merged
	for node in all_nodes():
		if not node.was_merged:
			remove_node(node)

	# connect all within a polygon
	for poly in all_polygons():
		var nodes_in_polygon: Array[NavigationNode] = get_nodes_in_polygon(poly)
		connect_all_nodes(nodes_in_polygon)


func connect_all_nodes(nodes: Array[NavigationNode]) -> void:
	for i in range(len(nodes)):
		for j in range(len(nodes)):
			if i != j:
				add_connection(nodes[i], nodes[j])


func _draw() -> void:
	if draw_nodes or draw_edges:
		for from in all_nodes():
			if draw_edges:
				for to in get_connections(from):
					draw_line(from.position, to.position, Color.RED, 1, false)
			if draw_nodes:
				draw_circle(from.position, 5, Color.RED)

	if draw_polygons:
		for poly in all_polygons():
			draw_colored_polygon(poly.polygon, Color(0.5, 0.4, 0.9, 0.3))
			draw_polyline(poly.polygon, Color.WHITE, 1, true)

	if latest_navigation_result != null:
		if latest_navigation_result.path.size() > 1:
			for i in range(latest_navigation_result.path.size() - 1):
				draw_line(latest_navigation_result.path[i].position, latest_navigation_result.path[i + 1].position, Color.GREEN, 1, false)
		draw_circle(latest_navigation_result.next_position, 5, Color.GREEN)


func draw_pathfinding_result(result: PathfindingResult) -> void:
	if latest_navigation_result and latest_navigation_result.is_identical_to(result):
		return
	latest_navigation_result = result
	queue_redraw()


func determine_next_position(current_position: Vector2, target_position: Vector2) -> PathfindingResult:
	var result: PathfindingResult = PathfindingResult.new()

	# find both polygons containing the current and target positions
	var current_polygon: CNavigationPolygon = null
	var target_polygon: CNavigationPolygon  = null

	for poly in all_polygons():
		if Geometry2D.is_point_in_polygon(current_position, poly.polygon):
			current_polygon = poly
		if Geometry2D.is_point_in_polygon(target_position, poly.polygon):
			target_polygon = poly

	# if the current position is not in any polygon, navigate to the closest node
	if not current_polygon:
		var closest_node: NavigationNode = find_closest_node_with_threshold(current_position, 100000)
		result.next_position = closest_node.position
		return result

	# if the target position is not in any polygon, return current position (cannot navigate)
	if not target_polygon:
		result.is_next_target = true
		result.next_position = current_position
		return result

	# if the current and target positions are in the same polygon, return the target position
	if current_polygon == target_polygon:
		result.is_next_target = true
		result.next_position = target_position
		return result

	# we will have to insert the start node into the graph and connect it to the nodes within the polygon,
	# and remove it later on again
	var start_node: NavigationNode                      = add_node(current_position.x, current_position.y, -1)
	var nodes_in_current_polygon: Array[NavigationNode] = get_nodes_in_polygon(current_polygon)
	polygon_nodes[current_polygon].append(start_node)
	for node in nodes_in_current_polygon:
		add_connection(start_node, node)

	# the target position is simple, just find the closest node in the polygon to the target position,
	# the alternate algorithm for within a polygon above will take care of the rest
	var end_node: NavigationNode = null
	var min_distance: float      = INF

	var nodes_in_target_polygon: Array[NavigationNode] = get_nodes_in_polygon(target_polygon)
	for node in nodes_in_target_polygon:
		var distance: float = target_position.distance_to(node.position)
		if distance < min_distance:
			min_distance = distance
			end_node = node

	var path: Array[NavigationNode] = dijkstra(start_node, end_node)
	result.path = path

	# remove the start node again
	remove_node(start_node)

	# if a path is found, return the position of the next node in the path
	if path.size() > 1:
		# next node in the path
		result.next_position = path[1].position
		return result
	elif path.size() == 1:
		# directly reachable
		result.is_next_target = true
		result.next_position = target_position
	else:
		# no path found; return current position
		result.is_next_target = true
		result.next_position = current_position

	return result


func array_contains_node(arr: Array, node: NavigationNode) -> bool:
	for item in arr:
		if item == node:
			return true
	return false


func dijkstra(start_node: NavigationNode, end_node: NavigationNode) -> Array[NavigationNode]:
	var open_set: Array[NavigationNode]   = []
	var closed_set: Array[NavigationNode] = []
	var distances: Dictionary             = {}
	var previous_nodes: Dictionary        = {}

	distances[start_node] = 0
	open_set.append(start_node)

	while open_set.size() > 0:
		var current_node: NavigationNode = open_set[0]
		var current_distance: float      = distances[current_node]
		for node in open_set:
			if distances[node] < current_distance:
				current_node = node
				current_distance = distances[node]

		# if the end node is reached, reconstruct path
		if current_node == end_node:
			var path: Array[NavigationNode] = []
			var node: NavigationNode        = end_node
			while node != null:
				path.insert(0, node)
				node = previous_nodes.get(node, null)
			return path

		open_set.erase(current_node)
		closed_set.append(current_node)

		# neighbors of the current node
		var neighbors = get_connections(current_node)
		for neighbor in neighbors:
			if array_contains_node(closed_set, neighbor):
				continue

			var tentative_distance: float = distances[current_node] + current_node.position.distance_to(neighbor.position)

			if not array_contains_node(open_set, neighbor) or tentative_distance < distances.get(neighbor, INF):
				distances[neighbor] = tentative_distance
				previous_nodes[neighbor] = current_node
				if not array_contains_node(open_set, neighbor):
					open_set.append(neighbor)

	# no path found
	return []