def a_star(board, heuristic):
"""
A*算法主题
:param board: 要解决的游戏
:param heuristic: 选择的启发函数
:return: 返回的解的路径
"""
frontier = PriorityQueue()
node = Node(board)
frontier.add(node, heuristic(node.data) + len(node.path()) - 1)
explored = []
while frontier.has_next():
node = frontier.pop()
if node.data.is_solved():
return node.path()
for move in node.data.legal_moves():
child = Node(node.data.forecast(move), node)
if (not frontier.has(child)) and (child.data not in explored):
frontier.add(child,
heuristic(child.data) + len(child.path()) - 1)
elif frontier.has(child):
child_value = heuristic(child.data) + len(child.path()) - 1
if child_value < frontier.get_value(child):
frontier.remove(child)
frontier.add(child, child_value)
explored.append(node.data)
return None
def greedy_best_first(board, heuristic):
"""
an implementation of the greedy best first search algorithm. it uses a heuristic function to find the quickest
way to the destination
:param board: (Board) the board you start at
:param heuristic: (function) the heuristic function
:return: (list) path to solution, (int) number of explored boards
"""
frontier = PriorityQueue()
node = Node(board)
frontier.add(node, heuristic(node.data))
explored = []
while frontier.has_next():
node = frontier.pop()
if node.data.is_solved():
return node.path(), len(explored) + 1
for move in node.data.legal_moves():
child = Node(node.data.forecast(move), node)
if (not frontier.has(child)) and (child.data not in explored):
frontier.add(child, heuristic(child.data))
explored.append(node.data)
return None, len(explored)
def a_star(board, heuristic):
"""
solves the board using the A* approach accompanied by the heuristic function
:param board: board to solve
:param heuristic: heuristic function
:return: path to solution, and number of explored nodes
"""
frontier = PriorityQueue()
node = Node(board)
frontier.add(node, heuristic(node.data) + len(node.path()) - 1)
explored = []
while frontier.has_next():
node = frontier.pop()
# check if solved
if node.data.is_solved():
return node.path(), len(explored) + 1
# add children to frontier
for move in node.data.legal_moves():
child = Node(node.data.forecast(move), node)
# child must not have already been explored
if (not frontier.has(child)) and (child.data not in explored):
frontier.add(child,
heuristic(child.data) + len(child.path()) - 1)
# if the child is already in the frontier, it can be added only if it's better
elif frontier.has(child):
child_value = heuristic(child.data) + len(child.path()) - 1
if child_value < frontier.get_value(child):
frontier.remove(child)
frontier.add(child, child_value)
explored.append(node.data)
return None, len(explored)
def compute_path(grid, start, goal, cost, heuristic):
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use thePriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
# Keep track of the parent of each node. Since the car can take 4 distinct orientations,
# for each orientation we can store a 2D array indicating the grid cells.
# E.g. parent[0][2][3] will denote the parent when the car is at (2,3) facing up
parent = [[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))]]
# The path of the car
path = [['-' for row in range(len(grid[0]))] for col in range(len(grid))]
x = start[0]
y = start[1]
theta = start[2]
h = heuristic[x][y]
g = 0
f = g + h
# your code: implement A*
open_set.put(start, Value(f=f, g=g))
while open_set.__len__() != 0:
node = open_set.pop()
g = node[1].g
x = node[0][0]
y = node[0][1]
theta = node[0][2]
#if reach the goal, break
if (node[0] == goal):
closed_set.add(node[0])
break
closed_set.add(node[0])
for idx, act in enumerate(action):
new_theta = (theta + act) % 4
#use orientation to determine the position
new_x = x + forward[new_theta][0]
new_y = y + forward[new_theta][1]
#to make sure it won't break the node is naviable
if (new_x < 0 or new_x > 4 or new_y < 0 or new_y > 5
or grid[new_x][new_y] == 1):
continue
new_g = g + cost[idx]
new_h = heuristic[new_x][new_y]
new_f = new_g + new_h
child = [(new_x, new_y, new_theta), Value(f=new_f, g=new_g)]
if (child[0] not in closed_set and child[0] not in open_set):
open_set.put(child[0], child[1])
parent[new_theta][new_x][new_y] = (action_name[idx], node[0])
#if the cost decreased, add it to the openlist
elif (open_set.has(child[0]) and open_set.get(child[0]).g > new_g):
open_set.put(child[0], child[1])
parent[new_theta][new_x][new_y] = (action_name[idx], node[0])
#recording the path in parent
#reach the goal
path[x][y] = '*'
#find out the path by recalling step by step
while (x, y, theta) != start:
pre_step = parent[theta][x][y]
x = pre_step[1][0]
y = pre_step[1][1]
theta = pre_step[1][2]
path[x][y] = pre_step[0]
# Initially you may want to ignore theta, that is, plan in 2D.
# To do so, set actions=forward, cost = [1, 1, 1, 1], and action_name = ['U', 'L', 'R', 'D']
# Similarly, set parent=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
return path, closed_set
def compute_path(grid, start, goal, cost, heuristic):
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use thePriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
# Keep track of the parent of each node. Since the car can take 4 distinct orientations,
# for each orientation we can store a 2D array indicating the grid cells.
# E.g. parent[0][2][3] will denote the parent when the car is at (2,3) facing up
parent = [[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))]]
# The path of the car
path = [['-' for row in range(len(grid[0]))] for col in range(len(grid))]
x = start[0]
y = start[1]
theta = start[2]
h = heuristic[x][y]
g = 0
f = g + h
open_set.put(start, Value(f=f, g=g))
# your code: implement A*
# Initially you may want to ignore theta, that is, plan in 2D.
# To do so, set actions=forward, cost = [1, 1, 1, 1], and action_name = ['U', 'L', 'R', 'D']
# Similarly, set parent=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
path[goal[0]][goal[1]] = '*'
l = []
l.append(start)
while (len(open_set) != 0):
curr_node = open_set.pop()
if (curr_node[0][:2] != start[:2]):
path[l[-1][0]][l[-1][1]] = curr_node[1].g
l.append(curr_node[0])
if (curr_node[0] == goal):
print("get path!")
closed_set.add(curr_node)
break
closed_set.add(curr_node)
x_current = curr_node[0][0] #row of current node
y_current = curr_node[0][1] #col of current node
o_current = curr_node[0][2] #orientation of current node
for i_act, act in enumerate(action):
o_next = (o_current + act) % 4 #orientation of next node
x_next = x_current + forward[o_next][0] #row of next node
y_next = y_current + forward[o_next][1] #col of next node
n_act = action_name[
i_act] #action to get this next node, this cause the one step delay in the display
if (x_next >= 0 and x_next < len(grid) and y_next >= 0
and y_next < len(grid[0])
): #filter the available child (map boundery and barrier)
if (grid[x_next][y_next] == 0):
F_cost = cost[i_act] + heuristic[x_next][
y_next] # abs(x_next - goal[0]) + abs(y_next - goal[1]) #calculate the f(n) = g(n) + h(n)
if ((not open_set.has((x_next, y_next, o_next)))
or (not closed_set.has((x_next, y_next, o_next)))):
open_set.put((x_next, y_next, o_next),
Value(f=F_cost, g=n_act))
#parent[x_next][y_next] = (x_next, y_next, o_next)
elif (open_set.has((x_next, y_next, o_next))
and F_cost < open_set.get(
(x_next, y_next, o_next)).f):
open_set.get((x_next, y_next, o_next)).f = F_cost
open_set.get((x_next, y_next, o_next)).g = n_act
if (curr_node[0] != goal): print("fail to find the path!")
return path, closed_set
