class_name MPNavigationGraph extends Node2D # godot does not support types on dictionaries, actual type is Dictionary[MPNavigationNode, Array[MPNavigationNode]] var navigation_nodes: Dictionary = {} var latest_navigation_result: MPPathfindingResult = null var draw_nodes: bool = false var draw_edges: bool = false func all_nodes() -> Array[MPNavigationNode]: # 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[MPNavigationNode] 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[MPNavigationNode] = [] for key in keys: if key is MPNavigationNode: nodes.append(key) else: push_error("Key is not a MPNavigationNode: %s" % key) return nodes func get_connections(from: MPNavigationNode) -> Array[MPNavigationNode]: # the same problem as the all_nodes() function var connections: Array = navigation_nodes[from] var nodes: Array[MPNavigationNode] = [] for connection in connections: if connection is MPNavigationNode: nodes.append(connection) else: push_error("Connection is not a MPNavigationNode: %s" % connection) return nodes func add_connection(from: MPNavigationNode, to: MPNavigationNode) -> 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) -> MPNavigationNode: if merge_threshold > 0: var closest_node: MPNavigationNode = find_closest_node_with_threshold(Vector2(x, y), merge_threshold) if closest_node: closest_node.was_merged = true return closest_node var node: MPNavigationNode = MPNavigationNode.new() node.set_position(Vector2(x, y)) navigation_nodes[node] = [] return node func find_closest_node_with_threshold(position: Vector2, threshold: float) -> MPNavigationNode: var closest_node: MPNavigationNode = 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: MPNavigationNode, to: MPNavigationNode) -> void: if all_nodes().has(from): navigation_nodes[from].erase(to) func remove_node(node: MPNavigationNode) -> void: navigation_nodes.erase(node) for other_node in all_nodes(): if other_node != node: remove_connection(other_node, node) 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(local_to_world(from.position), local_to_world(to.position), Color.RED, 1, false) if draw_nodes: draw_circle(local_to_world(from.position), 5, Color.RED) if latest_navigation_result != null: if latest_navigation_result.path.size() > 1: for i in range(latest_navigation_result.path.size() - 1): draw_line(local_to_world(latest_navigation_result.path[i].position), local_to_world(latest_navigation_result.path[i + 1].position), Color.GREEN, 1, false) for node in latest_navigation_result.path: draw_circle(local_to_world(node.position), 5, Color.GREEN) func local_to_world(location: Vector2) -> Vector2: return location * 32 + Vector2(16, 16) func draw_pathfinding_result(result: MPPathfindingResult) -> void: if latest_navigation_result and latest_navigation_result.is_identical_to(result): return latest_navigation_result = result queue_redraw() func clear_pathfinding_result() -> void: latest_navigation_result = null queue_redraw() func determine_next_position(current_position: Vector2, target_position: Vector2) -> MPPathfindingResult: var result: MPPathfindingResult = MPPathfindingResult.new() # Find the closest node to the current position var start_node: MPNavigationNode = find_closest_node_with_threshold(current_position, INF) if start_node == null: # No nodes exist; cannot navigate result.is_next_target = true result.next_position = current_position return result # Find the closest node to the target position var end_node: MPNavigationNode = find_closest_node_with_threshold(target_position, INF) if end_node == null: # No nodes exist; cannot navigate result.is_next_target = true result.next_position = current_position return result # If the start and end nodes are the same, go directly to the target position if start_node == end_node: result.is_next_target = true result.next_position = target_position return result # Run Dijkstra's algorithm to find the path var path: Array[MPNavigationNode] = dijkstra(start_node, end_node) result.path = path # 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: MPNavigationNode) -> bool: for item in arr: if item == node: return true return false func dijkstra(start_node: MPNavigationNode, end_node: MPNavigationNode) -> Array[MPNavigationNode]: var open_set: Array[MPNavigationNode] = [] var closed_set: Array[MPNavigationNode] = [] var distances: Dictionary = {} var previous_nodes: Dictionary = {} distances[start_node] = 0 open_set.append(start_node) while open_set.size() > 0: # Find the node with the smallest tentative distance var current_node: MPNavigationNode = 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 the path if current_node == end_node: var path: Array[MPNavigationNode] = [] var node: MPNavigationNode = 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) # Examine 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 an empty array return []