From d6bf267bdcec75d77c04c806fe5b7c1425d4a0f0 Mon Sep 17 00:00:00 2001 From: Yan Wittmann Date: Tue, 12 Nov 2024 15:39:09 +0100 Subject: [PATCH] Pathfinding works now --- .../scenes/custom-solver/CustomNavMovement.gd | 19 ++++- .../scenes/custom-solver/NavigationGraph.gd | 84 ++++++++++++++----- .../scenes/custom-solver/PathfindingResult.gd | 21 +++++ .../custom-solver/custom-graph-solver.tscn | 4 +- .../custom-solver/custom_graph_solver.gd | 3 +- 5 files changed, 103 insertions(+), 28 deletions(-) create mode 100644 pathfinding-algorithms/scenes/custom-solver/PathfindingResult.gd diff --git a/pathfinding-algorithms/scenes/custom-solver/CustomNavMovement.gd b/pathfinding-algorithms/scenes/custom-solver/CustomNavMovement.gd index cc8854c..fa2b948 100644 --- a/pathfinding-algorithms/scenes/custom-solver/CustomNavMovement.gd +++ b/pathfinding-algorithms/scenes/custom-solver/CustomNavMovement.gd @@ -4,13 +4,24 @@ const MAX_SPEED: float = 300.0 const ACCELERATION: int = 2400 @onready var graph: NavigationGraph = $"../NavGraph" +var frozen: bool = false + func _physics_process(delta: float) -> void: - var movement: Vector2 = Vector2() + # check if space is pressed + if Input.is_action_just_pressed("ui_select"): + frozen = !frozen - var next_pos: Vector2 = graph.determine_next_position(position, get_global_mouse_position()) - movement = next_pos - global_position - if movement.length() < 20: + var movement: Vector2 = Vector2() + if frozen: + return + + var pathfinding_result: PathfindingResult = graph.determine_next_position(position, get_global_mouse_position()) + graph.draw_pathfinding_result(pathfinding_result) + + movement = pathfinding_result.next_position - global_position + + if pathfinding_result.is_next_target and movement.length() < 20: movement = -velocity * 0.2 else: movement = movement.normalized() * ACCELERATION * delta diff --git a/pathfinding-algorithms/scenes/custom-solver/NavigationGraph.gd b/pathfinding-algorithms/scenes/custom-solver/NavigationGraph.gd index d2f063a..a590f72 100644 --- a/pathfinding-algorithms/scenes/custom-solver/NavigationGraph.gd +++ b/pathfinding-algorithms/scenes/custom-solver/NavigationGraph.gd @@ -4,7 +4,8 @@ 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 polygon_nodes: Dictionary = {} +var latest_navigation_result: PathfindingResult = null func all_nodes() -> Array[NavigationNode]: @@ -137,10 +138,14 @@ func erase_and_create_nodes_from_polygons(new_polys: Array[PackedVector2Array]) # 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]) + 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: @@ -153,9 +158,24 @@ func _draw() -> void: 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 determine_next_position(current_position: Vector2, target_position: Vector2) -> Vector2: - # find both polygons + +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 @@ -165,32 +185,56 @@ func determine_next_position(current_position: Vector2, target_position: Vector2 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 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) - return closest_node.position + result.next_position = closest_node.position + return result - if not current_polygon or not target_polygon: - return current_position + # 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 - # check if the polygons are the same, if so, just return the target position + # if the current and target positions are in the same polygon, return the target position if current_polygon == target_polygon: - return target_position + 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_polygon: Array[NavigationNode] = get_nodes_in_polygon(current_polygon) + polygon_nodes[current_polygon].append(start_node) + for node in nodes_in_polygon: + add_connection(start_node, node) # 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 end_node: NavigationNode = target_polygon.center_node 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 + # 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: - return path[1].position # Next node in the path + # next node in the path + result.next_position = path[1].position + return result elif path.size() == 1: - return target_position # Directly reachable + # directly reachable + result.is_next_target = true + result.next_position = target_position else: - # No path found; return current position - return current_position + # 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: diff --git a/pathfinding-algorithms/scenes/custom-solver/PathfindingResult.gd b/pathfinding-algorithms/scenes/custom-solver/PathfindingResult.gd new file mode 100644 index 0000000..876e314 --- /dev/null +++ b/pathfinding-algorithms/scenes/custom-solver/PathfindingResult.gd @@ -0,0 +1,21 @@ +class_name PathfindingResult +extends Node + +var path: Array[NavigationNode] = [] +var next_position: Vector2 = Vector2.ZERO +var is_next_target: bool = false + +func _init() -> void: + path = [] + next_position = Vector2.ZERO + is_next_target = false + +func is_identical_to(other: PathfindingResult) -> bool: + if path.size() != other.path.size(): + return false + for i in range(path.size()): + if path[i] != other.path[i]: + return false + if next_position != other.next_position: + return false + return true diff --git a/pathfinding-algorithms/scenes/custom-solver/custom-graph-solver.tscn b/pathfinding-algorithms/scenes/custom-solver/custom-graph-solver.tscn index 9c49a16..2d2d80c 100644 --- a/pathfinding-algorithms/scenes/custom-solver/custom-graph-solver.tscn +++ b/pathfinding-algorithms/scenes/custom-solver/custom-graph-solver.tscn @@ -12,14 +12,14 @@ height = 49.9999 [node name="custom-graph-solver" type="Node2D"] script = ExtResource("1_tis1c") -[node name="NavPolygon2D" type="Polygon2D" parent="."] +[node name="NavPolygon2D_1" 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) +position = Vector2(310, 140) scale = Vector2(0.640001, 0.640001) collision_mask = 3 script = ExtResource("3_4rjft") diff --git a/pathfinding-algorithms/scenes/custom-solver/custom_graph_solver.gd b/pathfinding-algorithms/scenes/custom-solver/custom_graph_solver.gd index c0519a0..ecb1c2c 100644 --- a/pathfinding-algorithms/scenes/custom-solver/custom_graph_solver.gd +++ b/pathfinding-algorithms/scenes/custom-solver/custom_graph_solver.gd @@ -1,7 +1,6 @@ extends Node2D -# @onready var navigation_polygon: Polygon2D = $TestPolygon2D -@onready var navigation_polygon: Polygon2D = $NavPolygon2D +@onready var navigation_polygon: Polygon2D = $NavPolygon2D_1 @onready var graph: NavigationGraph = $NavGraph @onready var character: CharacterBody2D = $CharacterBody2D