minor implementations of state logic

decision/StateTree.py

@@ -23,7 +23,6 @@ class StateTree:
 
     def expansion(self, from_state: State) -> List[State]:
         return []
-    #
     # a* na przestrzeni stanow
     # heura -> np manhatan
     # funkcja kosztu zroznicowany

test/dlaMarcina.py

@@ -1,23 +1,25 @@
 import queue
 from typing import List
+from typing import Optional, Dict, Tuple
 
 from data.Direction import Direction
 from data.GameConstants import GameConstants
 from decision.ActionType import ActionType
 from util.PathDefinitions import GridLocation
 from util.PriorityQueue import PriorityQueue
-
+from decision.StateTree import StateTree
 
 
 class PathFinderState:
 
-    def __init__(self, agent_position: GridLocation, agent_direction: Direction, cost: float):
+    def __init__(self, agent_position: GridLocation, agent_direction: Direction, cost: float,
+                 last_action: ActionType, action_taken: List[ActionType]):
         super().__init__()
         self.agent_position = agent_position
         self.agent_direction = agent_direction
         self.cost = cost
-        self.last_action: ActionType
+        self.last_action = last_action
-        self.action_taken: List[ActionType]
+        self.action_taken = action_taken
 
     def getActionTaken(self):
         return self.getActionTaken()
@@ -36,23 +38,125 @@ class PathFinderOnStates:
         self.queue = PriorityQueue()
         self.queue.put(root_state, root_state.cost)
 
-    def heuristic(self) -> float:
+    def heuristic(self, a: Tuple[int, int], b: Tuple[int, int]) -> float:
         # tutaj mozna uzyc heury np. manhatan distance (zmodyfikowany bo masz obroty a to zmienia oplacalnosc)
-        pass
+        (x1, y1) = a
+        (x2, y2) = b
+        return abs(x1 - x2) + abs(y1 - y2)
 
-    def evaluate(self, state: PathFinderState) -> float:
+    def evaluate(self, currState: PathFinderState) -> float:
-        # koszt dojscia do danego stanu
+        # koszt dojscia do danego stanu+ heura
-        pass
+        return currState.cost + self.heuristic(currState.agent_position, self.goal)
 
-    def expansion(self, state: PathFinderState) -> List[PathFinderState]:
+    def getPositionAfterMove(self, currState: PathFinderState) -> GridLocation:
+        result : GridLocation
+        if currState.agent_position == Direction.top:
+            currState.agent_position[0] += 1
+            result = currState.agent_position
+        elif currState.agent_position == Direction.down:
+            currState.agent_position[0] -= 1
+            result= currState.agent_position
+        elif currState.agent_position == Direction.right:
+            currState.agent_position[1] += 1
+            result = currState.agent_position
+        elif currState.agent_position == Direction.right:
+            currState.agent_position[1] -= 1
+            result = currState.agent_position
+        return result
+
+    def isMovePossible(self, currState: PathFinderState)-> bool:
+        positionAfterMove =self.getPositionAfterMove(currState)
+        if positionAfterMove in self.game_constants.walls:
+            return False
+        elif positionAfterMove in self.game_constants.grid_height:
+            return False
+        elif positionAfterMove in self.game_constants.grid_width:
+            return False
+        else:
+            return True
+
+    def createState(self, currState: PathFinderState, action: ActionType) -> PathFinderState:
+        cost = currState.cost + 1
+        last_action = action
+        action_taken = currState.action_taken.append(action)
+        agent_position = currState.agent_position
+        agent_direction = currState.agent_direction
+
+        if action == ActionType.ROTATE_UP:
+            agent_direction = Direction.top
+        elif action == ActionType.ROTATE_DOWN:
+            agent_direction = Direction.down
+        elif action == ActionType.ROTATE_LEFT:
+            agent_direction = Direction.left
+        elif action == ActionType.ROTATE_RIGHT:
+            agent_direction = Direction.right
+        elif action == ActionType.MOVE:
+            agent_position = self.getPositionAfterMove(currState)
+
+        return PathFinderState(agent_position, agent_direction, cost, last_action, action_taken)
+
+    def expansion(self, currState: PathFinderState) -> List[PathFinderState]:
         # dla stanu sprawdzamy jakie akcje z tego miejsca mozemy podjac (ActionType)
-        pass
+        # reprezentacja kazdego stanu co moge podjac z tego miejsca
+        # generowanie stanu
+        # sprawdz w ktorym kierunku obrocony
+        possibleNextStates: List[PathFinderState]
+        if currState.agent_direction == Direction.top:
+            possibleNextStates.append(self.createState(currState, ActionType.ROTATE_RIGHT))
+            possibleNextStates.append(self.createState(currState, ActionType.ROTATE_LEFT))
+            possibleNextStates.append(self.createState(currState, ActionType.ROTATE_DOWN))
+            if self.isMovePossible(currState):
+                possibleNextStates.append(self.createState(currState,ActionType.MOVE))
+        elif currState.agent_direction == Direction.down:
+            possibleNextStates.append(self.createState(currState, ActionType.ROTATE_RIGHT))
+            possibleNextStates.append(self.createState(currState, ActionType.ROTATE_LEFT))
+            possibleNextStates.append(self.createState(currState, ActionType.ROTATE_UP))
+            if self.isMovePossible(currState):
+                possibleNextStates.append(self.createState(currState, ActionType.MOVE))
+        elif currState.agent_direction == Direction.left:
+            possibleNextStates.append(self.createState(currState, ActionType.ROTATE_RIGHT))
+            possibleNextStates.append(self.createState(currState, ActionType.ROTATE_UP))
+            possibleNextStates.append(self.createState(currState, ActionType.ROTATE_DOWN))
+            if self.isMovePossible(currState):
+                possibleNextStates.append(self.createState(currState, ActionType.MOVE))
+        elif currState.agent_direction == Direction.right:
+            possibleNextStates.append(self.createState(currState, ActionType.ROTATE_UPT))
+            possibleNextStates.append(self.createState(currState, ActionType.ROTATE_LEFT))
+            possibleNextStates.append(self.createState(currState, ActionType.ROTATE_DOWN))
+            if self.isMovePossible(currState):
+                possibleNextStates.append(self.createState(currState, ActionType.MOVE))
+        return possibleNextStates
 
     def getActionList(self) -> List[ActionType]:
         best_state: PathFinderState = self.queue.get()
 
-        while best_state.agent_position != self.goal or self.queue.empty():
+        while best_state.agent_position != self.goal and not self.queue.empty():
             #dodajesz do kolejki stany z expansion (po cost)
+            for state in self.expansion(best_state):
+                self.queue.put(state, self.evaluate(state))
+
             best_state = self.queue.get()
 
         return best_state.getActionTaken()
+    # do kosztu dokładam koszt starego stanu plus 1
+    # def a_star(self,stateTree:StateTree, start: Tuple[int, int], goal: Tuple[int, int]):
+    #     frontier = PriorityQueue()
+    #     frontier.put(start, 0)
+    #     came_from = dict()
+    #     cost_so_far = dict()
+    #     came_from[start] = None
+    #     cost_so_far[start] = 0
+    #
+    #     while not frontier.empty():
+    #         current = frontier.get()
+    #
+    #         if current == goal:
+    #             break
+    #
+    #         for next in graph.neighbors(current):
+    #             new_cost = cost_so_far[current] + graph.cost(current, next)
+    #             if next not in cost_so_far or new_cost < cost_so_far[next]:
+    #                 cost_so_far[next] = new_cost
+    #                 priority = new_cost + heuristic(goal, next)
+    #                 frontier.put(next, priority)
+    #                 came_from[next] = current