SI_InteligentnyWozekWidlowy/pathfinding/PathfinderOnStates.py

140 lines
6.6 KiB
Python
Raw Normal View History

2022-04-28 21:22:19 +02:00
from typing import List
from typing import Tuple
from data.GameConstants import GameConstants
2022-05-11 19:05:44 +02:00
from data.enum.Direction import Direction
2022-05-22 16:27:36 +02:00
from decision.Action import Action
2022-04-28 21:22:19 +02:00
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
2022-05-22 16:27:36 +02:00
def create_state(self, curr_state: PathFinderState, action: Action) -> PathFinderState:
2022-04-28 21:22:19 +02:00
2022-05-22 16:27:36 +02:00
if action == action.action_type.MOVE:
2022-04-28 21:22:19 +02:00
if curr_state.agent_position in self.game_constants.diffTerrain:
cost = curr_state.cost + 20
# tutaj koszt kaluzy
else:
cost = curr_state.cost + 1
2022-05-22 16:27:36 +02:00
# TODO: jezeli bedziemy rozpatrywac rozne stany to nalezy rozbic na kazdy mozliwy obrot
2022-04-28 21:22:19 +02:00
else:
cost = curr_state.cost + 10
last_action = action
2022-05-22 16:27:36 +02:00
action_taken: List[Action] = []
2022-04-28 21:22:19 +02:00
action_taken.extend(curr_state.action_taken)
action_taken.append(last_action)
agent_position = curr_state.agent_position
agent_direction = curr_state.agent_direction
2022-05-22 16:27:36 +02:00
if action.action_type == ActionType.ROTATE_UP:
2022-04-28 21:22:19 +02:00
agent_direction = Direction.top
2022-05-22 16:27:36 +02:00
elif action.action_type == ActionType.ROTATE_DOWN:
2022-04-28 21:22:19 +02:00
agent_direction = Direction.down
2022-05-22 16:27:36 +02:00
elif action.action_type == ActionType.ROTATE_LEFT:
2022-04-28 21:22:19 +02:00
agent_direction = Direction.left
2022-05-22 16:27:36 +02:00
elif action.action_type == ActionType.ROTATE_RIGHT:
2022-04-28 21:22:19 +02:00
agent_direction = Direction.right
2022-05-22 16:27:36 +02:00
elif action.action_type == ActionType.MOVE:
2022-04-28 21:22:19 +02:00
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):
2022-05-22 16:27:36 +02:00
possible_next_states.append(self.create_state(curr_state, Action(ActionType.MOVE)))
2022-04-28 21:22:19 +02:00
if curr_state.agent_direction == Direction.top:
2022-05-22 16:27:36 +02:00
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)))
2022-04-28 21:22:19 +02:00
elif curr_state.agent_direction == Direction.down:
2022-05-22 16:27:36 +02:00
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)))
2022-04-28 21:22:19 +02:00
elif curr_state.agent_direction == Direction.left:
2022-05-22 16:27:36 +02:00
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)))
2022-04-28 21:22:19 +02:00
elif curr_state.agent_direction == Direction.right:
2022-05-22 16:27:36 +02:00
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)))
2022-04-28 21:22:19 +02:00
return possible_next_states
2022-05-22 16:27:36 +02:00
def get_action_list(self) -> List[Action]:
2022-04-28 21:22:19 +02:00
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