from typing import List from typing import Tuple from data.GameConstants import GameConstants from data.enum.Direction import Direction from decision.Action import Action from decision.ActionType import ActionType from pathfinding.PathFinderState import PathFinderState from pathfinding.PrioritizedItem import PrioritizedItem from util.PathDefinitions import GridLocation from util.PriorityQueue import PriorityQueue class PathFinderOnStates: def __init__(self, game_constants: GameConstants, goal: GridLocation, root_state: PathFinderState): super().__init__() self.game_constants = game_constants self.goal = goal self.queue = PriorityQueue() self.queue.put(PrioritizedItem(root_state.cost, root_state), root_state.cost) 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) (x1, y1) = a (x2, y2) = b return abs(x1 - x2) + abs(y1 - y2) def evaluate(self, curr_state: PathFinderState) -> float: # koszt dojscia do danego stanu+ heura return curr_state.cost + self.heuristic(curr_state.agent_position, self.goal) def get_position_after_move(self, curr_state: PathFinderState) -> GridLocation: if curr_state.agent_direction == Direction.top: return curr_state.agent_position[0], curr_state.agent_position[1] + 1 elif curr_state.agent_direction == Direction.down: return curr_state.agent_position[0], curr_state.agent_position[1] - 1 elif curr_state.agent_direction == Direction.right: return curr_state.agent_position[0] + 1, curr_state.agent_position[1] elif curr_state.agent_direction == Direction.left: return curr_state.agent_position[0] - 1, curr_state.agent_position[1] def is_move_possible(self, curr_state: PathFinderState) -> bool: position_after_move = self.get_position_after_move(curr_state) if position_after_move in self.game_constants.walls: return False elif position_after_move[0] < 0 or position_after_move[0] > self.game_constants.grid_width: return False elif position_after_move[1] < 0 or position_after_move[1] > self.game_constants.grid_height: return False else: return True def create_state(self, curr_state: PathFinderState, action: Action) -> PathFinderState: if action == action.action_type.MOVE: if curr_state.agent_position in self.game_constants.diffTerrain: cost = curr_state.cost + 20 # tutaj koszt kaluzy else: cost = curr_state.cost + 1 # TODO: jezeli bedziemy rozpatrywac rozne stany to nalezy rozbic na kazdy mozliwy obrot else: cost = curr_state.cost + 10 last_action = action action_taken: List[Action] = [] action_taken.extend(curr_state.action_taken) action_taken.append(last_action) agent_position = curr_state.agent_position agent_direction = curr_state.agent_direction if action.action_type == ActionType.ROTATE_UP: agent_direction = Direction.top elif action.action_type == ActionType.ROTATE_DOWN: agent_direction = Direction.down elif action.action_type == ActionType.ROTATE_LEFT: agent_direction = Direction.left elif action.action_type == ActionType.ROTATE_RIGHT: agent_direction = Direction.right elif action.action_type == ActionType.MOVE: agent_position = self.get_position_after_move(curr_state) return PathFinderState(agent_position, agent_direction, cost, last_action, action_taken) # Funkcja następnika def expansion(self, curr_state: PathFinderState) -> List[PathFinderState]: # dla stanu sprawdzamy jakie akcje z tego miejsca mozemy podjac (ActionType) # reprezentacja kazdego stanu co moge podjac z tego miejsca # generowanie stanu # sprawdz w ktorym kierunku obrocony possible_next_states: List[PathFinderState] = [] if self.is_move_possible(curr_state): possible_next_states.append(self.create_state(curr_state, Action(ActionType.MOVE))) if curr_state.agent_direction == Direction.top: possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_RIGHT))) possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_LEFT))) possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_DOWN))) elif curr_state.agent_direction == Direction.down: possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_RIGHT))) possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_LEFT))) possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_UP))) elif curr_state.agent_direction == Direction.left: possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_RIGHT))) possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_UP))) possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_DOWN))) elif curr_state.agent_direction == Direction.right: possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_UP))) possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_LEFT))) possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_DOWN))) return possible_next_states def get_action_list(self) -> List[Action]: already_visited = {} while not self.queue.empty(): item: PrioritizedItem = self.queue.get() best_state: PathFinderState = item.item if best_state.agent_position == self.goal or (self.heuristic(best_state.agent_position, self.goal) == 1 and self.goal in self.game_constants.walls): break for state in self.expansion(best_state): s_tuple = (state.agent_position[0], state.agent_position[1], state.agent_direction) if s_tuple not in already_visited: priority = self.evaluate(state) self.queue.put(PrioritizedItem(priority, state), priority) already_visited[s_tuple] = state return best_state.action_taken