Skip to content

Instantly share code, notes, and snippets.

@beothorn
Created September 6, 2020 19:39
Show Gist options
  • Save beothorn/bef3eba6c5430c54ab90f7764dbe3a63 to your computer and use it in GitHub Desktop.
Save beothorn/bef3eba6c5430c54ab90f7764dbe3a63 to your computer and use it in GitHub Desktop.
TileNavigation.gd
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()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment