-
Notifications
You must be signed in to change notification settings - Fork 0
/
main.py
311 lines (230 loc) · 8.85 KB
/
main.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
# A Star Algorithm Visualization implementation in Python
# Importing the required libraries
import pygame
import math
from queue import PriorityQueue
# Importing all the colors from colors module
from colors import *
# Global Variables
# Width of the pygame window
WIDTH = 800
# Setting the Width and Height of the pygame window
WIN = pygame.display.set_mode((WIDTH, WIDTH))
# Setting the title the pygame window
pygame.display.set_caption("A Star Path Finding Algorithm")
# Class Node
class Node:
def __init__(self, row, col, width, totalRows):
self.row = row
self.col = col
self.width = width
self.totalRows = totalRows
self.x = row * width
self.y = col * width
self.color = WHITE
self.neighbors = []
# Method which return the position of the node within the pygame window
def getPosition(self):
return self.row, self.col
# Method which checks if the node is open
def isOpen(self):
return self.color == GREEN
# Method which checks if the node is closed
def isClosed(self):
return self.color == RED
# Method which checks if the node is a barrier
def isBarrier(self):
return self.color == BLACK
# Method which checks if the node is the starting node
def isStart(self):
return self.color == ORANGE
# Method which checks if the node is the ending node
def isEnd(self):
return self.color == TURQUOISE
# Method which resets the node
def reset(self):
self.color = WHITE
# Method which makes the node open
def makeOpen(self):
self.color = GREEN
# Method which makes the node closed
def makeClosed(self):
self.color = RED
# Method which makes the node a barrier
def makeBarrier(self):
self.color = BLACK
# Method which makes the node the starting node
def makeStart(self):
self.color = ORANGE
# Method which makes the node the ending node
def makeEnd(self):
self.color = TURQUOISE
# Method which makes the node a part of the path from source to destination node
def makePath(self):
self.color = PURPLE
# Method which draws the node to the pygame window
def draw(self, win):
pygame.draw.rect(win, self.color, (self.x, self.y, self.width, self.width))
# Method which updates the neighbors of the node
def updateNeighbors(self, grid):
self.neighbors = []
# North Neighbor
if self.row > 0 and not grid[self.row - 1][self.col].isBarrier():
self.neighbors.append(grid[self.row - 1][self.col])
# South Neighbor
if self.row < self.totalRows - 1 and not grid[self.row + 1][self.col].isBarrier():
self.neighbors.append(grid[self.row + 1][self.col])
# East Neighbor
if self.col < self.totalRows - 1 and not grid[self.row][self.col + 1].isBarrier():
self.neighbors.append(grid[self.row][self.col + 1])
# West Neighbor
if self.col > 0 and not grid[self.row][self.col - 1].isBarrier():
self.neighbors.append(grid[self.row][self.col - 1])
def __lt__(self, other):
return False
# Function which draws the path from the source to the destination node
def drawPath(predecessor, currentNode, draw):
while currentNode in predecessor:
currentNode = predecessor[currentNode]
currentNode.makePath()
draw()
# A Star Algorithm Function
def AStarAlgorithm(draw, grid, startNode, endNode):
count = 0
# OpenSet is a priority queue
openSet = PriorityQueue()
openSet.put((0, count, startNode))
predecessor = {}
# Global Score (gScore)
gScore = {node: float("inf") for row in grid for node in row}
gScore[startNode] = 0
# Heuristic Score (fScore)
fScore = {node: float("inf") for row in grid for node in row}
fScore[startNode] = heuristicFunction(startNode.getPosition(), endNode.getPosition())
openSetHash = {startNode}
while not openSet.empty():
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
current = openSet.get()[2]
openSetHash.remove(current)
if current == endNode:
drawPath(predecessor, endNode, draw)
endNode.makeEnd()
print(f"The End Node is {temp_gScore} units away from the Source Node")
return True
for neighbor in current.neighbors:
temp_gScore = gScore[current] + 1
if temp_gScore < gScore[neighbor]:
predecessor[neighbor] = current
gScore[neighbor] = temp_gScore
fScore[neighbor] = temp_gScore + heuristicFunction(neighbor.getPosition(), endNode.getPosition())
if neighbor not in openSetHash:
count += 1
openSet.put((fScore[neighbor], count, neighbor))
openSetHash.add(neighbor)
neighbor.makeOpen()
draw()
if current != startNode:
current.makeClosed()
return False
# Function for calculating the heuristic value
def heuristicFunction(d1, d2):
x1, y1 = d1
x2, y2 = d2
# Euclidean Distance
# return math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2)
# Diagonal Distance
# return max(abs(x1 - x2), abs(y1 - y2))
# Manhattan Distance
return abs(x1 - x2) + abs(y1 - y2)
# Function which makes a grid of nodes
def makeGrid(rows, width):
grid = []
gap = width // rows
for i in range(rows):
grid.append([])
for j in range(rows):
node = Node(i, j, gap, rows)
grid[i].append(node)
return grid
# Function for drawing the border of the nodes in the grid
def drawGrid(win, rows, width):
gap = width // rows
for i in range(rows):
pygame.draw.line(win, GREY, (0, i * gap), (width, i * gap))
for j in range(rows):
pygame.draw.line(win, GREY, (j * gap, 0), (j * gap, width))
# Function which draws the nodes from the grid on the pygame window
def draw(win, grid, rows, width):
win.fill(WHITE)
for row in grid:
for node in row:
node.draw(win)
drawGrid(win, rows, width)
pygame.display.update()
# Function which gets the mouse position within the pygame window
def getMouseClickPosition(position, rows, width):
gap = width // rows
y, x = position
row = y // gap
col = x // gap
return row, col
# Main Function
def main(win, width):
# Number of rows
ROWS = 50
grid = makeGrid(ROWS, width)
# Initializing the starting and the ending node
startNode = None
endNode = None
run = True
while run:
# Drawing the grid onto the pygame window
draw(win, grid, ROWS, width)
for event in pygame.event.get():
if event.type == pygame.QUIT:
run = False
# Checking for Left Mouse Button click
if pygame.mouse.get_pressed()[0]:
position = pygame.mouse.get_pos()
row, col = getMouseClickPosition(position, ROWS, width)
node = grid[row][col]
# If start node has not been set,
# we set the node which is being clicked as starting node
if not startNode and node != endNode:
startNode = node
startNode.makeStart()
# If end node has not been set,
# we set the node which is being clicked as ending node
elif not endNode and node != startNode:
endNode = node
endNode.makeEnd()
# If barrier node has not been set,
# we set the node which is being clicked as barrier node
elif node != startNode and node != endNode:
node.makeBarrier()
# Checking for Right Mouse Button click
elif pygame.mouse.get_pressed()[2]:
position = pygame.mouse.get_pos()
row, col = getMouseClickPosition(position, ROWS, width)
node = grid[row][col]
node.reset()
if node == startNode:
startNode = None
elif node == endNode:
endNode = None
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_SPACE and startNode and endNode:
for row in grid:
for node in row:
node.updateNeighbors(grid)
AStarAlgorithm(lambda: draw(win, grid, ROWS, width), grid, startNode, endNode)
if event.key == pygame.K_c:
startNode = None
endNode = None
grid = makeGrid(ROWS, width)
pygame.quit()
# Main Loop
if __name__ == "__main__":
main(WIN, WIDTH)