def getPath(self, start, goal): self.connectToGraph(start); self.connectToGraph(goal); openset = minHeap(self.Length,self.Width) g_score =[] f_score =[] came_from=[] close_set=[] for i in xrange(self.Length): g_score.append([0]*self.Width) f_score.append([0]*self.Width) came_from.append([None]*self.Width) close_set.append([False]*self.Width) f_score[start.Y][start.X] = g_score[start.Y][start.X ] + self.heuristic_cost_estimate(start, goal); openset.add(f_score[start.Y][start.X], start); neighbors=None; while openset.count() != 0: current = openset.removemin(); if current==goal: return self.reconstruct_path(came_from, goal, start); close_set[current.Y][current.X] = True; neighbors = self.getNeighbours(current); for neighbor in neighbors.list: tentative_g_score = g_score[current.Y][current.X] + Point.distantTo(current, neighbor); if close_set[neighbor.Y][neighbor.X]: if tentative_g_score >= g_score[neighbor.Y][neighbor.X]: continue; if not openset.Contains(neighbor) or tentative_g_score < g_score[neighbor.Y][neighbor.X]: came_from[neighbor.Y][neighbor.X] = current; g_score[neighbor.Y][neighbor.X] = tentative_g_score; f_score[neighbor.Y][neighbor.X] = g_score[neighbor.Y][neighbor.X] + self.heuristic_cost_estimate(neighbor, goal); if not openset.Contains(neighbor): openset.add(f_score[neighbor.Y][neighbor.X], neighbor); return None;
def heuristic_cost_estimate(self, p1, p2): return Point.distantTo(p1, p2)