-
Notifications
You must be signed in to change notification settings - Fork 0
/
astar_algorithm.py
343 lines (268 loc) · 10.8 KB
/
astar_algorithm.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
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
import pygame
import math
from queue import PriorityQueue
# Define width of window - square, so no height variable needed
WIDTH = 800
# Setting up the display (win for window)
WIN = pygame.display.set_mode((WIDTH, WIDTH))
# Caption for the display
pygame.display.set_caption("A* Path Finding Algorithm")
# Colors to use
RED = (255, 0, 0)
GREEN = (0, 255, 0)
BLUE = (0, 255, 0)
YELLOW = (255, 255, 0)
WHITE = (255, 255, 255)
BLACK = (0, 0, 0)
PURPLE = (128, 0, 128)
ORANGE = (255, 165, 0)
GREY = (128, 128, 128)
TURQUOISE = (64, 224, 208)
# Defining a class, creating a visualization tool
# It needs to be able to keep track of all of these nodes
# Where it is (row, col position), width of itself (to draw itself), its neighbors, color (to differentiate itself)
class Node:
def __init__(self, row, col, width, total_rows):
self.row = row
self.col = col
self.width = width
# To keep track of position
self.x = row * width
self.y = col * width
self.color = WHITE
self.neighbors = width
self.total_rows = total_rows
# To get its position
def get_position(self):
return self.row, self.col
# To check if node was already checked
def is_closed(self):
# If the color of the node is red, it was checked
return self.color == RED
# To check if node can be analyzed by algorithm
def is_open(self):
# If the color of the node is green, it can be checked
return self.color == GREEN
# To check if the node is a barrier
def is_barrier(self):
# If the color of the node is black, it is a barrier
return self.color == BLACK
# Start node
def is_start(self):
return self.color == ORANGE
# End node
def is_end(self):
return self.color == TURQUOISE
# Method to reset the color back to white
def reset(self):
self.color = WHITE
# The next series of methods essentially makes the node to the desired color
def make_closed(self):
self.color = RED
def make_open(self):
self.color = GREEN
def make_barrier(self):
self.color = BLACK
def make_start(self):
self.color = ORANGE
def make_end(self):
self.color = TURQUOISE
def make_path(self):
self.color = PURPLE
# Method to actually draw the node
def draw(self, win):
# Parameters is the window, the color, and the rectangle. Pretty self explanatory
pygame.draw.rect(win, self.color, (self.x, self.y, self.width, self.width))
# Check if neighbors are barriers, and if not keep them
def update_neighbors(self, grid):
self.neighbors = []
if self.row < self.total_rows - 1 and not grid[self.row + 1][self.col].is_barrier(): # Down
self.neighbors.append(grid[self.row + 1][self.col])
if self.row > 0 and not grid[self.row - 1][self.col].is_barrier(): # Up
self.neighbors.append(grid[self.row - 1][self.col])
if self.col < self.total_rows - 1 and not grid[self.row][self.col + 1].is_barrier(): # Right
self.neighbors.append(grid[self.row][self.col + 1])
if self.col > 0 and not grid[self.row][self.col - 1].is_barrier(): # Left
self.neighbors.append(grid[self.row][self.col - 1])
# How to handle comparing two nodes together, lt for less than
def __lt__(self, other):
return False
# Defining the H score heuristic function, p1 and p2 are points (row, col)
def h(p1, p2):
# Using manhattan distance, quickest L to the shape, because no diagonals on this
x1, y1 = p1
x2, y2 = p2
return abs(x1 - x2) + abs(y1 - y2)
# To display the actual path
def reconstruct_path(came_from, current, draw):
while current in came_from:
current = came_from[current]
current.make_path()
draw()
def algorithm(draw, grid, start, end):
count = 0
# Priority Queue is just an efficient way to get the smallest element out of it, already has sorting algorithm in it
open_set = PriorityQueue()
# First step to put the start node into the open set
open_set.put((0, count, start))
# Dictionary to check which node we came from
came_from = {}
# Keep track of the g score - current shortest distance fromm current node
g_score = {node: float("inf") for row in grid for node in row}
g_score[start] = 0
# Keep track of the f score - with the heuristic
f_score = {node: float("inf") for row in grid for node in row}
f_score[start] = h(start.get_position(), end.get_position())
# Keep track of all items in and out of the priority queue
open_set_hash = {start}
while not open_set.empty():
for event in pygame.event.get():
# Way to exit the loop in case the user wants to
if event.type == pygame.QUIT:
pygame.quit()
# 2 so that can get node
current = open_set.get()[2]
# Take whatever from the priority queue and sync with the hash
open_set_hash.remove(current)
# Construct path if reached the end
if current == end:
reconstruct_path(came_from, end, draw)
# Prevent purple over end node
end.make_end()
# Prevent purple over start
start.make_start()
return True
for neighbor in current.neighbors:
# Assume all edges are 1, temp g score, and add 1 (because assuming the edge is 1 and not weighted)
temp_g_score = g_score[current] + 1
# If found a better way, then update the g score
if temp_g_score < g_score[neighbor]:
came_from[neighbor] = current
g_score[neighbor] = temp_g_score
# f(n) = g(n) + h(n)
f_score[neighbor] = temp_g_score + h(neighbor.get_position(), end.get_position())
if neighbor not in open_set_hash:
count += 1
# Put in this neighbor into the open set
open_set.put((f_score[neighbor], count, neighbor))
open_set_hash.add(neighbor)
# Now this neighbor is open
neighbor.make_open()
draw()
# If the node just considered, is not the start node, close it
if current != start:
current.make_closed()
return None
# Data structure to hold all of the nodes so you can actually do things
def make_grid(rows, width):
grid = []
# Gap between each of the rows
gap = width // rows
# In grid row i, append the node into it. Bunch of lists inside of lists that each have nodes
for i in range(rows):
grid.append([])
for j in range(rows):
node = Node(i, j, gap, rows)
grid[i].append(node)
return grid
# Need a way to draw the grid
def draw_grid(win, rows, width):
gap = width // rows
# Drawing horizontal lines through the window
for i in range(rows):
# Parameters to draw line are the window, color, the position for start of the line, and end of the line
# Multiplying the index by the gap, so we get accurate places
pygame.draw.line(win, GREY, (0, i * gap), (width, i * gap))
# Exact same thing but with vertical lines
for j in range(rows):
pygame.draw.line(win, GREY, (j * gap, 0), (j * gap, width))
# Main draw function to draw everything
def draw(win, grid, rows, width):
# Fills the window with one color, every frame do this and then repaint with what we want
win.fill(WHITE)
# Loop through the window and draw the spots
for row in grid:
for node in row:
node.draw(win)
# Drawing the gridlines
draw_grid(win, rows, width)
# Updating the display every frame
pygame.display.update()
# Translating the mouse position into a row column position
def clicked_position(pos, rows, width):
# Go through the math to understand, remember // is floor division!!
gap = width // rows
y, x = pos
row = y // gap
col = x // gap
return row, col
# Main loop to check everything
def main(win, width):
# Can change this to whatever
rows = 50
# Make the actual grid
grid = make_grid(rows, width)
start = None
end = None
run = True
started = False
# While running, check events that are happening
while run:
draw(win, grid, rows, width)
for event in pygame.event.get():
# If the player exits, stop running
if event.type == pygame.QUIT:
run = False
# If the algorithm started, user should not be able to do anything but quit - it will mess the algo up
if started:
continue
# If the mouse was pressed, left side
if pygame.mouse.get_pressed()[0]:
pos = pygame.mouse.get_pos()
row, col = clicked_position(pos, rows, width)
node = grid[row][col]
# If the starting position hasn't been established yet...
if not start and node != end:
start = node
start.make_start()
# If the ending position hasn't been established yet...
elif not end and node != start:
end = node
end.make_end()
# If neither above, has to be made a barrier (obviously if clicked)
elif node != end and node != start:
node.make_barrier()
# If the mouse was pressed, right side
elif pygame.mouse.get_pressed()[2]:
pos = pygame.mouse.get_pos()
row, col = clicked_position(pos, rows, width)
node = grid[row][col]
# If right click, then will reset to white
node.reset()
# In case you reset the start or end nodes
if node == start:
start = None
elif node == end:
end = None
# Start running the algorithm by pressing space
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_SPACE and not started:
# Update neighbors
for row in grid:
for node in row:
node.update_neighbors(grid)
# Calling the algorithm
'''
Lambda is an anonymous function
x = Lambda: print("hello")
x() --> will print hello
'''
algorithm(lambda: draw(win, grid, rows, width), grid, start, end)
# If press 'c' clear the board
if event.key == pygame.K_c:
start = None
end = None
grid = make_grid(rows, width)
# Self explanatory, exits the game window
pygame.quit()
main(WIN, WIDTH)