249 lines
9.2 KiB
GDScript
249 lines
9.2 KiB
GDScript
|
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 = {}
|
||
|
|
|
||
|
|
|
||
|
|
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] = []
|
||
|
|
|
||
|
|
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)
|
||
|
|
for i in range(len(nodes_in_polygon)):
|
||
|
|
for j in range(len(nodes_in_polygon)):
|
||
|
|
if i != j:
|
||
|
|
add_connection(nodes_in_polygon[i], nodes_in_polygon[j])
|
||
|
|
|
||
|
|
|
||
|
|
func _draw() -> void:
|
||
|
|
for from in all_nodes():
|
||
|
|
for to in get_connections(from):
|
||
|
|
draw_line(from.position, to.position, Color.RED, 1, false)
|
||
|
|
draw_circle(from.position, 5, Color.RED)
|
||
|
|
|
||
|
|
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)
|
||
|
|
|
||
|
|
|
||
|
|
func determine_next_position(current_position: Vector2, target_position: Vector2) -> Vector2:
|
||
|
|
# find both polygons
|
||
|
|
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, find the closest node and navigate to it
|
||
|
|
if not current_polygon:
|
||
|
|
var closest_node: NavigationNode = find_closest_node_with_threshold(current_position, 100000)
|
||
|
|
return closest_node.position
|
||
|
|
|
||
|
|
if not current_polygon or not target_polygon:
|
||
|
|
return current_position
|
||
|
|
|
||
|
|
# check if the polygons are the same, if so, just return the target position
|
||
|
|
if current_polygon == target_polygon:
|
||
|
|
return target_position
|
||
|
|
|
||
|
|
# the target position is simple, just take the center of the target polygon since we just need any point
|
||
|
|
# in the polygon to roughly navigate to it, the alternate algorithm above will take care of the rest
|
||
|
|
var end_node: NavigationNode = target_polygon.center_node
|
||
|
|
|
||
|
|
var path: Array[NavigationNode] = dijkstra(start_node, end_node)
|
||
|
|
|
||
|
|
# If a path is found, return the position of the next node in the path
|
||
|
|
if path.size() > 1:
|
||
|
|
return path[1].position # Next node in the path
|
||
|
|
elif path.size() == 1:
|
||
|
|
return target_position # Directly reachable
|
||
|
|
else:
|
||
|
|
# No path found; return current position
|
||
|
|
return current_position
|
||
|
|
|
||
|
|
|
||
|
|
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 []
|