81 lines
2.4 KiB
Python
81 lines
2.4 KiB
Python
|
from domain.commands.vacuum_move_command import VacuumMoveCommand
|
||
|
|
from domain.world import World
|
||
|
|
|
||
|
|
|
||
|
|
class State:
|
||
|
|
def __init__(self, x, y):
|
||
|
|
self.x = x
|
||
|
|
self.y = y
|
||
|
|
|
||
|
|
def __hash__(self):
|
||
|
|
return hash((self.x, self.y))
|
||
|
|
|
||
|
|
def __eq__(self, other):
|
||
|
|
return self.x == other.x and self.y == other.y
|
||
|
|
|
||
|
|
|
||
|
|
class GoAnyDirectionBFS:
|
||
|
|
def __init__(self, world: World, start_state: State, goal_state: State):
|
||
|
|
self.start_state = start_state
|
||
|
|
self.goal_state = goal_state
|
||
|
|
self.visited = set()
|
||
|
|
self.parent = {}
|
||
|
|
self.actions = []
|
||
|
|
self.path = []
|
||
|
|
self.world = world
|
||
|
|
self.queue = []
|
||
|
|
|
||
|
|
def search(self):
|
||
|
|
self.queue.append(self.start_state)
|
||
|
|
self.visited.add(self.start_state)
|
||
|
|
while self.queue:
|
||
|
|
state = self.queue.pop(0)
|
||
|
|
if state == self.goal_state:
|
||
|
|
self.actions = self.get_actions()
|
||
|
|
self.path = self.get_path()
|
||
|
|
return True
|
||
|
|
for successor in self.successors(state):
|
||
|
|
if successor not in self.visited:
|
||
|
|
self.visited.add(successor)
|
||
|
|
self.parent[successor] = state
|
||
|
|
self.queue.append(successor)
|
||
|
|
return False
|
||
|
|
|
||
|
|
def successors(self, state):
|
||
|
|
new_successors = [
|
||
|
|
State(state.x + dx, state.y + dy)
|
||
|
|
for dx, dy in [(1, 0), (0, 1), (-1, 0), (0, -1)]
|
||
|
|
if self.world.accepted_move(state.x + dx, state.y + dy)
|
||
|
|
]
|
||
|
|
|
||
|
|
return new_successors
|
||
|
|
|
||
|
|
def get_actions(self):
|
||
|
|
actions = []
|
||
|
|
state = self.goal_state
|
||
|
|
while state != self.start_state:
|
||
|
|
parent_state = self.parent[state]
|
||
|
|
dx = state.x - parent_state.x
|
||
|
|
dy = state.y - parent_state.y
|
||
|
|
if dx == 1:
|
||
|
|
actions.append("RIGHT")
|
||
|
|
elif dx == -1:
|
||
|
|
actions.append("LEFT")
|
||
|
|
elif dy == 1:
|
||
|
|
actions.append("DOWN")
|
||
|
|
elif dy == -1:
|
||
|
|
actions.append("UP")
|
||
|
|
state = parent_state
|
||
|
|
actions.reverse()
|
||
|
|
return actions
|
||
|
|
|
||
|
|
def get_path(self):
|
||
|
|
path = []
|
||
|
|
state = self.goal_state
|
||
|
|
while state != self.start_state:
|
||
|
|
path.append((state.x, state.y))
|
||
|
|
state = self.parent[state]
|
||
|
|
path.append((self.start_state.x, self.start_state.y))
|
||
|
|
path.reverse()
|
||
|
|
return path
|