60 lines
1.9 KiB
Python
60 lines
1.9 KiB
Python
|
def is_border(x, y, max_x, max_y):
|
||
|
|
return 0 <= x < max_x and 0 <= y < max_y
|
||
|
|
|
||
|
|
def is_obstacle(x, y, obstacles):
|
||
|
|
return (x, y) in obstacles
|
||
|
|
|
||
|
|
def succ(current_state, max_x, max_y, obstacles):
|
||
|
|
successors = []
|
||
|
|
x, y, direction = current_state
|
||
|
|
|
||
|
|
# Akcja: Do przodu
|
||
|
|
direction_x, direction_y = {'N': (0, -1), 'E': (1, 0), 'S': (0, 1), 'W': (-1, 0)}[direction] # Słownik przesunięć w zależności od kierunku
|
||
|
|
new_x, new_y = x + direction_x, y + direction_y
|
||
|
|
|
||
|
|
if is_border(new_x, new_y, max_x, max_y) and not(is_obstacle(new_x, new_y, obstacles)):
|
||
|
|
successors.append(((new_x, new_y, direction), 'Go Forward'))
|
||
|
|
|
||
|
|
# Akcja: Obrót w lewo
|
||
|
|
left_turns = {'N': 'W', 'W': 'S', 'S': 'E', 'E': 'N'} # Słownik kierunków po obrocie w lewo
|
||
|
|
successors.append(((x, y, left_turns[direction]), 'Turn Left'))
|
||
|
|
|
||
|
|
# Akcja: Obrót w prawo
|
||
|
|
right_turns = {'N': 'E', 'E': 'S', 'S': 'W', 'W': 'N'} # Słownik kierunków po obrocie w prawo
|
||
|
|
successors.append(((x, y, right_turns[direction]), 'Turn Right'))
|
||
|
|
|
||
|
|
return successors
|
||
|
|
|
||
|
|
def graphsearch(istate, goal, max_x, max_y, obstacles):
|
||
|
|
fringe = [{"state": istate, "parent": None, "action": None}]
|
||
|
|
explored = set()
|
||
|
|
|
||
|
|
while fringe:
|
||
|
|
elem = fringe.pop(0)
|
||
|
|
state = elem["state"]
|
||
|
|
|
||
|
|
if goaltest(state, goal):
|
||
|
|
return build_action_sequence(elem)
|
||
|
|
|
||
|
|
explored.add(state)
|
||
|
|
|
||
|
|
successors = succ(state, max_x, max_y, obstacles)
|
||
|
|
|
||
|
|
for new_state, action in successors:
|
||
|
|
if new_state not in fringe and new_state not in explored:
|
||
|
|
fringe.append({"state": new_state, "parent": elem, "action": action})
|
||
|
|
|
||
|
|
return False
|
||
|
|
|
||
|
|
def build_action_sequence(node):
|
||
|
|
actions = []
|
||
|
|
while node["parent"]:
|
||
|
|
actions.append(node["action"])
|
||
|
|
node = node["parent"]
|
||
|
|
actions.reverse()
|
||
|
|
return actions
|
||
|
|
|
||
|
|
def goaltest(state, goal):
|
||
|
|
x, y, _ = state
|
||
|
|
goal_x, goal_y = goal
|
||
|
|
return (x,y) == (goal_x, goal_y)
|