import collections
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()
class Graph:
def __init__(self, collisions):
self.collisions = collisions
self.width = len(collisions[0])
self.height = len(collisions)
def walkable(self, coor):
x, y = coor
if x < 0 or x > self.width - 1:
return False
if y < 0 or y > self.height - 1:
return False
if self.collisions[self.height - y - 1][x] > 0:
return False
return True
def neighbors(self, coor):
n = []
x, y = coor
for dx, dy in ((-1, 0), (1, 0), (0, -1), (0, 1)):
nx = x + dx
ny = y + dy
if self.walkable((nx, ny)):
n.append((nx, ny))
for dx, dy in ((-1, -1), (-1, 1), (1, -1), (1, 1)):
nx = x + dx
ny = y + dy
if (self.walkable((nx, ny)) and
self.walkable((nx, y)) and
self.walkable((x, ny))):
n.append((nx, ny))
return n
def breadth_first_search(collisions, start, goal):
graph = Graph(collisions)
frontier = Queue()
frontier.put(start)
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:
frontier.put(next)
came_from[next] = current
current = goal
path = [current]
while current != start:
current = came_from[current]
path.append(current)
path.reverse()
# simplify path
spath = path[:1]
for i in range(1, len(path) - 1):
px, py = path[i - 1]
cx, cy = path[i]
nx, ny = path[i + 1]
if nx - cx == cx - px and ny - cy == cy - py:
continue
spath.append(path[i])
spath.append(path[-1])
return spath
