fix pathfinding with passable borders

Author: andreyd41

pathfinding/finder/best_first.py

@@ -32,6 +32,6 @@ def __init__(self, heuristic=None, weight=1,
 
         self.weighted = False
 
-    def apply_heuristic(self, node_a, node_b, heuristic=None):
+    def apply_heuristic(self, node_a, node_b, heuristic=None, graph=None):
         return super(BestFirst, self).apply_heuristic(
-            node_a, node_b, heuristic) * 1000000
+            node_a, node_b, heuristic, graph=graph) * 1000000

pathfinding/finder/dijkstra.py

@@ -15,7 +15,7 @@ def __init__(self, weight=1,
             time_limit=time_limit,
             max_runs=max_runs)
 
-    def apply_heuristic(self, node_a, node_b, heuristic=None):
+    def apply_heuristic(self, node_a, node_b, heuristic=None, graph=None):
         """
         helper function to apply heuristic
         """

pathfinding/finder/finder.py

@@ -1,5 +1,6 @@
 import heapq  # used for the so colled "open list" that stores known nodes
 import time  # for time limitation
+from ..core.grid import Grid
 from ..core.diagonal_movement import DiagonalMovement
 from ..core.heap import SimpleHeap
 
@@ -56,15 +57,24 @@ def __init__(self, heuristic=None, weight=1,
         self.start_time = 0  # execution time limitation
         self.runs = 0  # count number of iterations
 
-    def apply_heuristic(self, node_a, node_b, heuristic=None):
+    def apply_heuristic(self, node_a, node_b, heuristic=None, graph=None):
         """
         helper function to apply heuristic
         """
         if not heuristic:
             heuristic = self.heuristic
-        return heuristic(
-            abs(node_a.x - node_b.x),
-            abs(node_a.y - node_b.y))
+
+        dx = abs(node_a.x - node_b.x)
+        dy = abs(node_a.y - node_b.y)
+
+        if isinstance(graph, Grid):
+            if graph.passable_left_right_border and dx > graph.width / 2:
+                dx = graph.width - dx
+
+            if graph.passable_up_down_border and dy > graph.height / 2:
+                dy = graph.height - dy
+
+        return heuristic(dx, dy)
 
     def find_neighbors(self, grid, node, diagonal_movement=None):
         '''
@@ -110,7 +120,7 @@ def process_node(
         if not node.opened or ng < node.g:
             old_f = node.f
             node.g = ng
-            node.h = node.h or self.apply_heuristic(node, end)
+            node.h = node.h or self.apply_heuristic(node, end, graph=graph)
             # f is the estimated total cost from start to goal
             node.f = node.g + node.h
             node.parent = parent

pathfinding/finder/ida_star.py

@@ -47,7 +47,7 @@ def search(self, node, g, cutoff, path, depth, end, grid):
 
         self.nodes_visited += 1
 
-        f = g + self.apply_heuristic(node, end) * self.weight
+        f = g + self.apply_heuristic(node, end, graph=grid) * self.weight
 
         # We've searched too deep for this iteration.
         if f > cutoff:
@@ -104,9 +104,9 @@ def find_path(self, start, end, grid):
 
         self.nodes_visited = 0  # for statistics
 
-        # initial search depth, given the typical heuristic contraints,
+        # initial search depth, given the typical heuristic constraints,
         # there should be no cheaper route possible.
-        cutoff = self.apply_heuristic(start, end)
+        cutoff = self.apply_heuristic(start, end, graph=grid)
 
         while True:
             path = []

test/path_test_scenarios.json

@@ -85,5 +85,20 @@
         "expectedDiagonalLength": 12,
         "inverse": false,
         "weighted": true
+    },
+    {
+        "name": "s6",
+        "startX": 3,
+        "startY": 2,
+        "endX": 0,
+        "endY": 0,
+        "matrix": [[0, 0, 0, 0, 0],
+                  [0, 0, 0, 0, 0],
+                  [0, 0, 0, 0, 0],
+                  [0, 0, 0, 0, 0],
+                  [0, 0, 0, 0, 0]],
+        "expectedLength": 5,
+        "expectedDiagonalLength": 3,
+        "passableLeftRightBorder": true
     }
 ]

test/test_path.py

@@ -29,6 +29,10 @@
 def grid_from_scenario(scenario):
     inverse = scenario['inverse'] if 'inverse' in scenario else True
     grid = Grid(matrix=scenario['matrix'], inverse=inverse)
+    if scenario.get('passableLeftRightBorder'):
+        grid.set_passable_left_right_border()
+    if scenario.get('passableUpDownBorder'):
+        grid.set_passable_up_down_border()
     start = grid.node(scenario['startX'], scenario['startY'])
     end = grid.node(scenario['endX'], scenario['endY'])
     return grid, start, end
@@ -67,7 +71,6 @@ def test_path_diagonal():
             weighted = False
             if 'weighted' in scenario:
                 weighted = scenario['weighted']
-            print(dir(find))
             if weighted and not finder.weighted:
                 continue