Pathfinding-Visualizer/a_star.py at master · huzefa26/Pathfinding-Visualizer · 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
84
from sortedcontainers import SortedList
from math import sqrt
from time import sleep

MAXINT = 1 << 20


class Astar:
    def __init__(self, master):
        self.master = master
        self.src = self.dest = None

    def G(self, cell):
        # distance from source
        return int(10 * sqrt(pow(self.src.x - cell.x, 2) + pow(self.src.y - cell.y, 2)))

    def H(self, cell):
        # distance from destination
        return int(10 * sqrt(pow(self.dest.x - cell.x, 2) + pow(self.dest.y - cell.y, 2)))

    def get_neighbors(self, cell):
        x, y = cell.x, cell.y
        padosi = []
        if x > 0:
            padosi.append([x - 1, y])
            if y > 0:
                padosi.append([x - 1, y - 1])
            if y < self.master.row_num:
                padosi.append([x - 1, y + 1])
        if y > 0:
            padosi.append([x, y - 1])
        if y < self.master.row_num:
            padosi.append([x, y + 1])
        if x < self.master.col_num:
            padosi.append([x + 1, y])
            if y > 0:
                padosi.append([x + 1, y - 1])
            if y < self.master.row_num:
                padosi.append([x + 1, y + 1])
        return padosi

    def trace(self):
        if self.master.start_cell and self.master.dest_cells:
            OPEN = SortedList(key=lambda cell: self.G(cell) + self.H(cell))
            CLOSED = []
            self.dest = [i for i in self.master.dest_cells][0]
            self.src = self.master.start_cell
            distance = [[MAXINT for c in range(self.master.col_num)] for r in range(self.master.row_num)]
            parent = [[None for c in range(self.master.col_num)] for r in range(self.master.row_num)]
            distance[self.src.y][self.src.x] = 0
            OPEN.add(self.src)
            # distance = 0
            cnt = 6
            while True:
                cell = OPEN.pop(0)
                print(cell.x, cell.y)
                CLOSED.append(cell)
                # distance += self.G(cell) + self.H(cell)
                if cell == self.dest:
                    break
                temp_dist = MAXINT
                for n in self.get_neighbors(cell):
                    temp = self.master.grid[n[1]][n[0]]
                    if temp.typ == 1 or temp in CLOSED:
                        continue
                    f = self.G(temp) + self.H(temp)
                    if distance[n[1]][n[0]] > f and temp not in OPEN:
                        distance[n[1]][n[0]] = f
                        print(temp.x, temp.y, f)
                        parent[n[1]][n[0]] = cell
                        OPEN.add(temp)
                        if temp.typ == 0:
                            temp.draw(cnt)
                # distance += temp_dist
                if cell is not self.dest:
                    cell.draw(5)
                cnt += 1
            cell = self.dest
            while cell != self.src:
                cell = parent[cell.y][cell.x]
                cell.draw(4)
            self.src.draw(2)
        # OPEN.clear()
        # CLOSED.clear()