Naprawa ruchu i A*

res/map.txt

@@ -1,12 +1,12 @@
 MP--H---------------------
 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--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-------

src/agent.py

@@ -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_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 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)
                 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] == 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
                 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] == 0:
-                if move[0] == 1:
+                if move[1] == -1:
-                    return 90
-                else:
-                    return 270
-            elif move[1] != 0:
-                if move[1] == 1:
                     return 0
                 else:
+                    self.path.pop(0)
                     return pg.Vector2(move)
+            elif move[1] == 0:
+                if move[0] == -1:
+                    return 270
                 else:
-            if move[0] != 0:
-                if move[0] == 1:
                     return 90
         else:
-                    return pg.Vector2(move)
+            if move[0] == 0:
-            elif move[1] != 0:
+                if move[1] == -1:
-                if move[1] == 0:
                     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):
+    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):  
         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):

src/simulation.py

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