Orientacja i A*

res/map.txt

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

src/agent.py

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

src/simulation.py

@@ -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")
+        texture_file = Path(r"..\res\tiles.png")
-        self.layers = [StructureLayer(self, texture_file, self.state, self.state.roads_pos, ROAD_SPRITE),
+        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: