.gitignore

*.pyc
env/*

astar.py

from Queue import PriorityQueue
from math import sqrt
import numpy as np

# A* Search Algorithm
"""
A* Search Algorithm

f(x) = g(x) + h(x)

f(x) is the cost function.
g(x) is the actual cost thus far.
h(x) is the admissible heuristic function.
 In our case, h(x) will be the euclidean dist from x
 to the goal g.



"""

def heuristic(node, goal):
    return np.linalg.norm(np.array(node) - np.array(goal))

def astar(start, goal, graph):
    frontier = PriorityQueue()
    frontier.put(start, 0)
    came_from = {}
    cost_so_far = {}

    came_from[start] = None
    cost_so_far[start] = 0

    while not frontier.empty():
        current = frontier.get()

        if current == goal:
            break

        for next in graph[current]:
            new_cost = cost_so_far[current] + heuristic(current, next)
            if next not in cost_so_far or new_cost < cost_so_far[next]:
                cost_so_far[next] = new_cost
                priority = new_cost + heuristic(next, goal)
                frontier.put(next, priority)
                came_from[next] = current

    # Traceback and figure out what the actual path was
    current = goal
    path = [current]
    while current != start:
        current = came_from[current]
        path.append(current)
    path.reverse()
    return path


G = {
    (0, 0): [(0, 1), (1, -1)],
    (1, 0): [(1, 1)],
    (1, -1): [(1, 0)],
    (0, 1): []
}


print astar((0, 0), (1, 1), G)