34 lines
1.1 KiB
Python
34 lines
1.1 KiB
Python
|
class A_Star(object):
|
||
|
|
def __init__(self, starts, goals, h, d, neighbors):
|
||
|
|
"""
|
||
|
|
:param h: heuristic function
|
||
|
|
:param d: cost from node to node function
|
||
|
|
:param neighbors: neighbors function
|
||
|
|
"""
|
||
|
|
open_set = []
|
||
|
|
g_score = {}
|
||
|
|
f_score = {}
|
||
|
|
|
||
|
|
for start in starts:
|
||
|
|
open_set.append(start)
|
||
|
|
g_score[start] = d(0, start)
|
||
|
|
f_score[start] = h(start)
|
||
|
|
|
||
|
|
while open_set:
|
||
|
|
open_set.sort(key=lambda node: f_score[node])
|
||
|
|
current = open_set[0]
|
||
|
|
|
||
|
|
if current in goals:
|
||
|
|
self.cost = g_score[current]
|
||
|
|
break
|
||
|
|
|
||
|
|
open_set = open_set[1:] # remove current
|
||
|
|
|
||
|
|
for neighbor in neighbors(current):
|
||
|
|
tentative_g_score = g_score[current] + d(current, neighbor)
|
||
|
|
if neighbor not in g_score or \
|
||
|
|
tentative_g_score < g_score[neighbor]:
|
||
|
|
g_score[neighbor] = tentative_g_score
|
||
|
|
f_score[neighbor] = g_score[neighbor] + h(neighbor)
|
||
|
|
open_set.append(neighbor)
|