User:Keiji/aystar.py

From Rosetta Code
 
def bsearch(haystack, needle, rev=False, func=None):
p1 = 0
p2 = len(haystack)
while p2 > p1:
m = (p2+p1)/2
ht = haystack[m]
st = needle
if (ht == st): return m
if func:
ht = func(ht)
st = func(st)
if (ht < st) != rev:
p1 = m+1
else:
p2 = m
return p1
 
class BiList:
def __init__(self, fore):
self.fore = fore
self.flen = len(self.fore)
self.back = []
i = 0
while i < self.num:
t = self.fore[i]
t1 = t - len(self.back) + 1
if (t1 > 0): self.back += [-1]*t1
self.back[t] = i
i += 1
self.blen = len(self.back)
 
def __getitem__(self, index):
return self.fore[index]
 
def find(self, item):
if (item < 0) or (item >= self.blen):
return -1
return self.back[item]
 
def pop(self, index=-1):
x = self.fore.pop(index)
self.back[x] = -1
self.flen -= 1
if (index >= 0):
while (index < self.flen):
self.back[self.fore[index]] = index
index += 1
return x
 
def insert(self, index, item):
# If item is already in list, remove it
pos = self.find(item)
if (pos == index):
return
if (pos >= 0):
self.pop(pos)
if (pos < index): index -= 1
 
# Now insert it in the desired place
self.fore.insert(index, item)
t1 = item - self.blen + 1
if (t1 > 0):
self.back += [-1]*t1
self.blen += t1
self.back[item] = index
self.flen += 1
index += 1
while (index < self.flen):
self.back[self.fore[index]] = index
index += 1
 
def __len__(self):
return self.flen
 
def __contains__(self, item):
if (item < 0) or (item >= self.blen):
return False
return self.back[item] >= 0
 
class AyStar:
def __init__(self, cost, est):
# cost: dict from tuple (x, y) to cost to travel between x and y
# no entry in dict <=> no arc between nodes
self.cost = cost
 
# est: function to calculate estimated distance between x and y
self.est = est
 
# make a lookup table for all neighbors of each node
t = -1
for x, y in self.cost:
if (t < x): t = x
if (t < y): t = y
self.num = t+1
 
self.neighbors = []
i = 0
while i < self.num:
self.neighbors.append([])
i += 1
 
for x, y in self.cost:
self.neighbors[x].append(y)
self.neighbors[y].append(x)
 
def reconstruct_path(self, came_from, current_node):
r = [current_node]
 
while came_from[current_node] >= 0:
current_node = came_from[current_node]
r = [current_node] + r
return r
 
def run(self, start, goal):
# start: index of start node
# goal: index of goal node
 
# The set of nodes already evaluated.
closedset = []
 
# The set of tentative nodes to be evaluated.
openset = range(0, self.num)
 
# initialize arrays
came_from = [-1]*self.num
f_score = [0]*self.num
g_score = [0]*self.num
h_score = [0]*self.num
 
# Distance from start along optimal path.
g_score[start] = 0
h_score[start] = self.est(start, goal)
 
# Estimated total distance from start to goal through y.
f_score[start] = h_score[start]
 
# move start to start of openset, so that node with lowest f_score value is at the end
# (at this point, all nodes except start have zero f_score)
openset.pop(start)
openset = [start] + openset
 
# make lookup table for openset (for efficiency)
openset = BiList(openset)
 
while len(openset):
x = openset.pop()
 
if x == goal:
return self.reconstruct_path(came_from, goal)
 
closedset.append(x)
 
for y in self.neighbors[x]:
if y in closedset:
continue
 
tentative_g_score = g_score[x] + self.cost[(x,y)]
 
if (y not in openset) or (tentative_g_score < g_score[y]):
came_from[y] = x
 
g_score[y] = tentative_g_score
h_score[y] = self.est(y, goal)
f_score[y] = g_score[y] + h_score[y]
 
pos = bsearch(openset, y, True, f_score.__getitem__)
openset.insert(pos, y)
return None