Improve pathfinding

assets/Scenes/AI_core.tscn

@@ -25,9 +25,12 @@ texture = ExtResource( 1 )
 [node name="CollisionShape2D" type="CollisionShape2D" parent="."]
 position = Vector2( -0.5, 0 )
 shape = SubResource( 1 )
+disabled = true
 
 [node name="Ressource_depot" type="Area2D" parent="."]
 position = Vector2( 72, 0 )
+collision_layer = 0
+collision_mask = 2
 
 [node name="Sprite" type="Sprite" parent="Ressource_depot"]
 texture = ExtResource( 2 )

assets/Scenes/Actor.tscn

@@ -7,6 +7,7 @@ extents = Vector2( 6.5, 10 )
 
 [node name="Actor" type="RigidBody2D" groups=["Actor"]]
 z_index = 1
+collision_layer = 5
 mode = 2
 mass = 70.0
 linear_damp = 2.0
@@ -20,3 +21,4 @@ shape = SubResource( 1 )
 
 [node name="RayCast2D" type="RayCast2D" parent="."]
 enabled = true
+collision_mask = 4

assets/Scenes/Body.tscn

@@ -9,6 +9,7 @@ height = 6.0
 
 [node name="Body" type="RigidBody2D" groups=["Ressource"]]
 z_index = 1
+collision_layer = 7
 mass = 40.0
 script = ExtResource( 2 )
 _body_size = 11

assets/Scenes/Player.tscn

@@ -9,6 +9,7 @@ height = 3.0
 
 [node name="Player" type="RigidBody2D" groups=["Actor"]]
 z_index = 1
+collision_layer = 5
 mode = 2
 mass = 70.0
 linear_damp = 2.0
@@ -24,3 +25,4 @@ shape = SubResource( 1 )
 
 [node name="RayCast2D" type="RayCast2D" parent="."]
 enabled = true
+collision_mask = 4

assets/Scenes/Robot.tscn

@@ -1,17 +1,20 @@
-[gd_scene load_steps=5 format=2]
+[gd_scene load_steps=6 format=2]
 
 [ext_resource path="res://assets/cdda_sprites/mon_eyebot_season_winter.png" type="Texture" id=1]
 [ext_resource path="res://src/Robot.gd" type="Script" id=2]
 
-[sub_resource type="CapsuleShape2D" id=3]
+[sub_resource type="CircleShape2D" id=3]
 radius = 6.0
-height = 4.0
+
+[sub_resource type="CircleShape2D" id=4]
+radius = 9.0
 
 [sub_resource type="CircleShape2D" id=2]
 radius = 150.0
 
 [node name="Robot" type="RigidBody2D" groups=["Actor"]]
 z_index = 1
+collision_layer = 5
 mode = 2
 mass = 70.0
 linear_damp = 2.0
@@ -25,11 +28,20 @@ shape = SubResource( 3 )
 
 [node name="RayCast2D" type="RayCast2D" parent="."]
 enabled = true
+collision_mask = 4
 
 [node name="Line2D" type="Line2D" parent="."]
 width = 2.0
 default_color = Color( 0.894118, 0.623529, 0.909804, 1 )
 
+[node name="Collision_avoidance" type="Area2D" parent="."]
+collision_layer = 8
+collision_mask = 8
+
+[node name="Collision_avoidance_Shape2D" type="CollisionShape2D" parent="Collision_avoidance"]
+self_modulate = Color( 0.862745, 0.145098, 0.945098, 1 )
+shape = SubResource( 4 )
+
 [node name="Area2D" type="Area2D" parent="." groups=["Depot"]]
 modulate = Color( 0.796078, 0.913725, 0.219608, 1 )
 show_behind_parent = true
@@ -39,3 +51,5 @@ monitorable = false
 shape = SubResource( 2 )
 
 [node name="NavigationAgent2D" type="NavigationAgent2D" parent="."]
+
+[connection signal="area_entered" from="Collision_avoidance" to="." method="_on_Collision_avoidance_area_entered"]

project.godot

@@ -92,6 +92,13 @@ Exit={
  ]
 }
 
+[layer_names]
+
+2d_physics/layer_1="Collision"
+2d_physics/layer_2="Ressource"
+2d_physics/layer_3="Grab layer"
+2d_physics/layer_4="Collision avoidance"
+
 [physics]
 
 common/enable_pause_aware_picking=true

src/AI_core.gd

@@ -8,7 +8,7 @@ onready var depot:= $Ressource_depot
 onready var spawner:= $Spawner
 
 onready var world:= get_parent()
-onready var map:= get_node("../Navigation2D/TileMap")
+onready var map:= get_node("../TileMap")
 
 func _ready() -> void:
 	place_depot()
@@ -36,17 +36,20 @@ func update_ressources_in_depot()->void:
 			_ressources.append(body)
 
 
-#TODO: replace group check with smarter layer management
 func _on_Ressource_depot_body_entered(body: RigidBody2D) -> void:
-	#TODO: Wait for ressource to be released before counting it
-	if body.is_in_group("Ressource"):
-		#Disable collision with Actors
-		body.set_collision_layer_bit(0,false)
-		body.set_collision_mask_bit(0,false)
-
-		body.set_modulate(Color.dimgray)
-
-		_ressources.append(body)
+	#Make robot release the ressource and go back fetching more
+	for actor in body.get_grabbers():
+		if actor.is_robot():
+			actor.release_grab()
+			actor.new_path()
+
+	#Disable collision with Actors
+	body.set_collision_layer_bit(0,false)
+	body.set_collision_mask_bit(0,false)
+
+	body.set_modulate(Color.dimgray)
+
+	_ressources.append(body)
 
 func make_new_bot()->void:
 	var new_robot = robot_template.instance()

src/Actor.gd

@@ -27,9 +27,8 @@ func release_grab()->void:
 		emit_signal("grab_release", _grabbed_object,self)
 		_grabbed_object = null
 
-func move(force: Vector2)-> Vector2:
+func move(force: Vector2)-> void:
 	apply_central_impulse(force)
-	return force
 
 func get_grab_target_pos(pos_to_try: Vector2,for_line_draw:=false)->Vector2:
 	var pos:= to_local( position)

src/Body.gd

@@ -42,3 +42,6 @@ func get_body_size()->int:
 
 func is_grabbed()->bool:
 	return !_grabbers.empty()
+
+func get_grabbers()->Array:
+	return _grabbers

src/Player.gd

@@ -20,9 +20,11 @@ func _draw() -> void:
 func _physics_process(_delta) ->void:
 	var direction:= Vector2(Input.get_axis("move_left","move_right"), Input.get_axis("move_up","move_down"))
 	move_force = _speed * direction
-	move_force = move(move_force)
+	move(move_force)
 
 	if trying_grab:
 		if try_grab(get_global_mouse_position()) && Input.is_action_just_pressed("interact"):
 			confirm_grab()
 
+func is_robot()->bool:
+	return false

src/Robot.gd

@@ -1,25 +1,39 @@
 extends Actor
 
 var _current_path: PoolVector2Array
+var _curr_dest: Vector2
 
 var grab_target: Vector2
 
 var _home_depot_pos: Vector2
 var _home_depot_area: Area2D
 
-onready var Nav:= get_node("../Navigation2D")
+var _last_pos: Vector2
+var _stuck_counter:= 0.0
+var _heading: float
+
+export var _step_size:= 40
+
 onready var line:= $Line2D
 onready var awarness_area:= $Area2D
+onready var collision_avoidance_area:= $Collision_avoidance
 onready var collision_shape:= $CollisionShape2D
-#TODO: figure out navigation agent
-#onready var nav_agent:= $NavigationAgent2D
+onready var nav_agent:= $NavigationAgent2D
+
+onready var map := get_node("../TileMap")
+
+var STUCK_THRESHOLD:= 0.4
 
 func _ready() -> void:
 	set_process(true)
 
 	var AI_core = get_tree().get_nodes_in_group("Core")[0]
 	_home_depot_area = AI_core.get_child(2)
 	_home_depot_pos = _home_depot_area.global_position
+
+	_last_pos = global_position
+	pick_heading()
+	new_path()
 
 func _draw() -> void:
 	if trying_grab || _grabbed_object!= null:
@@ -28,7 +42,9 @@ func _draw() -> void:
 
 func _process(_delta: float) -> void:
 	line.global_position = Vector2.ZERO
-
+	update()
+
+func _physics_process(_delta: float) -> void:
 	var closest_ressource = get_closest_ressource_in_awarness()
 	if closest_ressource!= null && !is_carrying():
 		grab_target = closest_ressource.position
@@ -44,35 +60,47 @@ func _process(_delta: float) -> void:
 	if is_carrying():
 		if close_enough(_home_depot_pos):
 			release_grab()
+			pick_heading()
 			new_path()
 
 
 	if _current_path.empty():
 		new_path()
-	var direction:= (_current_path[0] - position).normalized()
-	move_force = _speed * direction
-	move_force = move(move_force)
+
+	if position.distance_to(_last_pos)<= 0.2:
+		_stuck_counter += _delta
+	else:
+		_stuck_counter = 0.0
+
+	_last_pos = global_position	
 
 
-	if close_enough(_current_path[0]):
+	var next_location= _current_path[0]
+	var direction:= position.direction_to(next_location)
+	var target_vel:= direction * _speed
+	move(target_vel)
+
+	#TODO: unstuck
+	if _stuck_counter > STUCK_THRESHOLD:
+		pass
+
+	if close_enough(next_location):
 		_current_path.remove(0)
 		line.remove_point(0)
-
-	update()
 
-func set_path_to(dest: Vector2)->void:
-	_current_path = make_path(dest)
+func pick_heading():
+	_heading = rand_range(0,2*PI)
 
 func new_path()->void:
-	_current_path = make_path(pick_destination())
+	_curr_dest = pick_destination()
+	_current_path = make_path(_curr_dest)
 
 func make_path(destination: Vector2)->PoolVector2Array:
-	var path= Nav.get_simple_path(position,destination,false)
-	#TODO: figure out navigation agent
-	#var path = Navigation2DServer.map_get_path(nav_agent.get_navigation_map() ,position,destination, false)
+	var path = Navigation2DServer.map_get_path(nav_agent.get_navigation_map() ,position,destination, false)
 	line.points = path
 	return path
 
+
 func close_enough(vect: Vector2, bod = null)->bool:
 	if bod == null:
 		return position.distance_to(vect)<=_body_size/2.0
@@ -90,12 +118,15 @@ func pick_destination()->Vector2:
 		if closest_body != null:
 			return closest_body.position
 		else:
-			var home_direction:= (_home_depot_pos - position).normalized()
-			dest = position - home_direction*40
+			var cell_index:= -1
+			while cell_index != 1:
+				var rand_angle:= rand_range(_heading-PI/3, _heading+ PI/3)
+				var rand_vector:= Vector2(cos(rand_angle),sin(rand_angle))
+				dest = global_position + rand_vector * _step_size
+				cell_index = map.get_cellv(map.world_to_map(dest))
 	else:
 		dest = _home_depot_pos
 
-
 	return dest
 
 func get_closest_ressource_in_awarness()->Body:
@@ -105,3 +136,18 @@ func get_closest_ressource_in_awarness()->Body:
 			if body.is_in_group("Ressource"):
 				closest_body= body
 	return closest_body
+
+func _on_Collision_avoidance_area_entered(area: Area2D) -> void:
+	var into_colllider= area.global_position - global_position
+	var tangent_out = into_colllider.tangent().normalized() * _body_size
+	var new_point = area.global_position + tangent_out
+	_current_path.insert(0,new_point)
+	line.add_point(new_point,0)
+
+
+func _on_Collision_avoidance_area_exited(area: Area2D) -> void:
+	_current_path = make_path(_curr_dest)
+	line.points = _current_path
+
+func is_robot()->bool:
+	return true

src/World.gd

@@ -15,7 +15,7 @@ onready var timer:= $Timer
 
 func _ready() -> void:
 	connect_to_actors()
-	for k in range(0,1000):
+	for _k in range(0,1000):
 		_add_ressource()
 
 func connect_to_actors() ->void:
@@ -30,9 +30,10 @@ func connect_to_actor(body: Actor)->void:
 
 
 func _on_grab_ray_hit(collider: Object, emitter: Object)->void:
-	#if !collider.is_class("Body"):
-	#	print("%s tried to grab a non body %s" % [emitter.name, collider.name])
-	#	return
+	if !collider.is_class("RigidBody2D"):
+		print("%s tried to grab a non body %s" % [emitter.name, collider.name])
+		print("Check that collision layers are correct, layer 3 is for grabbing only")
+		return
 	collider.add_grabber(emitter)
 	emitter.set_grabbed_object(collider)