Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #!/usr/bin/python
- import gridgraph
- import priorityqueue
- import vector
- def heuristic_distance(a, b):
- return vector.distance_between(a.value, b.value)
- def distance(a, b):
- return 1
- def astar(start, end, d, hd):
- visited = set()
- dist = dict()
- dist[start] = 0
- hdist = dict()
- hdist[start] = hd(start, end)
- previous = dict()
- previous[start] = None
- queue = priorityqueue.Min()
- queue.push(start, 0)
- while queue.length > 0:
- n = queue.pop()
- if n == end:
- #reconstruct the path
- path = []
- while n in previous:
- path.insert(0, n)
- n = previous[n]
- path.insert(0, start)
- return path
- visited.add(n)
- #dist to each child
- for neighbor in n.children:
- if neighbor in visited:
- continue
- alt = dist[n] + d(n, neighbor)
- if neighbor not in dist or alt < dist[neighbor]:
- dist[neighbor] = alt
- previous[neighbor] = n
- #full distance calc:
- hdist[neighbor] = dist[neighbor] + hd(neighbor, end)
- queue.push(neighbor, hdist[neighbor])
- #end not found
- return None
- def main():
- graph = gridgraph.GridGraph(20, 20)
- path = astar(graph.grid[0][0], graph.grid[19][19], distance, heuristic_distance)
- graph.marked[path[0].value] = "S "
- for n in range(1, len(path)-1):
- graph.marked[path[n].value] = "O "
- graph.marked[path[-1].value] = "E "
- print graph
- if __name__ == "__main__":
- main()
- OUTPUT:
- /usr/bin/python2.7 /home/jesse/Development/Python/goog/playground.py
- O - - - - - - - - - - - - - - - - - - -
- O O O - - - - - - - - - - - - - - - - -
- - - O O - - - - - - - - - - - - - - - -
- - - - O - - - - - - - - - - - - - - - -
- - - - O O O - - - - - - - - - - - - - -
- - - - - - O O - - - - - - - - - - - - -
- - - - - - - O O - - - - - - - - - - - -
- - - - - - - - O - - - - - - - - - - - -
- - - - - - - - O O O - - - - - - - - - -
- - - - - - - - - - O O - - - - - - - - -
- - - - - - - - - - - O - - - - - - - - -
- - - - - - - - - - - O O O - - - - - - -
- - - - - - - - - - - - - O - - - - - - -
- - - - - - - - - - - - - O O - - - - - -
- - - - - - - - - - - - - - O O - - - - -
- - - - - - - - - - - - - - - O O O - - -
- - - - - - - - - - - - - - - - - O - - -
- - - - - - - - - - - - - - - - - O O O -
- - - - - - - - - - - - - - - - - - - O O
- - - - - - - - - - - - - - - - - - - - E
- Process finished with exit code 0
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement