Unit Instability Issues

Godot Version

Godot 3.5

Question

My question is how would you handle a pikmin like unit following a player up to 100units, or more if possible, in the Godot Engine. I tried making pikmin units into dynamic groups, managed by a group manager, and the groups take all the pikmin units in each group and follow the player, with flocking behavious, and other attempts at fixing the issues… The issue is I get rampant instability above 8-10 units when I’d like to achieve at least 100 units just like the original Pikmen. I will share my code below, and I know it is not perfect, this is my first attempt at a game with this much going on.

if anyone wants to help me solve the problem I’d appreciate it.
Wempik ManagerV


export var group_size: int = 10
export var wempik_group_scene: PackedScene

var groups: Array = []
var player: Node = null

func _ready():
	player = get_tree().get_root().find_node("Player", true, false)
	if player:
		print("Player found: ", player.name)
	else:
		print("Player not found!")
		return
	
	create_new_group()  # Create the initial group

func _process(delta: float):
	if player:
		for i in range(groups.size()):
			if i == 0:
				# The first group follows the player
				groups[i].update_group(delta, player.global_transform.origin, groups)
			else:
				# Subsequent groups follow the previous group
				groups[i].update_group(delta, groups[i-1].calculate_group_center(), groups)

func create_new_group():
	var new_group = wempik_group_scene.instance()
	add_child(new_group)
	groups.append(new_group)
	new_group.name = "WempikGroup_%d" % groups.size()
	new_group.add_to_group("WempikGroups")

func add_wempik_to_nearest_group(wempik: Node) -> Node:
	if groups.size() == 0 or groups[-1].wempiks.size() >= group_size:
		create_new_group()

	var nearest_group = null
	var shortest_distance = INF

	for group in groups:
		if group and group.is_inside_tree():
			var group_position = group.global_transform.origin
			var wempik_position = wempik.global_transform.origin
			var distance = wempik_position.distance_to(group_position)
			if distance < shortest_distance and group.wempiks.size() < group_size:
				shortest_distance = distance
				nearest_group = group

	if nearest_group:
		nearest_group.add_wempik(wempik)
	else:
		create_new_group()
		groups[-1].add_wempik(wempik)
		nearest_group = groups[-1]

	return nearest_group

WempikGroup

extends Node

export var group_size: int = 10
export var follow_distance: float = 5.0
export var follow_speed: float = 3.0
export var min_distance: float = 2.0
export var gravity: float = 9.8
export var separation_weight: float = 2.5
export var alignment_weight: float = 5.0
export var cohesion_weight: float = 5.0

var wempiks: Array = []
var target_position: Vector3 = Vector3.ZERO  # The position this group is following

func _ready():
	disable_collisions_within_group(true)

func add_wempik(wempik: Node):
	if wempiks.size() < group_size:
		wempiks.append(wempik)
		wempik.group = self
	else:
		print("Group is full, cannot add more Wempiks")

func remove_wempik(wempik: Node):
	if wempiks.has(wempik):
		wempiks.erase(wempik)
		wempik.group = null

func update_group(delta: float, target_position: Vector3, all_groups: Array):
	if wempiks.size() > 0:
		self.target_position = target_position
		var group_center = calculate_group_center()
		move_group_towards_target(delta, group_center)
		flocking_behavior(delta)
		apply_gravity(delta)

func calculate_group_center() -> Vector3:
	var center = Vector3.ZERO
	for wempik in wempiks:
		center += wempik.global_transform.origin
	return center / wempiks.size()

func move_group_towards_target(delta: float, group_center: Vector3):
	var direction_to_target = (target_position - group_center).normalized()
	var target_pos = target_position - direction_to_target * follow_distance

	for i in range(wempiks.size()):
		var offset = Vector3((i % 2) * min_distance - min_distance / 2, 0, ((i / 2) % 2) * min_distance - min_distance / 2)
		var move_target = target_pos + offset
		var wempik = wempiks[i]
		var move_dir = (move_target - wempik.global_transform.origin).normalized() * follow_speed
		var new_velocity = wempik.velocity.linear_interpolate(move_dir * 100, 0.1)
		new_velocity.y = -gravity * delta
		wempik.move_and_slide(new_velocity, Vector3.UP)

func flocking_behavior(delta: float):
	for wempik in wempiks:
		var separation = Vector3.ZERO
		var alignment = Vector3.ZERO
		var cohesion = Vector3.ZERO
		var neighbors = 0

		for other_wempik in wempiks:
			if wempik != other_wempik:
				var distance = wempik.global_transform.origin.distance_to(other_wempik.global_transform.origin)
				if distance < min_distance:
					separation += (wempik.global_transform.origin - other_wempik.global_transform.origin).normalized() / distance
					alignment += other_wempik.velocity
					cohesion += other_wempik.global_transform.origin
					neighbors += 1

		if neighbors > 0:
			alignment /= neighbors
			cohesion = (cohesion / neighbors - wempik.global_transform.origin).normalized()

			var separation_force = separation * separation_weight
			var alignment_force = alignment.normalized() * alignment_weight
			var cohesion_force = cohesion * cohesion_weight

			var flocking_force = separation_force + alignment_force + cohesion_force
			var new_velocity = wempik.velocity.linear_interpolate(flocking_force * 100, 0.1)
			new_velocity.y = -gravity * delta
			wempik.move_and_slide(new_velocity, Vector3.UP)

func apply_gravity(delta: float):
	for wempik in wempiks:
		if not wempik.is_on_floor():
			wempik.velocity.y -= gravity * delta

func disable_collisions_within_group(disable: bool):
	for wempik in wempiks:
		for other_wempik in wempiks:
			if wempik != other_wempik:
				wempik.set_collision_mask_bit(1, not disable)
				wempik.set_collision_layer_bit(1, not disable)
				other_wempik.set_collision_mask_bit(1, not disable)
				other_wempik.set_collision_layer_bit(1, not disable)

WempikUnit

extends KinematicBody

# Parameters
export var follow_distance: float = 5.0
export var follow_speed: float = 3.0
export var gravity: float = 9.8
export var deceleration: float = 2.0
export var collision_check_distance: float = 1.0
export var post_delivery_radius: float = 2.0
export var ray_length: float = 1.5
export var attack_speed: float = 300.0
export var retreat_speed: float = 150.0
export var retreat_distance: float = 3.0
export var attack_interval: float = 1.0
export var attack_damage: int = 5

onready var health = $HealthComponent

var player: Node = null
var home_base: Node = null
var navigation: Navigation = null
var wempik_manager: Node = null
var group: Node = null

onready var follow_area = $FollowArea
onready var pickup_area = $PickupArea
onready var follow_bounds = $FollowArea
onready var collision_area = $CollisionArea
onready var attack_zone = $AttackZone

var is_held: bool = false
var velocity: Vector3 = Vector3.ZERO
var is_throwing: bool = false
var is_delivering: bool = false
var held_item: Node = null
var player_in_bounds: bool = true
var is_summoned: bool = false
var summoned_by_spawner: bool = false
var colliding_wempiks: Array = []
var path: PoolVector3Array = []
var path_index: int = 0
var post_delivery_position: Vector3 = Vector3.ZERO

var attack_timer = 0.0
var is_attacking = false
var is_retreating = false
var original_position: Vector3 = Vector3.ZERO
var target_enemy = null

var move_target: Vector3 = Vector3.ZERO
var moving_away: bool = false
var move_speed: float = 8.0
var is_whistled: bool = false

var previous_position: Vector3 = Vector3.ZERO
var stuck_timer: float = 0.0
var stuck_threshold: float = 0.1
var stuck_time_limit: float = 1.0

func _ready():
	
	player = get_tree().get_root().find_node("Player", true, false)
	home_base = get_tree().get_root().find_node("HomeBase", true, false)
	navigation = get_tree().get_root().find_node("Navigation", true, false)
	# ... rest of the function remains unchanged

	wempik_manager = get_tree().get_root().find_node("WempikManager", true, false)
	
	if health:
		health.connect("health_changed", self, "_on_health_changed")
		health.connect("died", self, "_on_died")
	
	pickup_area.connect("body_entered", self, "_on_pickup_area_entered")
	follow_bounds.connect("body_entered", self, "_on_follow_bounds_entered")
	follow_bounds.connect("body_exited", self, "_on_follow_bounds_exited")
	collision_area.connect("area_entered", self, "_on_collision_area_entered")
	collision_area.connect("area_exited", self, "_on_collision_area_exited")

	if should_add_to_group_on_spawn():
		add_to_nearest_group()




func should_add_to_group_on_spawn() -> bool:
	# Add your custom logic to determine if the Wempik should be added to the group on spawn
	return true

func add_to_nearest_group():
	if wempik_manager:
		var nearest_group = wempik_manager.call("add_wempik_to_nearest_group", self)
		if nearest_group and nearest_group.is_in_group("WempikGroups"):
			nearest_group.call("add_wempik", self)
			group = nearest_group
		else:
			print("No WempikGroup found to add Wempik: ", self.name)
	else:
		print("WempikManager not found")

func remove_from_groups(wempik: String):
	if group:
		group.call("remove_wempik", self)
		group = null

func _physics_process(delta: float):
	if is_held or is_delivering:
		return

	if moving_away:
		move_away(delta)
	elif is_throwing:
		apply_gravity(delta)
		move_and_slide(velocity, Vector3.UP)
		if is_on_floor():
			is_throwing = false
			detect_collision_with_enemy()
			if target_enemy:
				is_attacking = true
	elif is_attacking:
		attack_enemy(delta)
	elif is_retreating:
		retreat(delta)
	elif post_delivery_position != Vector3.ZERO:
		move_towards_post_delivery_position(delta)
	else:
		stop_movement()
		attack_timer -= delta
		if not is_attacking and not is_retreating:
			detect_collision_with_enemy()

func move_to_position(target_position: Vector3, speed: float):
	move_target = target_position
	move_speed = speed
	moving_away = true

func stop_moving():
	moving_away = false
	velocity = Vector3.ZERO

func move_away(delta: float):
	var direction = (move_target - global_transform.origin).normalized()
	var move_vector = direction * move_speed * delta * 100
	move_and_slide(move_vector, Vector3.UP)
	if global_transform.origin.distance_to(move_target) < 0.1:
		stop_moving()

func adjust_path_points(path: PoolVector3Array) -> PoolVector3Array:
	var adjusted_path = PoolVector3Array()
	var space_state = get_world().direct_space_state
	for point in path:
		var avoid_direction = Vector3.ZERO
		var origin = point
		var directions = [
			Vector3(1, 0, 0), Vector3(-1, 0, 0), Vector3(0, 0, 1), Vector3(0, 0, -1),
			Vector3(0.707, 0, 0.707), Vector3(-0.707, 0, 0.707), Vector3(0.707, 0, -0.707), Vector3(-0.707, 0, -0.707)
		]
		for direction in directions:
			var ray_end = origin + direction.normalized() * ray_length
			var result = space_state.intersect_ray(origin, ray_end)
			if result:
				avoid_direction -= direction.normalized()
		var adjusted_point = origin + avoid_direction * ray_length
		adjusted_path.append(adjusted_point)
	return adjusted_path

func generate_path_to_home_base():
	path = navigation.get_simple_path(global_transform.origin, home_base.global_transform.origin, true)
	path = adjust_path_points(path)
	path_index = 0

func move_towards_home_base(delta: float):
	if is_delivering and home_base:
		remove_from_group(self.name)  # Remove from group when starting delivery
		if path.empty():
			generate_path_to_home_base()
		if path.size() > 0 and path_index < path.size():
			var target_position = path[path_index]
			var direction = (target_position - global_transform.origin).normalized()
			var move_dir = direction * follow_speed * delta * 100
			move_and_slide(move_dir, Vector3.UP)

			if global_transform.origin.distance_to(target_position) < 1.0:
				path_index += 1
			if is_stuck():
				generate_path_to_home_base()
			velocity = move_dir
			var drop_off_point = home_base.get_node("DropOffPoint")
			var distance_to_drop_off = global_transform.origin.distance_to(drop_off_point.global_transform.origin)
			if distance_to_drop_off < 1.0:
				drop_off_item()
				calculate_post_delivery_position()
		else:
			var distance_to_home = global_transform.origin.distance_to(home_base.global_transform.origin)
			if distance_to_home < 1.0:
				drop_off_item()
				calculate_post_delivery_position()
			else:
				generate_path_to_home_base()

func drop_off_item():
	if held_item:
		var drop_off_point = home_base.get_node("DropOffPoint")
		var distance_to_drop_off = global_transform.origin.distance_to(drop_off_point.global_transform.origin)
		if distance_to_drop_off < 1.0:
			if home_base.has_method("handle_dropped_item"):
				home_base.call("handle_dropped_item", held_item)
			held_item.queue_free()
			reset_after_delivery()

func reset_after_delivery():
	velocity = Vector3.ZERO
	path_index = 0
	is_retreating = false
	is_attacking = false
	is_held = false
	moving_away = false
	move_speed = follow_speed  # Reset to default move speed

func start_delivery(item: Node):
	if item:
		held_item = item
		is_delivering = true
		item.get_parent().remove_child(item)
		add_child(item)
		remove_from_group(self.name)
		item.global_transform.origin = global_transform.origin + Vector3(0, 2, 0)
		if item is RigidBody:
			item.mode = RigidBody.MODE_KINEMATIC
		item.set_collision_layer(0)
		item.set_collision_mask(0)
		set_process(true)

func apply_gravity(delta: float):
	if not is_on_floor():
		velocity.y -= gravity * delta
	else:
		velocity.y = 0

func is_on_floor() -> bool:
	var collision = move_and_collide(Vector3.DOWN * 0.1)
	return collision and collision.collider != null

func set_held_state(held: bool):
	is_held = held
	is_throwing = false
	if is_held:
		remove_from_group(self.name)  # Remove from group when held
		global_transform.origin = player.global_transform.origin + Vector3(0, 1, 2)
		set_process(false)
	else:
		set_process(true)
		velocity = Vector3.ZERO

func initiate_throw(throw_velocity: Vector3):
	is_held = false
	is_throwing = true
	velocity = throw_velocity
	remove_from_group(self.name)  # Remove from group when thrown

func respond_to_whistle():
	is_whistled = true
	is_summoned = true

	# Ensure the Wempik is not attacking or retreating
	if is_attacking or is_retreating:
		is_attacking = false
		is_retreating = false

	add_to_nearest_group()  # Add to nearest group when whistled

func _on_pickup_area_entered(body):
	if is_throwing and held_item == null and body.is_in_group("Items"):
		held_item = body
		is_throwing = false
		start_delivery(body)

func _on_follow_bounds_entered(area):
	if area.name == "Player":
		player_in_bounds = true

func _on_follow_bounds_exited(area):
	if area.name == "Player":
		player_in_bounds = false
		stop_movement()
		is_summoned = false

func _on_collision_area_entered(area):
	if is_delivering or is_held:
		return
	if area.is_in_group("Wempik"):
		colliding_wempiks.append(area)
	elif area.is_in_group("Enemy"):
		target_enemy = area
		original_position = global_transform.origin
		if is_throwing:
			is_throwing = false
		is_attacking = true
		attack_enemy(0)

func _on_collision_area_exited(area):
	if is_delivering or is_held:
		return
	if area.is_in_group("Wempik"):
		colliding_wempiks.erase(area)
	elif area == target_enemy:
		target_enemy = null
		is_attacking = false
		is_retreating = false

func detect_collision_with_enemy():
	if attack_zone is Area:
		for body in attack_zone.get_overlapping_bodies():
			if body.is_in_group("Enemy"):
				if not is_attacking and not is_retreating:
					var attack_direction = (body.global_transform.origin - global_transform.origin).normalized()
					original_position = global_transform.origin - attack_direction * retreat_distance  # Calculate retreat position
				target_enemy = body
				is_attacking = true
				break

func attack_enemy(delta):
	if target_enemy and is_instance_valid(target_enemy):
		var target_position = target_enemy.global_transform.origin
		var direction = (target_position - global_transform.origin).normalized()
		var move_vector = direction * attack_speed * delta * 100
		move_and_slide(move_vector, Vector3.UP)

		# Continuously check for collisions with the enemy within the AttackZone
		detect_collision_with_enemy()

		# Check if close enough to attack
		if global_transform.origin.distance_to(target_position) < 2.0:
			if target_enemy.has_method("take_damage"):
				target_enemy.call("take_damage", attack_damage)
			is_attacking = false
			is_retreating = true
			attack_timer = attack_interval
	else:
		target_enemy = null
		is_attacking = false
		is_retreating = false

func retreat(delta):
	var direction = (original_position - global_transform.origin).normalized()
	var move_vector = direction * retreat_speed * delta * 100
	move_and_slide(move_vector, Vector3.UP)

	# Check if retreated far enough
	if global_transform.origin.distance_to(original_position) < retreat_distance * 100:
		is_retreating = false
		yield(get_tree().create_timer(attack_interval), "timeout")
		is_attacking = true

func move_towards_post_delivery_position(delta):
	var direction = (post_delivery_position - global_transform.origin).normalized()
	var move_dir = direction * follow_speed
	move_and_slide(move_dir, Vector3.UP)

	if global_transform.origin.distance_to(post_delivery_position) < 1.0:
		post_delivery_position = Vector3.ZERO

func stop_movement():
	velocity = Vector3.ZERO
	if colliding_wempiks.size() > 0:
		move_away_from_collision()
	else:
		move_and_slide(velocity, Vector3.UP)

func is_stuck() -> bool:
	if previous_position.distance_to(global_transform.origin) < stuck_threshold:
		stuck_timer += get_process_delta_time()
		if stuck_timer > stuck_time_limit:
			stuck_timer = 0.0
			return true
	else:
		stuck_timer = 0.0
	previous_position = global_transform.origin
	return false

func calculate_post_delivery_position():
	var angle = randf() * 2.0 * PI
	post_delivery_position = home_base.global_transform.origin + Vector3(cos(angle) * post_delivery_radius, 0, sin(angle) * post_delivery_radius)

func move_away_from_collision():
	if colliding_wempiks.size() > 0:
		var other_wempik = colliding_wempiks[0]
		var other_velocity = other_wempik.velocity
		if other_velocity.length() > 0:
			var move_dir = -other_velocity.normalized() * follow_speed * get_process_delta_time() * 100
			move_and_slide(move_dir, Vector3.UP)
			velocity = move_dir

Instability Issue
Thank you for anyone’s help on the topic, I’d appreciate any help on different approaches I could try. I am not against starting from scratch on the pikmen units as well.