This commit is contained in:
s473575 2023-05-31 21:03:00 +02:00
parent 019c38c344
commit 88eff2c4b7
3 changed files with 125 additions and 166 deletions

Binary file not shown.

Binary file not shown.

View File

@ -3,6 +3,20 @@ import pygame as pg
import heapq
class Node:
def __init__(self, state, parent, cost):
self.state = state
self.parent = parent
self.cost = cost
def __lt__(self, other):
return self.cost < other.cost
class State:
def __init__(self, position, house_list):
self.position = position
self.house_list = house_list
def vector_to_tuple(vector):
tup = (int(vector.x), int(vector.y))
return tup
@ -27,17 +41,19 @@ class Agent:
self.weights = {}
self.orientation = 90
# utworzenie grafu dróg
roads_pos = [vector_to_tuple(pos) for pos in self.simulation.state.road_pos_g + self.simulation.state.road_pos_r]
roads_pos = [vector_to_tuple(pos) for pos in
self.simulation.state.road_pos_g + self.simulation.state.road_pos_r]
roads_pos.sort()
for index, pos in enumerate(roads_pos):
if pos[0] < 0 or pos[0] >= 27 or pos[1] < 0 or pos[1] >= 14:
continue
for another_pos in roads_pos[index:]:
if pos == another_pos or another_pos[0] < 0 or another_pos[0] >= 27 or another_pos[1] < 0 or another_pos[1] >= 14:
if pos == another_pos or another_pos[0] < 0 or another_pos[0] >= 27 or another_pos[1] < 0 or \
another_pos[1] >= 14:
continue
if ((abs(pos[0] - another_pos[0]) == 1 and abs(pos[1] - another_pos[1]) == 0) or (abs(pos[0] - another_pos[0]) == 0 and abs(pos[1] - another_pos[1]) == 1)):
if ((abs(pos[0] - another_pos[0]) == 1 and abs(pos[1] - another_pos[1]) == 0) or (
abs(pos[0] - another_pos[0]) == 0 and abs(pos[1] - another_pos[1]) == 1)):
if pos not in self.graph.keys():
self.graph[pos] = set()
if another_pos not in self.graph.keys():
@ -45,7 +61,7 @@ class Agent:
if another_pos in self.simulation.state.road_pos_r:
weight = 1
elif another_pos in self.simulation.state.road_pos_g:
weight = 3
weight = 100
else:
weight = 1
self.graph[pos].add(another_pos)
@ -53,9 +69,6 @@ class Agent:
self.weights[(pos, another_pos)] = weight
self.weights[(another_pos, pos)] = weight
# dołączenie domów i składowisk do grafu dróg
entities = self.simulation.state.entities
for entity in entities:
@ -78,10 +91,9 @@ class Agent:
if entity.entity_type == 'house':
self.houses[vector_to_tuple(entity.position)] = HousePOI()
self.path = self.A_star()
self.path = self.a_star()
pass
def update(self):
entities = self.simulation.state.entities
for entity in entities:
@ -90,8 +102,8 @@ class Agent:
self.fullness = entity.fullness
self.orientation = entity.orientation
def decide_move(self):
if self.path:
move_a = self.path[0]
move = (move_a[0] - self.current_pos[0], move_a[1] - self.current_pos[1])
@ -143,130 +155,77 @@ class Agent:
return pg.Vector2(move)
else:
return 90
else:
return pg.Vector2(0, 0)
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)]
def get_move_cost(self, start_pos, end_pos):
return self.heuristic(start_pos, end_pos) + self.weight_cost(start_pos, end_pos)
def get_start_state(self):
# entities = self.simulation.state.entities
# orientation - self.orientation
position = self.current_pos
house_list = tuple(self.houses)
start_state = (position, house_list)
return start_state
def get_end_state(self):
position = (0,1)
house_list = ()
end_state = (position, house_list)
return end_state
def succesor(self, state):
def successors(self, state):
successors_pos = self.graph[state[0]]
house_list = state[1]
successors = ()
successors = []
for pos in successors_pos:
if pos in house_list:
house_list = list(house_list)
house_list.remove(pos)
house_list = tuple(house_list)
successors = list(successors)
successors.append((pos, tuple(house_list)))
successors = tuple(successors)
else:
successors = list(successors)
successors.append((pos, tuple(house_list)))
successors = tuple(successors)
return successors
def cost(self, current_node, succ_state):
if succ_state[0] not in current_node.state[1]:
cost = current_node.cost + self.weights[current_node.state[0], succ_state[0]]
else:
cost = float(1)
return cost
'''def A_star(self):
def heuristic(self, succ_state, goal_state):
position = succ_state[0]
if len(succ_state[1]) != 0:
estimated_cost = 0
for house in succ_state[1]:
estimated_cost += abs(position[0] - house[0]) + abs(position[1] - house[1])
else:
goal_position = goal_state[0]
estimated_cost = abs(position[0] - goal_position[0]) + abs(position[1] - goal_position[1])
return estimated_cost
def a_star(self):
fringe = []
explored = set()
istate = self.get_start_state()
goaltest = self.get_end_state()
node = (istate, None)
actions = []
initial_state = (self.current_pos, tuple(self.houses))
goal_state = (self.current_pos, ())
start_node = Node(initial_state, None, 0)
heapq.heappush(fringe, (self.heuristic(istate[0], goaltest[0]), node))
heapq.heappush(fringe, start_node)
while fringe:
_, el = heapq.heappop(fringe)
elem = el[0]
parent = el[1]
current_node = heapq.heappop(fringe)
if elem == goaltest:
while parent is not None:
actions.append(elem[0])
elem = parent
actions.reverse()
return actions
explored.add(elem)
succs = self.succesor(elem)
for succ in succs:
node = (succ, elem)
p = self.heuristic(succ[0], goaltest[0]) + self.weight_cost(elem[0], succ[0])
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] == succ for tup in fringe):
i = next(i for node in enumerate(fringe) if node[1][0] == succ)
if fringe[i][0] > p:
fringe[i] = (p,node)
return actions'''
def A_star(self):
fringe = []
explored = set()
istate = self.get_start_state()
goaltest = self.get_end_state()
node = (istate, None)
if current_node.state == goal_state:
actions = []
while current_node:
actions.append(current_node.state[0])
current_node = current_node.parent
c = (4, 0) in actions
return list(reversed(actions))
heapq.heappush(fringe, (self.heuristic(istate, goaltest), node))
explored.add(current_node.state)
while fringe:
_, el = heapq.heappop(fringe)
elem = el[0]
parent = el[1]
succ_states = self.successors(current_node.state)
if elem == goaltest:
while parent is not None:
actions.append(elem[0])
elem = parent[0]
parent = parent[1]
actions.reverse()
return actions
for succ_state in succ_states:
if succ_state in explored:
continue
explored.add(elem)
g = self.cost(current_node, succ_state)
h = self.heuristic(succ_state, goal_state)
succ_cost = g + h
succ_node = Node(succ_state, current_node, succ_cost)
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
heapq.heappush(fringe, succ_node)
return None
def discover(self):
if self.current_pos in self.houses.keys():