├── A-Star.py ├── A-Star_opt.py ├── A-Star_precompute_h.py ├── Bidirectional_A-Star.py ├── LICENSE └── README.md /A-Star.py: -------------------------------------------------------------------------------- 1 | """ 2 | *** Unidirectional A-Star algorithm *** 3 | 4 | Useful for real road network graph problems, on a planar map/grid. 5 | 6 | n - number of nodes 7 | m - number of edges 8 | 9 | If the graph isn't dense, ie. it's sparse, it's better to implement 10 | priority queue as heap than as array. 11 | A graph is sparse when n and m are of the same order of magnitude. 12 | 13 | Here, priority queue is implemented by using module heapq. 14 | 15 | We put (dist, name) into heap, where dist is f(v), and f(v) = g(v) + h(v), 16 | that is, known part + heuristic part. 17 | Known part is correct distance from starting vertex, s, to this one, v. 18 | Heuristic part is estimation from this vertex, v, to the target vertex, t. 19 | 20 | This version of the algorithm assumes that we are given 21 | coordinates of vertices, so we can calculate Euclidean 22 | distances and use it as our heuristics function. 23 | An alternative heuristic would be Manhattan distance, 24 | but Euclidean is used in a case like this (road network). 25 | 26 | This is plain unidirectional A-Star algorithm, without landmarks. 27 | This is directed search, thanks to heuristic (potential) function. 28 | It represents Dijkstra with Potentials. 29 | This means that this is just plain Dijkstra with new edge weights. 30 | Actually, if heuristic (potential) function is identical to 0, 31 | this is identical to the UCS Dijkstra. 32 | The higher the potential function, the smaller the set of vertices 33 | scanned (in the closed set). Of course, it has to be feasible. 34 | It can be shown that any lower-bounding algorithm with a nonnegative 35 | potential function visits no more vertices than Dijkstra's algorithm. 36 | 37 | It's probably the best to compute the heuristic on the fly. 38 | A-Star is directed search, which means not many vertices 39 | will be processed. If, one the other hand, we want to precompute 40 | the heuristic function, we have to do that for every vertex of the graph, 41 | because we don't know in advance which vertices will be processed. 42 | In each query, the heuristic will be different, because target vertex 43 | will be different, and heuristic is target-dependent. 44 | 45 | A-Star with heuristics doesn't need the preprocessing phase. 46 | We can have it, but it will only make the algorithm slower. 47 | In other words, it doesn't make sense. That's because heuristic values depend 48 | on a single query - they are not common to many queries in a graph. 49 | 50 | This version of the algorithm doesn't reconstruct the shortest path. 51 | It only computes its length and returns it. 52 | 53 | Fastest if f & g are arrays. 54 | Slightly slower if f & g are maps (dictionaries) 55 | Way slower if f & g are arrays and h == 0 (plain Dijkstra UCS). 56 | Memory consumption is the same in all three cases. 57 | """ 58 | 59 | 60 | import sys 61 | import math 62 | import heapq 63 | 64 | 65 | class AStar: 66 | def __init__(self, n, adj, cost, x, y): 67 | self.n = n; 68 | self.adj = adj 69 | self.cost = cost 70 | self.inf = n*10**6 71 | self.f = [self.inf] * n # f(v) = g(v) + h(v); these are new distances, with potential h (h(v) is computed on the fly); Dijkstra UCS now works with them (this is self.distance in Dijkstra); self.f can be a map (dictionary) 72 | self.g = [self.inf] * n # this is the known part of the distance; h is the heuristic part; this is the true distance from starting vertex to v; self.g can be a map (dictionary) 73 | #self.f = {} 74 | #self.g = {} 75 | self.closed = set() 76 | self.valid = [True] * n # is vertex (name) valid or not - it's valid while name (vertex) is in open set (in heap) 77 | # Coordinates of the nodes 78 | self.x = x 79 | self.y = y 80 | 81 | def clear(self): 82 | self.f = [self.inf] * self.n 83 | self.g = [self.inf] * self.n 84 | #for i in range(self.n): # we have to "reset" dictionaries this way, even for the first query; this is proper initialization; we can't use method clear() on these dictionaries here, because that would be a mistake 85 | # self.f[i] = self.inf 86 | # self.g[i] = self.inf 87 | self.closed.clear() 88 | self.valid = [True] * self.n 89 | 90 | # Returns the distance from s to t in the graph, or -1 if t is unreachable from s 91 | def query(self, s, t): 92 | self.clear() 93 | open = [] # elements are tuples (f(v), v) 94 | 95 | self.g[s] = 0 96 | self.f[s] = self.g[s] + math.sqrt((self.x[s] - self.x[t])**2 + (self.y[s] - self.y[t])**2) 97 | heapq.heappush(open, (self.f[s], s)) 98 | 99 | while open: 100 | # the inner while loop removes and returns the best vertex 101 | best = None 102 | name = None 103 | while open: 104 | best = heapq.heappop(open) 105 | name = best[1] 106 | if self.valid[name]: 107 | self.valid[name] = False; 108 | break 109 | if name == t: 110 | break 111 | self.closed.add(name) # also ok: self.closed.add(best) 112 | for i in range(len(self.adj[name])): 113 | neighbor = self.adj[name][i] 114 | if neighbor in self.closed: # also ok: if (self.f[neighbor], neighbor) in self.closed: 115 | continue 116 | temp_g = self.g[name] + self.cost[name][i] 117 | if (self.f[neighbor], neighbor) not in open: 118 | h = math.sqrt((self.x[neighbor] - self.x[t])**2 + (self.y[neighbor] - self.y[t])**2) 119 | self.g[neighbor] = temp_g 120 | self.f[neighbor] = temp_g + h 121 | heapq.heappush(open, (self.f[neighbor], neighbor)) 122 | continue 123 | if self.g[neighbor] > temp_g: 124 | h = math.sqrt((self.x[neighbor] - self.x[t])**2 + (self.y[neighbor] - self.y[t])**2) 125 | self.g[neighbor] = temp_g 126 | self.f[neighbor] = temp_g + h 127 | heapq.heappush(open, (self.f[neighbor], neighbor)) 128 | 129 | return self.g[t] if self.g[t] < self.inf else -1 #same as: return int(self.f[t]) if self.f[t] < self.inf else -1 130 | 131 | 132 | if __name__ == '__main__': 133 | input = sys.stdin.read() # Python 2 134 | data = list(map(int, input.split())) 135 | n, m = data[0:2] # number of nodes, number of edges; nodes are numbered from 1 to n 136 | data = data[2:] 137 | x = [0] * n 138 | y = [0] * n 139 | adj = [[] for _ in range(n)] 140 | cost = [[] for _ in range(n)] 141 | # coordinates x and y of the corresponding node 142 | for i in range(n): 143 | x[i] = data[i << 1] 144 | y[i] = data[(i << 1) + 1] 145 | data = data[2*n:] 146 | # directed edge (u, v) of length c from the node number u to the node number v 147 | for e in range(m): 148 | u = data[3*e] 149 | v = data[3*e+1] 150 | c = data[3*e+2] 151 | adj[u-1].append(v-1) 152 | cost[u-1].append(c) 153 | astar = AStar(n, adj, cost, x, y) 154 | data = data[3*m:] 155 | # the number of queries for computing the distance 156 | q = data[0] 157 | data = data[1:] 158 | # s and t are numbers ("names") of two nodes to compute the distance from s to t 159 | for i in range(q): 160 | s = data[i << 1] 161 | t = data[(i << 1) + 1] 162 | print(astar.query(s-1, t-1)) 163 | -------------------------------------------------------------------------------- /A-Star_opt.py: -------------------------------------------------------------------------------- 1 | """ 2 | *** Unidirectional A-Star algorithm. Optimized version. *** 3 | 4 | Useful for real road network graph problems, on a planar map/grid. 5 | 6 | n - number of nodes 7 | m - number of edges 8 | 9 | If the graph isn't dense, ie. it's sparse, it's better to implement 10 | priority queue as heap than as array. 11 | A graph is sparse when n and m are of the same order of magnitude. 12 | 13 | Here, priority queue is implemented by using module heapq. 14 | 15 | We put (dist, name) into heap, where dist is f(v), and f(v) = g(v) + h(v), 16 | that is, known part + heuristic part. 17 | Known part is correct distance from starting vertex, s, to this one, v. 18 | Heuristic part is estimation from this vertex, v, to the target vertex, t. 19 | 20 | This version of the algorithm assumes that we are given 21 | coordinates of vertices, so we can calculate Euclidean 22 | distances and use it as our heuristics function. 23 | An alternative heuristic would be Manhattan distance, 24 | but Euclidean is used in a case like this (road network). 25 | 26 | This is plain unidirectional A-Star algorithm, without landmarks. 27 | This is directed search, thanks to heuristic (potential) function. 28 | It represents Dijkstra with Potentials. 29 | This means that this is just plain Dijkstra with new edge weights. 30 | Actually, if heuristic (potential) function is identical to 0, 31 | this is identical to the UCS Dijkstra. 32 | 33 | It's probably the best to compute the heuristic on the fly. 34 | A-Star is directed search, which means not many vertices 35 | will be processed. If, one the other hand, we want to precompute 36 | the heuristic function, we have to do that for every vertex of the graph, 37 | because we don't know in advance which vertices will be processed. 38 | In each query, the heuristic will be different, because target vertex 39 | will be different, and heuristic is target-dependent. 40 | 41 | A-Star with heuristics doesn't need the preprocessing phase. 42 | We can have it, but it will only make the algorithm slower. 43 | In other words, it doesn't make sense. That's because heuristic values depend 44 | on a single query - they are not common to many queries in a graph. 45 | 46 | This version of the algorithm doesn't reconstruct the shortest path. 47 | It only computes its length and returns it. 48 | 49 | Data structures f & g are implemented as arrays. 50 | 51 | Ties in priority queue are broken so the queue behaves in a LIFO manner. This should make it faster. 52 | https://en.wikipedia.org/wiki/A*_search_algorithm#Implementation_details 53 | """ 54 | 55 | 56 | import sys 57 | import math 58 | import heapq 59 | import itertools 60 | 61 | 62 | class AStar: 63 | def __init__(self, n, adj, cost, x, y): 64 | self.n = n; 65 | self.adj = adj 66 | self.cost = cost 67 | self.inf = n*10**6 68 | self.f = [self.inf] * n # f(v) = g(v) + h(v); these are new distances, with potential h (h(v) is computed on the fly); Dijkstra UCS now works with them (this is self.distance in Dijkstra); self.f can be a map (dictionary) 69 | self.g = [self.inf] * n # this is the known part of the distance; h is the heuristic part; this is the true distance from starting vertex to v; self.g can be a map (dictionary) 70 | self.closed = set() 71 | self.valid = [True] * n # is vertex (name) valid or not - it's valid while name (vertex) is in open set (in heap) 72 | self.counter = itertools.count() # unique sequence count 73 | self.cnt = [self.inf] * n 74 | # Coordinates of the nodes 75 | self.x = x 76 | self.y = y 77 | 78 | def clear(self): 79 | self.f = [self.inf] * n 80 | self.g = [self.inf] * n 81 | self.closed.clear() 82 | self.valid = [True] * n 83 | 84 | # Returns the distance from s to t in the graph 85 | def query(self, s, t): 86 | self.clear() 87 | open = [] # elements are tuples (f(v), count, v); count is decrementing for each new insertion, because we want the queue to behave as LIFO; self.n size should do 88 | 89 | self.g[s] = 0 90 | self.f[s] = self.g[s] + math.sqrt((self.x[s] - self.x[t])**2 + (self.y[s] - self.y[t])**2) 91 | count = self.inf - next(self.counter) 92 | self.cnt[s] = count 93 | heapq.heappush(open, (self.f[s], count, s)) 94 | 95 | while open: 96 | # the inner while loop removes and returns the best vertex 97 | best = None 98 | name = None 99 | while open: 100 | best = heapq.heappop(open) 101 | name = best[2] 102 | if self.valid[name]: 103 | self.valid[name] = False; 104 | break 105 | if name == t: 106 | break 107 | self.closed.add(name) # also ok: self.closed.add(best) 108 | for i in xrange(len(self.adj[name])): 109 | neighbor = self.adj[name][i] 110 | if neighbor in self.closed: # also ok: if (self.f[neighbor], neighbor) in self.closed: 111 | continue 112 | temp_g = self.g[name] + self.cost[name][i] 113 | if (self.f[neighbor], self.cnt[neighbor], neighbor) not in open: 114 | h = math.sqrt((self.x[neighbor] - self.x[t])**2 + (self.y[neighbor] - self.y[t])**2) 115 | self.g[neighbor] = temp_g 116 | self.f[neighbor] = temp_g + h 117 | count = self.inf - next(self.counter) 118 | self.cnt[neighbor] = count 119 | heapq.heappush(open, (self.f[neighbor], count, neighbor)) 120 | continue 121 | if self.g[neighbor] > temp_g: 122 | h = math.sqrt((self.x[neighbor] - self.x[t])**2 + (self.y[neighbor] - self.y[t])**2) 123 | self.g[neighbor] = temp_g 124 | self.f[neighbor] = temp_g + h 125 | heapq.heappush(open, (self.f[neighbor], self.cnt[neighbor], neighbor)) 126 | 127 | return self.g[t] if self.g[t] < self.inf else -1 #same as: return int(self.f[t]) if self.f[t] < self.inf else -1 128 | 129 | 130 | if __name__ == '__main__': 131 | input = sys.stdin.read() # Python 2 132 | data = list(map(int, input.split())) 133 | n, m = data[0:2] # number of nodes, number of edges; nodes are numbered from 1 to n 134 | data = data[2:] 135 | x = [0] * n 136 | y = [0] * n 137 | adj = [[] for _ in range(n)] 138 | cost = [[] for _ in range(n)] 139 | # coordinates x and y of the corresponding node 140 | for i in range(n): 141 | x[i] = data[i << 1] 142 | y[i] = data[(i << 1) + 1] 143 | data = data[2*n:] 144 | # directed edge (u, v) of length c from the node number u to the node number v 145 | for e in range(m): 146 | u = data[3*e] 147 | v = data[3*e+1] 148 | c = data[3*e+2] 149 | adj[u-1].append(v-1) 150 | cost[u-1].append(c) 151 | astar = AStar(n, adj, cost, x, y) 152 | data = data[3*m:] 153 | # the number of queries for computing the distance 154 | q = data[0] 155 | data = data[1:] 156 | # s and t are numbers ("names") of two nodes to compute the distance from s to t 157 | for i in range(q): 158 | s = data[i << 1] 159 | t = data[(i << 1) + 1] 160 | print(astar.query(s-1, t-1)) 161 | 162 | -------------------------------------------------------------------------------- /A-Star_precompute_h.py: -------------------------------------------------------------------------------- 1 | """ 2 | *** Unidirectional A-Star algorithm *** 3 | ** Heuristic function precomputed ** 4 | 5 | Useful for real road network graph problems, on a planar map/grid. 6 | 7 | n - number of nodes 8 | m - number of edges 9 | 10 | If the graph isn't dense, ie. it's sparse, it's better to implement 11 | priority queue as heap than as array. 12 | A graph is sparse when n and m are of the same order of magnitude. 13 | 14 | Here, priority queue is implemented by using module heapq. 15 | 16 | We put (dist, name) into heap, where dist is f(v), and f(v) = g(v) + h(v), 17 | that is, known part + heuristic part. 18 | Known part is correct distance from starting vertex, s, to this one, v. 19 | Heuristic part is estimation from this vertex, v, to the target vertex, t. 20 | 21 | This version of the algorithm assumes that we are given 22 | coordinates of vertices, so we can calculate Euclidean 23 | distances and use it as our heuristics function. 24 | An alternative heuristic would be Manhattan distance, 25 | but Euclidean is used in a case like this (road network). 26 | 27 | This is plain unidirectional A-Star algorithm, without landmarks. 28 | This is directed search, thanks to heuristic (potential) function. 29 | It represents Dijkstra with Potentials. 30 | This means that this is just plain Dijkstra with new edge weights. 31 | Actually, if heuristic (potential) function is identical to 0, 32 | this is identical to the UCS Dijkstra. 33 | 34 | It's probably the best to compute the heuristic on the fly. 35 | A-Star is directed search, which means not many vertices 36 | will be processed. If, one the other hand, we want to precompute 37 | the heuristic function, we have to do that for every vertex of the graph, 38 | because we don't know in advance which vertices will be processed. 39 | In each query, the heuristic will be different, because target vertex 40 | will be different, and heuristic is target-dependent. 41 | 42 | A-Star with heuristics doesn't need the precomputation phase. 43 | We can have it, like here, but it will only make the algorithm slower. 44 | In other words, it doesn't make sense. That's because heuristic values depend 45 | on a single query - they are not common to many queries in a graph. 46 | 47 | This version of the algorithm doesn't reconstruct the shortest path. 48 | It only computes its length and returns it. 49 | 50 | Data structures f & g are implemented as arrays. 51 | It's way slower if h == 0 (plain Dijkstra UCS). 52 | Memory consumption is the same in both cases. 53 | """ 54 | 55 | 56 | import sys 57 | import math 58 | import heapq 59 | 60 | 61 | class AStar: 62 | def __init__(self, n, adj, cost, x, y): 63 | self.n = n; 64 | self.adj = adj 65 | self.cost = cost 66 | self.inf = n*10**6 67 | self.f = [self.inf] * n # f(v) = g(v) + h(v); these are new distances, with potential h (h(v) is computed on the fly); Dijkstra UCS now works with them (this is self.distance in Dijkstra); self.f can be a map (dictionary) 68 | self.g = [self.inf] * n # this is the known part of the distance; h is the heuristic part; this is the true distance from starting vertex to v; self.g can be a map (dictionary) 69 | self.h = [self.inf] * n # h(v) 70 | self.closed = set() 71 | self.valid = [True] * n # is vertex (name) valid or not - it's valid while name (vertex) is in open set (in heap) 72 | # Coordinates of the nodes 73 | self.x = x 74 | self.y = y 75 | 76 | def clear(self): 77 | self.f = [self.inf] * self.n 78 | self.g = [self.inf] * self.n 79 | self.h = [self.inf] * self.n 80 | self.closed.clear() 81 | self.valid = [True] * self.n 82 | 83 | def precompute_h(self, t): 84 | tx = self.x[t] 85 | ty = self.y[t] 86 | for i in xrange(self.n): 87 | self.h[i] = math.sqrt((self.x[i] - tx)**2 + (self.y[i] - ty)**2) 88 | 89 | # Returns the distance from s to t in the graph 90 | def query(self, s, t): 91 | self.clear() 92 | self.precompute_h(t) 93 | open = [] # elements are tuples (f(v), v) 94 | 95 | self.g[s] = 0 96 | self.f[s] = self.g[s] + self.h[s] 97 | heapq.heappush(open, (self.f[s], s)) 98 | 99 | while open: 100 | # the inner while loop removes and returns the best vertex 101 | best = None 102 | name = None 103 | while open: 104 | best = heapq.heappop(open) 105 | name = best[1] 106 | if self.valid[name]: 107 | self.valid[name] = False; 108 | break 109 | if name == t: 110 | break 111 | self.closed.add(name) # also ok: self.closed.add(best) 112 | for i in xrange(len(self.adj[name])): 113 | neighbor = self.adj[name][i] 114 | if neighbor in self.closed: # also ok: if (self.f[neighbor], neighbor) in self.closed: 115 | continue 116 | temp_g = self.g[name] + self.cost[name][i] 117 | if (self.f[neighbor], neighbor) not in open: 118 | self.g[neighbor] = temp_g 119 | self.f[neighbor] = temp_g + self.h[neighbor] 120 | heapq.heappush(open, (self.f[neighbor], neighbor)) 121 | continue 122 | if self.g[neighbor] > temp_g: 123 | self.g[neighbor] = temp_g 124 | self.f[neighbor] = temp_g + self.h[neighbor] 125 | heapq.heappush(open, (self.f[neighbor], neighbor)) 126 | 127 | return self.g[t] if self.g[t] < self.inf else -1 #same as: return int(self.f[t]) if self.f[t] < self.inf else -1 128 | 129 | 130 | if __name__ == '__main__': 131 | input = sys.stdin.read() # Python 2 132 | data = list(map(int, input.split())) 133 | n, m = data[0:2] # number of nodes, number of edges; nodes are numbered from 1 to n 134 | data = data[2:] 135 | x = [0] * n 136 | y = [0] * n 137 | adj = [[] for _ in range(n)] 138 | cost = [[] for _ in range(n)] 139 | # coordinates x and y of the corresponding node 140 | for i in range(n): 141 | x[i] = data[i << 1] 142 | y[i] = data[(i << 1) + 1] 143 | data = data[2*n:] 144 | # directed edge (u, v) of length c from the node number u to the node number v 145 | for e in range(m): 146 | u = data[3*e] 147 | v = data[3*e+1] 148 | c = data[3*e+2] 149 | adj[u-1].append(v-1) 150 | cost[u-1].append(c) 151 | astar = AStar(n, adj, cost, x, y) 152 | data = data[3*m:] 153 | # the number of queries for computing the distance 154 | q = data[0] 155 | data = data[1:] 156 | # s and t are numbers ("names") of two nodes to compute the distance from s to t 157 | for i in range(q): 158 | s = data[i << 1] 159 | t = data[(i << 1) + 1] 160 | print(astar.query(s-1, t-1)) 161 | 162 | -------------------------------------------------------------------------------- /Bidirectional_A-Star.py: -------------------------------------------------------------------------------- 1 | """ 2 | *** Bidirectional A-Star algorithm *** 3 | 4 | Useful for real road network graph problems, on a planar map/grid. 5 | 6 | n - number of nodes 7 | m - number of edges 8 | 9 | If the graph isn't dense, ie. it's sparse, it's better to implement 10 | priority queue as heap than as array. 11 | A graph is sparse when n and m are of the same order of magnitude. 12 | 13 | Here, priority queue is implemented by using module heapq. 14 | 15 | We put (dist, name) into heap, where dist is f(v), and f(v) = g(v) + h(v), 16 | that is, known part + heuristic part. 17 | Known part is correct distance from starting vertex, s, to this one, v. 18 | Heuristic part is estimation from this vertex, v, to the target vertex, t. 19 | 20 | This version of the algorithm assumes that we are given 21 | coordinates of vertices, so we can calculate Euclidean 22 | distances and use it as our heuristics function. 23 | An alternative heuristic would be Manhattan distance, 24 | but Euclidean is used in a case like this (road network). 25 | 26 | This is Bidirectional A-Star algorithm, without landmarks. 27 | This is directed search, thanks to heuristic (potential) function. 28 | It represents Dijkstra with Potentials. 29 | This means that this is just plain Dijkstra with new edge weights. 30 | Actually, if heuristic (potential) function is identical to 0, 31 | this is identical to the UCS Dijkstra. 32 | 33 | It's probably the best to compute the heuristic on the fly. 34 | A-Star is directed search, which means not many vertices 35 | will be processed. If, on the other hand, we want to precompute 36 | the heuristic function, we have to do that for every vertex of the graph, 37 | because we don't know in advance which vertices will be processed. 38 | In each query, the heuristic will be different, because target vertex 39 | will be different, and heuristic is target-dependent. 40 | 41 | A-Star with heuristics doesn't need the preprocessing phase. 42 | We can have it, but it will only make the algorithm slower. 43 | In other words, it doesn't make sense. That's because heuristic values depend 44 | on a single query - they are not common to many queries in a graph. 45 | 46 | This version of the algorithm doesn't reconstruct the shortest path. 47 | It only computes its length and returns it. 48 | 49 | This bidirectional variant is the fastest one. 50 | """ 51 | 52 | 53 | import sys 54 | import math 55 | import heapq 56 | 57 | 58 | class AStar: 59 | def __init__(self, n, adj, cost, x, y): 60 | self.n = n; 61 | self.adj = adj 62 | self.cost = cost 63 | self.inf = n*10**6 64 | self.f = [[self.inf]*n, [self.inf]*n] # forward & backward; f(v) = g(v) + h(v); these are new distances, with potential h (h(v) is computed on the fly); Dijkstra UCS now works with them (this is self.distance in Dijkstra); self.f can be a map (dictionary) 65 | self.g = [[self.inf]*n, [self.inf]*n] # forward & backward; this is the known part of the distance; h is the heuristic part; this is the true distance from starting vertex to v; self.g can be a map (dictionary) 66 | self.closedF = set() # forward closed set (processed nodes) 67 | self.closedR = set() # backward closed set (processed nodes) 68 | self.valid = [[True] * n, [True] * n] # is vertex (name) valid or not - it's valid while name (vertex) is in open set (in heap) 69 | # Coordinates of the nodes 70 | self.x = x 71 | self.y = y 72 | 73 | def clear(self): 74 | """ Reinitialize the data structures for the next query after the previous query. """ 75 | self.f = [[self.inf]*n, [self.inf]*n] 76 | self.g = [[self.inf]*n, [self.inf]*n] 77 | self.closedF.clear() 78 | self.closedR.clear() 79 | self.valid = [[True] * n, [True] * n] 80 | 81 | def heur(self, s, t, v): 82 | """ The so called "average function" by Goldberg. """ 83 | pi_f = math.sqrt((self.x[v] - self.x[t])**2 + (self.y[v] - self.y[t])**2) 84 | pi_r = math.sqrt((self.x[s] - self.x[v])**2 + (self.y[s] - self.y[v])**2) 85 | pf = (pi_f - pi_r)/2.0 86 | pr = -pf 87 | return pf, pr 88 | 89 | def heurMax(self, s, t, v): 90 | """ Computing the Shortest Path - A-Star Search Meets Graph Theory (Microsoft, Goldberg) 91 | Goldberg calls it "max function". 92 | We can see it's slower than the above average function, which is consistent with his 93 | findings (BLA vs BLM, though that's ALT algorithm, but I guess that doesn't make a difference). 94 | """ 95 | pi_t_v = math.sqrt((self.x[v] - self.x[t])**2 + (self.y[v] - self.y[t])**2) # forward 96 | pi_s_v = math.sqrt((self.x[s] - self.x[v])**2 + (self.y[s] - self.y[v])**2) # backward 97 | pi_s_t = self.pi_s_t 98 | pi_t_s = pi_s_t 99 | beta = int(pi_t_s) >> 3 # a constant that depends on pi_t(s) and/or pi_s(t) (their implementation uses a constant fraction of pi_t(s)) 100 | pf = max(pi_t_v, pi_s_t - pi_s_v + beta) 101 | pr = -pf 102 | return pf, pr 103 | 104 | def visit(self, open, side, name, s, t): 105 | closed = self.closedF if side == 0 else self.closedR 106 | for i in range(len(self.adj[side][name])): 107 | neighbor = self.adj[side][name][i] 108 | if neighbor in closed: # also ok: if (self.f[neighbor], neighbor) in closed: 109 | continue 110 | temp_g = self.g[side][name] + self.cost[side][name][i] 111 | if (self.f[side][neighbor], neighbor) not in open[side]: 112 | hf, hr = self.heur(s, t, neighbor) 113 | self.g[side][neighbor] = temp_g 114 | self.f[side][neighbor] = temp_g + (hf if side == 0 else hr) # h depends on the side! 115 | heapq.heappush(open[side], (self.f[side][neighbor], neighbor)) 116 | continue 117 | if self.g[side][neighbor] > temp_g: 118 | hf, hr = self.heur(s, t, neighbor) 119 | self.g[side][neighbor] = temp_g 120 | self.f[side][neighbor] = temp_g + (hf if side == 0 else hr) # h depends on the side! 121 | heapq.heappush(open[side], (self.f[side][neighbor], neighbor)) 122 | 123 | def query(self, s, t): 124 | """ Returns the distance from s to t in the graph (-1 if there's no path). """ 125 | self.clear() 126 | open = [[], []] # list of two priority queues (that are implemented as min-heaps); open[0] is forward, open[1] is reverse - those are the two "open" sets; elements are tuples (f(v), v) 127 | 128 | #self.pi_s_t = math.sqrt((self.x[s] - self.x[t])**2 + (self.y[s] - self.y[t])**2) # need only for heurMax(); comment it out for regular heur (heurAvg); backward 129 | 130 | hf, hr = self.heur(s, t, s) 131 | self.g[0][s] = 0 132 | self.f[0][s] = self.g[0][s] + hf 133 | heapq.heappush(open[0], (self.f[0][s], s)) 134 | 135 | hf, hr = self.heur(s, t, t) 136 | self.g[1][t] = 0 137 | self.f[1][t] = self.g[1][t] + hr 138 | heapq.heappush(open[1], (self.f[1][t], t)) 139 | 140 | while open[0] or open[1]: 141 | # the inner while loop removes and returns the best vertex 142 | best = None 143 | name = None 144 | while open[0]: 145 | best = heapq.heappop(open[0]) 146 | name = best[1] 147 | if self.valid[0][name]: 148 | self.valid[0][name] = False; 149 | break 150 | self.visit(open, 0, name, s, t) # forward 151 | if name in self.closedR: 152 | break 153 | self.closedF.add(name) # also ok: self.closedF.add(best) 154 | 155 | # the inner while loop removes and returns the best vertex 156 | best = None 157 | name = None 158 | while open[1]: 159 | best = heapq.heappop(open[1]) 160 | name = best[1] 161 | if self.valid[1][name]: 162 | self.valid[1][name] = False; 163 | break 164 | self.visit(open, 1, name, s, t) # forward 165 | if name in self.closedF: 166 | break 167 | self.closedR.add(name) # also ok: self.closedR.add(best) 168 | 169 | distance = self.inf 170 | 171 | # merge closedF & closedR 172 | self.closedF = self.closedF | self.closedR # sets - O(1) lookup 173 | 174 | for u in self.closedF: 175 | if (self.g[0][u] + self.g[1][u] < distance): 176 | distance = self.g[0][u] + self.g[1][u] 177 | 178 | return distance if distance < self.inf else -1 179 | 180 | 181 | if __name__ == '__main__': 182 | input = sys.stdin.read() # Python 2 183 | data = list(map(int, input.split())) 184 | n, m = data[0:2] # number of nodes, number of edges; nodes are numbered from 1 to n 185 | data = data[2:] 186 | x = [0] * n 187 | y = [0] * n 188 | adj = [[[] for _ in range(n)], [[] for _ in range(n)]] # holds adjacency lists for every vertex in the graph; contains both forward and reverse arrays 189 | cost = [[[] for _ in range(n)], [[] for _ in range(n)]] # holds weights of the edges; contains both forward and reverse arrays 190 | # coordinates x and y of the corresponding node 191 | for i in range(n): 192 | x[i] = data[i << 1] 193 | y[i] = data[(i << 1) + 1] 194 | data = data[2*n:] 195 | # directed edge (u, v) of length c from the node number u to the node number v 196 | for e in range(m): 197 | u = data[3*e] 198 | v = data[3*e+1] 199 | c = data[3*e+2] 200 | adj[0][u-1].append(v-1) 201 | cost[0][u-1].append(c) 202 | adj[1][v-1].append(u-1) 203 | cost[1][v-1].append(c) 204 | astar = AStar(n, adj, cost, x, y) 205 | data = data[3*m:] 206 | # the number of queries for computing the distance 207 | q = data[0] 208 | data = data[1:] 209 | # s and t are numbers ("names") of two nodes to compute the distance from s to t 210 | for i in range(q): 211 | s = data[i << 1] 212 | t = data[(i << 1) + 1] 213 | print(astar.query(s-1, t-1)) 214 | 215 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 Ivan Lazarevic 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # A-Star (A*) Algorithm 2 | 3 | Several different implementations of the [A-Star search algorithm](https://en.wikipedia.org/wiki/A*_search_algorithm), including a Bidirectional version. 4 | 5 | The algorithm is used for graph traversal and for finding the shortest path in a graph. 6 | 7 | It is an improved version of the [Dijkstra's Shortest path algorithm](https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm). 8 | --------------------------------------------------------------------------------