User:Keiji/aystar.py

<lang python> 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 </lang>