Combine functionality in Finder, implement BreadthFirstFinder #3

pathfinding/core/grid.py

@@ -157,7 +157,7 @@ def grid_str(self, path=None, start=None, end=None,
                     line += start_chr
                 elif node == end:
                     line += end_chr
-                elif path and (node.x, node.y) in path:
+                elif path and ((node.x, node.y) in path or node in path):
                     line += path_chr
                 elif node.walkable:
                     line += empty_chr  # empty field

pathfinding/core/heuristic.py

@@ -1,6 +1,6 @@
 # -*- coding: utf-8 -*-
 import math
-
+from .util import SQRT2
 
 def manhatten(dx, dy):
     """manhatten heuristics"""
@@ -18,8 +18,8 @@ def chebyshev(dx, dy):
 
 
 def octile(dx, dy):
-    f = (2 ** 0.5) - 1
+    f = SQRT2 - 1
     if dx < dy:
         return f * dx + dy
     else:
-        return f * dy + dx
+        return f * dy + dx

pathfinding/core/node.py

@@ -31,6 +31,7 @@ def __init__(self, x=0, y=0, walkable=True):
 
         # used for recurion tracking of IDA*
         self.retain_count = 0
+        # used for IDA* and Jump-Point-Search
         self.tested = False
 
     def __lt__(self, other):

pathfinding/core/util.py

@@ -1,4 +1,9 @@
 # -*- coding: utf-8 -*-
+import math
+
+# square root of 2 for diagonal distance
+SQRT2 = math.sqrt(2)
+
 def backtrace(node):
     """
     Backtrace according to the parent records and return the path.
@@ -21,3 +26,47 @@ def bi_backtrace(node_a, node_b):
     path_b = backtrace(node_b)
     path_b.reverse()
     return path_a + path_b
+
+
+def interpolate(coords_a, coords_b):
+    '''
+    Given the start and end coordinates, return all the coordinates lying
+    on the line formed by these coordinates, based on Bresenham's algorithm.
+    http://en.wikipedia.org/wiki/Bresenham's_line_algorithm#Simplification
+    '''
+    line = []
+    x0, y0 = coords_a
+    x1, y1 = coords_b
+    dx = abs(x1 - x0)
+    dy = abs(y1 - y0)
+    sx = 1 if x0 < x1 else -1
+    sy = 1 if y0 < y1 else -1
+    err = dx - dy
+
+    while True:
+        line += [(x0, y0)]
+        if coords_a.x == coords_b.x and coords_a.y == coords_b.y:
+            break
+        e2 = err * 2
+        if e2 > -dy:
+            err = err - dy
+            x0 = x0 + sx
+        if e2 < dx:
+            err = err + dx
+            y0 = y0 + sy
+
+    return line
+
+
+def expand_path(path):
+    '''
+    Given a compressed path, return a new path that has all the segments
+    in it interpolated.
+    '''
+    expanded = []
+    if len(path) < 2:
+        return expanded
+    for i in range(len(path)-1):
+        expanded += interpolate(path[i], path[i + 1])
+    expanded += [path[:-1]]
+    return expanded

pathfinding/finder/a_star.py

@@ -6,22 +6,9 @@
 from pathfinding.core.heuristic import manhatten, octile
 from pathfinding.core.util import backtrace, bi_backtrace
 from pathfinding.core.diagonal_movement import DiagonalMovement
+from .finder import Finder, TIME_LIMIT, MAX_RUNS, BY_START, BY_END
 
-
-# max. amount of tries we iterate until we abort the search
-MAX_RUNS = float('inf')
-# max. time after we until we abort the search (in seconds)
-TIME_LIMIT = float('inf')
-
-# square root of 2
-SQRT2 = math.sqrt(2)
-
-# used for backtrace of bi-directional A*
-BY_START = 1
-BY_END = 2
-
-
-class AStarFinder(object):
+class AStarFinder(Finder):
     def __init__(self, heuristic=None, weight=1,
                  diagonal_movement=DiagonalMovement.never,
                  time_limit=TIME_LIMIT,
@@ -39,11 +26,12 @@ def __init__(self, heuristic=None, weight=1,
             <=0 means there are no constrains and the code might run on any
             large map.
         """
-        self.time_limit = time_limit
-        self.max_runs = max_runs
-
-        self.diagonal_movement = diagonal_movement
-        self.weight = weight
+        super(AStarFinder, self).__init__(
+            heuristic=heuristic,
+            weight=weight,
+            diagonal_movement=diagonal_movement,
+            time_limit=time_limit,
+            max_runs=max_runs)
 
         if not heuristic:
             if diagonal_movement == DiagonalMovement.never:
@@ -54,29 +42,6 @@ def __init__(self, heuristic=None, weight=1,
                 self.heuristic = octile
 
 
-    def calc_cost(self, node_a, node_b):
-        """
-        get the distance between current node and the neighbor (cost)
-        """
-        ng = node_a.g
-        if node_b.x - node_a.x == 0 or node_b.y - node_a.y == 0:
-            # direct neighbor - distance is 1
-            ng += 1
-        else:
-            # not a direct neighbor - diagonal movement
-            ng += SQRT2
-        return ng
-
-
-    def apply_heuristic(self, node_a, node_b):
-        """
-        helper function to calculate heuristic
-        """
-        return self.heuristic(
-            abs(node_a.x - node_b.x),
-            abs(node_a.y - node_b.y))
-
-
     def check_neighbors(self, start, end, grid, open_list,
             open_value=True, backtrace_by=None):
         """
@@ -95,7 +60,7 @@ def check_neighbors(self, start, end, grid, open_list,
             return backtrace(end)
 
         # get neighbors of the current node
-        neighbors = grid.neighbors(node, self.diagonal_movement)
+        neighbors = self.find_neighbors(grid, node)
         for neighbor in neighbors:
             if neighbor.closed:
                 # already visited last minimum f value
@@ -107,47 +72,13 @@ def check_neighbors(self, start, end, grid, open_list,
                 else:
                     return bi_backtrace(neighbor, node)
 
-            ng = self.calc_cost(node, neighbor)
-
             # check if the neighbor has not been inspected yet, or
             # can be reached with smaller cost from the current node
-            if not neighbor.opened or ng < neighbor.g:
-                neighbor.g = ng
-                neighbor.h = neighbor.h or \
-                    self.apply_heuristic(neighbor, end) *  self.weight
-                # f is the estimated total cost from start to goal
-                neighbor.f = neighbor.g + neighbor.h
-                neighbor.parent = node
-
-                if not neighbor.opened:
-                    heapq.heappush(open_list, neighbor)
-                    neighbor.opened = open_value
-                else:
-                    # the neighbor can be reached with smaller cost.
-                    # Since its f value has been updated, we have to
-                    # update its position in the open list
-                    open_list.remove(neighbor)
-                    heapq.heappush(open_list, neighbor)
+            self.process_node(neighbor, node, end, open_list, open_value)
 
         # the end has not been reached (yet) keep the find_path loop running
         return None
 
-    def keep_running(self):
-        """
-        check, if we run into time or iteration constrains.
-        """
-        if self.runs >= self.max_runs:
-            logging.error('{} run into barrier of {} iterations without '
-                          'finding the destination'.format(
-                            self.__name__, self.max_runs))
-            return False
-        if time.time() - self.start_time >= self.time_limit:
-            logging.error('{} took longer than {} '
-                          'seconds, aborting!'.format(
-                            self.__name__, self.time_limit))
-            return False
-        return True
-
 
     def find_path(self, start, end, grid):
         """
@@ -157,22 +88,6 @@ def find_path(self, start, end, grid):
         :param grid: grid that stores all possible steps/tiles as 2D-list
         :return:
         """
-        self.start_time = time.time() # execution time limitation
-        self.runs = 0 # count number of iterations
-
-        open_list = []
         start.g = 0
         start.f = 0
-        heapq.heappush(open_list, start)
-
-        while len(open_list) > 0:
-            self.runs += 1
-            if not self.keep_running():
-                break
-
-            path = self.check_neighbors(start, end, grid, open_list)
-            if path:
-                return path, self.runs
-
-        # failed to find path
-        return [], self.runs
+        return super(AStarFinder, self).find_path(start, end, grid)

pathfinding/finder/bi_a_star.py

@@ -1,9 +1,10 @@
 # -*- coding: utf-8 -*-
-import heapq # used for the so colled "open list" that stores known nodes
 import logging
+import time
 from pathfinding.core.heuristic import manhatten, octile
 from pathfinding.core.diagonal_movement import DiagonalMovement
-from .a_star import *
+from .finder import Finder, BY_START, BY_END
+from .a_star import AStarFinder
 
 class BiAStarFinder(AStarFinder):
     """
@@ -21,16 +22,14 @@ def find_path(self, start, end, grid):
         self.start_time = time.time() # execution time limitation
         self.runs = 0 # count number of iterations
 
-        start_open_list = []
+        start_open_list = [start]
         start.g = 0
         start.f = 0
-        heapq.heappush(start_open_list, start)
         start.opened = BY_START
 
-        end_open_list = []
+        end_open_list = [end]
         end.g = 0
         end.f = 0
-        heapq.heappush(end_open_list, end)
         end.opened = BY_END
 
         while len(start_open_list) > 0 and len(end_open_list) > 0:

pathfinding/finder/breadth_first.py

@@ -0,0 +1,33 @@
+from .finder import Finder, TIME_LIMIT, MAX_RUNS
+from pathfinding.core.util import backtrace
+from pathfinding.core.diagonal_movement import DiagonalMovement
+
+class BreadthFirstFinder(Finder):
+    def __init__(self, heuristic=None, weight=1,
+                 diagonal_movement=DiagonalMovement.never,
+                 time_limit=TIME_LIMIT,
+                 max_runs=MAX_RUNS):
+        super(BreadthFirstFinder, self).__init__(
+            heuristic=heuristic,
+            weight=weight,
+            diagonal_movement=diagonal_movement,
+            time_limit=time_limit,
+            max_runs=max_runs)
+        if not diagonal_movement:
+            self.diagonalMovement = DiagonalMovement.never
+
+    def check_neighbors(self, start, end, grid, open_list):
+        node = open_list.pop(0)
+        node.closed = True
+
+        if node == end:
+            return backtrace(end)
+
+        neighbors = self.find_neighbors(grid, node)
+        for neighbor in neighbors:
+            if neighbor.closed or neighbor.opened:
+                continue
+
+            open_list.append(neighbor)
+            neighbor.opened = True
+            neighbor.parent = node