import queue from domain.world import World class State: def __init__(self, x, y, direction=(1, 0)): self.x = x self.y = y self.direction = direction def __hash__(self): return hash((self.x, self.y)) def __eq__(self, other): return (self.x == other.x and self.y == other.y and self.direction == other.direction) class Node: def __init__(self, state: State): self.state = state self.parent = None self.action = None def action_sequence(node: Node): actions = [] while node.parent: actions.append(node.action) node = node.parent actions.reverse() return actions class RotateAndGoBFS: def __init__(self, world: World, start_state: State, goal_state: State): self.world = world self.start_state = start_state self.goal_state = goal_state self.fringe = queue.Queue() self.enqueued_states = set() self.explored = set() self.actions = [] def search(self): self.fringe.put(Node(self.start_state)) while self.fringe: elem = self.fringe.get() if self.is_goal(elem.state): self.actions = action_sequence(elem) return True self.explored.add(elem.state) for (action, state) in self.successors(elem.state): if state in self.explored or state in self.enqueued_states: continue next_node = Node(state) next_node.action = action next_node.parent = elem self.fringe.put(next_node) self.enqueued_states.add(state) return False def successors(self, state: State): new_successors = [ # rotate right ("RR", State(state.x, state.y, (-state.direction[1], state.direction[0]))), # rotate left ("RL", State(state.x, state.y, (state.direction[1], -state.direction[0]))), ] if self.world.accepted_move(state.x + state.direction[0], state.y + state.direction[1]): new_successors.append( ("GO", State(state.x + state.direction[0], state.y + state.direction[1], state.direction))) return new_successors def is_goal(self, state: State) -> bool: return ( state.x == self.goal_state.x and state.y == self.goal_state.y )