-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathAstarSolver.py
197 lines (149 loc) · 6.37 KB
/
AstarSolver.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
# coding: utf-8
import heapq
from Direction import Direction
class AstarSolver(object):
def __init__(self,grid,start_x,start_y):
self.opened = []
heapq.heapify(self.opened)
self.closed = set()
self.cells = grid
self.grid_height = len(grid)
self.grid_width = len(grid[0])
self.start = self.cells[start_y][start_x]
self.direction = Direction.EAST
self.currentDirection = Direction.EAST
self.end = self.cells[self.grid_height - 1][self.grid_width -1]
def get_heuristic(self, cell):
return (abs(cell.x - self.end.x) + abs(cell.y - self.end.y))
def get_cell(self, x, y):
return self.cells[x][y]
def get_adjacent_cells_2(self, x, y): # Size of "board"
Y = self.grid_width - 1
X = self.grid_height - 1
skewNeighbors = [(x - 1, y - 1),(x + 1, y - 1),
(x - 1, y + 1), (x + 1, y + 1)]
neighbors = lambda x, y : [(x2, y2) for x2 in range(x-1, x+2)
for y2 in range(y-1, y+2)
if (-1 < x <= X and
-1 < y <= Y and
(x != x2 or y != y2) and
(0 <= x2 <= X) and
(0 <= y2 <= Y))]
toCheck = list(set(neighbors(x,y)) - set(skewNeighbors))
return [self.cells[x][y] for (x,y) in toCheck]
def get_adjacent_cells(self, x, y):
# Size of "board"
Y = self.grid_width - 1
X = self.grid_height - 1
skewNeighbors = [(x - 1, y - 1),(x + 1, y - 1),
(x - 1, y + 1), (x + 1, y + 1)]
neighbors = lambda x, y : [(x2, y2) for x2 in range(x-1, x+2)
for y2 in range(y-1, y+2)
if (-1 < x <= X and
-1 < y <= Y and
(x != x2 or y != y2) and
(0 <= x2 <= X) and
(0 <= y2 <= Y))]
toCheck = list(set(neighbors(x,y)) - set(skewNeighbors))
#poprzednio ta linijka byla zwracana
adjFields = [self.cells[x][y] for (x,y) in toCheck]
print("adjfields: ", adjFields)
adjFieldsWithAction=[]
for element in adjFields:
# print(element)
parent = self.get_cell(x,y).get_parent()
print(parent)
if parent is not None:
cur_direction = self.computeStateFrom(x,y,parent.x,parent.y)
else:
cur_direction=self.direction
new_direction = self.computeState(x,y,element.x,element.y)
if(cur_direction == new_direction):
adjFieldsWithAction.append((element,[("Move",1)]))
print("jestem na: ",x,y,"\nide na: ",element.x,element.y,"\nkierunek obecny: ",cur_direction," nowy kierunek: ", new_direction,"\n")
else:
adjFieldsWithAction.append((element,[("Rotate",new_direction),("Move",1)]))
print("jestem na: ",x,y,"\nrotuje na: ",element.x,element.y,"\nkierunek obecny: ",cur_direction," nowy kierunek: ", new_direction,"\n")
return adjFieldsWithAction
def get_path(self):
cell = self.end
path = [cell.action]
if cell.parent is not None:
while cell.parent is not self.start:
cell = cell.parent
print(cell)
path.append(cell.action)
else:
return path
path.append(self.start.action)
path.reverse()
# print('end path',path)
return [y for x in path for y in x]
def get_path_fields(self):
cell = self.end
path = [cell]
if cell.parent is not None:
while cell.parent is not self.start:
cell = cell.parent
# print(cell)
path.append(cell)
else:
return path
path.append(self.start)
return path
def computeState(self,Y,X,y,x):
dir_x = x - X
dir_y = y-Y
if(dir_x == 1):
return Direction.EAST
if(dir_y == 1):
return Direction.NORTH
if(dir_x == -1):
return Direction.WEST
return Direction.SOUTH
def computeStateFrom(self,Y,X,y,x):
dir_x = x - X
dir_y = y-Y
if(dir_x == 1):
return Direction.WEST
if(dir_y == 1):
return Direction.SOUTH
if(dir_x == -1):
return Direction.EAST
return Direction.NORTH
def update_cell(self, adj, cell,actions):
"""Update adjacent cell.
@param adj adjacent cell to current cell
@param cell current cell being processed
"""
adj.g = cell.g + 10
adj.h = self.get_heuristic(adj)
adj.parent = cell
#lista krotek akcji jakie wykonamy by dojść do adj
#cell.action = adj[1]
adj.action = actions
#print(actions)
if actions[0][0] == 'Rotate':
self.direction = actions[0][1]
adj.f = adj.h + adj.g
#print('Updated coordinates: ', adj.x, adj.y)
def solve(self):
heapq.heappush(self.opened, (self.start.f, self.start))
while len(self.opened):
f, cell = heapq.heappop(self.opened)
if cell is self.end:
return self.get_path()
self.closed.add(cell)
adj_cells = self.get_adjacent_cells(cell.x,cell.y)
for adj_cell in adj_cells:
field = adj_cell[0]
actions = adj_cell[1]
if field.reachable and field not in self.closed:
if (field.f, field) in self.opened:
if field.g > cell.g + 100:
self.update_cell(field, cell, actions)
print("aktualizuje: ",field.x,field.y,"\nakcja: ",actions,"\n")
else:
self.update_cell(field, cell, actions)
print("dodaje nowe: ",field.x,field.y,"\nakcja: ",actions,"\n")
heapq.heappush(self.opened, (field.f, field))