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 []