-
Notifications
You must be signed in to change notification settings - Fork 0
/
AStar.py
88 lines (58 loc) · 1.82 KB
/
AStar.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
#!/usr/bin/env python
#
# A* PATHFINDING ALGORITHM
#
#
import math
import time
import pygame
from queue import PriorityQueue
def AStar(start, end, grid):
start_node = grid.grid[start]
end_node = grid.grid[end]
queue = PriorityQueue()
queue.put(start_node)
start_node.list = True
start_node.g = 0
start_node.h = math.sqrt((end_node.position[0] - start_node.position[0])**2 + (end_node.position[1] - start_node.position[1])**2)
start_node.f = start_node.g + start_node.h
while not queue.empty():
current_node = queue.get()
current_node.list = False
if grid.grid[current_node.position].type != 'start_pos':
grid.grid[current_node.position].type = "closed_node"
directions = [(1, 0), (0, 1), (-1, 0), (0, -1), (1, 1), (-1, 1), (1, -1), (-1, -1)]
for x, y in directions:
try:
child_node = grid.grid[(current_node.position[0] + x, current_node.position[1] + y)]
except:
continue
if child_node.type == "wall":
continue
if child_node.list == False:
continue
g = 1
if x != 0 and y != 0:
g = math.sqrt(2)
if child_node.list == True:
if child_node.f <= child_node.h + current_node.g + g:
continue
child_node.g = current_node.g + g
child_node.h = math.sqrt((end_node.position[0] - child_node.position[0])**2 + (end_node.position[1] - child_node.position[1])**2)
child_node.f = child_node.g + child_node.h
child_node.parent = current_node
if child_node == end_node:
path = []
child_node = child_node.parent
while child_node.parent:
path.append(child_node)
child_node = child_node.parent
for node in path[::-1]:
node.type = 'final_path'
time.sleep(0.05)
grid.path = path[::-1]
return None
child_node.list = True
queue.put(child_node)
grid.grid[(child_node.position)].type = "open_node"
time.sleep(0.01)