Naprawa ruchu i A*

This commit is contained in:
Wit Uta 2023-05-15 13:12:27 +02:00
parent c443087ab0
commit 2fe5d96595
3 changed files with 93 additions and 51 deletions

View File

@ -1,12 +1,12 @@
MP--H--------------------- MP--H---------------------
RRRRRRRRRRRRRRRRRRR---G--- RRRRRRRRRRRRRRRRRRR---G---
SZ--R------G------R---GGG- SZ--G------G------R---GGG-
----R------G------R---G--- ----G------G------R---G---
-RRRR------GGGGGGGRRRRR--- -RRRG------GGGGGGGRRRRR---
-R--G-------------R------- -R--G-------------R-------
-R--GGGGGGGGGGGGGGRH------ -R--GGGGGGGGGRRRRRRH------
-R--G---G---GH----R------- -R--G---G---GH----R-------
-R--G---G---GH----RRRRRRR- -R--G---G---G-----RRRRRRR-
-R--G-------H-----R------- -R--G-------H-----R-------
-R--G-----G----G--R------- -R--G-----G----G--R-------
-R--G-----G----G--R------- -R--G-----G----G--R-------

View File

@ -43,7 +43,7 @@ class Agent:
if another_pos not in self.graph.keys(): if another_pos not in self.graph.keys():
self.graph[another_pos] = set() self.graph[another_pos] = set()
if another_pos in self.simulation.state.road_pos_r: if another_pos in self.simulation.state.road_pos_r:
weight = 2 weight = 1
elif another_pos in self.simulation.state.road_pos_g: elif another_pos in self.simulation.state.road_pos_g:
weight = 3 weight = 3
else: else:
@ -79,7 +79,7 @@ class Agent:
self.houses[vector_to_tuple(entity.position)] = HousePOI() self.houses[vector_to_tuple(entity.position)] = HousePOI()
self.path = self.A_star() self.path = self.A_star()
pass
def update(self): def update(self):
@ -92,58 +92,60 @@ class Agent:
def decide_move(self): def decide_move(self):
move_a = self.path[0]
move = (move_a[0] - self.current_pos[0], move_a[1] - self.current_pos[1])
move = self.path.pop
if self.orientation == 0: if self.orientation == 0:
if move[0] != 0: if move[0] == 0:
if move[0] == 1: if move[1] == -1:
return 90 self.path.pop(0)
else:
return 270
elif move[1] != 0:
if move[1] == 1:
return pg.Vector2(move) return pg.Vector2(move)
else: else:
return 180 return 180
elif move[1] == 0:
if move[0] == -1:
return 270
else:
return 90
elif self.orientation == 90: elif self.orientation == 90:
if move[0] != 0: if move[0] == 0:
if move[0] == 1: if move[1] == -1:
return pg.Vector2(move)
else:
return 270
elif move[1] != 0:
if move[1] == 1:
return 0 return 0
else: else:
return 180 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: elif self.orientation == 180:
if move[0] != 0: if move[0] == 0:
if move[0] == 1: if move[1] == -1:
return 90
else:
return 270
elif move[1] != 0:
if move[1] == 1:
return 0 return 0
else: else:
self.path.pop(0)
return pg.Vector2(move) return pg.Vector2(move)
else: elif move[1] == 0:
if move[0] != 0: if move[0] == -1:
if move[0] == 1: return 270
return 90
else: else:
return pg.Vector2(move) return 90
elif move[1] != 0: else:
if move[1] == 0: if move[0] == 0:
if move[1] == -1:
return 0 return 0
else: else:
return 180 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): def heuristic(self, start, end):
return abs((end_pos[0] - start_pos[0])) + abs((end_pos[1] - start_pos[1])) 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): def weight_cost(self, start_pos, end_pos):
return self.weights[(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) return self.heuristic(start_pos, end_pos) + self.weight_cost(start_pos, end_pos)
def get_start_state(self): def get_start_state(self):
entities = self.simulation.state.entities # entities = self.simulation.state.entities
# orientation - self.orientation # orientation - self.orientation
position = self.current_pos position = self.current_pos
house_list = tuple(self.houses) house_list = tuple(self.houses)
@ -186,12 +188,13 @@ class Agent:
return successors return successors
def A_star(self): '''def A_star(self):
fringe = [] fringe = []
explored = set() explored = set()
istate = self.get_start_state() istate = self.get_start_state()
goaltest = self.get_end_state() goaltest = self.get_end_state()
node = (istate, None) node = (istate, None)
actions = []
heapq.heappush(fringe, (self.heuristic(istate[0], goaltest[0]), node)) heapq.heappush(fringe, (self.heuristic(istate[0], goaltest[0]), node))
@ -201,7 +204,6 @@ class Agent:
parent = el[1] parent = el[1]
if elem == goaltest: if elem == goaltest:
actions = []
while parent is not None: while parent is not None:
actions.append(elem[0]) actions.append(elem[0])
elem = parent elem = parent
@ -222,8 +224,48 @@ class Agent:
i = next(i for node in enumerate(fringe) if node[1][0] == succ) i = next(i for node in enumerate(fringe) if node[1][0] == succ)
if fringe[i][0] > p: if fringe[i][0] > p:
fringe[i] = (p,node) 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): def discover(self):

View File

@ -276,9 +276,9 @@ class Interface:
def update(self): def update(self):
self.state.update(self.move_truck) self.state.update(self.move_truck)
self.agent.update() self.agent.update()
if isinstance(self.move_truck, int): '''if isinstance(self.move_truck, int):
self.state.update(self.move_truck) self.state.update(self.move_truck)
self.agent.update() self.agent.update()'''
def render(self): def render(self):
if not self.debug_mode: if not self.debug_mode:
@ -298,5 +298,5 @@ class Interface:
self.processAgentInput() self.processAgentInput()
self.update() self.update()
self.render() self.render()
self.clock.tick(24) self.clock.tick(12)
pg.quit() pg.quit()