astar-pseudocode.py

Node:
  position: (x,y,z)
  G_cost = INFINITY // costo local
  H_cost = INFINITY // distancia heuristica
  F_cost = () => G_cost + H_cost
  neighbors = [] // vecinos
  is_walkable  = true
  is_closed = false
  
  
Grid:
  x_count = 100
  y_count = 100
  nodes = []
  generate()
    for x in range(x_count):
      for y in range(y_count):
        createNode()
        
    # connect neighbors

# C# LIST comprehension
List<Node> nodes = new List<Node>();
var open = from n in nodes where !n.is_closed select n;      

def find_path(start, target):
  
  OPEN = new list
  CLOSED = new list
  add start to OPEN
  
  while OPEN is not empty:
    # busco el candidato mas prometedor de la lista abierta
    current = n from OPEN with lowest f cost
    remove current from OPEN
    add current to CLOSED
    
    if current is target
      return generatePath(current)
    
    foreach(var n in current.neighbours)
      # check if n is obstacle or CLOSED
      if n is not obstacle or n is in CLOSED
        continue    
      
      # check if new path is shorter
      g_cost = current.g_cost + distance(current,n)
      if g_cost < n.g_cost OR n is not in OPEN
        n.g_cost = g_cost // update since we
                          // got a better path
        n.h_cost = heuristic(n, target)
        # f_cost is just the sum...
        n.parent = current
        if n is not in OPEN
          add n to OPEN
   
   # no encontre ningun camino...
   return NULL   
    
# calculo aproximado que nos resulte
# suficientemente aceptable  
def heuristic(n, target):
  distance(n.position, target.position)   
    
# recorrer los nodos hasta el comienzo
# para devolver el camino
def generatePath(current):
  PATH = new list
  n = current
  while n has a parent:
    add n.position to PATH
    n = n.parent
  return PATH