class PriorityQueue: def __init__(self): self.heap = [] def get_parent(self, node_idx): return (node_idx-1) // 2 def get_left_child(self, node_idx): return node_idx * 2 + 1 def get_right_child(self, node_idx): return node_idx * 2 + 2

def sift_down(self, node_idx): left_idx = self.get_left_child(node_idx) right_idx = self.get_right_child(node_idx) smallest_idx = node_idx if left_idx < len(self.heap) and self.heap[left_idx] < self.heap[smallest_idx]: smallest_idx = left_idx if right_idx < len(self.heap) and self.heap[right_idx] < self.heap[smallest_idx]: smallest_idx = right_idx if smallest_idx != node_idx: self.heap[node_idx], self.heap[smallest_idx] = self.heap[smallest_idx], self.heap[node_idx] return self.sift_down(smallest_idx) def sift_up(self, node_idx): if node_idx == 0: return root_idx = self.get_parent(node_idx) if self.heap[node_idx] < self.heap[root_idx]: self.heap[node_idx], self.heap[root_idx] = self.heap[root_idx], self.heap[node_idx] return self.sift_up(root_idx) """ Extracts the minimum value from the priority queue """ def extract_min(self): last = len(self.heap)-1 self.heap[0], self.heap[last] = self.heap[last], self.heap[0] extract = self.heap.pop() self.sift_down(0) return extract """ Adds a new value to the priority queue """ def append(self, value): self.heap.append(value) self.sift_up(len(self.heap) - 1) def is_empty(self): return len(self.heap) == 0

""" This function returns a tuple of (neighbor_cost, neighbor_cell) """ def get_neighbors(graph, node): def is_valid(node): return ( node[0] >= 0 and node[1] >= 0 and \ node[1] < len(graph) and node[0] < len(graph[0]) and \ graph[node[1]][node[0]] != -1 ) neighbors = [] for direction in [(0, 1), (0, -1), (1, 0), (-1, 0)]: neighbor_x, neighbor_y = [node[i] + direction[i] for i in range(2)] potential_neighbor = (neighbor_x, neighbor_y) if is_valid(potential_neighbor): neighbors.append((graph[neighbor_y][neighbor_x], potential_neighbor)) return neighbors

def manhattan_distance(node1, node2): return abs(node1[0] - node2[0]) + abs(node1[1] - node2[1])

def a_star(graph, start, goal): # initialize variables parent = {} # holds mapping between every node and it's parent in the path current_node = start closed_set = set() cost_dict = {} # initialize open queue open_queue = PriorityQueue() open_queue.append((manhattan_distance(start, goal), start)) # while loop setting current node while not open_queue.is_empty(): current_node_cost, current_node = open_queue.extract_min() """ The cost on the queue includes the heuristic value so we need to subtract it to get the real cost """ current_node_cost -= manhattan_distance(current_node, goal) """ Check if the node is on the closed set, if it's then do nothing """ if current_node in closed_set: continue

if current_node == goal: """ Return the cost """ path = [goal] node = goal while node != start: node = parent[node] path.append(node) path.reverse() return (cost_dict[goal], path) # iterate over neighbors of current node # and add to open queue if not already there or in closed for neighbor in get_neighbors(graph, current_node): neighbor_cost, neighbor_node = neighbor neighbor_heuristic_value = manhattan_distance(neighbor_node, goal) """ Now we need to calculate the cost to reach this neighbor via current_node remember, the cost is the cost to the current_node + the cost of the edge (neighbor cost) + heuristic value """ # fill the following line new_cost = # your code here if neighbor_node in closed_set: continue if neighbor_node in cost_dict and cost_dict[neighbor_node] <= new_cost: continue cost_dict[neighbor_node] = new_cost """ Store parent info """ # fill this line """ Now we need to add the new cost and the neighbor node to the queue, make a tuple of the new cost and the neighbor node """ # Fill the following line open_queue.append(()) closed_set.add(current_node) return None

A*

In this assignment we are going to implement A*. Remember, A* is exactly the same as dijkstra with the only difference that when we add the cost to the queue, we add the heuristic value to the cost of the node.

We will use Manhattan distance heuristic for this assignment, I have included the function for you so you only need to call it.

The algorithm is mostly implemented for you with only few lines for you to fill, I have provided comments above the lines.

You need to fill the following three lines

Lines 140, 151, 158

graph1 = [ [ 1, 1, -1, 1, 1], [ 1, 1, -1, 1, 1], [ 1, 1, 25, 9, 1], [ 1, -1, -1, 1, 1], [ 1, 1, -1, 1, 7], [12, 1, 50, 1, 1], [ 1, 1, 39, 1, 1], ] start = (0, 0) goal = (4, 0) a_star(graph1, start, goal) => (40, [(0, 0), (1, 0), (1, 1), (1, 2), (2, 2), (3, 2),

(3, 1), (3, 0), (4, 0)]) graph2 = [ [ 1, 1, -1, 1, 1], [ 1, 1, -1, 1, 1], [ 1, 1, 1, 9, 1], [ 1, -1, -1, 1, 1], [ 1, 1, -1, 1, 7], [12, 1, 50, 1, 1], [ 1, 1, 39, 1, 1], ] a_star(graph2, start, goal) => (16, [(0, 0), (1, 0), (1, 1), (1, 2), (2, 2), (3, 2),

(3, 1), (3, 0), (4, 0)]) graph3 = [ [ 1, 1, -1, 1, 1], [ 1, 1, -1, 1, 1], [ 1, 1, 25, 9, 1], [ 1, -1, -1, 1, 1], [ 1, 1, -1, 1, 7], [12, 1, 1, 1, 1], [ 1, 1, 1, 1, 1], ] a_star(graph3, start, goal) => (14, [(0, 0), (0, 1), (0, 2), (0, 3), (0, 4), (1, 4),

(1, 5), (2, 5), (3, 5), (3, 4), (3, 3), (4, 3), (4, 2), (4, 1), (4, 0)])

graph4 = [

[ 1, 1, 1, 1, 1, 1, 1, 1],

[ 1, 1, 1, 1, 1, 1, 1, 1],

[-1, -1, -1, -1, -1, -1, -1, 1],

[ 1, 1, 1, 1, 1, 1, -1, 1],

[ 1, 1, 1, 1, 1, 1, -1, 1],

[ 1, 1, 1, 1, 1, 1, -1, 1],

[ 1, 1, 1, 1, 1, 1, -1, 1],

[ 1, 1, 1, 1, 1, 1, -1, 1],

[ 1, 1, 1, 1, 1, 1, 1, 1],

]

a_star(graph4, (0,8), (7,0))

=> (15, [(0, 8), (1, 8), (2, 8), (3, 8), (4, 8),

(5, 8), (6, 8), (7, 8), (7, 7), (7, 6), (7, 5), (7, 4),

(7, 3), (7, 2), (7, 1), (7, 0)])