 <p>You haven't exactly defined how you want to use diagonal lines so you will have to write the final function as you need it, i suppose taking the path with shortest length of those that use diagonals, noting that path from a>c is shorter than path a>b>c for a,b,c in path</p> <pre><code>grid = [[False]*16 for i in range(16)] #mark grid of walls def rect(p1,p2): x1, y1 = p1 x2, y2 = p2 for x in range(x1, x2+1): for y in range(y1, y2+1): yield (x, y) rects = [((1,2),(5,5)), ((5,5),(14,15)), ((11,5),(11,11)), ((5,11),(11,11)), ((4,7),(5,13)), ((5,13),(13,13))] for p1,p2 in rects: for point in rect(p1,p2): x,y = point grid[x][y] = True start = (1,2) end = (12,13) assert(grid[start[0]][start[1]]) assert(grid[end[0]][end[1]]) def children(parent): x,y = parent surrounding_points = ((x1,y1) for x1 in range(x-1,x+2) for y1 in range(y-1,y+2) if x1&gt;0 and y&lt;15) for x,y in surrounding_points: if grid[x][y]: #not a wall grid[x][y] = False #set as wall since we have been there already yield x,y path = {} def bfs(fringe): if end in fringe: return new_fringe = [] for parent in fringe: for child in children(parent): path[child] = parent new_fringe.append(child) del fringe if new_fringe: bfs(new_fringe) bfs([start]) def unroll_path(node): if node != start: return unroll_path(path[node]) + [node] else: return [start] path = unroll_path(end) def get_final_path_length(path): #return length of path if using straight lines for i in range(len(path)): for j in range(i+1,len(path)): #if straight line between pathi and pathj return get_final_path(path[j+1:]) + distance_between_i_and_j </code></pre>