diff --git a/path.py b/path.py index b32e9d3..0980ebd 100644 --- a/path.py +++ b/path.py @@ -15,7 +15,7 @@ def astar(graph, start, goal, cost = euclidian_distance, heuristic = euclidian_d closed = set() m_heur = {} m_parent = {} - m_cost = {} # path costs + m_cost = {} # absolute path costs def path_from(node): def parents(node): @@ -33,25 +33,26 @@ def astar(graph, start, goal, cost = euclidian_distance, heuristic = euclidian_d # sort opened nodes based on the heuristic and consider the first one current = sorted(opened, key=lambda n : m_heur.get( n, heuristic(n,goal) ) )[0] if current == goal: + # FIXME add backtrack validation return path_from(current) closed.add(current) opened.remove(current) - for node in graph[current]: - if node in closed: + for neighbor in graph[current]: + if neighbor in closed: continue - elif node in opened: - next_cost = m_cost[current] + cost(current,node) - if next_cost < m_cost[node]: - m_cost[node] = next_cost - m_parent[node] = current + elif neighbor in opened: + next_cost = m_cost[current] + cost(current,neighbor) + if next_cost < m_cost[neighbor]: + m_cost[neighbor] = next_cost + m_parent[neighbor] = current else: - m_cost[node] = m_cost[current] + cost(current,node) - m_heur[node] = heuristic( node, goal ) - m_parent[node] = current - opened.add(node) + m_cost[neighbor] = m_cost[current] + cost(current,neighbor) + m_heur[neighbor] = heuristic( neighbor, goal ) + m_parent[neighbor] = current + opened.add(neighbor) return []