Add 1-dir A* pathfinding

This commit adds one-directional A* pathfinding; it also adds an
image to aid with one of the explanations. I wasn't 100% confident on
where these types of images should reside.

I also noticed this dijkstra pathfinding algorithm rechecks nodes in the
open list many times. I left it because dijkstra is mostly just an example
algorithm (though you do use similiar algorithms for precomputing paths).
I did change it to name the dict so it's slightly easier to read.

* docs/Pathfinding.rst - Add astar documentation
+ imgs/onedirectionalastar_riverexample.png - referenced in comments
* pygorithm/pathfinding/__init__.py - add astar
+ pygorithm/pathfinding/astar.py - add one-directional astar
* pygorithm/pathfinding/dijkstra - add comments on reverse_path
* tests/test_pathing - add timing tests. This is the same test as before
but such a way that it can be reused for all graph-based pathfinding
algorithms. This method won't work for smarter algorithms like Theta*
because they won't work on these style of graphs (there require additional
assumptions like a square or octogonal grid), but the TimedTestCase is
reusable.

I updated the test to check based on total weight rather than a specific
path which is more accurate to what is being tested. This indirectly
ensures that there is a way between each node in the path (otherwise
get_edge_weight would return None and an exception would be raised).

Author: Tjstretchalot

docs/Pathfinding.rst

@@ -40,6 +40,7 @@ Features
 
 * Algorithms available:
     - Dijkstra (dijkstra)
+    - Unidirectional AStar (astar)
 
 
 * To see all the available functions in a module there is a `modules()` function available. For example,
@@ -48,7 +49,7 @@ Features
 
     >>> from pygorithm.pathfinding import modules
     >>> modules.modules()
-    ['dijkstra']
+    ['dijkstra', 'astar']
 
 * Get the code used for any of the algorithm
 
@@ -64,7 +65,7 @@ Dijkstra
 
 * Functions and their uses
 
-.. function:: dijkstra.find_path(pygorithm.data_structures.WeightedUndirectedGraph, vertex, vertex)
+.. function:: dijkstra.Dijkstra.find_path(pygorithm.data_structures.WeightedUndirectedGraph, vertex, vertex)
 
 - **pygorithm.data_structures.WeightedUndirectedGraph** : acts like an object with `graph` (see WeightedUndirectedGraph)
 - **vertex** : any hashable type for the start of the path
@@ -74,3 +75,16 @@ Dijkstra
 .. function:: dijkstra.get_code()
 
 - **Return Value**    : returns the code for the ``Dijkstra`` object
+
+Unidirectional AStar
+--------------------
+
+* Functions and their uses
+
+.. function:: astar.OneDirectionalAStar.find_path(pygorithm.data_structures.WeightedUndirectedGraph, vertex, vertex, function)
+
+- **pygorithm.data_structures.WeightedUndirectedGraph** : acts like an object with `graph` and `get_edge_weight` (see WeightedUndirectedGraph)
+- **vertex** : any hashable type for the start of the path
+- **vertex** : any hashable type for the end of the path
+- **function** : `function(graph, vertex, vertex)` returns numeric - a heuristic function for distance between two vertices
+- **Return Value** : returns a `List` of vertexes (of the same type of the graph) starting from from and going to to. This algorithm respects weights, but is only guarranteed to be optimal if the heuristic is admissable. An admissable function will never *overestimate* the cost from one node to another (in other words, it is optimistic).

imgs/onedirectionalastar_riverexample.png

@@ -2,7 +2,9 @@
 Collection of pathfinding examples
 """
 from . import dijkstra
+from . import astar
 
 __all__ = [
-    'dijkstra'
+    'dijkstra',
+    'astar'
 ]

pygorithm/pathfinding/__init__.py

@@ -0,0 +1,207 @@
+import heapq
+
+class OneDirectionalAStar(object):
+    """AStar object
+    Finds the optimal path between two nodes on
+    a graph while taking into account weights.
+    """
+    
+    # Some miscellaneous notes:
+    
+    # River example neighbors
+    # Imagine you had a graph that was constructed by the time it
+    # would take to get to different strategic locations on a map.
+    # Suppose there is a river that cuts the map in half vertically,
+    # and two bridges that allow crossing at the top and bottom of
+    # the map, but swimming is an option but very slow.
+    # For simplicity, on each side there is 1 base that acts as a 
+    # strategic location, both sides of the each bridge, and both
+    # sides of the river directly in the vertical center, for a total
+    # graph of 8 nodes (see imgs/onedirectionalastar_riverexample.png)
+    #
+    # Now suppose the heuristic naively used euclidean distance while 
+    # the actual weights were based on precalculated paths.
+    #
+    # Looking at the picture, if you were going from one base to the other
+    # middle side of the river, you would first expand the base and find
+    # 3 nodes: top (15 + 10), your side of center (5 + 3), bottom (15 + 10).
+    #
+    # You would expand your side of center and find the top and bottom at 
+    # (5 + 12) - WORSE than just going to them. This is the case where we
+    # would NOT add the path base->center->top to the open list because
+    # (for these weights) it will never be better than base->top.
+    #
+    # You would also add the new node (55 + 0) or the destination.
+    #
+    # Then you expand the top node (or bottom) on the other side of
+    # river with a cost of (18 + 12).
+    #
+    # You expand the top node on the other side of the river next and find
+    # one of the neighbors is already on the open list (the destination)
+    # at a score of (55 + 0), but your cost to get there is (30 + 0). This
+    # is where you would REPLACE the old path with yourself.
+    
+    def __init__(self):
+        pass
+    
+    def reverse_path(self, node):
+        """
+        Walks backward from an end node to the start
+        node and reconstructs a path. Meant for internal
+        use.
+        :param node: dict containing { 'vertex': any hashable, 'parent': dict or None }
+        :return:     a list of vertices ending on the node
+        """
+        result = []
+        while node is not None:
+            result.insert(0, node['vertex'])
+            node = node['parent']
+        return result
+    
+    def find_path(self, graph, start, end, heuristic_fn):
+        """
+        Calculates the optimal path from start to end
+        on the graph. Weights are taken into account.
+        This implementation is one-directional expanding
+        from the start to the end. This implementation is
+        faster than dijkstra based on how much better the
+        heuristic is than flooding.
+        
+        The heuristic must never overestimate the distance
+        between two nodes (in other words, the heuristic
+        must be "admissible"). Note however that, in practice,
+        it is often acceptable to relax this requirement and
+        get very slightly incorrect paths if:
+            - The distance between nodes are small
+            - There are too many nodes for an exhaustive search 
+              to ever be feasible.
+            - The world is mostly open (ie there are many paths
+              from the start to the end that are acceptable)
+            - Execution speed is more important than accuracy.
+        The best way to do this is to make the heuristic slightly
+        pessimistic (typically by multiplying by small value such
+        as 1.1). This will have the algorithm favor finishing its
+        path rather than finding a better one. This optimization
+        needs to be tested based on the map.
+        
+        :param graph:        object contains `graphs` as per pygorithm.data_structures.WeightedUndirectedGraph
+                             and `get_edge_weight` in the same manner.
+        :param start:        the start vertex (which is the same type of the verticies in the graph)
+        :param end:          the end vertex (which is the same type of the vertices in the graph)
+        :param heuristic_fn: function(graph, start, end) that when given two vertices returns an expected cost to get
+                             to get between the two vertices.
+        :return:      a list starting with `start` and ending with `end`, or None if no path is possible.
+        """
+        
+        # It starts off very similiar to Dijkstra. However, we will need to lookup
+        # nodes in the open list before. There can be thousands of nodes in the open
+        # list and any unordered search is too expensive, so we trade some memory usage for
+        # more consistent performance by maintaining a dictionary (O(1) lookup) between
+        # vertices and their nodes. 
+        open_lookup = {}
+        open = []
+        closed = set()
+        
+        # We require a bit more information on each node than Dijkstra
+        # and we do slightly more calculation, so the heuristic must
+        # prune enough nodes to offset those costs. In practice this
+        # is almost always the case if their are any large open areas
+        # (nodes with many connected nodes).
+        
+        # Rather than simply expanding nodes that are on the open list
+        # based on how close they are to the start, we will expand based
+        # on how much distance we predict is between the start and end 
+        # node IF we go through that parent. That is a combination of 
+        # the distance from the start to the node (which is certain) and
+        # the distance from the node to the end (which is guessed).
+        
+        # We use the counter to enforce consistent ordering between nodes
+        # with the same total predicted distance.
+        
+        counter = 0 
+        heur = heuristic_fn(graph, start, end)
+        open_lookup[start] = { 'vertex': start, 'dist_start_to_here': 0, 'pred_dist_here_to_end': heur, 'pred_total_dist': heur, 'parent': None }
+        heapq.heappush(open, (heur, counter, start))
+        counter += 1
+        
+        while len(open) > 0:
+            current = heapq.heappop(open)
+            current_vertex = current[2]
+            current_dict = open_lookup[current_vertex]
+            del open_lookup[current_vertex]
+            closed.update(current_vertex)
+            
+            if current_vertex == end:
+                return self.reverse_path(current_dict)
+            
+            neighbors = graph.graph[current_vertex]
+            for neighbor in neighbors:
+                if neighbor in closed:
+                    # If we already expanded it it's definitely not better
+                    # to go through this node, or we would have expanded this
+                    # node first.
+                    continue
+                
+                cost_start_to_neighbor = current_dict['dist_start_to_here'] + graph.get_edge_weight(current_vertex, neighbor)
+                neighbor_from_lookup = open_lookup.get(neighbor, None) # avoid searching twice
+                if neighbor_from_lookup is not None:
+                    # If our heuristic is NOT consistent or the grid is NOT uniform,
+                    # it is possible that there is a better path to a neighbor of a 
+                    # previously expanded node. See above, ctrl+f "river example neighbors"
+                    
+                    # Note that the heuristic distance from here to end will be the same for
+                    # both, so the only difference will be in start->here through neighbor
+                    # and through the old neighbor.
+                    
+                    old_dist_start_to_neighbor = neighbor_from_lookup['dist_start_to_here']
+                    
+                    if cost_start_to_neighbor < old_dist_start_to_neighbor:
+                        pred_dist_neighbor_to_end = neighbor_from_lookup['pred_dist_here_to_end']
+                        pred_total_dist_through_neighbor_to_end = cost_start_to_neighbor + pred_dist_neighbor_to_end
+                        # Note, we've already shown that neighbor (the vector) is already in the open list,
+                        # but unfortunately we don't know where and we have to do a slow lookup to fix the 
+                        # key its sorting by to the new predicted total distance.
+                        
+                        # In case we're using a fancy debugger we want to search in user-code so when 
+                        # this lookup freezes we can see how much longer its going to take.
+                        found = None
+                        for i in range(0, len(open)):
+                            if open[i][2] == neighbor:
+                                found = i
+                                break
+                        if found is None:
+                            raise Exception('A vertex is in the open lookup but not in open. This is impossible, please submit an issue + include the graph!')
+                        # todo I'm not certain about the performance characteristics of doing this with heapq, nor if 
+                        # it would be better to delete heapify and push or rather than replace
+                        open[i] = (pred_total_dist_through_neighbor_to_end, counter, neighbor)
+                        counter += 1
+                        heapq.heapify(open)
+                        open_lookup[neighbor] = { 'vertex': neighbor,
+                                                  'dist_start_to_here': cost_start_to_neighbor, 
+                                                  'pred_dist_here_to_end': pred_dist_neighbor_to_end, 
+                                                  'pred_total_dist': pred_total_dist_through_neighbor_to_end, 
+                                                  'parent': current_dict }
+                    continue
+                    
+                
+                # We've found the first possible way to the path!
+                pred_dist_neighbor_to_end = heuristic_fn(graph, neighbor, end)
+                pred_total_dist_through_neighbor_to_end = cost_start_to_neighbor + pred_dist_neighbor_to_end
+                heapq.heappush(open, (pred_total_dist_through_neighbor_to_end, counter, neighbor))
+                open_lookup[neighbor] = { 'vertex': neighbor,
+                                          'dist_start_to_here': cost_start_to_neighbor,
+                                          'pred_dist_here_to_end': pred_dist_neighbor_to_end,
+                                          'pred_total_dist': pred_total_dist_through_neighbor_to_end,
+                                          'parent': current_dict }
+        
+        return None
+            
+    @staticmethod
+    def get_code(self):
+        """
+        returns the code for the current class
+        """
+        return inspect.getsource(OneDirectionalAStar)
+        
+        
+        

pygorithm/pathfinding/astar.py

@@ -8,6 +8,13 @@ def __init__(self):
         pass
 
     def reverse_path(self, node):
+        """
+        Walks backward from an end node to the start
+        node and reconstructs a path. Meant for internal
+        use.
+        :param node: dict containing { 'vertex': any hashable, 'parent': dict or None }
+        :return:     a list of vertices ending on the node
+        """
         result = []
         while node is not None:
             result.insert(0, node['vertex'])
@@ -33,21 +40,25 @@ def find_path(self, graph, start, end):
         # key for sorting. The second element in the tuple is just a counter and is used to avoid having
         # to hash the dictionary when the distance from the source is not unique.
 
+        # performance might be improved by also searching the open list and avoiding adding those nodes
+        # but since this algorithm is typically for examples only performance improvements are not made
+        
         counter = 0
         heapq.heappush(open, (0, counter, { 'vertex': start, 'parent': None }))
         counter += 1
 
         while len(open) > 0:
             current = heapq.heappop(open)
-            closed.update(current[2]['vertex'])
+            current_dict = current[2]
+            closed.update(current_dict['vertex'])
 
-            if current[2]['vertex'] == end:
-                return self.reverse_path(current[2])
+            if current_dict['vertex'] == end:
+                return self.reverse_path(current_dict)
 
-            neighbors = graph.graph[current[2]['vertex']]
+            neighbors = graph.graph[current_dict['vertex']]
             for neighbor in neighbors:
                 if neighbor not in closed:
-                    heapq.heappush(open, (current[0] + 1, counter, { 'vertex': neighbor, 'parent': current[2] }))
+                    heapq.heappush(open, (current[0] + 1, counter, { 'vertex': neighbor, 'parent': current_dict }))
                     counter += 1
 
 

pygorithm/pathfinding/dijkstra.py

@@ -1,22 +1,23 @@
 import unittest
+import math
+import time
 
-from pygorithm.pathing import (dijkstra)
+from pygorithm.pathfinding import (dijkstra, astar)
+from pygorithm.data_structures import graph
 
-class TestDijkstra(unittest.TestCase):
-    def test_exists(self):
-        pather = dijkstra.Dijkstra()
+class TimedTestCase(unittest.TestCase):
+    # https://hackernoon.com/timing-tests-in-python-for-fun-and-profit-1663144571
+    def setUp(self):
+        self._started_at = time.time()
+    def tearDown(self):
+        elapsed = time.time() - self._started_at
+        print('{} ({}s)'.format(self.id(), round(elapsed, 2)))
 
-        self.assertIsNotNone(pather)
-        self.assertIsNotNone(pather.find_path)
-        self.assertIsNotNone(pather.get_code)
+class SimplePathfindingTestCaseTimed(TimedTestCase):
+    def find_path(self, my_graph, v1, v2):
+        return [ (0, 0), (0, 1), (0, 2), (1, 3), (2, 4), (3, 3), (3, 2), (3, 1), (3, 0) ]
 
     def test_find_path_package_example(self):
-        # import the required pathing
-        from pygorithm.pathing import dijkstra
-        
-        # import a graph data structure
-        from pygorithm.data_structures import graph
-        
         # initialize the graph with nodes from (0, 0) to (4, 4)
         # with weight corresponding to distance (orthogonal 
         # is 1, diagonal is sqrt(2))
@@ -30,12 +31,31 @@ def test_find_path_package_example(self):
         my_graph.remove_edge((2, 2))
         my_graph.remove_edge((2, 3))
 
-        # create a pathfinder
-        my_pathfinder = dijkstra.Dijkstra()
-        
         # calculate a path
-        my_path = my_pathfinder.find_path(my_graph, (0, 0), (3, 0))
+        my_path = self.find_path(my_graph, (0, 0), (3, 0))
 
         # check path:
-        expected_path = [ (0, 0), (0, 1), (0, 2), (1, 3), (2, 4), (3, 3), (3, 2), (3, 1), (3, 0) ]
-        self.assertListEqual(expected_path, my_path)
+        self.assertIsNotNone(my_path)
+        
+        total_weight = 0
+        for i in range(1, len(my_path)):
+            total_weight += my_graph.get_edge_weight(my_path[i - 1], my_path[i])
+            
+        self.assertAlmostEqual(9.242640687119284, total_weight)
+        
+class TestDijkstraTimed(SimplePathfindingTestCaseTimed):
+    def find_path(self, my_graph, v1, v2):
+       my_pathfinder = dijkstra.Dijkstra()
+       return my_pathfinder.find_path(my_graph, (0, 0), (3, 0))
+
+class TestAStarUnidirectionalTimed(SimplePathfindingTestCaseTimed):
+    def find_path(self, my_graph, v1, v2):
+        my_pathfinder = astar.OneDirectionalAStar()
+        
+        def my_heuristic(graph, v1, v2):
+            dx = v2[0] - v1[0]
+            dy = v2[1] - v1[1]
+            return math.sqrt(dx * dx + dy * dy)
+        
+        return my_pathfinder.find_path(my_graph, v1, v2, my_heuristic)
+