def astar_search(begin, end, graph):
global _infinite_weight
start = graph.node(begin)
if start is None:
return Path()
finish = graph.node(end)
if finish is None or finish.id() == start.id():
return Path()
nodes = {}
for node_id in graph.nodes():
node = graph.node(node_id)
cost = 0 if node_id == begin else _infinite_weight
nodes[node_id] = _AstarNode(node, cost, None, None)
visited_nodes = []
priority_queue = sortedList([nodes[start.id()]], key=lambda x: x.cost, load=len(nodes))
iterations = 0
while priority_queue:
iterations += 1
current = priority_queue.pop(0)
visited_nodes.append(current.id)
edges = current.node.edges()
for edge_id in edges:
edge = graph.edge(edge_id)
if edge is None:
continue
edge_info = edges[edge_id]
if edge_info.tail() in visited_nodes:
continue
tail = nodes.get(edge_info.tail(), None)
if tail is None:
continue
if tail.heuristics_cost is None:
tail.heuristics_cost = astar_heuristics(tail.node, finish)
cost = current.cost + edge.weight() + tail.node.weight() + tail.heuristics_cost
if cost < tail.cost:
tail.cost = cost
tail.parent_id = current.id
tail.parent = current.node
tail.edge = edge
priority_queue.discard(tail)
priority_queue.add(tail)
elif tail not in priority_queue:
priority_queue.add(tail)
if tail.id == end:
del priority_queue[:]
break
path = []
current = nodes[end]
while current.parent_id != 0 and current.edge is not None:
path.append((current.parent, current.node, current.edge))
current = nodes[current.parent_id]
if path and current.id == begin:
path.reverse()
return Path(path, iterations)
return Path()
def dijkstra_search(begin, end, graph):
global _infinite_weight
start = graph.node(begin)
if start is None:
return Path()
finish = graph.node(end)
if finish is None or finish.id() == start.id():
return Path()
nodes = {}
for node_id in graph.nodes():
node = graph.node(node_id)
cost = 0 if node_id == begin else _infinite_weight
nodes[node_id] = _DijkstraNode(node, cost, None, None)
visited_nodes = []
priority_queue = sortedList([nodes[start.id()]], key=lambda x: x.cost, load=len(nodes))
iterations = 0
# "resulting_cost" is used to reduce total number of visited nodes.
# Explanation: when you have already found target node you can check if other search paths are shorter than
# the found one, and if not then there is no need to explore such path further.
resulting_cost = _infinite_weight
while priority_queue:
iterations += 1
current = priority_queue.pop(0)
visited_nodes.append(current.id)
edges = current.node.edges()
for edge_id in edges:
edge = graph.edge(edge_id)
if edge is None:
continue
edge_info = edges[edge_id]
if edge_info.tail() in visited_nodes:
continue
tail = nodes.get(edge_info.tail(), None)
if tail is None:
continue
cost = current.cost + edge.weight() + tail.node.weight()
if tail.id == end:
resulting_cost = min(cost, resulting_cost)
if cost < tail.cost:
tail.cost = cost
tail.parent_id = current.id
tail.parent = current.node
tail.edge = edge
priority_queue.discard(tail)
if cost < resulting_cost:
priority_queue.add(tail)
elif cost < resulting_cost and tail not in priority_queue:
priority_queue.add(tail)
path = []
current = nodes[end]
while current.parent_id != 0 and current.edge is not None:
path.append((current.parent, current.node, current.edge))
current = nodes[current.parent_id]
if path and current.id == begin:
path.reverse()
return Path(path, iterations)
return Path()
