Naprawa ruchu i A*

This commit is contained in:
s473575 2023-05-28 17:33:02 +02:00
parent c443087ab0
commit cc3bad67d5
3 changed files with 93 additions and 51 deletions

View File

@ -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-------

View File

@ -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):

View File

@ -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()