1 files changed, 91 insertions, 0 deletions
diff --git a/gui/pathfind.py b/gui/pathfind.py
new file mode 100644
index 0000000..2c870e1
--- /dev/null
+++ b/gui/pathfind.py
@@ -0,0 +1,91 @@
+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