Recipe 578919 revision 3

Diff to Previous Revision

--- revision 2 2014-08-05 23:28:11
+++ revision 3 2014-08-06 10:03:01
@@ -5,84 +5,58 @@
 import numpy
 from heapq import *
 
+
 def heuristic(a, b):
-    """calculates the H cost using the Manhattan Method"""
-    return 10*(abs(a[0] - b[0]) + abs(a[1] - b[1]))
+    return (b[0] - a[0]) ** 2 + (b[1] - a[1]) ** 2
 
-def getG(current, neighbor):
-    """Function to calculate G cost, 10 for ortogonal movement, and 14 for diagonal movement"""
-    vector_sub = [abs(neighbor[0] - current[0]), abs(neighbor[1] - current[1])]
-    if sum(vector_sub) > 1:
-        move_cost = 14
-    else:
-        move_cost = 10
-    return move_cost
+def astar(array, start, goal):
 
-def astar(array, start, dest):
-    """ 
-    Function to calculate the shortest path from point a to point
-    b in a numpy array, using A* pathfinding algorithm and a binary heap.
-    0's in the array are walkable nodes, while 1's are solid obstacles
-    """
     neighbors = [(0,1),(0,-1),(1,0),(-1,0),(1,1),(1,-1),(-1,1),(-1,-1)]
-    closed = []
-    open_list = []
 
+    close_set = set()
     came_from = {}
-
-    gscore = {start : 0}
-    fscore = {start : heuristic(start, dest)}
+    gscore = {start:0}
+    fscore = {start:heuristic(start, goal)}
     oheap = []
 
-    #place start and its fscore to heap and add start to open_set
     heappush(oheap, (fscore[start], start))
-    open_list.append(start)
+    
+    while oheap:
 
-    while len(open_list):
-        #Grab fscore and corresponding position off top of heap
         current = heappop(oheap)[1]
-        open_list.append(current)
-        
-        if current == dest: #check if path is done and trace it back
-            path = []
+
+        if current == goal:
+            data = []
             while current in came_from:
-                path.append(current)
+                data.append(current)
                 current = came_from[current]
-            return list(reversed(path))
+            return data
 
-        #drop current and place it in closed list
-        open_list.remove(current)
-        closed.append(current)
-
-        for x, y in neighbors: #check current nodes neighbors
-            current_neighbor = current[0]+x , current[1]+y
-            if current_neighbor in closed:
-                continue
-
-            new_g = gscore[current] + getG(current, current_neighbor)
- 
-            #check if neighbors are outside array bounds or they are obstacles
-            if 0 <= current_neighbor[0] < array.shape[0]:
-                if 0 <= current_neighbor[1] < array.shape[1]:                
-                    if array[current_neighbor[0]][current_neighbor[1]] == 1:
-                        closed.append(current_neighbor)
-                        continue # solid obstacle
+        close_set.add(current)
+        for i, j in neighbors:
+            neighbor = current[0] + i, current[1] + j            
+            tentative_g_score = gscore[current] + heuristic(current, neighbor)
+            if 0 <= neighbor[0] < array.shape[0]:
+                if 0 <= neighbor[1] < array.shape[1]:                
+                    if array[neighbor[0]][neighbor[1]] == 1:
+                        continue
                 else:
                     # array bound y walls
                     continue
             else:
                 # array bound x walls
                 continue
-     
-            if current_neighbor not in open_list or new_g < gscore.get(current_neighbor, 0):
-                #make current node parent of neighbor node, and update G and F costs
-                came_from[current_neighbor] = current
-                gscore[current_neighbor] = new_g
-                fscore[current_neighbor] = new_g + getG(current, current_neighbor)
-
-                #add neighbors to open_set and to bin heap
-                open_list.append(current_neighbor)
-                heappush(oheap, (fscore[current_neighbor], current_neighbor))
+                
+            if neighbor in close_set and tentative_g_score >= gscore.get(neighbor, 0):
+                continue
+                
+            if  tentative_g_score < gscore.get(neighbor, 0) or neighbor not in [i[1]for i in oheap]:
+                came_from[neighbor] = current
+                gscore[neighbor] = tentative_g_score
+                fscore[neighbor] = tentative_g_score + heuristic(neighbor, goal)
+                heappush(oheap, (fscore[neighbor], neighbor))
+                
+    return False
 
 '''Here is an example of using my algo with a numpy array,
    astar(array, start, destination)