Pretty much finished custom pathfinding

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

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 poly_center: Vector2        = navpoly.center()
-        var center_node: NavigationNode = add_node(poly_center.x, poly_center.y, -1)
+        # var center_node: NavigationNode = add_node(poly_center.x, poly_center.y, -1)
-        center_node.was_merged = true
+        # center_node.was_merged = true
-        polygon_nodes[navpoly].append(center_node)
+        # polygon_nodes[navpoly].append(center_node)
-        navpoly.center_node = 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():
+    if draw_nodes or draw_edges:
-        for to in get_connections(from):
+        for from in all_nodes():
-            draw_line(from.position, to.position, Color.RED, 1, false)
+            if draw_edges:
-        draw_circle(from.position, 5, Color.RED)
+                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():
+    if draw_polygons:
-        draw_colored_polygon(poly.polygon, Color(0.5, 0.4, 0.9, 0.3))
+        for poly in all_polygons():
-        draw_polyline(poly.polygon, Color.WHITE, 1, true)
+            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 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 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
+    # the target position is simple, just find the closest node in the polygon to the target position,
-    # in the polygon to roughly navigate to it, the alternate algorithm above will take care of the rest
+    # the alternate algorithm for within a polygon above will take care of the rest
-    var end_node: NavigationNode    = target_polygon.center_node
+    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
 

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)

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
 
-func _ready() -> void:
+var seleted_polygon: int = 0
-	navigation_polygon.hide()
-	var polygons = Geometry2D.decompose_polygon_in_convex(navigation_polygon.polygon)
 
-	graph.erase_and_create_nodes_from_polygons(polygons)
+
+func _ready() -> void:
+    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