Naprawa ruchu i A*
This commit is contained in:
parent
c443087ab0
commit
cc3bad67d5
10
res/map.txt
10
res/map.txt
@ -1,12 +1,12 @@
|
||||
MP--H---------------------
|
||||
RRRRRRRRRRRRRRRRRRR---G---
|
||||
SZ--R------G------R---GGG-
|
||||
----R------G------R---G---
|
||||
-RRRR------GGGGGGGRRRRR---
|
||||
SZ--G------G------R---GGG-
|
||||
----G------G------R---G---
|
||||
-RRRG------GGGGGGGRRRRR---
|
||||
-R--G-------------R-------
|
||||
-R--GGGGGGGGGGGGGGRH------
|
||||
-R--GGGGGGGGGRRRRRRH------
|
||||
-R--G---G---GH----R-------
|
||||
-R--G---G---GH----RRRRRRR-
|
||||
-R--G---G---G-----RRRRRRR-
|
||||
-R--G-------H-----R-------
|
||||
-R--G-----G----G--R-------
|
||||
-R--G-----G----G--R-------
|
||||
|
122
src/agent.py
122
src/agent.py
@ -43,7 +43,7 @@ class Agent:
|
||||
if another_pos not in self.graph.keys():
|
||||
self.graph[another_pos] = set()
|
||||
if another_pos in self.simulation.state.road_pos_r:
|
||||
weight = 2
|
||||
weight = 1
|
||||
elif another_pos in self.simulation.state.road_pos_g:
|
||||
weight = 3
|
||||
else:
|
||||
@ -79,7 +79,7 @@ class Agent:
|
||||
self.houses[vector_to_tuple(entity.position)] = HousePOI()
|
||||
|
||||
self.path = self.A_star()
|
||||
|
||||
pass
|
||||
|
||||
|
||||
def update(self):
|
||||
@ -92,58 +92,60 @@ class Agent:
|
||||
|
||||
|
||||
def decide_move(self):
|
||||
|
||||
|
||||
|
||||
move = self.path.pop
|
||||
move_a = self.path[0]
|
||||
move = (move_a[0] - self.current_pos[0], move_a[1] - self.current_pos[1])
|
||||
|
||||
if self.orientation == 0:
|
||||
if move[0] != 0:
|
||||
if move[0] == 1:
|
||||
return 90
|
||||
else:
|
||||
return 270
|
||||
elif move[1] != 0:
|
||||
if move[1] == 1:
|
||||
if move[0] == 0:
|
||||
if move[1] == -1:
|
||||
self.path.pop(0)
|
||||
return pg.Vector2(move)
|
||||
else:
|
||||
return 180
|
||||
elif move[1] == 0:
|
||||
if move[0] == -1:
|
||||
return 270
|
||||
else:
|
||||
return 90
|
||||
elif self.orientation == 90:
|
||||
if move[0] != 0:
|
||||
if move[0] == 1:
|
||||
return pg.Vector2(move)
|
||||
else:
|
||||
return 270
|
||||
elif move[1] != 0:
|
||||
if move[1] == 1:
|
||||
if move[0] == 0:
|
||||
if move[1] == -1:
|
||||
return 0
|
||||
else:
|
||||
return 180
|
||||
elif move[1] == 0:
|
||||
if move[0] == -1:
|
||||
return 270
|
||||
else:
|
||||
self.path.pop(0)
|
||||
return pg.Vector2(move)
|
||||
elif self.orientation == 180:
|
||||
if move[0] != 0:
|
||||
if move[0] == 1:
|
||||
return 90
|
||||
else:
|
||||
return 270
|
||||
elif move[1] != 0:
|
||||
if move[1] == 1:
|
||||
if move[0] == 0:
|
||||
if move[1] == -1:
|
||||
return 0
|
||||
else:
|
||||
self.path.pop(0)
|
||||
return pg.Vector2(move)
|
||||
else:
|
||||
if move[0] != 0:
|
||||
if move[0] == 1:
|
||||
return 90
|
||||
elif move[1] == 0:
|
||||
if move[0] == -1:
|
||||
return 270
|
||||
else:
|
||||
return pg.Vector2(move)
|
||||
elif move[1] != 0:
|
||||
if move[1] == 0:
|
||||
return 90
|
||||
else:
|
||||
if move[0] == 0:
|
||||
if move[1] == -1:
|
||||
return 0
|
||||
else:
|
||||
return 180
|
||||
elif move[1] == 0:
|
||||
if move[0] == -1:
|
||||
self.path.pop(0)
|
||||
return pg.Vector2(move)
|
||||
else:
|
||||
return 90
|
||||
|
||||
def heuristic(self, start_pos, end_pos):
|
||||
return abs((end_pos[0] - start_pos[0])) + abs((end_pos[1] - start_pos[1]))
|
||||
def heuristic(self, start, end):
|
||||
return abs((end[0][0] - start[0][0])) + abs((end[0][1] - start[0][1])) // (abs(len(start[1]) - len(end[1])) + 1)
|
||||
|
||||
def weight_cost(self, start_pos, end_pos):
|
||||
return self.weights[(start_pos, end_pos)]
|
||||
@ -152,7 +154,7 @@ class Agent:
|
||||
return self.heuristic(start_pos, end_pos) + self.weight_cost(start_pos, end_pos)
|
||||
|
||||
def get_start_state(self):
|
||||
entities = self.simulation.state.entities
|
||||
# entities = self.simulation.state.entities
|
||||
# orientation - self.orientation
|
||||
position = self.current_pos
|
||||
house_list = tuple(self.houses)
|
||||
@ -186,12 +188,13 @@ class Agent:
|
||||
return successors
|
||||
|
||||
|
||||
def A_star(self):
|
||||
'''def A_star(self):
|
||||
fringe = []
|
||||
explored = set()
|
||||
istate = self.get_start_state()
|
||||
goaltest = self.get_end_state()
|
||||
node = (istate, None)
|
||||
actions = []
|
||||
|
||||
heapq.heappush(fringe, (self.heuristic(istate[0], goaltest[0]), node))
|
||||
|
||||
@ -201,7 +204,6 @@ class Agent:
|
||||
parent = el[1]
|
||||
|
||||
if elem == goaltest:
|
||||
actions = []
|
||||
while parent is not None:
|
||||
actions.append(elem[0])
|
||||
elem = parent
|
||||
@ -222,8 +224,48 @@ class Agent:
|
||||
i = next(i for node in enumerate(fringe) if node[1][0] == succ)
|
||||
if fringe[i][0] > p:
|
||||
fringe[i] = (p,node)
|
||||
return False
|
||||
return actions'''
|
||||
|
||||
def A_star(self):
|
||||
fringe = []
|
||||
explored = set()
|
||||
istate = self.get_start_state()
|
||||
goaltest = self.get_end_state()
|
||||
node = (istate, None)
|
||||
actions = []
|
||||
|
||||
heapq.heappush(fringe, (self.heuristic(istate, goaltest), node))
|
||||
|
||||
while fringe:
|
||||
_, el = heapq.heappop(fringe)
|
||||
elem = el[0]
|
||||
parent = el[1]
|
||||
|
||||
if elem == goaltest:
|
||||
while parent is not None:
|
||||
actions.append(elem[0])
|
||||
elem = parent[0]
|
||||
parent = parent[1]
|
||||
actions.reverse()
|
||||
return actions
|
||||
|
||||
explored.add(elem)
|
||||
|
||||
succs = self.succesor(elem)
|
||||
for succ in succs:
|
||||
node = (succ, el)
|
||||
heuristic = self.heuristic(succ, goaltest)
|
||||
cost = 7*self.weight_cost(elem[0], succ[0])
|
||||
p = heuristic + cost
|
||||
|
||||
if succ not in explored and not any(tup[1][0] == succ for tup in fringe):
|
||||
heapq.heappush(fringe, (p, node))
|
||||
elif any(tup[1][0] == succ for tup in fringe):
|
||||
i = next(i for i, node in enumerate(fringe) if node[1][0] == succ)
|
||||
if fringe[i][0] > p:
|
||||
fringe[i] = (p, node)
|
||||
|
||||
return actions
|
||||
|
||||
|
||||
def discover(self):
|
||||
|
@ -276,9 +276,9 @@ class Interface:
|
||||
def update(self):
|
||||
self.state.update(self.move_truck)
|
||||
self.agent.update()
|
||||
if isinstance(self.move_truck, int):
|
||||
'''if isinstance(self.move_truck, int):
|
||||
self.state.update(self.move_truck)
|
||||
self.agent.update()
|
||||
self.agent.update()'''
|
||||
|
||||
def render(self):
|
||||
if not self.debug_mode:
|
||||
@ -298,5 +298,5 @@ class Interface:
|
||||
self.processAgentInput()
|
||||
self.update()
|
||||
self.render()
|
||||
self.clock.tick(24)
|
||||
self.clock.tick(12)
|
||||
pg.quit()
|
Loading…
Reference in New Issue
Block a user