diff --git a/algorithms/a_star.py b/algorithms/a_star.py index c18872d..82c9812 100644 --- a/algorithms/a_star.py +++ b/algorithms/a_star.py @@ -4,6 +4,10 @@ from dataclasses import dataclass, field from enum import Enum, unique from typing import Tuple, Optional, List +from common.constants import ROWS, COLUMNS + +FREE_FIELD = ' ' + @unique class Direction(Enum): @@ -54,21 +58,57 @@ def child_node(node: Node, action: Action) -> Node: return Node(state=next_state, parent=node, action=action) -def actions(state: State) -> List[Action]: - pass +def next_position(current_position: Tuple[int, int], direction: Direction) -> Tuple[int, int]: + x1, y1 = direction.value + x2, y2 = current_position + return x1 + x2, y1 + y2 -def result(state: State, action: Action) -> State: +def valid_move(position: Tuple[int, int], grid: List[List[str]]) -> bool: + row, col = position + return grid[row][col] == FREE_FIELD + + +def actions(state: State, grid: List[List[str]]) -> List[Action]: + possible_actions = [Action.FORWARD, Action.TURN_LEFT, Action.TURN_RIGHT] + row, col = state.position + direction = state.direction + + if direction == Direction.UP and row == 0: + remove_forward(possible_actions) + if direction == Direction.DOWN and row == ROWS - 1: + remove_forward(possible_actions) + if direction == Direction.LEFT and col == 0: + remove_forward(possible_actions) + if direction == Direction.RIGHT and col == COLUMNS - 1: + remove_forward(possible_actions) + + if not valid_move(next_position(state.position, direction), grid): + remove_forward(possible_actions) + + return possible_actions + + +def remove_forward(possible_actions: List[Action]) -> None: + if Action.FORWARD in possible_actions: + possible_actions.remove(Action.FORWARD) + + +def result(state: State, action: Action, grid: List[List[str]]) -> State: pass def goal_test(state: State, goal_list: List[Tuple[int, int]]) -> bool: - pass + return state.position in goal_list -def h(state: State) -> int: - pass +def h(state: State, goal: Tuple[int, int]) -> int: + """heuristics that calculates Manhattan distance between current position and goal""" + x1, y1 = state.position + x2, y2 = goal + return abs(x1 - x2) + abs(y1 - y2) # Manhattan distance -def f(state: State) -> int: - pass +def f(current_node: Node, goal: Tuple[int, int]) -> int: + """f(n) = g(n) + h(n), g stands for current cost, h for heuristics""" + return current_node.cost + h(state=current_node.state, goal=goal)