TileNavigation.gd

gistfile1.txt

extends Node

class_name TileNavigation

var discarded: Array
export var speed: float = 60.0

func pos_to_tile_pos(pos: Vector2, cell_size: Vector2 ) -> Vector2: 
	var x : int = floor(pos.x / cell_size.x) * cell_size.x
	var y : int = floor(pos.y / cell_size.y) * cell_size.y
	return Vector2(x, y)

# A star, stops on first hit
func start_path_from_to(
						character, 
						allowed_tiles: TileMap, 
						movables: Array, 
						movables_allowed_tiles: TileMap, 
						destination_arrow: DestinationArrow,
						end: Vector2
	) -> void:
	var start = character.get_tile()
	_reset_queue()
	discarded = []
	
	var movables_count = movables.size()
	
	var movables_pos: Array = [] #Can't use PoolVector2Array
	#var a = []
	#var b = [] #(Try PoolVector2Array )
	#a.append(b)
	#print("a = "+ str(a))
	#print("a[0] = "+ str(a[0]))
	#a[0].insert(0, Vector2(42, 42)) # here, a[0] passes by value, it is a copy
	#print("a = "+ str(a))
	#print("a[0] = "+ str(a[0]))
	
	for m in movables:
		movables_pos.append(m.get_tile())
		
	var starting_node = {
		"pos": start,
		"movables_pos": movables_pos,
		"parent": null,
		"g": 0,
		"h": start.distance_to(end),
		"predicted": 0 + start.distance_to(end)
	}
	
	_insert(starting_node)
	
	var start_time = OS.get_ticks_msec()
	var count_tries = 0
	
	while(not _empty()):
		count_tries = count_tries + 1
		var current_time = OS.get_ticks_msec()
		if current_time - start_time > 600:
			print("Goal is too hard to reach")
			return
		var current_node = _del_min()
		if end.x == current_node.pos.x and end.y == current_node.pos.y:
			var new_path : PoolVector2Array = PoolVector2Array()
			var new_paths_movables : Array = []
			for i in range(movables_count):
				new_paths_movables.append([])
			
			while(current_node.parent):
				new_path.insert(0, current_node.pos)
				for movable_positions_i in range(movables_count):
					new_paths_movables[movable_positions_i].insert(0, current_node.movables_pos[movable_positions_i])
					
				current_node = current_node.parent
				
			new_path.insert(0, current_node.pos)
			for movable_positions_i in range(movables_count):
				new_paths_movables[movable_positions_i].insert(0, current_node.movables_pos[movable_positions_i])
				
			destination_arrow.show_at(new_path[new_path.size() - 1])
			character.set_path(new_path, speed)
			for m_i in range(movables_count):
				movables[m_i].set_path(new_paths_movables[m_i], speed)
			return
			
		var new_possible_paths = _expand(current_node, end, allowed_tiles, movables_allowed_tiles)
		for i in new_possible_paths:
			_insert(i)
		discarded.append(current_node)
			
	print("No path found "+str(count_tries)+" tries")

func _is_allowed(node, allowed_tiles: TileMap, movables_allowed_tiles: TileMap) -> bool:
	var tile_value = allowed_tiles.get_cell(
		node.pos.x / allowed_tiles.cell_size.x, 
		node.pos.y / allowed_tiles.cell_size.y
	) 
	if tile_value == -1:
		return false
	for mp_i in range(node.movables_pos.size()):
		var mp = node.movables_pos[mp_i]
		for mp_j in range(mp_i + 1, node.movables_pos.size()):
			if mp.is_equal_approx(node.movables_pos[mp_j]):
				return false
		var movable_tile_value = movables_allowed_tiles.get_cell(
			mp.x / movables_allowed_tiles.cell_size.x, 
			mp.y / movables_allowed_tiles.cell_size.y
		) 
		if movable_tile_value == -1:
			return false
		
	return _not_calculated_yet(node.pos)

func _not_calculated_yet(new_node):
	for node in heaplist:
		if node.pos.x == new_node.x and node.pos.y == new_node.y:
			return false
	for node in discarded:
		if node.pos.x == new_node.x and node.pos.y == new_node.y:
			return false
	
	return true

func _nnode(parent, new_total_travelled: float, new_heuristic: float, new_pos: Vector2, movables_pos: Array):
	var movables_pos_copy: Array = []
	movables_pos_copy.resize(movables_pos.size())
	for mp_i in range(movables_pos.size()):
		var mp = movables_pos[mp_i]
		movables_pos_copy[mp_i] = Vector2(mp.x, mp.y)
		
	return {
		"pos": new_pos,
		"movables_pos": movables_pos_copy,
		"parent": parent,
		"g": new_total_travelled,
		"h": new_heuristic,
		"predicted": new_total_travelled + new_heuristic
	}

func create_new_node(
	parent, 
	new_pos: Vector2, 
	end_goal_pos: Vector2,
	cell_size_x,
	cell_size_y
):
	var new_total_travelled = parent.g + new_pos.distance_to(parent.pos)
	var new_heuristic = new_pos.distance_to(end_goal_pos)
	var new_mps: Array = []
	new_mps.resize(parent.movables_pos.size())
	for mp_i in range(parent.movables_pos.size()):
		var mp = parent.movables_pos[mp_i]
		var new_mp = Vector2(mp.x, mp.y)
		if new_mp.is_equal_approx(new_pos):
			if parent.pos.x < new_pos.x:
				new_mp.x = new_mp.x + cell_size_x
			if parent.pos.x > new_pos.x:
				new_mp.x = new_mp.x - cell_size_x
			if parent.pos.y < new_pos.y:
				new_mp.y = new_mp.y + cell_size_y
			if parent.pos.y > new_pos.y:
				new_mp.y = new_mp.y - cell_size_y
		new_mps[mp_i] = new_mp
	return _nnode(parent, new_total_travelled, new_heuristic, new_pos, new_mps)

func _expand(
	node, 
	end_goal_pos: Vector2, 
	allowed_tiles: TileMap,
	movables_allowed_tiles: TileMap
) -> Array:
	var new_nodes = Array()
	
	var up = create_new_node(
		node, 
		Vector2(node.pos.x, node.pos.y - allowed_tiles.cell_size.y), 
		end_goal_pos, 
		allowed_tiles.cell_size.x, 
		allowed_tiles.cell_size.y
	)
	
	var down = create_new_node(
		node, 
		Vector2(node.pos.x, node.pos.y + allowed_tiles.cell_size.y), 
		end_goal_pos, 
		allowed_tiles.cell_size.x, 
		allowed_tiles.cell_size.y
	)
	
	var left = create_new_node(
		node, 
		Vector2(node.pos.x - allowed_tiles.cell_size.x, node.pos.y), 
		end_goal_pos, 
		allowed_tiles.cell_size.x, 
		allowed_tiles.cell_size.y
	)
	
	var right = create_new_node(
		node, 
		Vector2(node.pos.x + allowed_tiles.cell_size.x, node.pos.y), 
		end_goal_pos, 
		allowed_tiles.cell_size.x, 
		allowed_tiles.cell_size.y
	)
	
	if _is_allowed(up, allowed_tiles, movables_allowed_tiles):
		new_nodes.append(up)
	if _is_allowed(down, allowed_tiles, movables_allowed_tiles):
		new_nodes.append(down)
	if _is_allowed(left, allowed_tiles, movables_allowed_tiles):
		new_nodes.append(left)
	if _is_allowed(right, allowed_tiles, movables_allowed_tiles):
		new_nodes.append(right)
	
	return new_nodes
	
# Priority Queue

var heaplist : Array

func _reset_queue():
	heaplist = []

func _perc_up(i):
	while floor(i / 2) >= 0:
		if heaplist[i].predicted < heaplist[floor(i / 2)].predicted:
			var tmp = heaplist[floor(i / 2)]
			heaplist[floor(i / 2)] = heaplist[i]
			heaplist[i] = tmp
		else:
			return
		i = floor(i / 2)

func _insert(k):
	heaplist.append(k)
	_perc_up(heaplist.size() - 1)

func _perc_down(i):
	while (i * 2) + 1 < heaplist.size():
		var mc = _min_child(i)
		if heaplist[i].predicted > heaplist[mc].predicted:
			var tmp = heaplist[i]
			heaplist[i] = heaplist[mc]
			heaplist[mc] = tmp
		i = mc

func _min_child(i):
	if (i * 2) + 2 >= heaplist.size():
		return (i * 2) + 1
	else:
		if heaplist[(i * 2) + 1].predicted < heaplist[(i * 2) + 2].predicted:
			return (i * 2) + 1
		else:
			return (i * 2) + 2

func _del_min():
	var retval = heaplist.pop_front()
	if heaplist.size() == 0:
		return retval
	heaplist.append(heaplist.pop_back())
	_perc_down(0)
	return retval

func _empty():
	return heaplist.empty()