pgRouting
pgRouting extends the PostGIS / PostgreSQL geospatial database to provide geospatial routing functionality.
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
pgr_astar.hpp
Go to the documentation of this file.
1 /*PGR-GNU*****************************************************************
2 
3 File: pgr_astar.hpp
4 
5 Copyright (c) 2015 Vicky Vergara
6 Mail: vicky_vergara@hotmail.com
7 Mail: project@pgrouting.org
8 
9 ------
10 
11 This program is free software; you can redistribute it and/or modify
12 it under the terms of the GNU General Public License as published by
13 the Free Software Foundation; either version 2 of the License, or
14 (at your option) any later version.
15 
16 This program is distributed in the hope that it will be useful,
17 but WITHOUT ANY WARRANTY; without even the implied warranty of
18 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 GNU General Public License for more details.
20 
21 You should have received a copy of the GNU General Public License
22 along with this program; if not, write to the Free Software
23 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
24 
25 ********************************************************************PGR-GNU*/
26 
27 // Include C header first for windows build issue
28 
29 #if 0
30 #if defined(__MINGW32__) || defined(_MSC_VER)
31 #include <winsock2.h>
32 #include <windows.h>
33 #endif
34 #endif
35 
36 #include <boost/config.hpp>
37 #include <boost/graph/graph_traits.hpp>
38 #include <boost/graph/adjacency_list.hpp>
39 #include <boost/graph/astar_search.hpp>
40 
41 #include <cmath>
42 
43 
44 #include <deque>
45 #include <limits>
46 #include <algorithm>
47 #include <vector>
48 #include <set>
49 
50 #include "./../../common/src/basePath_SSEC.hpp"
51 #include "./../../common/src/pgr_base_graph.hpp"
52 
53 
54 
55 
56 
57 template < class G >
58 class Pgr_astar {
59  public:
60  typedef typename G::V V;
61  typedef typename G::B_G B_G;
62 
63 
65 
66  void astar(
68  G &graph,
69  Path &path,
70  int64_t start_vertex,
71  int64_t end_vertex,
72  int heuristic,
73  double factor,
74  double epsilon,
75  bool only_cost = false);
76 
78  void astar(
79  G &graph,
80  std::deque< Path > &paths,
81  int64_t start_vertex,
82  const std::vector< int64_t > &end_vertex,
83  int heuristic,
84  double factor,
85  double epsilon,
86  bool only_cost = false);
87 
89  void astar(
90  G &graph,
91  std::deque< Path > &paths,
92  std::vector< int64_t > start_vertex,
93  const std::vector< int64_t > &end_vertex,
94  int heuristic,
95  double factor,
96  double epsilon,
97  bool only_cost = false);
98 
100 
101  private:
103  bool astar_1_to_1(
104  G &graph,
105  V source,
106  V target,
107  int heuristic,
108  double factor,
109  double epsilon);
110 
112  bool astar_1_to_many(
113  G &graph,
114  V source,
115  const std::vector< V > &targets,
116  int heuristic,
117  double factor,
118  double epsilon);
119 
120 
121 
122  void clear() {
123  predecessors.clear();
124  distances.clear();
125  }
126 
127  void get_path(
128  const G &graph,
129  V source,
130  V target,
131  Path &path) const;
132  void get_cost(
133  const G &graph,
134  V source,
135  V target,
136  Path &path) const;
137 
138  // used when multiple goals
139  void get_path(
140  const G &graph,
141  std::deque< Path > &paths,
142  V source,
143  std::vector< V > &targets) const;
144 
145  void get_cost(
146  const G &graph,
147  std::deque< Path > &paths,
148  V source,
149  std::vector< V > &targets) const;
150 
152 
153  struct found_goals{};
154  std::vector< V > predecessors;
155  std::vector< double > distances;
156  std::deque< V > nodesInDistance;
158 
159  // euclidean distance heuristic for one goal
160  class distance_heuristic : public boost::astar_heuristic< B_G, double > {
161  public:
162  distance_heuristic(B_G &g, V goal, int heuristic, double factor)
163  : m_g(g),
164  m_factor(factor),
165  m_heuristic(heuristic) {
166  m_goals.insert(goal);
167  }
169  B_G &g,
170  std::vector< V > goals,
171  int heuristic,
172  double factor)
173  : m_g(g),
174  m_goals(goals.begin(), goals.end()),
175  m_factor(factor),
176  m_heuristic(heuristic) {}
177 
178  double operator()(V u) {
179  if (m_heuristic == 0) return 0;
180  if (m_goals.empty()) return 0;
181  double best_h(std::numeric_limits<double>::max());
182  for (auto goal : m_goals) {
183  double current(std::numeric_limits<double>::max());
184  double dx = m_g[goal].x() - m_g[u].x();
185  double dy = m_g[goal].y() - m_g[u].y();
186  switch (m_heuristic) {
187  case 0:
188  current = 0;
189  case 1:
190  current = std::fabs(std::max(dx, dy)) * m_factor;
191  case 2:
192  current = std::fabs(std::min(dx, dy)) * m_factor;
193  case 3:
194  current = (dx * dx + dy * dy) * m_factor * m_factor;
195  case 4:
196  current = std::sqrt(dx * dx + dy * dy) * m_factor;
197  case 5:
198  current = (std::fabs(dx) + std::fabs(dy)) * m_factor;
199  default:
200  current = 0;
201  }
202  if (current < best_h) {
203  best_h = current;
204  }
205  }
206  {
207  auto s_it = m_goals.find(u);
208  if (!(s_it == m_goals.end())) {
209  // found one more goal
210  m_goals.erase(s_it);
211  }
212  }
213  return best_h;
214  }
215 
216  private:
218  std::set< V > m_goals;
219  double m_factor;
221  }; // class distance_heuristic
222 
223 
225  class astar_one_goal_visitor : public boost::default_astar_visitor {
226  public:
227  explicit astar_one_goal_visitor(V goal) : m_goal(goal) {}
228  template <class B_G>
229  void examine_vertex(V u, B_G &g) {
230  if (u == m_goal)
231  throw found_goals();
232  // using g, otherwise is throws a warning
233  num_edges(g);
234  }
235  private:
237  }; // class astar_one_goal_visitor
238 
240  class astar_many_goals_visitor : public boost::default_astar_visitor {
241  public:
242  explicit astar_many_goals_visitor(std::vector< V > goals)
243  :m_goals(goals.begin(), goals.end()) {}
244  template <class B_G>
245  void examine_vertex(V u, B_G &g) {
246  auto s_it = m_goals.find(u);
247  if (s_it == m_goals.end()) return;
248  // found one more goal
249  m_goals.erase(s_it);
250  if (m_goals.size() == 0) throw found_goals();
251  num_edges(g);
252  }
253  private:
254  std::set< V > m_goals;
255  };
256 }; // pgr_astar
257 
258 /******************** IMPLEMENTTION ******************/
259 
261 template < class G >
262 void
264  G &graph,
265  Path &path,
266  int64_t start_vertex,
267  int64_t end_vertex,
268  int heuristic,
269  double factor,
270  double epsilon,
271  bool only_cost) {
272  clear();
273 
274  // adjust predecessors and distances vectors
275  predecessors.resize(graph.num_vertices());
276  distances.resize(graph.num_vertices());
277 
278 
279  if (!graph.has_vertex(start_vertex)
280  || !graph.has_vertex(end_vertex)) {
281  path.clear();
282  return;
283  }
284 
285  // get the graphs source and target
286  auto v_source(graph.get_V(start_vertex));
287  auto v_target(graph.get_V(end_vertex));
288 
289  // perform the algorithm
290  astar_1_to_1(graph, v_source, v_target, heuristic, factor, epsilon);
291 
292  // get the results
293  if (only_cost) {
294  get_cost(graph, v_source, v_target, path);
295  } else {
296  get_path(graph, v_source, v_target, path);
297  }
298  return;
299 }
300 
302 template < class G >
303 void
305  G &graph,
306  std::deque< Path > &paths,
307  int64_t start_vertex,
308  const std::vector< int64_t > &end_vertex,
309  int heuristic,
310  double factor,
311  double epsilon,
312  bool only_cost) {
313  clear();
314 
315  // adjust predecessors and distances vectors
316  predecessors.resize(graph.num_vertices());
317  distances.resize(graph.num_vertices());
318 
319  // get the graphs source and targets
320  if (!graph.has_vertex(start_vertex)) return;
321  auto v_source(graph.get_V(start_vertex));
322 
323  std::set< V > s_v_targets;
324  for (const auto &vertex : end_vertex) {
325  if (graph.has_vertex(vertex)) {
326  s_v_targets.insert(graph.get_V(vertex));
327  }
328  }
329 
330  std::vector< V > v_targets(s_v_targets.begin(), s_v_targets.end());
331  // perform the algorithm
332  astar_1_to_many(graph, v_source, v_targets, heuristic, factor, epsilon);
333 
334  // get the results // route id are the targets
335  if (only_cost) {
336  get_cost(graph, paths, v_source, v_targets);
337  } else {
338  get_path(graph, paths, v_source, v_targets);
339  }
340 
341  std::stable_sort(paths.begin(), paths.end(),
342  [](const Path &e1, const Path &e2)->bool {
343  return e1.end_id() < e2.end_id();
344  });
345 
346  return;
347 }
348 
349 // preparation for many to many
350 template < class G >
351 void
353  G &graph, std::deque< Path > &paths,
354  std::vector< int64_t > start_vertex,
355  const std::vector< int64_t > &end_vertex,
356  int heuristic,
357  double factor,
358  double epsilon,
359  bool only_cost) {
360  std::stable_sort(start_vertex.begin(), start_vertex.end());
361  start_vertex.erase(
362  std::unique(start_vertex.begin(), start_vertex.end()),
363  start_vertex.end());
364 
365  for (const auto &start : start_vertex) {
366  astar(graph, paths, start, end_vertex,
367  heuristic, factor, epsilon, only_cost);
368  }
369 
370  std::sort(paths.begin(), paths.end(),
371  [](const Path &e1, const Path &e2)->bool {
372  return e1.end_id() < e2.end_id();
373  });
374  std::stable_sort(paths.begin(), paths.end(),
375  [](const Path &e1, const Path &e2)->bool {
376  return e1.start_id() < e2.start_id();
377  });
378  return;
379 }
380 
381 
382 
383 
385 template < class G >
386 bool
388  G &graph,
389  V source,
390  V target,
391  int heuristic,
392  double factor,
393  double epsilon) {
394  bool found = false;
395  try {
396  // Call A* named parameter interface
397  boost::astar_search(
398  graph.graph, source,
399  distance_heuristic(graph.graph, target,
400  heuristic, factor * epsilon),
401  boost::predecessor_map(&predecessors[0])
402  .weight_map(get(&pgrouting::Basic_edge::cost, graph.graph))
403  .distance_map(&distances[0])
404  .visitor(astar_one_goal_visitor(target)));
405  }
406  catch(found_goals &) {
407  found = true; // Target vertex found
408  }
409  return found;
410 }
411 
412 
414 template <class G>
415 bool
417  G &graph,
418  V source,
419  const std::vector< V > &targets,
420  int heuristic,
421  double factor,
422  double epsilon) {
423  bool found = false;
424  try {
425  boost::astar_search(
426  graph.graph, source,
428  graph.graph, targets,
429  heuristic, factor * epsilon),
430  boost::predecessor_map(&predecessors[0])
431  .weight_map(get(&pgrouting::Basic_edge::cost, graph.graph))
432  .distance_map(&distances[0])
433  .visitor(astar_many_goals_visitor(targets)));
434  }
435  catch(found_goals &fg) {
436  found = true; // Target vertex found
437  }
438  return found;
439 }
440 
441 
442 /*
443  * GET_COST
444  */
445 
446 
447 
448 template < class G >
449 void
451  const G &graph,
452  V source,
453  V target,
454  Path &r_path) const {
455  // backup of the target
456  int64_t from(graph.graph[source].id);
457  int64_t to(graph.graph[target].id);
458 
459  // no path was found
460  if (target == predecessors[target]) {
461  r_path.clear();
462  } else {
463  Path path(from, to);
464  path.push_front(
465  {to, -1, distances[target], distances[target]});
466  r_path = path;
467  }
468 }
469 
470 template < class G >
471 void
473  const G &graph,
474  std::deque< Path > &paths,
475  V source,
476  std::vector< V > &targets) const {
477  Path path;
478  for (auto s_it = targets.begin(); s_it != targets.end(); ++s_it) {
479  path.clear();
480  get_cost(graph, source, *s_it, path);
481  paths.push_back(path);
482  }
483 }
484 
485 
486 /*
487  * GET_PATH
488  */
489 
490 
491 template < class G >
492 void
494  const G &graph,
495  std::deque< Path > &paths,
496  V source,
497  std::vector< V > &targets) const {
498  Path path;
499  typename std::vector< V >::iterator s_it;
500  for (s_it = targets.begin(); s_it != targets.end(); ++s_it) {
501  path.clear();
502  get_path(graph, source, *s_it, path);
503  paths.push_back(path);
504  }
505 }
506 
507 template < class G >
508 void
510  const G &graph,
511  V source,
512  V target,
513  Path &r_path) const {
514  // backup of the target
515  V target_back = target;
516  uint64_t from(graph.graph[source].id);
517  uint64_t to(graph.graph[target].id);
518 
519  // no path was found
520  if (target == predecessors[target]) {
521  r_path.clear();
522  return;
523  }
524 
525  // find out how large is the path
526  int64_t result_size = 1;
527  while (target != source) {
528  if (target == predecessors[target]) break;
529  result_size++;
530  target = predecessors[target];
531  }
532 
533  // recover the target
534  target = target_back;
535 
536  // variables that are going to be stored
537  int64_t vertex_id;
538  int64_t edge_id;
539  double cost;
540 
541  // working from the last to the beginning
542 
543  // initialize the sequence
544  auto seq = result_size;
545  // the last stop is the target
546  Path path(from, to);
547  path.push_front(
548  {graph.graph[target].id, -1,
549  0, distances[target]});
550 
551  while (target != source) {
552  // we are done when the predecesor of the target is the target
553  if (target == predecessors[target]) break;
554  // values to be inserted in the path
555  --seq;
556  cost = distances[target] - distances[predecessors[target]];
557  vertex_id = graph.graph[predecessors[target]].id;
558  edge_id = graph.get_edge_id(predecessors[target], target, cost);
559 
560  path.push_front({vertex_id, edge_id,
561  cost, (distances[target] - cost)});
562  target = predecessors[target];
563  }
564  r_path = path;
565  return;
566 }
567 
void get_cost(const G &graph, V source, V target, Path &path) const
Definition: pgr_astar.hpp:450
bool astar_1_to_many(G &graph, V source, const std::vector< V > &targets, int heuristic, double factor, double epsilon)
Call to astar 1 source to many targets.
Definition: pgr_astar.hpp:416
void push_front(Path_t data)
exception for termination
Definition: pgr_astar.hpp:153
std::vector< V > predecessors
Definition: pgr_astar.hpp:154
bool astar_1_to_1(G &graph, V source, V target, int heuristic, double factor, double epsilon)
Call to Astar 1 source to 1 target.
Definition: pgr_astar.hpp:387
visitor that terminates when we find the goal
Definition: pgr_astar.hpp:225
distance_heuristic(B_G &g, std::vector< V > goals, int heuristic, double factor)
Definition: pgr_astar.hpp:168
void get_path(const G &graph, V source, V target, Path &path) const
Definition: pgr_astar.hpp:509
G::B_G B_G
Definition: pgr_astar.hpp:61
void clear()
Definition: pgr_astar.hpp:122
int64_t end_id() const
void astar(G &graph, Path &path, int64_t start_vertex, int64_t end_vertex, int heuristic, double factor, double epsilon, bool only_cost=false)
one to one
Definition: pgr_astar.hpp:263
class for stopping when all targets are found
Definition: pgr_astar.hpp:240
astar_many_goals_visitor(std::vector< V > goals)
Definition: pgr_astar.hpp:242
int64_t start_id() const
path_element_t * path
Definition: BDATester.cpp:49
std::deque< V > nodesInDistance
Definition: pgr_astar.hpp:156
void examine_vertex(V u, B_G &g)
Definition: pgr_astar.hpp:229
void clear()
static void get_path(int route_id, int path_id, const Path &path, Routes_t **postgres_data, double &route_cost, size_t &sequence)
distance_heuristic(B_G &g, V goal, int heuristic, double factor)
Definition: pgr_astar.hpp:162
std::vector< double > distances
Definition: pgr_astar.hpp:155