Fix double counting of heuristic

Author: jobh

a_star/a_star.py

@@ -54,26 +54,25 @@ def backtrack(self, current):
             yield current
             current = self.visited[current].came_from
 
-Node = collections.namedtuple("Node", ["cost_estimate", "vertex", "came_from"])
+Node = collections.namedtuple("Node", ["cost_estimate", "cost_so_far", "vertex", "came_from"])
 
 def a_star_search(graph, start, end, heuristic):
     """
     Calculates the shortest path from start to end.
 
-    :param graph: A graph object. The graph object can be anything that implements the following methods for a vertex of any comparable and hashable type V:
+    :param graph: A graph object. The graph object can be anything that implements the following method for a vertex of any comparable and hashable type V:
 
-        graph.neighbors( from_vertex:V ) : Iterable( to_vertex:V, to_vertex:V, ...)
-        graph.cost( from_vertex:V, to_vertex:V ) : float
+        graph.neighbors( from_vertex:V ) : Iterable( (to_vertex:V,edge_cost:float), (to_vertex:V,edge_cost:float), ...)
 
     :param start: The starting vertex, as type V.
     :param end: The ending vertex, as type V.
-    :param heuristic: Heuristic lower-bound cost function taking arguments ( from_vertex:V, end:V ).
+    :param heuristic: Heuristic lower-bound cost function taking arguments ( from_vertex:V, end:V ) and returning float.
     :returns: A DijkstraHeap object.
 
     """
 
     frontier = DijkstraHeap()
-    frontier.insert( Node(heuristic(start, end), start, None) )
+    frontier.insert( Node(heuristic(start, end), 0, start, None) )
 
     while True:
 
@@ -84,14 +83,11 @@ def a_star_search(graph, start, end, heuristic):
         if current_node.vertex == end:
             return frontier
 
-        for neighbor in graph.neighbors( current_node.vertex ):
+        for neighbor, edge_cost in graph.neighbors( current_node.vertex ):
 
-            cost_so_far = current_node.cost_estimate - heuristic(current_node.vertex, end)
-            new_cost = ( cost_so_far
-                         + graph.cost(current_node.vertex, neighbor)
-                         + heuristic(neighbor, end) )
+            new_cost = current_node.cost_so_far + edge_cost
 
-            new_node = Node(new_cost, neighbor, current_node.vertex)
+            new_node = Node(new_cost + heuristic(neighbor, end), new_cost, neighbor, current_node.vertex)
 
             frontier.insert(new_node)
 

a_star/tools.py

@@ -23,6 +23,9 @@ def is_wall(self, point):
         """
         return point not in self.walls
 
+    def cost(self, from_node, to_node):
+        return 1
+
     def neighbors(self, point):
         """ Yields the valid neighbours of a given point.
 
@@ -34,7 +37,7 @@ def neighbors(self, point):
         candidates = [(x + 1, y), (x, y - 1), (x - 1, y), (x, y + 1)]
         candidates = filter(self.is_inside, candidates)
         candidates = filter(self.is_wall, candidates)
-        yield from candidates
+        yield from ((pt, self.cost(point, pt)) for pt in candidates)
 
     def _draw_tile(self, point, style, width):
         """ Returns a symbol for the current point given the style dictionary and the drawing width.

examples/maze_solving_example.py

@@ -47,10 +47,10 @@ def from_id_width(point, width):
 
      graph.draw(width=5, point_to = {k:v.came_from for k,v in frontier.visited.items()}, start=(1, 4), goal=(7, 8))
 
+     print("[costs]")
+
+     graph.draw(width=5, number = {k:v.cost_so_far for k,v in frontier.visited.items()}, start=(1, 4), goal=(7, 8))
+
      print("[total cost estimates]")
-     costs = {k:v.cost_estimate for k,v in frontier.visited.items()}
-     graph.draw(width=5, number = costs, start=(1, 4), goal=(7, 8))
 
-     print("[costs]")
-     costs_so_far = { k: v - manhattan_distance(k, (7, 8)) for k,v in costs.items() }
-     graph.draw(width=5, number = costs_so_far, start=(1, 4), goal=(7, 8))
+     graph.draw(width=5, number = {k:v.cost_estimate for k,v in frontier.visited.items()}, start=(1, 4), goal=(7, 8))

tests/test_dijkstra_heap.py

@@ -9,7 +9,7 @@ class DijkstraHeapTests(unittest.TestCase):
 
     def test_sorted_unique(self):
 
-        test_data = [a_star.Node(x,x,None) for x in (5,7,3,2,5,3,5,7,2,1,3,1)]
+        test_data = [a_star.Node(x,x,x,None) for x in (5,7,3,2,5,3,5,7,2,1,3,1)]
         sorted_unique = sorted(set(test_data))
 
         # Prepare and insert elements in DijkstraHeap
@@ -30,7 +30,7 @@ def test_sorted_unique(self):
 
     def test_delete_repeated(self):
 
-        test_data = [a_star.Node(x,x,None) for x in (1,1,1,1,1,1,1,1,1)]
+        test_data = [a_star.Node(x,x,x,None) for x in (1,1,1,1,1,1,1,1,1)]
 
         # Prepare and insert elements in DijkstraHeap
 
@@ -45,12 +45,12 @@ def test_delete_repeated(self):
             elem = frontier.pop()
             if elem:
                 result.append(elem)
-        self.assertTrue( result == [a_star.Node(1,1,None)]  )
+        self.assertTrue( result == [a_star.Node(1,1,1,None)]  )
 
 
     def test_came_from(self):
 
-        test_data = [a_star.Node(x,x,x+1) for x in range(10)]
+        test_data = [a_star.Node(x,x,x,x+1) for x in range(10)]
 
         # Prepare and insert elements in DijkstraHeap
 
@@ -70,7 +70,7 @@ def test_came_from(self):
 
     def test_came_from_unique(self):
 
-        test_data = [a_star.Node(x,x,x+1) for x in [0,1,2,3,2,4,1]]
+        test_data = [a_star.Node(x,x,x,x+1) for x in [0,1,2,3,2,4,1]]
 
         # Prepare and insert elements in DijkstraHeap