Algorithms Library Toolkit
A toolkit for algorithms, especially for algorithms on formal languages
AStar.hpp
Go to the documentation of this file.
1
6// Copyright (c) 2017 Czech Technical University in Prague | Faculty of Information Technology. All rights reserved.
7
8#pragma once
9
10#include <alib/list>
11#include <alib/set>
12#include <queue>
13#include <alib/map>
14#include <alib/vector>
15#include <stdexcept>
16#include <functional>
17
18#include <common/ReconstructPath.hpp>
19#include <common/SupportFunction.hpp>
20
21namespace graph {
22
23namespace shortest_path {
24
25class AStar {
26// ---------------------------------------------------------------------------------------------------------------------
27
28 public:
29
49 template<
50 typename TGraph,
51 typename TNode,
52 typename F1 = std::function<typename TGraph::edge_type::weight_type(const TNode &)>,
53 typename F2 = std::function<void(const TNode &, const typename TGraph::edge_type::weight_type &)>>
54 static
55 ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
56 findPath(const TGraph &graph,
57 const TNode &start,
58 const TNode &goal,
59 F1 f_heuristic,
60 F2 f_user = [](const TNode &,
61 const typename TGraph::edge_type::weight_type &) {});
62
63 template<
64 typename TGraph,
65 typename TNode,
66 typename F1 = std::function<typename TGraph::edge_type::weight_type(const TNode &, const TNode &)>
67 >
68 static
69 ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
70 findPathRegistration(const TGraph &graph,
71 const TNode &start,
72 const TNode &goal,
73 F1 f_heuristic) {
74 return findPath(graph, start, goal, [&](const TNode &n) { return f_heuristic(goal, n); });
75 }
76
77// ---------------------------------------------------------------------------------------------------------------------
78
100 template<
101 typename TGraph,
102 typename TNode,
103 typename F1 = std::function<typename TGraph::edge_type::weight_type(const TNode &)>,
104 typename F2 = std::function<typename TGraph::edge_type::weight_type(const TNode &)>,
105 typename F3 = std::function<void(const TNode &, const typename TGraph::edge_type::weight_type &)>>
106 static
107 ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
108 findPathBidirectional(const TGraph &graph,
109 const TNode &start,
110 const TNode &goal,
111 F1 f_heuristic_forward,
112 F2 f_heuristic_backward,
113 F3 f_user = [](const TNode &,
114 const typename TGraph::edge_type::weight_type &) {});
115
116 template<
117 typename TGraph,
118 typename TNode,
119 typename F1 = std::function<typename TGraph::edge_type::weight_type(const TNode &, const TNode &)>
120 >
121 static
122 ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
123 findPathBidirectionalRegistration(const TGraph &graph,
124 const TNode &start,
125 const TNode &goal,
126 F1 f_heuristic) {
127 return findPathBidirectional(graph, start, goal,
128 [&](const TNode &n) { return f_heuristic(goal, n); },
129 [&](const TNode &n) { return f_heuristic(start, n); });
130 }
131
132
133// =====================================================================================================================
134
135 private:
136
137 template<typename TNode, typename TWeight>
138 struct Data {
139 ext::set<ext::pair<TWeight, TNode>> queue; // priority queue
140 ext::map<TNode, TWeight> g; // G score
141 ext::map<TNode, TWeight> f; // F score
142 ext::map<TNode, TNode> p; // parents
143 };
144
145// ---------------------------------------------------------------------------------------------------------------------
146
147 template<typename TGraph, typename TNode, typename F1, typename F2, typename F3>
148 static
149 ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
150 impl(const TGraph &graph,
151 const TNode &start,
152 const TNode &goal,
153 F1 f_heuristic,
154 F2 f_user,
155 F3 f_stop);
156
157
158// ---------------------------------------------------------------------------------------------------------------------
159
160 template<typename FSuccEdge, typename TNode, typename TWeight, typename F1, typename F2, typename F3, typename F4>
161 static bool relaxation(FSuccEdge successor_edges,
162 Data<TNode, TWeight> &data,
163 F1 f_heuristic,
164 F2 f_user,
165 F3 f_stop,
166 F4 f_update);
167
168// ---------------------------------------------------------------------------------------------------------------------
169
170 template<typename TGraph, typename TNode, typename F1, typename F2, typename F3, typename F4>
171 static
172 ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
173 implBidirectional(const TGraph &graph,
174 const TNode &start,
175 const TNode &goal,
176 F1 f_heuristic_forward,
177 F2 f_heuristic_backward,
178 F3 f_user,
179 F4 f_stop);
180
181// ---------------------------------------------------------------------------------------------------------------------
182
183 template<typename TNode, typename TWeight, typename F>
184 inline static void init(AStar::Data<TNode, TWeight> &data, const TNode &start, F f_heuristic);
185
186// ---------------------------------------------------------------------------------------------------------------------
187};
188
189// =====================================================================================================================
190
191template<typename TGraph, typename TNode, typename F1, typename F2>
192ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
193AStar::findPath(const TGraph &graph,
194 const TNode &start,
195 const TNode &goal,
196 F1 f_heuristic,
197 F2 f_user) {
198 // We need to change user function to return false for every node in order to have only one relaxation function
199 return impl(graph, start, goal,
200 f_heuristic,
201 f_user,
202 [&goal](const TNode &n) -> bool { return goal == n; });
203}
204
205// ---------------------------------------------------------------------------------------------------------------------
206
207template<typename TGraph, typename TNode, typename F1, typename F2, typename F3>
208ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
209AStar::findPathBidirectional(const TGraph &graph,
210 const TNode &start,
211 const TNode &goal,
212 F1 f_heuristic_forward,
213 F2 f_heuristic_backward,
214 F3 f_user) {
215 // We need to change user function to return false for every node in order to have only one relaxation function
216 return implBidirectional(graph,
217 start,
218 goal,
219 f_heuristic_forward,
220 f_heuristic_backward,
221 f_user,
222 [](const TNode &) -> bool { return false; });
223}
224
225// =====================================================================================================================
226
227template<typename TGraph, typename TNode, typename F1, typename F2, typename F3>
228ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
229AStar::impl(const TGraph &graph,
230 const TNode &start,
231 const TNode &goal,
232 F1 f_heuristic,
233 F2 f_user,
234 F3 f_stop) {
235 using weight_type = typename TGraph::edge_type::weight_type;
236
237 Data<TNode, weight_type> data;
238
239 // Init search
240 init(data, start, f_heuristic);
241
242 while (!data.queue.empty()) {
243 bool stop = relaxation([&](const auto &node) -> auto { return graph.successorEdges(node); },
244 data,
245 f_heuristic,
246 f_user,
247 f_stop,
248 [](const TNode &) -> void {});
249
250 if (stop) {
251 break;
252 }
253 }
254
255 return common::ReconstructPath::reconstructWeightedPath(data.p, data.g, start, goal);
256}
257
258// ---------------------------------------------------------------------------------------------------------------------
259
260template<typename FSuccEdge, typename TNode, typename TWeight, typename F1, typename F2, typename F3, typename F4>
261bool AStar::relaxation(FSuccEdge successor_edges,
262 Data<TNode, TWeight> &data,
263 F1 f_heuristic,
264 F2 f_user,
265 F3 f_stop,
266 F4 f_update) {
267 TNode n = data.queue.begin()->second;
268 data.queue.erase(data.queue.begin());
269
270 // Run user's function
271 f_user(n, data.g[n]);
272
273 // Stop if reach the goal
274 if (f_stop(n)) {
275 return true;
276 }
277
278 for (const auto &s_edge: successor_edges(n)) {
279 const TNode &s = common::SupportFunction::other(s_edge, n); // successor
280
281 // Check for negative edge
282 if (s_edge.weight() < 0) {
283 throw std::out_of_range("AStar: Detect negative weight on edge in graph.");
284 }
285
286 // Calculate new G score
287 TWeight gscore = data.g.at(n) + s_edge.weight();
288
289 // Search if the node s was already visited
290 auto search_g = data.g.find(s);
291
292 // If not or the G score can be improve do relaxation
293 if (search_g == data.g.end() || data.g.at(s) > gscore) {
294 // Search if the node s is in OPEN
295 auto search_q = data.queue.find(ext::make_pair(data.f[s], s));
296 if (search_q != data.queue.end()) {
297 // Erase node from priority queue
298 data.queue.erase(search_q);
299 }
300
301 data.g[s] = gscore;
302 data.f[s] = gscore + f_heuristic(s);
303 data.p.insert_or_assign(s, n);
304 data.queue.insert(ext::make_pair(data.f[s], s));
305
306 f_update(s); // Update currently best path
307 }
308 }
309
310 return false;
311}
312
313// ---------------------------------------------------------------------------------------------------------------------
314
315template<typename TGraph, typename TNode, typename F1, typename F2, typename F3, typename F4>
316ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
317AStar::implBidirectional(const TGraph &graph,
318 const TNode &start,
319 const TNode &goal,
320 F1 f_heuristic_forward,
321 F2 f_heuristic_backward,
322 F3 f_user,
323 F4 f_stop) {
324 using TWeight = typename TGraph::edge_type::weight_type;
325
326 TWeight p = std::numeric_limits<TWeight>::max(); // Currently best path weight
327 ext::vector<TNode> intersection_nodes; // Last one is currently best intersection node
328 Data<TNode, TWeight> forward_data;
329 Data<TNode, TWeight> backward_data;
330
331 // Init forward search
332 init(forward_data, start, f_heuristic_forward);
333 auto f_forward_update = [&](const auto &s) -> void {
334 if (backward_data.g.find(s) != backward_data.g.end()) {
335 if (forward_data.g.at(s) + backward_data.g.at(s) < p) {
336 p = forward_data.g.at(s) + backward_data.g.at(s);
337 intersection_nodes.push_back(s);
338 }
339 }
340 };
341
342 // Init backward search
343 init(backward_data, goal, f_heuristic_backward);
344 auto f_backward_update = [&](const auto &s) -> void {
345 if (forward_data.g.find(s) != forward_data.g.end()) {
346 if (backward_data.g.at(s) + forward_data.g.at(s) < p) {
347 p = backward_data.g.at(s) + forward_data.g.at(s);
348 intersection_nodes.push_back(s);
349 }
350 }
351 };
352
353 while (!forward_data.queue.empty() && !backward_data.queue.empty()) {
354 if (std::max(forward_data.queue.begin()->first, backward_data.queue.begin()->first) >= p) {
355 return common::ReconstructPath::reconstructWeightedPath(forward_data.p,
356 backward_data.p,
357 forward_data.g,
358 backward_data.g,
359 start,
360 goal,
361 intersection_nodes.back());
362 }
363
364 // Expand the lower value
365 if (forward_data.queue.begin()->first < backward_data.queue.begin()->first) {
366 // Forward search relaxationBidirectional
367 relaxation([&](const auto &node) -> auto { return graph.successorEdges(node); },
368 forward_data,
369 f_heuristic_forward,
370 f_user,
371 f_stop,
372 f_forward_update);
373 } else {
374 // Backward search relaxationBidirectional
375 relaxation([&](const auto &node) -> auto { return graph.predecessorEdges(node); },
376 backward_data,
377 f_heuristic_backward,
378 f_user,
379 f_stop,
380 f_backward_update);
381 }
382 }
383
384 return ext::make_pair(ext::vector<TNode>(), p);
385}
386
387// ---------------------------------------------------------------------------------------------------------------------
388
389template<typename TNode, typename TWeight, typename F>
390void AStar::init(AStar::Data<TNode, TWeight> &data, const TNode &start, F f_heuristic) {
391 data.g[start] = 0;
392 data.f[start] = data.g[start] + f_heuristic(start);
393 data.p.insert_or_assign(start, start);
394 data.queue.insert(ext::make_pair(data.f[start], start));
395}
396
397// ---------------------------------------------------------------------------------------------------------------------
398
399} // namespace shortest_path
400
401} // namespace graph
402
ext::map
Class extending the map class from the standard library. Original reason is to allow printing of the ...
Definition: map.hpp:48
ext::pair
Class extending the pair class from the standard library. Original reason is to allow printing of the...
Definition: pair.hpp:43
ext::set
Definition: set.hpp:44
ext::vector
Class extending the vector class from the standard library. Original reason is to allow printing of t...
Definition: vector.hpp:45
graph::common::ReconstructPath::reconstructWeightedPath
static ext::pair< ext::vector< TNode >, TWeight > reconstructWeightedPath(const ext::map< TNode, TNode > &p, const ext::map< TNode, TWeight > &g, const TNode &start, const TNode &goal)
Definition: ReconstructPath.hpp:132
graph::common::SupportFunction::other
static TNode const & other(const TEdge &e, const TNode &n)
graph::shortest_path::AStar
Definition: AStar.hpp:25
graph::shortest_path::AStar::findPathBidirectional
static ext::pair< ext::vector< TNode >, typename TGraph::edge_type::weight_type > findPathBidirectional(const TGraph &graph, const TNode &start, const TNode &goal, F1 f_heuristic_forward, F2 f_heuristic_backward, F3 f_user=[](const TNode &, const typename TGraph::edge_type::weight_type &) {})
Definition: AStar.hpp:209
graph::shortest_path::AStar::findPath
static ext::pair< ext::vector< TNode >, typename TGraph::edge_type::weight_type > findPath(const TGraph &graph, const TNode &start, const TNode &goal, F1 f_heuristic, F2 f_user=[](const TNode &, const typename TGraph::edge_type::weight_type &) {})
Definition: AStar.hpp:193
graph::shortest_path::AStar::findPathBidirectionalRegistration
static ext::pair< ext::vector< TNode >, typename TGraph::edge_type::weight_type > findPathBidirectionalRegistration(const TGraph &graph, const TNode &start, const TNode &goal, F1 f_heuristic)
Definition: AStar.hpp:123
graph::shortest_path::AStar::findPathRegistration
static ext::pair< ext::vector< TNode >, typename TGraph::edge_type::weight_type > findPathRegistration(const TGraph &graph, const TNode &start, const TNode &goal, F1 f_heuristic)
Definition: AStar.hpp:70
ext::max
constexpr const T & max(const T &a)
Root case of maximum computation. The maximum from one value is the value itself.
Definition: algorithm.hpp:278
ext::make_pair
constexpr auto make_pair(T1 &&x, T2 &&y)
Definition: pair.hpp:79
graph
Definition: ReconstructPath.hpp:14
measurements::start
void start(measurements::stealth_string name, measurements::Type type)
Definition: measurements.cpp:14
node
Definition: Node.cpp:11
Generated on Mon Dec 27 2021 10:21:54 for Algorithms Library Toolkit by doxygen 1.9.2