-
Notifications
You must be signed in to change notification settings - Fork 1
/
dijkstra.py
46 lines (40 loc) · 1.41 KB
/
dijkstra.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
import math
import pygame
from queue import PriorityQueue
from node import Node
HEIGHT, WIDTH = 900, 900
def reconstructPath(came_from, current, draw):
while current in came_from:
current = came_from[current]
current.makePath()
draw()
def dijkstra(draw, grid, start, end):
visited = {node: False for row in grid for node in row}
distance = {node: math.inf for row in grid for node in row}
distance[start] = 0
came_from = {}
priority_queue = PriorityQueue()
priority_queue.put((0, start))
while not priority_queue.empty():
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
current = priority_queue.get()[1]
if visited[current]:
continue
visited[current] = True
if current == end:
reconstructPath(came_from, end, draw)
return True
if current != start:
current.makeVisited()
for neighbor in current.neighbors:
weight = 1
if distance[current] + weight < distance[neighbor]:
came_from[neighbor] = current
distance[neighbor] = distance[current] + weight
priority_queue.put((distance[neighbor], neighbor))
if neighbor != end and neighbor != start and not visited[neighbor]:
neighbor.makeVisiting()
draw()
return False