From 2ca824a2e35560f739e31fae37961420ca047e1c Mon Sep 17 00:00:00 2001 From: Yan Wittmann Date: Tue, 12 Nov 2024 15:00:58 +0100 Subject: [PATCH] Started custom navmesh logic --- pathfinding-algorithms/project.godot | 2 +- .../custom-solver/CNavigationPolygon.gd | 21 ++ .../scenes/custom-solver/CustomNavMovement.gd | 22 ++ .../scenes/custom-solver/NavigationGraph.gd | 248 ++++++++++++++++++ .../scenes/custom-solver/NavigationNode.gd | 4 + .../custom-solver/custom-graph-solver.tscn | 41 +++ .../custom-solver/custom_graph_solver.gd | 12 + 7 files changed, 349 insertions(+), 1 deletion(-) create mode 100644 pathfinding-algorithms/scenes/custom-solver/CNavigationPolygon.gd create mode 100644 pathfinding-algorithms/scenes/custom-solver/CustomNavMovement.gd create mode 100644 pathfinding-algorithms/scenes/custom-solver/NavigationGraph.gd create mode 100644 pathfinding-algorithms/scenes/custom-solver/NavigationNode.gd create mode 100644 pathfinding-algorithms/scenes/custom-solver/custom-graph-solver.tscn create mode 100644 pathfinding-algorithms/scenes/custom-solver/custom_graph_solver.gd diff --git a/pathfinding-algorithms/project.godot b/pathfinding-algorithms/project.godot index f8fa103..2eff004 100644 --- a/pathfinding-algorithms/project.godot +++ b/pathfinding-algorithms/project.godot @@ -11,7 +11,7 @@ config_version=5 [application] config/name="pathfinding-algorithms" -run/main_scene="res://scenes/tilemap_nav.tscn" +run/main_scene="res://scenes/custom-solver/custom-graph-solver.tscn" config/features=PackedStringArray("4.3", "Forward Plus") config/icon="res://icon.svg" diff --git a/pathfinding-algorithms/scenes/custom-solver/CNavigationPolygon.gd b/pathfinding-algorithms/scenes/custom-solver/CNavigationPolygon.gd new file mode 100644 index 0000000..5b2aeea --- /dev/null +++ b/pathfinding-algorithms/scenes/custom-solver/CNavigationPolygon.gd @@ -0,0 +1,21 @@ +class_name CNavigationPolygon +extends Node + +var polygon: PackedVector2Array +var center_node: NavigationNode + +func _init() -> void: + polygon = PackedVector2Array() + +func set_polygon(new_polygon: PackedVector2Array) -> PackedVector2Array: + var new_polygon_clone: PackedVector2Array = new_polygon.duplicate() + new_polygon_clone.append(new_polygon_clone[0]) + polygon = new_polygon_clone + return polygon + +func center() -> Vector2: + var center: Vector2 = Vector2() + for point in polygon: + center += Vector2(point.x, point.y) + center /= polygon.size() + return center diff --git a/pathfinding-algorithms/scenes/custom-solver/CustomNavMovement.gd b/pathfinding-algorithms/scenes/custom-solver/CustomNavMovement.gd new file mode 100644 index 0000000..cc8854c --- /dev/null +++ b/pathfinding-algorithms/scenes/custom-solver/CustomNavMovement.gd @@ -0,0 +1,22 @@ +extends CharacterBody2D + +const MAX_SPEED: float = 300.0 +const ACCELERATION: int = 2400 +@onready var graph: NavigationGraph = $"../NavGraph" + + +func _physics_process(delta: float) -> void: + var movement: Vector2 = Vector2() + + var next_pos: Vector2 = graph.determine_next_position(position, get_global_mouse_position()) + movement = next_pos - global_position + if movement.length() < 20: + movement = -velocity * 0.2 + else: + movement = movement.normalized() * ACCELERATION * delta + + velocity += movement + if velocity.length() > MAX_SPEED: + velocity = velocity.normalized() * MAX_SPEED + + move_and_slide() diff --git a/pathfinding-algorithms/scenes/custom-solver/NavigationGraph.gd b/pathfinding-algorithms/scenes/custom-solver/NavigationGraph.gd new file mode 100644 index 0000000..d2f063a --- /dev/null +++ b/pathfinding-algorithms/scenes/custom-solver/NavigationGraph.gd @@ -0,0 +1,248 @@ +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 [] diff --git a/pathfinding-algorithms/scenes/custom-solver/NavigationNode.gd b/pathfinding-algorithms/scenes/custom-solver/NavigationNode.gd new file mode 100644 index 0000000..6202aec --- /dev/null +++ b/pathfinding-algorithms/scenes/custom-solver/NavigationNode.gd @@ -0,0 +1,4 @@ +class_name NavigationNode +extends Node2D + +var was_merged: bool = false diff --git a/pathfinding-algorithms/scenes/custom-solver/custom-graph-solver.tscn b/pathfinding-algorithms/scenes/custom-solver/custom-graph-solver.tscn new file mode 100644 index 0000000..9c49a16 --- /dev/null +++ b/pathfinding-algorithms/scenes/custom-solver/custom-graph-solver.tscn @@ -0,0 +1,41 @@ +[gd_scene load_steps=6 format=3 uid="uid://g4ggp2goh0di"] + +[ext_resource type="Script" path="res://scenes/custom-solver/NavigationGraph.gd" id="1_5s4ud"] +[ext_resource type="Script" path="res://scenes/custom-solver/custom_graph_solver.gd" id="1_tis1c"] +[ext_resource type="Script" path="res://scenes/custom-solver/CustomNavMovement.gd" id="3_4rjft"] +[ext_resource type="Texture2D" uid="uid://c53ftc05so8rx" path="res://icon.svg" id="3_ibv8u"] + +[sub_resource type="CapsuleShape2D" id="CapsuleShape2D_gqxbx"] +radius = 24.9999 +height = 49.9999 + +[node name="custom-graph-solver" type="Node2D"] +script = ExtResource("1_tis1c") + +[node name="NavPolygon2D" type="Polygon2D" parent="."] +polygon = PackedVector2Array(164, 56, 379, 23, 603, 53, 684, 152, 759, 255, 572, 293, 598, 166, 422, 106, 344, 158, 509, 312, 438, 454, 489, 504, 610, 342, 734, 323, 834, 199, 786, 85, 958, 43, 1117, 48, 1109, 194, 1137, 565, 916, 550, 887, 386, 965, 359, 962, 473, 1000, 494, 1002, 204, 928, 184, 916, 304, 837, 382, 835, 541, 752, 563, 732, 421, 627, 450, 592, 618, 335, 540, 295, 412, 361, 311, 190, 169, 194, 329, 278, 510, 132, 589, 133, 459, 77, 311, 48, 130) + +[node name="NavGraph" type="Node2D" parent="."] +script = ExtResource("1_5s4ud") + +[node name="CharacterBody2D" type="CharacterBody2D" parent="."] +position = Vector2(448, 182) +scale = Vector2(0.640001, 0.640001) +collision_mask = 3 +script = ExtResource("3_4rjft") +metadata/_edit_group_ = true + +[node name="NavigationAgent2D" type="NavigationAgent2D" parent="CharacterBody2D"] +debug_enabled = true + +[node name="Icon" type="Sprite2D" parent="CharacterBody2D"] +scale = Vector2(0.4, 0.4) +texture = ExtResource("3_ibv8u") + +[node name="CollisionShape2D" type="CollisionShape2D" parent="CharacterBody2D"] +shape = SubResource("CapsuleShape2D_gqxbx") + +[node name="TestPolygon2D" type="Polygon2D" parent="."] +visible = false +invert_border = 0.1 +polygon = PackedVector2Array(516, 218, 475, 104, 677, 121, 582, 247, 598, 333, 394, 315) diff --git a/pathfinding-algorithms/scenes/custom-solver/custom_graph_solver.gd b/pathfinding-algorithms/scenes/custom-solver/custom_graph_solver.gd new file mode 100644 index 0000000..c0519a0 --- /dev/null +++ b/pathfinding-algorithms/scenes/custom-solver/custom_graph_solver.gd @@ -0,0 +1,12 @@ +extends Node2D + +# @onready var navigation_polygon: Polygon2D = $TestPolygon2D +@onready var navigation_polygon: Polygon2D = $NavPolygon2D +@onready var graph: NavigationGraph = $NavGraph +@onready var character: CharacterBody2D = $CharacterBody2D + +func _ready() -> void: + navigation_polygon.hide() + var polygons = Geometry2D.decompose_polygon_in_convex(navigation_polygon.polygon) + + graph.erase_and_create_nodes_from_polygons(polygons)