Orientacja i A*

This commit is contained in:
473575 2023-05-15 10:58:18 +02:00
parent b55d836f59
commit 86c5ad6fd1
4 changed files with 220 additions and 34 deletions

View File

@ -1,14 +1,14 @@
MP--H---------------------
ORRRRRRRRRRRRRRRRRR---R---
SZ--R------R------R---RRR-
----R------R------R---R---
-RRRR------RRRRRRRRRRRR---
-R--R-------------R-------
-R--RRRRRRRRRRRRRRRH------
-R--R---R---RH----R-------
-R--R---R---RH----RRRRRRR-
-R--R-------H-----R-------
-R--R-----R----R--R-------
-R--R-----R----R--R-------
-RRRRRRRRRRRRRRRRRRRRRRR--
RRRRRRRRRRRRRRRRRRR---G---
SZ--R------G------R---GGG-
----R------G------R---G---
-RRRR------GGGGGGGRRRRR---
-R--G-------------R-------
-R--GGGGGGGGGGGGGGRH------
-R--G---G---GH----R-------
-R--G---G---GH----RRRRRRR-
-R--G-------H-----R-------
-R--G-----G----G--R-------
-R--G-----G----G--R-------
-RRRRRRRRRRRRRRRRRRRORRR--
--------------------------

Binary file not shown.

Before

Width:  |  Height:  |  Size: 14 KiB

After

Width:  |  Height:  |  Size: 16 KiB

View File

@ -1,5 +1,6 @@
from random import randint
import random
import pygame as pg
import heapq
def vector_to_tuple(vector):
@ -23,20 +24,37 @@ class Agent:
self.houses = {}
self.dumps = {}
self.fullness = None
self.weights = {}
self.orientation = 90
# utworzenie grafu dróg
roads_pos = [vector_to_tuple(pos) for pos in self.simulation.state.roads_pos]
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:
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]) <= 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():
self.graph[another_pos] = set()
if another_pos in self.simulation.state.road_pos_r:
weight = 2
elif another_pos in self.simulation.state.road_pos_g:
weight = 3
else:
weight = 1
self.graph[pos].add(another_pos)
self.graph[another_pos].add(pos)
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
@ -46,29 +64,160 @@ class Agent:
(entity_pos[0]+1, entity_pos[1]),
(entity_pos[0], entity_pos[1]-1),
(entity_pos[0], entity_pos[1]+1)]:
if neighbour_pos[0] < 0 or neighbour_pos[0] >= 27 or neighbour_pos[1] < 0 or neighbour_pos[1] >= 14:
continue
if neighbour_pos in roads_pos:
if entity_pos not in self.graph.keys():
self.graph[entity_pos] = set()
self.graph[entity_pos].add(neighbour_pos)
self.graph[neighbour_pos].add(entity_pos)
self.weights[(entity_pos, neighbour_pos)] = 1
self.weights[(neighbour_pos, entity_pos)] = 1
if entity.entity_type == 'dump':
self.dumps[entity.trash_type] = vector_to_tuple(entity.position)
if entity.entity_type == 'house':
self.houses[vector_to_tuple(entity.position)] = HousePOI()
self.path = self.A_star()
def update(self):
entities = self.simulation.state.entities
for entity in entities:
if entity.entity_type == 'truck':
self.current_pos = vector_to_tuple(entity.position)
self.fullness = entity.fullness
self.orientation = entity.orientation
def decide_move(self):
possible_positions = self.graph[self.current_pos]
possible_moves = [pg.Vector2(pos[0] - self.current_pos[0], pos[1] - self.current_pos[1]) for pos in possible_positions]
move = possible_moves[randint(0, len(possible_moves)-1)]
return move
move = self.path.pop
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:
return pg.Vector2(move)
else:
return 180
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:
return 0
else:
return 180
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:
return 0
else:
return pg.Vector2(move)
else:
if move[0] != 0:
if move[0] == 1:
return 90
else:
return pg.Vector2(move)
elif move[1] != 0:
if move[1] == 0:
return 0
else:
return 180
def heuristic(self, start_pos, end_pos):
return abs((end_pos[0] - start_pos[0])) + abs((end_pos[1] - start_pos[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
for entity in entities:
if entity.entity_type == 'truck':
orientation = entity.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):
successors_pos = self.graph[state[0]]
house_list = [state[1]]
successors = ()
for pos in successors_pos:
if pos in house_list:
house_list.remove(pos)
successors += ((pos, tuple(house_list)),)
else:
successors += ((pos, tuple(house_list)),)
return successors
def A_star(self):
fringe = []
explored = set()
istate = self.get_start_state()
goaltest = self.get_end_state()
node = (istate, None)
heapq.heappush(fringe, (self.heuristic(istate[0], goaltest[0]), node))
while fringe:
_, el = heapq.heappop(fringe)
elem = el[0]
parent = el[1]
if elem == goaltest:
actions = []
while parent is not None:
actions.append(elem[0])
elem = parent
actions.reverse()
return actions
explored.add(elem)
for succ in self.succesor(elem):
node = (succ, elem)
p = self.heuristic(succ[0], goaltest[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 False
def discover(self):
if self.current_pos in self.houses.keys():

View File

@ -4,14 +4,18 @@ from random import randint
import pygame as pg
from agent import Agent
ROAD_SPRITE = pg.Vector2(0, 0)
ROAD_SPRITE_R = pg.Vector2(0, 0)
ROAD_SPRITE_G = pg.Vector2(0, 1)
HOUSE_WITHOUT_TRASH_SPRITE = pg.Vector2(1, 0)
HOUSE_WITH_TRASH_SPRITE = pg.Vector2(1, 1)
HOUSE_SPRITES = {0: HOUSE_WITHOUT_TRASH_SPRITE,
1: HOUSE_WITH_TRASH_SPRITE}
TRUCK_SPRITE = pg.Vector2(2, 0)
TRUCK_SPRITE_R = pg.Vector2(2, 0)
TRUCK_SPRITE_D = pg.Vector2(2,1)
TRUCK_SPRITE_L = pg.Vector2(2,2)
TRUCK_SPRITE_U = pg.Vector2(2,3)
PAPER_DUMP_SPRITE = pg.Vector2(3, 0)
PLASTIC_DUMP_SPRITE = pg.Vector2(3, 1)
@ -34,18 +38,31 @@ class Entity:
class TruckEntity(Entity):
def __init__(self, state, position):
super().__init__(state, position)
self.tile = TRUCK_SPRITE
self.entity_type = 'truck'
self.tile = TRUCK_SPRITE_R
self.fullness = {'paper': 0,
'glass': 0,
'plastic': 0,
'mixed': 0}
self.orientation = 90
def rotate_img(self, orientation):
self.orientation = orientation
if orientation == 0:
self.tile = TRUCK_SPRITE_U
elif orientation == 90:
self.tile = TRUCK_SPRITE_R
elif orientation == 180:
self.tile = TRUCK_SPRITE_D
elif orientation == 270:
self.tile = TRUCK_SPRITE_L
def move(self, move_vector):
proposed_pos = self.position + move_vector
move_valid = True
if proposed_pos not in self.state.roads_pos \
if proposed_pos not in self.state.road_pos_r \
and proposed_pos not in self.state.road_pos_g \
and proposed_pos not in self.state.houses_pos \
and proposed_pos not in self.state.dumps_pos:
move_valid = False
@ -86,13 +103,14 @@ class DumpEntity(Entity):
class SimulationState:
def __init__(self):
self.roads_pos = []
self.road_pos_r = []
self.road_pos_g = []
self.houses_pos = []
self.dumps_pos = []
self.entities = []
# stworzenie mapy i jednostek na podstawie pliku txt
map_path = Path("../res/map.txt")
map_path = Path(r"..\res\map.txt")
with open(map_path, "r") as map_file:
map_data = map_file.readlines()
max_x = len(map_data[0].replace('\n', ''))
@ -103,9 +121,11 @@ class SimulationState:
if tile == "O":
self.truck_origin = pg.Vector2(x, y)
self.roads_pos.append(pg.Vector2(x, y))
self.road_pos_r.append(pg.Vector2(x, y))
if tile == "R":
self.roads_pos.append(pg.Vector2(x, y))
self.road_pos_r.append(pg.Vector2(x, y))
if tile == "G":
self.road_pos_g.append(pg.Vector2(x,y))
if tile == "H":
self.houses_pos.append(pg.Vector2(x, y))
self.entities.append(HouseEntity(self, pg.Vector2(x, y)))
@ -132,7 +152,12 @@ class SimulationState:
def update(self, move_agent):
for entity in self.entities:
if entity.entity_type == 'truck':
entity.move(move_agent)
if isinstance(move_agent, int):
entity.orientation = move_agent
entity.rotate_img(move_agent)
else:
entity.move(move_agent)
class Layer:
@ -140,7 +165,7 @@ class Layer:
self.sim = sim
self.texture_atlas = pg.image.load(texture_file)
def renderTile(self, surface, position, tile):
def renderTile(self, surface, position, tile, orientation = 90, rotate=False):
# pozycja na ekranie
sprite_pos = position.elementwise() * self.sim.cell_size
@ -151,6 +176,7 @@ class Layer:
self.sim.cell_size.x,
self.sim.cell_size.y)
# render
surface.blit(self.texture_atlas, sprite_pos, texture)
@ -166,7 +192,10 @@ class EntityLayer(Layer):
def render(self, surface):
for entity in self.entities:
self.renderTile(surface, entity.position, entity.tile)
if entity.entity_type == 'truck':
self.renderTile(surface, entity.position, entity.tile, entity.orientation, rotate=True)
else:
self.renderTile(surface, entity.position, entity.tile)
class StructureLayer(Layer):
@ -194,8 +223,9 @@ class Interface:
# rendering
self.cell_size = pg.Vector2(64, 64)
texture_file = Path("../res/tiles.png")
self.layers = [StructureLayer(self, texture_file, self.state, self.state.roads_pos, ROAD_SPRITE),
texture_file = Path(r"..\res\tiles.png")
self.layers = [StructureLayer(self, texture_file, self.state, self.state.road_pos_r, ROAD_SPRITE_R ),
StructureLayer(self, texture_file, self.state, self.state.road_pos_g, ROAD_SPRITE_G ),
EntityLayer(self, texture_file, self.state, self.state.entities)]
# okno
@ -227,12 +257,16 @@ class Interface:
if self.debug_mode:
if event.key == pg.K_RIGHT:
self.move_truck.x = 1
if event.key == pg.K_LEFT:
self.move_truck.x = -1
if event.key == pg.K_DOWN:
self.move_truck.y = 1
if event.key == pg.K_UP:
self.move_truck.y = -1
if event.key == pg.K_d:
self.agent.discover()
@ -242,6 +276,9 @@ class Interface:
def update(self):
self.state.update(self.move_truck)
self.agent.update()
if isinstance(self.move_truck, int):
self.state.update(self.move_truck)
self.agent.update()
def render(self):
if not self.debug_mode: