A_star_pathvisualizer/algorithms.py at main · utkarsh4tech/A_star_pathvisualizer · GitHub

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
"""
Project Name: A* Path Finder Visualizer
Author: Utkarsh Sahu
Email: sahuuutkarhs03@gamil.com
Twitter: @Utkarsh4tech ( https://twitter.com/Utkarsh4tech )

Project Description: This is a Visualization tool for helping learners understand
                    about the A* algorithm. The tech Stack used in Project is
                    - Pygame and Python.

File Descreption: This file has all the Algorithms needed to run the a_star.py file.
                    It has the heuristic function that guides the algorithm and the main algorithm.

A* Mathematical Definition : f (n) = g(n)+h(n),
                        where f(n) = total estimated cost of path through node
                        g(n) is the cost of the path from the start node to n,
                        and h(n) is a heuristic function that estimates the cost of
                        the cheapest path from n to the goal.
"""

from queue import PriorityQueue
import pygame as pg

def heuristic(point1,point2):
    x1,y1=point1
    x2,y2=point2
    manhattan_distance = abs(x1-x2) + abs(y1-y2)
    return manhattan_distance

def construct_path(came_from,curr,draw):
    while curr in came_from:
        curr=came_from[curr]
        curr.make_path()
        draw()

def a_star_algorithm(draw,grid,start,end):
    count=0
    open_set=PriorityQueue()
    open_set.put((0,count,start))
    came_from={}

    g_score={node: float("inf") for row in grid for node in row}
    g_score[start]=0

    f_score={node: float("inf") for row in grid for node in row}
    f_score[start]= heuristic(start.get_pos(),end.get_pos())

    open_set_hash={start}

    while not open_set.empty():

        for event in pg.event.get():
            if event.type == pg.QUIT:
                pg.quit()

        curr =open_set.get()[2]
        open_set_hash.remove(curr)

        if curr == end:
            construct_path(came_from,curr,draw)
            start.make_start()
            end.make_end()
            return True

        for neighbor in curr.neighbors:
            temp_g_score=g_score[curr]+1
            if temp_g_score < g_score[neighbor]:
                came_from[neighbor]=curr
                g_score[neighbor]=temp_g_score
                f_score[neighbor]=temp_g_score + heuristic(neighbor.get_pos(),end.get_pos())

                if neighbor not in open_set_hash:
                    count+=1
                    open_set.put((f_score[neighbor],count,neighbor))
                    open_set_hash.add(neighbor)
                    neighbor.make_open()

        draw()

        if curr != start:
             curr.make_closed()

    return False