Pathfinding works now

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

pathfinding-algorithms/scenes/custom-solver/NavigationGraph.gd

@@ -5,6 +5,7 @@ extends Node2D
 var navigation_nodes: Dictionary = {}
 # type is Dictionary[CNavigationPolygon, Array[NavigationNode]]
 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)):
+        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_in_polygon[i], nodes_in_polygon[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 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:

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

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")

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