Started custom navmesh logic

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"
 

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

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

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 []

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

@@ -0,0 +1,4 @@
+class_name NavigationNode
+extends Node2D
+
+var was_merged: bool = false

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)

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)