gistfile1.py

# Point : nametuple x : Real, y : Real, db: Real, de: Real, parent : Point
# db: real distance from the beginning
# de : heuristic distance from the end

def a_star(B, E, D, H, N):
  """
  B : begin Point
  E : end Point
  D : distance (Point, Point) -> Real
  H : heuristic distance (Point, Point) -> Real
  N : neighbors (Point) -> [Point]
  """
  
  # Init
  B.db = 0
  B.de = H(B, E)
  open_set = set(B)
  close_set = set()
  
  while open_set:
    if E in open_set:
      # Success
      path = []
      current = E
      while current.parent:
        path.append(current)
        current = current.parent
      return path
    
    # Chose the nearly point
    P = sorted(open_set, key=lambda p:  p.db + p.de)[0]
    close_set.add(P)
    
    # Add the neighbors
    for neighbor in (n for n in N(P) if n not in close_set):
      neighbor.db = P.db + D(P, neighbor)
      neighbor.de = H(neighbor, E)
      neighbor.parent = P
      open_set.add(neighbor)
    
  
  # Fail
  return None
  
def distance(p1, p2):
  """ Euclidian distance (squared) """
  return (p1.x - p2.x) ** 2 + (p1.y - p2.y) ** 2

heuristic_distance = distance

def neighbors(P):
  """ Neighbors """
  return [Point(x=P.x-1, y=P.y),
      Point(x=P.x+1, y=P.y),
      Point(x=P.x, y=P.y-1),
      Point(x=P.x, y=P.y+1)]