|
1 | 1 | import heapq
|
2 | 2 |
|
3 | 3 | class Cell(object):
|
4 |
| - def __init__(self, x, y, reachable): |
5 |
| - """ |
6 |
| - Initialize new cell |
7 |
| -
|
8 |
| - @param x cell x coordinate |
9 |
| - @param y cell y coordinate |
10 |
| - @param reachable is cell reachable? not a wall? |
11 |
| - """ |
12 |
| - self.reachable = reachable |
13 |
| - self.x = x |
14 |
| - self.y = y |
15 |
| - self.parent = None |
16 |
| - self.g = 0 |
17 |
| - self.h = 0 |
18 |
| - self.f = 0 |
| 4 | + def __init__(self, x, y, reachable): |
| 5 | + """ |
| 6 | + Initialize new cell |
| 7 | +
|
| 8 | + @param x cell x coordinate |
| 9 | + @param y cell y coordinate |
| 10 | + @param reachable is cell reachable? not a wall? |
| 11 | + """ |
| 12 | + self.reachable = reachable |
| 13 | + self.x = x |
| 14 | + self.y = y |
| 15 | + self.parent = None |
| 16 | + self.g = 0 |
| 17 | + self.h = 0 |
| 18 | + self.f = 0 |
19 | 19 |
|
20 | 20 | class AStar(object):
|
21 |
| - def __init__(self): |
22 |
| - self.op = [] |
23 |
| - heapq.heapify(self.op) |
24 |
| - self.cl = set() |
25 |
| - self.cells = [] |
26 |
| - self.gridHeight = 6 |
27 |
| - self.gridWidth = 6 |
28 |
| - |
29 |
| - def init_grid(self): |
30 |
| - walls = ((0, 5), (1, 0), (1, 1), (1, 5), (2, 3), |
31 |
| - (3, 1), (3, 2), (3, 5), (4, 1), (4, 4), (5, 1)) |
32 |
| - for x in range(self.gridWidth): |
33 |
| - for y in range(self.gridHeight): |
34 |
| - if (x, y) in walls: |
35 |
| - reachable = False |
36 |
| - else: |
37 |
| - reachable = True |
38 |
| - self.cells.append(Cell(x, y, reachable)) |
39 |
| - self.start = self.get_cell(0, 0) |
40 |
| - self.end = self.get_cell(5, 5) |
41 |
| - |
42 |
| - def get_heuristic(self, cell): |
43 |
| - """ |
44 |
| - Compute the heuristic value H for a cell: distance between |
45 |
| - this cell and the ending cell multiply by 10. |
46 |
| -
|
47 |
| - @param cell |
48 |
| - @returns heuristic value H |
49 |
| - """ |
50 |
| - return 10 * (abs(cell.x - self.end.x) + abs(cell.y - self.end.y)) |
51 |
| - |
52 |
| - def get_cell(self, x, y): |
53 |
| - """ |
54 |
| - Returns a cell from the cells list |
55 |
| -
|
56 |
| - @param x cell x coordinate |
57 |
| - @param y cell y coordinate |
58 |
| - @returns cell |
59 |
| - """ |
60 |
| - return self.cells[x * self.gridHeight + y] |
61 |
| - |
62 |
| - def get_adjacent_cells(self, cell): |
63 |
| - """ |
64 |
| - Returns adjacent cells to a cell. Clockwise starting |
65 |
| - from the one on the right. |
66 |
| -
|
67 |
| - @param cell get adjacent cells for this cell |
68 |
| - @returns adjacent cells list |
69 |
| - """ |
70 |
| - cells = [] |
71 |
| - if cell.x < self.gridWidth-1: |
72 |
| - cells.append(self.get_cell(cell.x+1, cell.y)) |
73 |
| - if cell.y > 0: |
74 |
| - cells.append(self.get_cell(cell.x, cell.y-1)) |
75 |
| - if cell.x > 0: |
76 |
| - cells.append(self.get_cell(cell.x-1, cell.y)) |
77 |
| - if cell.y < self.gridHeight-1: |
78 |
| - cells.append(self.get_cell(cell.x, cell.y+1)) |
79 |
| - return cells |
80 |
| - |
81 |
| - def display_path(self): |
82 |
| - cell = self.end |
83 |
| - while cell.parent is not self.start: |
84 |
| - cell = cell.parent |
85 |
| - print 'path: cell: %d,%d' % (cell.x, cell.y) |
86 |
| - |
87 |
| - def compare(self, cell1, cell2): |
88 |
| - """ |
89 |
| - Compare 2 cells F values |
90 |
| -
|
91 |
| - @param cell1 1st cell |
92 |
| - @param cell2 2nd cell |
93 |
| - @returns -1, 0 or 1 if lower, equal or greater |
94 |
| - """ |
95 |
| - if cell1.f < cell2.f: |
96 |
| - return -1 |
97 |
| - elif cell1.f > cell2.f: |
98 |
| - return 1 |
99 |
| - return 0 |
100 |
| - |
101 |
| - def update_cell(self, adj, cell): |
102 |
| - """ |
103 |
| - Update adjacent cell |
104 |
| -
|
105 |
| - @param adj adjacent cell to current cell |
106 |
| - @param cell current cell being processed |
107 |
| - """ |
108 |
| - adj.g = cell.g + 10 |
109 |
| - adj.h = self.get_heuristic(adj) |
110 |
| - adj.parent = cell |
111 |
| - adj.f = adj.h + adj.g |
112 |
| - |
113 |
| - def process(self): |
114 |
| - # add starting cell to open heap queue |
115 |
| - heapq.heappush(self.op, (self.start.f, self.start)) |
116 |
| - while len(self.op): |
117 |
| - # pop cell from heap queue |
118 |
| - h, cell = heapq.heappop(self.op) |
119 |
| - # add cell to closed list so we don't process it twice |
120 |
| - self.cl.add(cell) |
121 |
| - # if ending cell, display found path |
122 |
| - if cell is self.end: |
123 |
| - self.display_path() |
124 |
| - break |
125 |
| - # get adjacent cells for cell |
126 |
| - adj_cells = self.get_adjacent_cells(cell) |
127 |
| - for c in adj_cells: |
128 |
| - if c.reachable and c not in self.cl: |
129 |
| - if c in self.op: |
130 |
| - # if adj cell in open list, check if current path is |
131 |
| - # better than the one previously found for this adj cell. |
132 |
| - if c.g > cell.g + 10: |
133 |
| - self.update_cell(c, cell) |
134 |
| - else: |
135 |
| - self.update_cell(c, cell) |
136 |
| - # add adj cell to open list |
137 |
| - heapq.heappush(self.op, (c.f, c)) |
| 21 | + def __init__(self): |
| 22 | + self.op = [] |
| 23 | + heapq.heapify(self.op) |
| 24 | + self.cl = set() |
| 25 | + self.cells = [] |
| 26 | + self.gridHeight = 6 |
| 27 | + self.gridWidth = 6 |
| 28 | + |
| 29 | + def init_grid(self): |
| 30 | + walls = ((0, 5), (1, 0), (1, 1), (1, 5), (2, 3), |
| 31 | + (3, 1), (3, 2), (3, 5), (4, 1), (4, 4), (5, 1)) |
| 32 | + for x in range(self.gridWidth): |
| 33 | + for y in range(self.gridHeight): |
| 34 | + if (x, y) in walls: |
| 35 | + reachable = False |
| 36 | + else: |
| 37 | + reachable = True |
| 38 | + self.cells.append(Cell(x, y, reachable)) |
| 39 | + self.start = self.get_cell(0, 0) |
| 40 | + self.end = self.get_cell(5, 5) |
| 41 | + |
| 42 | + def get_heuristic(self, cell): |
| 43 | + """ |
| 44 | + Compute the heuristic value H for a cell: distance between |
| 45 | + this cell and the ending cell multiply by 10. |
| 46 | +
|
| 47 | + @param cell |
| 48 | + @returns heuristic value H |
| 49 | + """ |
| 50 | + return 10 * (abs(cell.x - self.end.x) + abs(cell.y - self.end.y)) |
| 51 | + |
| 52 | + def get_cell(self, x, y): |
| 53 | + """ |
| 54 | + Returns a cell from the cells list |
| 55 | +
|
| 56 | + @param x cell x coordinate |
| 57 | + @param y cell y coordinate |
| 58 | + @returns cell |
| 59 | + """ |
| 60 | + return self.cells[x * self.gridHeight + y] |
| 61 | + |
| 62 | + def get_adjacent_cells(self, cell): |
| 63 | + """ |
| 64 | + Returns adjacent cells to a cell. Clockwise starting |
| 65 | + from the one on the right. |
| 66 | +
|
| 67 | + @param cell get adjacent cells for this cell |
| 68 | + @returns adjacent cells list |
| 69 | + """ |
| 70 | + cells = [] |
| 71 | + if cell.x < self.gridWidth-1: |
| 72 | + cells.append(self.get_cell(cell.x+1, cell.y)) |
| 73 | + if cell.y > 0: |
| 74 | + cells.append(self.get_cell(cell.x, cell.y-1)) |
| 75 | + if cell.x > 0: |
| 76 | + cells.append(self.get_cell(cell.x-1, cell.y)) |
| 77 | + if cell.y < self.gridHeight-1: |
| 78 | + cells.append(self.get_cell(cell.x, cell.y+1)) |
| 79 | + return cells |
| 80 | + |
| 81 | + def display_path(self): |
| 82 | + cell = self.end |
| 83 | + while cell.parent is not self.start: |
| 84 | + cell = cell.parent |
| 85 | + print 'path: cell: %d,%d' % (cell.x, cell.y) |
| 86 | + |
| 87 | + def compare(self, cell1, cell2): |
| 88 | + """ |
| 89 | + Compare 2 cells F values |
| 90 | +
|
| 91 | + @param cell1 1st cell |
| 92 | + @param cell2 2nd cell |
| 93 | + @returns -1, 0 or 1 if lower, equal or greater |
| 94 | + """ |
| 95 | + if cell1.f < cell2.f: |
| 96 | + return -1 |
| 97 | + elif cell1.f > cell2.f: |
| 98 | + return 1 |
| 99 | + return 0 |
| 100 | + |
| 101 | + def update_cell(self, adj, cell): |
| 102 | + """ |
| 103 | + Update adjacent cell |
| 104 | +
|
| 105 | + @param adj adjacent cell to current cell |
| 106 | + @param cell current cell being processed |
| 107 | + """ |
| 108 | + adj.g = cell.g + 10 |
| 109 | + adj.h = self.get_heuristic(adj) |
| 110 | + adj.parent = cell |
| 111 | + adj.f = adj.h + adj.g |
| 112 | + |
| 113 | + def process(self): |
| 114 | + # add starting cell to open heap queue |
| 115 | + heapq.heappush(self.op, (self.start.f, self.start)) |
| 116 | + while len(self.op): |
| 117 | + # pop cell from heap queue |
| 118 | + h, cell = heapq.heappop(self.op) |
| 119 | + # add cell to closed list so we don't process it twice |
| 120 | + self.cl.add(cell) |
| 121 | + # if ending cell, display found path |
| 122 | + if cell is self.end: |
| 123 | + self.display_path() |
| 124 | + break |
| 125 | + # get adjacent cells for cell |
| 126 | + adj_cells = self.get_adjacent_cells(cell) |
| 127 | + for c in adj_cells: |
| 128 | + if c.reachable and c not in self.cl: |
| 129 | + if c in self.op: |
| 130 | + # if adj cell in open list, check if current path is |
| 131 | + # better than the one previously found |
| 132 | + # for this adj cell. |
| 133 | + if c.g > cell.g + 10: |
| 134 | + self.update_cell(c, cell) |
| 135 | + else: |
| 136 | + self.update_cell(c, cell) |
| 137 | + # add adj cell to open list |
| 138 | + heapq.heappush(self.op, (c.f, c)) |
138 | 139 |
|
139 | 140 | a = AStar()
|
140 | 141 | a.init_grid()
|
|
0 commit comments