Graph search algoritms sandbox

Initial imports


In [2]:
import matplotlib.pyplot as plt
import collections
import heapq

Simple function for creating graphs


In [3]:
class SimpleGraph:
    def __init__(self):
        self.edges = {}
    
    def neighbors(self, id):
        return self.edges[id]

example_graph = SimpleGraph()
example_graph.edges = {
    'A': ['B'],
    'B': ['A', 'C', 'D'],
    'C': ['A'],
    'D': ['E', 'A'],
    'E': ['B']
}

In [4]:
class Queue:
    def __init__(self):
        self.elements = collections.deque()
    
    def empty(self):
        return len(self.elements) == 0
    
    def put(self, x):
        self.elements.append(x)
    
    def get(self):
        return self.elements.popleft()

In [148]:
def breadth_first_search(graph, start):
        # print out what we find
        frontier = Queue()
        frontier.put(start)
        visited = {}
        visited[start] = True
        while not frontier.empty():
            current = frontier.get()
            print("Visiting %r" % current)
            for next in graph.neighbors(current):
                if next not in visited:
                    frontier.put(next)
                    visited[next] = True

breadth_first_search(example_graph, 'B')


Visiting 'B'
Visiting 'A'
Visiting 'C'
Visiting 'D'
Visiting 'E'

Handle more complicated graphs


In [6]:
# Create Grid that stores tuples instead of 
class SquareGrid:
    def __init__(self, width, height):
        self.width = width
        self.height = height
        self.walls = []
    
    def in_bounds(self, id):
        (x, y) = id
        return 0 <= x < self.width and 0 <= y < self.height
    
    def passable(self, id):
        return id not in self.walls
    
    def neighbors(self, id):
        (x, y) = id
        results = [(x+1, y), (x, y-1), (x-1, y), (x, y+1)]
        if (x + y) % 2 == 0: results.reverse() # aesthetics
        results = filter(self.in_bounds, results)
        results = filter(self.passable, results)
        return results

In [101]:
# Utility functions for dealing with square grids
def from_id_width(id, width):
    return (id % width, id // width)

def draw_tile(graph, id, style, width):
    r = "."
    if 'number' in style and id in style['number']: r = "%d" % style['number'][id]
    if 'point_to' in style and style['point_to'].get(id, None) is not None:
        (x1, y1) = id
        (x2, y2) = style['point_to'][id]
        if x2 == x1 + 1: r = "\u2192"
        if x2 == x1 - 1: r = "\u2190"
        if y2 == y1 + 1: r = "\u2193"
        if y2 == y1 - 1: r = "\u2191"
    if 'start' in style and id == style['start']: r = "A"
    if 'goal' in style and id == style['goal']: r = "Z"
    if 'path' in style and id in style['path']: r = "@"
    if id in graph.walls: r = "#" * width
    return r

# Each wall is double size (width = 2)
def draw_grid(graph, width=2, **style):
    for y in range(graph.height):
        for x in range(graph.width):
            print("%%-%ds" % width % draw_tile(graph, (x, y), style, width), end="")
        print()

# With is 50 so that if I want something in 2nd row I need to take its location in 1st row + 50, 
# for 3rd row is +100 etc.
DIAGRAM1_WALLS = [from_id_width(id, width=50) for id in [
    10,11,60,61,110,111,160,161,210,211,410,411,460,461,510,511,560,561,610,611,
    660,661,710,711,910,911,960,961,1010,1011,1060,1061,1110,1111,1160,1161,1210,1211,
    500,501,502,503,504,505,506,507,508,509,550,551,552,553,554,555,556,557,558,559,
    43,93,143,193,243,244,245,248,249,
    37,87,137,187,237,238,239,242,
    435,436,437,438,439,440,441,442,443,485,493,543,593,643,685,693,735,736,737,738,739,740,741,742,743,
    416,417,418,419,420,421,422,466,472,522,572,622,666,672,716,717,718,719,720,721,722
]]

In [102]:
g = SquareGrid(50, 25)
g.walls = DIAGRAM1_WALLS # long list, [(21, 0), (21, 2), ...]
draw_grid(g)


. . . . . . . . . . ####. . . . . . . . . . . . . . . . . . . . . . . . . ##. . . . . ##. . . . . . 
. . . . . . . . . . ####. . . . . . . . . . . . . . . . . . . . . . . . . ##. . . . . ##. . . . . . 
. . . . . . . . . . ####. . . . . . . . . . . . . . . . . . . . . . . . . ##. . . . . ##. . . . . . 
. . . . . . . . . . ####. . . . . . . . . . . . . . . . . . . . . . . . . ##. . . . . ##. . . . . . 
. . . . . . . . . . ####. . . . . . . . . . . . . . . . . . . . . . . . . ######. . ########. . ####
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 
. . . . . . . . . . ####. . . . ##############. . . . . . . . . . . . ##################. . . . . . 
. . . . . . . . . . ####. . . . ##. . . . . ##. . . . . . . . . . . . ##. . . . . . . ##. . . . . . 
########################. . . . . . . . . . ##. . . . . . . . . . . . . . . . . . . . ##. . . . . . 
########################. . . . . . . . . . ##. . . . . . . . . . . . . . . . . . . . ##. . . . . . 
. . . . . . . . . . ####. . . . . . . . . . ##. . . . . . . . . . . . . . . . . . . . ##. . . . . . 
. . . . . . . . . . ####. . . . ##. . . . . ##. . . . . . . . . . . . ##. . . . . . . ##. . . . . . 
. . . . . . . . . . ####. . . . ##############. . . . . . . . . . . . ##################. . . . . . 
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 
. . . . . . . . . . ####. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 
. . . . . . . . . . ####. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 
. . . . . . . . . . ####. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 
. . . . . . . . . . ####. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 
. . . . . . . . . . ####. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 
. . . . . . . . . . ####. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 
. . . . . . . . . . ####. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 

In [112]:
def early_exit(graph, start, goal):
    frontier = Queue()
    frontier.put(start)
    came_from = {}
    came_from[start] = None

    while not frontier.empty():
        current = frontier.get()
        
        # Early exit thing
        if current == goal: 
            break           
        
        for next in graph.neighbors(current):
            if next not in came_from:
                frontier.put(next)
                came_from[next] = current
                
    return came_from
    g = SquareGrid(50, 25)
    g.walls = DIAGRAM1_WALLS

In [113]:
# Start Point and Goal points
_start = (7, 2)
_goal = (48, 2)

parents = early_exit(g, _start, _goal )
draw_grid(g, width=2, point_to=parents, start=_start, goal=_goal)


→ → → → → → ↓ ↓ ↓ ↓ ####↓ ↓ ↓ ↓ ↓ ↓ ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ##↓ ↓ ↓ ↓ ↓ ##. . ↓ . . . 
→ → → → → → → ↓ ↓ ← ####↓ ↓ ↓ ↓ ↓ ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ##→ ↓ ↓ ↓ ↓ ##. ↓ ↓ ↓ . . 
→ → → → → → → A ← ← ####↓ ↓ ↓ ↓ ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ##→ → ↓ ↓ ← ##→ → ↓ ↓ Z . 
→ → → → → → ↑ ↑ ↑ ← ####↓ ↓ ↓ ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ##→ → ↓ ↓ ← ##→ → ↓ ↓ ← ← 
→ → → → → ↑ ↑ ↑ ↑ ↑ ####↓ ↓ ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ######↓ ↓ ########↓ ↓ ####
→ → → → ↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← 
→ → → ↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← 
→ → ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← 
→ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ####↑ ↑ ← ← ##############↑ ← ← ← ← ← ← ← ← ← ← ← ##################↑ ↑ ← ← ← ← 
↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ####↑ ↑ ↑ ← ##↓ ↓ ← ← ← ##↑ ↑ ← ← ← ← ← ← ← ← ← ← ##↓ ← ← ← ← ← ← ##↑ ↑ ↑ ← ← . 
########################↑ ↑ ↑ ↑ ← ← ← ← ← ← ##↑ ↑ ↑ ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ##↑ ↑ ↑ ↑ . . 
########################↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ##↑ ↑ ↑ ↑ ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ##↑ ↑ ↑ . . . 
→ → → → → → ↓ ↓ ↓ ↓ ####↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ##↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ##↑ ↑ . . . . 
→ → → → → → → ↓ ↓ ↓ ####↑ ↑ ↑ ↑ ##↑ ↑ ← ← ← ##↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ← ##↑ ← ← ← ← ← ← ##↑ . . . . . 
→ → → → → → → → ↓ ↓ ####↑ ↑ ↑ ↑ ##############↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ##################. . . . . . 
→ → → → → → → → → → → → ↑ ↑ ↑ ↑ ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← . . . . . . . 
→ → → → → → → → → → → ↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← . . . . . . . . 
→ → → → → → → → → → ↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← . . . . . . . . . 
→ → → → → → → → → ↑ ####↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← . . . . . . . . . . 
→ → → → → → → → ↑ ↑ ####↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← . . . . . . . . . . . 
→ → → → → → → ↑ ↑ ↑ ####↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← . . . . . . . . . . . . 
→ → → → → → ↑ ↑ ↑ ↑ ####↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← ← . . . . . . . . . . . . . 
→ → → → → ↑ ↑ ↑ ↑ ↑ ####↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ← ← ← ← ← ← ← ← ← . . . . . . . . . . . . . . 
→ → → → ↑ ↑ ↑ ↑ ↑ ↑ ####↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ← ← ← ← ← ← ← . . . . . . . . . . . . . . . 
→ → → ↑ ↑ ↑ ↑ ↑ ↑ ↑ ####↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ← ← ← ← ← . . . . . . . . . . . . . . . . 

Dijkstra


In [132]:
# Now we need to implement also weights to escalate cost of movement
class GridWithWeights(SquareGrid):
    
    def __init__(self, width, height):
        super().__init__(width, height)
        self.weights = {}
        
    # this function tells us the cost of moving
    def cost(self, from_node, to_node):
        return self.weights.get(to_node, 1)

diagram4 = GridWithWeights(20, 20)
diagram4.walls = [(8,0), (9,0), (8,1), (9,1), (8,2), (9,2),(8,6),(9,6),(8,7),(9,7), (8,8), (9,8), (8,9), (9,9),
                  (0,10), (1,10), (2,10), (3,10), (4,10), (5,10), (6,10), (7,10), (8,10), (9,10), (10,10), 
                  (11,10),(12,10), (13,10), (17,10),(18,10), (19,10), (20,10),
                  (0,11), (1,11), (2,11), (3,11), (4,11), (5,11), (6,11), (7,11), (8,11), (9,11), (10,11), 
                  (11,11),(12,11), (13,11), (17,11),(18,11), (19,11), (20,11),
                  (8,12), (9,12), (8,13), (9,13), (8,14), (9,14), (8,18), (9,18),(8,19), (9,19),(8,20), (9,20),
                 ]

# Weights
diagram4.weights = {loc: 5 for loc in [(3, 4), (3, 5), (4, 1), (4, 2),
                                       (4, 3), (4, 4), (4, 5), (4, 6), 
                                       (4, 7), (4, 8), (5, 1), (5, 2),
                                       (5, 3), (5, 4), (5, 5), (5, 6), 
                                       (5, 7), (5, 8), (6, 2), (6, 3), 
                                       (6, 4), (6, 5), (6, 6), (6, 7), 
                                       (7, 3), (7, 4), (7, 5)]}


class PriorityQueue:
    def __init__(self):
        self.elements = []
    
    def empty(self):
        return len(self.elements) == 0
    
    def put(self, item, priority):
        heapq.heappush(self.elements, (priority, item))
    
    def get(self):
        return heapq.heappop(self.elements)[1]

# Sometimes this variant is called Uniform Cost Search
def dijkstra_search(graph, start, goal):
    
    # Good thing is that we do not put all the nodes at the start
    frontier = PriorityQueue()
    frontier.put(start, 0)
    came_from = {}
    cost_so_far = {}
    came_from[start] = None
    cost_so_far[start] = 0

    while not frontier.empty():
        current = frontier.get()
        
        # In case to not insert all nodes I implemented EarlyExit also
        if current == goal:
            break

        for next in graph.neighbors(current):
            new_cost = cost_so_far[current] + graph.cost(current, next)
            if next not in cost_so_far or new_cost < cost_so_far[next]:
                cost_so_far[next] = new_cost
                priority = new_cost
                frontier.put(next, priority)
                came_from[next] = current
                
    return came_from, cost_so_far

def reconstruct_path(came_from, start, goal):
    current = goal
    path = [current]
    while current != start:
        current = came_from[current]
        path.append(current)
    path.append(start)
    path.reverse() 
    return path

In [133]:
# Test time
_start = (19,1)
_goal = (2,3)
came_from, cost_so_far = dijkstra_search(diagram4, _start, _goal)
draw_grid(diagram4, width=3, point_to=came_from, start=_start, goal=_goal)
print()
draw_grid(diagram4, width=3, number=cost_so_far, start=_start, goal=_goal)
print()
draw_grid(diagram4, width=3, path=reconstruct_path(came_from, start=_start, goal=_goal))


→  →  →  →  →  →  ↓  ↓  ######↓  ↓  ↓  ↓  ↓  ↓  ↓  ↓  ↓  ↓  
↑  ↑  ↑  ↑  ↑  →  →  ↓  ######→  →  →  →  →  →  →  →  →  A  
↑  ↑  ↑  ↑  ←  →  →  ↓  ######↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  
.  ↑  Z  ↑  ←  →  →  →  →  →  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  
.  .  .  ↑  .  →  →  →  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  
.  .  .  .  .  →  →  →  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  
.  .  .  .  .  →  →  ↑  ######↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  
.  .  .  .  .  →  →  ↑  ######↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  
.  .  .  ↓  ↓  →  →  ↑  ######↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  
.  .  →  →  →  →  ↑  ↑  ######↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  
##########################################↑  ↑  ↑  #########
##########################################↑  ↑  ↑  #########
.  .  .  .  .  .  .  ↓  ######→  →  →  →  ↑  ↑  ↑  ←  ←  ←  
.  .  .  .  .  .  ↓  ↓  ######↑  ↑  ↑  ↑  ↑  ↑  ↑  ←  ←  ←  
.  .  .  .  .  ↓  ↓  ↓  ######↑  ↑  ↑  ↑  ↑  ↑  ↑  ←  ←  ←  
.  .  .  .  →  →  →  →  →  →  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ←  ←  ←  
.  .  .  .  .  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ←  ←  ←  
.  .  .  .  .  .  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ←  ←  ←  
.  .  .  .  .  .  .  ↑  ######↑  ↑  ↑  ↑  ↑  ↑  ↑  ←  ←  ←  
.  .  .  .  .  .  .  .  ######↑  ↑  ↑  ↑  ↑  ↑  ↑  ←  ←  ←  

28 27 26 25 24 23 22 21 ######10 9  8  7  6  5  4  3  2  1  
29 28 27 26 29 26 21 20 ######9  8  7  6  5  4  3  2  1  A  
30 29 28 27 32 29 24 19 ######10 9  8  7  6  5  4  3  2  1  
.  30 Z  28 33 28 23 18 13 12 11 10 9  8  7  6  5  4  3  2  
.  .  .  33 .  29 24 19 14 13 12 11 10 9  8  7  6  5  4  3  
.  .  .  .  .  30 25 20 15 14 13 12 11 10 9  8  7  6  5  4  
.  .  .  .  .  31 26 21 ######14 13 12 11 10 9  8  7  6  5  
.  .  .  .  .  32 27 22 ######15 14 13 12 11 10 9  8  7  6  
.  .  .  29 32 29 24 23 ######16 15 14 13 12 11 10 9  8  7  
.  .  29 28 27 26 25 24 ######17 16 15 14 13 12 11 10 9  8  
##########################################14 13 12 #########
##########################################15 14 13 #########
.  .  .  .  .  .  .  29 ######20 19 18 17 16 15 14 15 16 17 
.  .  .  .  .  .  29 28 ######21 20 19 18 17 16 15 16 17 18 
.  .  .  .  .  29 28 27 ######22 21 20 19 18 17 16 17 18 19 
.  .  .  .  29 28 27 26 25 24 23 22 21 20 19 18 17 18 19 20 
.  .  .  .  .  29 28 27 26 25 24 23 22 21 20 19 18 19 20 21 
.  .  .  .  .  .  29 28 27 26 25 24 23 22 21 20 19 20 21 22 
.  .  .  .  .  .  .  29 ######26 25 24 23 22 21 20 21 22 23 
.  .  .  .  .  .  .  .  ######27 26 25 24 23 22 21 22 23 24 

.  .  @  @  @  @  @  .  ######.  .  .  .  .  .  .  .  .  .  
.  .  @  .  .  .  @  @  ######@  @  @  @  @  @  @  @  @  @  
.  .  @  .  .  .  .  @  ######@  .  .  .  .  .  .  .  .  .  
.  .  @  .  .  .  .  @  @  @  @  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  ######.  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  ######.  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  ######.  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  ######.  .  .  .  .  .  .  .  .  .  
##########################################.  .  .  #########
##########################################.  .  .  #########
.  .  .  .  .  .  .  .  ######.  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  ######.  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  ######.  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  ######.  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  ######.  .  .  .  .  .  .  .  .  .  

A*

Create Heuristics


In [136]:
def heuristic(a, b):
    (x1, y1) = a
    (x2, y2) = b
    return abs(x1 - x2) + abs(y1 - y2)

In [139]:
## Heuristics search (Greedy Best First Search)
def greedy_best_first_search(graph, start, goal):
    frontier = PriorityQueue()
    frontier.put(start, 0)
    came_from = {}
    came_from[start] = None

    while not frontier.empty():
        current = frontier.get()

        if current == goal:
            break

        for next in graph.neighbors(current):
            if next not in came_from:
                # We do not use cost_so_far from Dijsktra
                priority = heuristic(goal, next)
                frontier.put(next, priority)
                came_from[next] = current
                
    return cost_so_far

In [144]:
_start = (19,1)
_goal = (2,3)
cost_so_far = greedy_best_first_search(diagram4, _start, _goal)
draw_grid(diagram4, width=3, point_to=came_from, start=_start, goal= _goal)
print()
draw_grid(diagram4, width=3, number=cost_so_far, start=_start, goal=_goal)
print()
#draw_grid(diagram4, width=3, path=reconstruct_path(came_from, start=(1, 4), goal=(7, 8)))


→  →  →  →  →  →  ↓  ↓  ######↓  ↓  ↓  ↓  ↓  ↓  ↓  ↓  ↓  ↓  
↑  ↑  ↑  ↑  ↑  →  →  ↓  ######→  →  →  →  →  →  →  →  →  A  
↑  ↑  ↑  ↑  ←  →  →  ↓  ######↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  
.  ↑  Z  ↑  ←  →  →  →  →  →  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  
.  .  .  ↑  .  →  →  →  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  
.  .  .  .  .  →  →  →  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  
.  .  .  .  .  →  →  ↑  ######↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  
.  .  .  .  .  →  →  ↑  ######↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  
.  .  .  ↓  ↓  →  →  ↑  ######↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  
.  .  →  →  →  →  ↑  ↑  ######↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  
##########################################↑  ↑  ↑  #########
##########################################↑  ↑  ↑  #########
.  .  .  .  .  .  .  ↓  ######→  →  →  →  ↑  ↑  ↑  ←  ←  ←  
.  .  .  .  .  .  ↓  ↓  ######↑  ↑  ↑  ↑  ↑  ↑  ↑  ←  ←  ←  
.  .  .  .  .  ↓  ↓  ↓  ######↑  ↑  ↑  ↑  ↑  ↑  ↑  ←  ←  ←  
.  .  .  .  →  →  →  →  →  →  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ←  ←  ←  
.  .  .  .  .  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ←  ←  ←  
.  .  .  .  .  .  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ←  ←  ←  
.  .  .  .  .  .  .  ↑  ######↑  ↑  ↑  ↑  ↑  ↑  ↑  ←  ←  ←  
.  .  .  .  .  .  .  .  ######↑  ↑  ↑  ↑  ↑  ↑  ↑  ←  ←  ←  

28 27 26 25 24 23 22 21 ######10 9  8  7  6  5  4  3  2  1  
29 28 27 26 29 26 21 20 ######9  8  7  6  5  4  3  2  1  A  
30 29 28 27 32 29 24 19 ######10 9  8  7  6  5  4  3  2  1  
.  30 Z  28 33 28 23 18 13 12 11 10 9  8  7  6  5  4  3  2  
.  .  .  33 .  29 24 19 14 13 12 11 10 9  8  7  6  5  4  3  
.  .  .  .  .  30 25 20 15 14 13 12 11 10 9  8  7  6  5  4  
.  .  .  .  .  31 26 21 ######14 13 12 11 10 9  8  7  6  5  
.  .  .  .  .  32 27 22 ######15 14 13 12 11 10 9  8  7  6  
.  .  .  29 32 29 24 23 ######16 15 14 13 12 11 10 9  8  7  
.  .  29 28 27 26 25 24 ######17 16 15 14 13 12 11 10 9  8  
##########################################14 13 12 #########
##########################################15 14 13 #########
.  .  .  .  .  .  .  29 ######20 19 18 17 16 15 14 15 16 17 
.  .  .  .  .  .  29 28 ######21 20 19 18 17 16 15 16 17 18 
.  .  .  .  .  29 28 27 ######22 21 20 19 18 17 16 17 18 19 
.  .  .  .  29 28 27 26 25 24 23 22 21 20 19 18 17 18 19 20 
.  .  .  .  .  29 28 27 26 25 24 23 22 21 20 19 18 19 20 21 
.  .  .  .  .  .  29 28 27 26 25 24 23 22 21 20 19 20 21 22 
.  .  .  .  .  .  .  29 ######26 25 24 23 22 21 20 21 22 23 
.  .  .  .  .  .  .  .  ######27 26 25 24 23 22 21 22 23 24 

A-star search algorithm


In [145]:
def a_star_search(graph, start, goal):
    frontier = PriorityQueue()
    frontier.put(start, 0)
    came_from = {}
    cost_so_far = {}
    came_from[start] = None
    cost_so_far[start] = 0
    
    while not frontier.empty():
        current = frontier.get()
        
        if current == goal:
            break
        
        for next in graph.neighbors(current):
            new_cost = cost_so_far[current] + graph.cost(current, next)
            if next not in cost_so_far or new_cost < cost_so_far[next]:
                cost_so_far[next] = new_cost
                priority = new_cost + heuristic(goal, next)
                frontier.put(next, priority)
                came_from[next] = current
    
    return came_from, cost_so_far

In [147]:
_start = (19,1)
_goal = (2,3)

came_from, cost_so_far = a_star_search(diagram4, _start, _goal)
draw_grid(diagram4, width=3, point_to=came_from,  start=_start, goal= _goal)
print()
draw_grid(diagram4, width=3, number=cost_so_far,  start=_start, goal= _goal)


.  →  →  →  →  →  ↓  ↓  ######↓  ↓  ↓  ↓  ↓  ↓  ↓  ↓  ↓  ↓  
.  →  ↑  ↑  ↑  →  →  ↓  ######→  →  →  →  →  →  →  →  →  A  
.  →  ↑  ←  .  .  →  ↓  ######↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  
.  .  Z  .  .  →  →  →  →  →  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  
.  .  .  .  .  .  →  →  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  
.  .  .  .  .  .  →  →  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  
.  .  .  .  .  .  .  ↑  ######↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  
.  .  .  .  .  .  .  .  ######↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  
.  .  .  .  .  .  .  .  ######↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  ↑  
.  .  .  .  .  .  .  .  ######.  .  .  .  .  .  .  .  .  .  
##########################################.  .  .  #########
##########################################.  .  .  #########
.  .  .  .  .  .  .  .  ######.  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  ######.  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  ######.  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  ######.  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  ######.  .  .  .  .  .  .  .  .  .  

.  27 26 25 24 23 22 21 ######10 9  8  7  6  5  4  3  2  1  
.  28 27 26 29 26 21 20 ######9  8  7  6  5  4  3  2  1  A  
.  29 28 29 .  .  24 19 ######10 9  8  7  6  5  4  3  2  1  
.  .  Z  .  .  28 23 18 13 12 11 10 9  8  7  6  5  4  3  2  
.  .  .  .  .  .  24 19 14 13 12 11 10 9  8  7  6  5  4  3  
.  .  .  .  .  .  25 20 15 14 13 12 11 10 9  8  7  6  5  4  
.  .  .  .  .  .  .  21 ######14 13 12 11 10 9  8  7  6  5  
.  .  .  .  .  .  .  .  ######15 14 13 12 11 10 9  8  7  6  
.  .  .  .  .  .  .  .  ######16 15 14 13 12 11 10 9  8  7  
.  .  .  .  .  .  .  .  ######.  .  .  .  .  .  .  .  .  .  
##########################################.  .  .  #########
##########################################.  .  .  #########
.  .  .  .  .  .  .  .  ######.  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  ######.  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  ######.  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  ######.  .  .  .  .  .  .  .  .  .  
.  .  .  .  .  .  .  .  ######.  .  .  .  .  .  .  .  .  .  

In [ ]: