-
Notifications
You must be signed in to change notification settings - Fork 0
/
pathfinder.py
150 lines (115 loc) · 4.88 KB
/
pathfinder.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
""" PathFinder class """
from priorityqueueset import PriorityQueueSet
class PathFinder(object):
"""Computes a path in a graph using the A* algorithm.
Initialize the object and then repeatedly compute_path to
get the path between a start point and an end point.
The points on a graph are required to be hashable and
comparable with __eq__. Other than that, they may be
represented as you wish, as long as the functions
supplied to the constructor know how to handle them.
"""
def __init__(self, successors, move_cost, heuristic_to_goal):
"""Create a new PathFinder. Provided with several
functions that represent your graph and the costs of
moving through it.
successors:
A function that receives a point as a single
argument and returns a list of "successor" points,
the points on the graph that can be reached from
the given point.
move_cost:
A function that receives two points as arguments
and returns the numeric cost of moving from the
first to the second.
heuristic_to_goal:
A function that receives a point and a goal point,
and returns the numeric heuristic estimation of
the cost of reaching the goal from the point.
"""
self.successors = successors
self.move_cost = move_cost
self.heuristic_to_goal = heuristic_to_goal
def compute_path(self, start, goal):
"""Compute the path between the 'start' point and the
'goal' point.
The path is returned as an iterator to the points,
including the start and goal points themselves.
If no path was found, an empty list is returned.
"""
#
# Implementation of the A* algorithm.
#
closed_set = {}
start_node = self._Node(start)
start_node.g_cost = 0
start_node.f_cost = self._compute_f_cost(start_node, goal)
open_set = PriorityQueueSet()
open_set.add(start_node)
while len(open_set) > 0:
# Remove and get the node with the lowest f_score from
# the open set
#
curr_node = open_set.pop_smallest()
if curr_node.coord == goal:
return self._reconstruct_path(curr_node)
closed_set[curr_node] = curr_node
for succ_coord in self.successors(
curr_node.coord[0], curr_node.coord[1], curr_node.coord[2]
):
succ_node = self._Node(succ_coord)
succ_node.g_cost = self._compute_g_cost(curr_node, succ_node)
succ_node.f_cost = self._compute_f_cost(succ_node, goal)
if succ_node in closed_set:
continue
if open_set.add(succ_node):
succ_node.pred = curr_node
return []
########################## PRIVATE ##########################
def _compute_g_cost(self, from_node, to_node):
"""return the cost"""
return from_node.g_cost + self.move_cost(from_node.coord, to_node.coord)
def _compute_f_cost(self, node, goal):
"""return the cost"""
return node.g_cost + self._cost_to_goal(node, goal)
def _cost_to_goal(self, node, goal):
"""return the cost"""
return self.heuristic_to_goal(node.coord, goal)
def _reconstruct_path(self, node):
"""Reconstructs the path to the node from the start node
(for which .pred is None)
"""
pth = [node.coord]
n = node
while n.pred:
n = n.pred
pth.append(n.coord)
return reversed(pth)
class _Node(object):
"""Used to represent a node on the searched graph during
the A* search.
Each Node has its coordinate (the point it represents),
a g_cost (the cumulative cost of reaching the point
from the start point), a f_cost (the estimated cost
from the start to the goal through this point) and
a predecessor Node (for path construction).
The Node is meant to be used inside PriorityQueueSet,
so it implements equality and hashinig (based on the
coordinate, which is assumed to be unique) and
comparison (based on f_cost) for sorting by cost.
"""
def __init__(self, coord, g_cost=None, f_cost=None, pred=None):
self.coord = coord
self.g_cost = g_cost
self.f_cost = f_cost
self.pred = pred
def __eq__(self, other):
return self.coord == other.coord
def __lt__(self, other):
return self.f_cost < other.f_cost
def __hash__(self):
return hash(self.coord)
def __str__(self):
return "N(%s) -> g: %s, f: %s" % (self.coord, self.g_cost, self.f_cost)
def __repr__(self):
return self.__str__()