From 2e9b1264f047caa84c907e255f36d63a07252d7b Mon Sep 17 00:00:00 2001 From: Yan Wittmann Date: Tue, 12 Nov 2024 16:07:31 +0100 Subject: [PATCH] Pretty much finished custom pathfinding --- pathfinding-algorithms/project.godot | 18 +++++++ .../scenes/custom-solver/NavigationGraph.gd | 52 ++++++++++++------- .../custom-solver/custom-graph-solver.tscn | 17 +++--- .../custom-solver/custom_graph_solver.gd | 40 ++++++++++++-- 4 files changed, 98 insertions(+), 29 deletions(-) diff --git a/pathfinding-algorithms/project.godot b/pathfinding-algorithms/project.godot index 2eff004..d992e90 100644 --- a/pathfinding-algorithms/project.godot +++ b/pathfinding-algorithms/project.godot @@ -15,6 +15,24 @@ run/main_scene="res://scenes/custom-solver/custom-graph-solver.tscn" config/features=PackedStringArray("4.3", "Forward Plus") config/icon="res://icon.svg" +[input] + +draw_toggle_polygon={ +"deadzone": 0.5, +"events": [Object(InputEventKey,"resource_local_to_scene":false,"resource_name":"","device":-1,"window_id":0,"alt_pressed":false,"shift_pressed":false,"ctrl_pressed":false,"meta_pressed":false,"pressed":false,"keycode":0,"physical_keycode":80,"key_label":0,"unicode":112,"location":0,"echo":false,"script":null) +] +} +draw_toggle_nodes={ +"deadzone": 0.5, +"events": [Object(InputEventKey,"resource_local_to_scene":false,"resource_name":"","device":-1,"window_id":0,"alt_pressed":false,"shift_pressed":false,"ctrl_pressed":false,"meta_pressed":false,"pressed":false,"keycode":0,"physical_keycode":78,"key_label":0,"unicode":110,"location":0,"echo":false,"script":null) +] +} +draw_toggle_edges={ +"deadzone": 0.5, +"events": [Object(InputEventKey,"resource_local_to_scene":false,"resource_name":"","device":-1,"window_id":0,"alt_pressed":false,"shift_pressed":false,"ctrl_pressed":false,"meta_pressed":false,"pressed":false,"keycode":0,"physical_keycode":69,"key_label":0,"unicode":101,"location":0,"echo":false,"script":null) +] +} + [navigation] 2d/default_edge_connection_margin=40.0 diff --git a/pathfinding-algorithms/scenes/custom-solver/NavigationGraph.gd b/pathfinding-algorithms/scenes/custom-solver/NavigationGraph.gd index a590f72..99e89f1 100644 --- a/pathfinding-algorithms/scenes/custom-solver/NavigationGraph.gd +++ b/pathfinding-algorithms/scenes/custom-solver/NavigationGraph.gd @@ -6,6 +6,9 @@ var navigation_nodes: Dictionary = {} # type is Dictionary[CNavigationPolygon, Array[NavigationNode]] var polygon_nodes: Dictionary = {} var latest_navigation_result: PathfindingResult = null +var draw_polygons: bool = true +var draw_nodes: bool = false +var draw_edges: bool = false func all_nodes() -> Array[NavigationNode]: @@ -116,11 +119,11 @@ func erase_and_create_nodes_from_polygons(new_polys: Array[PackedVector2Array]) 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 + # 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 @@ -149,14 +152,18 @@ func connect_all_nodes(nodes: Array[NavigationNode]) -> void: 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) + if draw_nodes or draw_edges: + for from in all_nodes(): + if draw_edges: + for to in get_connections(from): + draw_line(from.position, to.position, Color.RED, 1, false) + if draw_nodes: + 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) + if draw_polygons: + 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) if latest_navigation_result != null: if latest_navigation_result.path.size() > 1: @@ -205,15 +212,24 @@ func determine_next_position(current_position: Vector2, target_position: Vector2 # 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) + var start_node: NavigationNode = add_node(current_position.x, current_position.y, -1) + var nodes_in_current_polygon: Array[NavigationNode] = get_nodes_in_polygon(current_polygon) polygon_nodes[current_polygon].append(start_node) - for node in nodes_in_polygon: + for node in nodes_in_current_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 + # the target position is simple, just find the closest node in the polygon to the target position, + # the alternate algorithm for within a polygon above will take care of the rest + var end_node: NavigationNode = null + var min_distance: float = INF + + var nodes_in_target_polygon: Array[NavigationNode] = get_nodes_in_polygon(target_polygon) + for node in nodes_in_target_polygon: + var distance: float = target_position.distance_to(node.position) + if distance < min_distance: + min_distance = distance + end_node = node + var path: Array[NavigationNode] = dijkstra(start_node, end_node) result.path = path diff --git a/pathfinding-algorithms/scenes/custom-solver/custom-graph-solver.tscn b/pathfinding-algorithms/scenes/custom-solver/custom-graph-solver.tscn index 2d2d80c..cbaeef5 100644 --- a/pathfinding-algorithms/scenes/custom-solver/custom-graph-solver.tscn +++ b/pathfinding-algorithms/scenes/custom-solver/custom-graph-solver.tscn @@ -9,12 +9,22 @@ radius = 24.9999 height = 49.9999 -[node name="custom-graph-solver" type="Node2D"] +[node name="custom-graph-solver" type="Node2D" node_paths=PackedStringArray("navigation_polygon")] script = ExtResource("1_tis1c") +navigation_polygon = [NodePath("NavPolygon2D_1"), NodePath("SmallPolygon2D"), NodePath("NavPolygon2D_2")] + +[node name="NavPolygon2D_2" type="Polygon2D" parent="."] +polygon = PackedVector2Array(474, 298, 379, 295, 381, 261, 479, 98, 422, 71, 336, 203, 334, 47, 255, 43, 49, 42, 48, 119, 55, 247, 109, 244, 112, 126, 253, 119, 249, 176, 153, 177, 153, 240, 253, 236, 289, 322, 69, 300, 74, 390, 475, 400, 484, 463, 104, 458, 108, 630, 222, 633, 222, 542, 476, 569, 748, 642, 984, 642, 979, 580, 911, 563, 910, 495, 831, 495, 847, 563, 701, 493, 656, 402, 926, 414, 995, 516, 1110, 517, 985, 319, 774, 292, 765, 244, 884, 266, 896, 206, 764, 167, 675, 226, 703, 295, 652, 295, 554, 179, 805, 106, 935, 128, 998, 249, 1105, 257, 934, 19, 616, 61, 476, 167) [node name="NavPolygon2D_1" type="Polygon2D" parent="."] +visible = false 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="SmallPolygon2D" type="Polygon2D" parent="."] +visible = false +invert_border = 0.1 +polygon = PackedVector2Array(516, 218, 475, 104, 677, 121, 582, 247, 598, 333, 394, 315) + [node name="NavGraph" type="Node2D" parent="."] script = ExtResource("1_5s4ud") @@ -34,8 +44,3 @@ 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 index ecb1c2c..0833d76 100644 --- a/pathfinding-algorithms/scenes/custom-solver/custom_graph_solver.gd +++ b/pathfinding-algorithms/scenes/custom-solver/custom_graph_solver.gd @@ -1,11 +1,41 @@ extends Node2D -@onready var navigation_polygon: Polygon2D = $NavPolygon2D_1 +@export var navigation_polygon: Array[Polygon2D] = [] + @onready var graph: NavigationGraph = $NavGraph @onready var character: CharacterBody2D = $CharacterBody2D +var seleted_polygon: int = 0 + + func _ready() -> void: - navigation_polygon.hide() - var polygons = Geometry2D.decompose_polygon_in_convex(navigation_polygon.polygon) - - graph.erase_and_create_nodes_from_polygons(polygons) + for polygon in navigation_polygon: + polygon.hide() + + update_polygon() + + +func update_polygon(): + var polygons: Array[PackedVector2Array] = Geometry2D.decompose_polygon_in_convex(navigation_polygon[seleted_polygon].polygon) + graph.erase_and_create_nodes_from_polygons(polygons) + + +func _process(delta: float) -> void: + # check for left and right arrow keys to change the selected polygon (0, len(navigation_polygon)-1) + if Input.is_action_just_pressed("ui_right"): + seleted_polygon += 1 + if seleted_polygon >= len(navigation_polygon): + seleted_polygon = 0 + update_polygon() + + if Input.is_action_just_pressed("ui_left"): + seleted_polygon -= 1 + if seleted_polygon < 0: + seleted_polygon = len(navigation_polygon) - 1 + update_polygon() + + if Input.is_action_just_pressed("draw_toggle_polygon"): + graph.draw_polygons = !graph.draw_polygons + if Input.is_action_just_pressed("draw_toggle_nodes") or Input.is_action_just_pressed("draw_toggle_edges"): + graph.draw_nodes = !graph.draw_nodes + graph.draw_edges = !graph.draw_edges