gai-godot-games/pathfinding-algorithms/scenes/mario-party/MPNavigationGraph.gd

class_name MPNavigationGraph
extends Node2D

# godot does not support types on dictionaries, actual type is Dictionary[NavigationNode, Array[NavigationNode]]
var navigation_nodes: Dictionary                = {}
var latest_navigation_result: PathfindingResult = null
var draw_nodes: bool                            = true
var draw_edges: bool                            = true


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)


func _draw() -> void:
    print("Drawing navigation graph")
    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)
        draw_circle(local_to_world(latest_navigation_result.next_position), 5, Color.GREEN)


func local_to_world(location: Vector2) -> Vector2:
    return location * 32 + Vector2(16, 16)

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 the closest node to the current position
    var start_node: NavigationNode = 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: NavigationNode = 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[NavigationNode] = 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: 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:
        # Find the node with the smallest tentative distance
        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 the 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)

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