In [2]:
import matplotlib.pyplot as plt
import collections
import heapq
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')
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)
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))
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)))
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)
In [ ]: