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)
