pgRouting  2.2
pgRouting extends the PostGIS / PostgreSQL geospatial database to provide geospatial routing functionality.
 All Classes Functions Variables Pages
astar_boost_wrapper.cpp
1 /*PGR-GNU*****************************************************************
2 
3  * A* Shortest path algorithm for PostgreSQL
4  *
5  * Copyright (c) 2006 Anton A. Patrushev, Orkney, Inc.
6 Mail: project@pgrouting.org
7 
8 ------
9 
10 This program is free software; you can redistribute it and/or modify
11 it under the terms of the GNU General Public License as published by
12 the Free Software Foundation; either version 2 of the License, or
13 (at your option) any later version.
14 
15 This program is distributed in the hope that it will be useful,
16 but WITHOUT ANY WARRANTY; without even the implied warranty of
17 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 GNU General Public License for more details.
19 
20 You should have received a copy of the GNU General Public License
21 along with this program; if not, write to the Free Software
22 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
23 
24 ********************************************************************PGR-GNU*/
25 
26 // Include C header first for windows build issue
27 
28 #ifdef __MINGW32__
29 #include <winsock2.h>
30 #include <windows.h>
31 #ifdef unlink
32 #undef unlink
33 #endif
34 #endif
35 
36 
37 #if 0
38 extern "C" {
39 #include "postgres.h"
40 }
41 #endif
42 
43 #include <boost/config.hpp>
44 
45 #include <boost/graph/graph_traits.hpp>
46 #include <boost/graph/adjacency_list.hpp>
47 #include <boost/graph/astar_search.hpp>
48 
49 #include <cmath> // for sqrt
50 #include "astar.h"
51 
52 using namespace std;
53 using namespace boost;
54 
55 // Maximal number of nodes in the path (to avoid infinite loops)
56 #define MAX_NODES 100000000
57 
58 struct Edge
59 {
60  int id;
61  double cost;
62  //double distance;
63 };
64 
65 struct Vertex
66 {
67  int id;
68  double x;
69  double y;
70 };
71 
72 
73 struct found_goal {}; // exception for termination
74 
75 // visitor that terminates when we find the goal
76 template <class Vertex>
77 class astar_goal_visitor : public boost::default_astar_visitor
78 {
79  public:
80  astar_goal_visitor(Vertex goal) : m_goal(goal) {}
81  template <class Graph>
82  void examine_vertex(Vertex u, Graph& g) {
83  if(u == m_goal)
84  throw found_goal();
85  num_vertices(g);
86  }
87  private:
88  Vertex m_goal;
89 };
90 
91 // Heuristic function which tells us how far the current node is
92 // from the target node.
93 //
94 // (|dx|+|dy|)/2 is used currently.
95 template <class Graph, class CostType>
96 class distance_heuristic : public astar_heuristic<Graph, CostType>
97 {
98  public:
99  typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
100  distance_heuristic(Graph& g, Vertex goal):m_g(g), m_goal(goal){}
101  CostType operator()(Vertex u)
102  {
103  CostType dx = m_g[m_goal].x - m_g[u].x;
104  CostType dy = m_g[m_goal].y - m_g[u].y;
105  //You can choose any heuristical function from below
106  //return ::max(dx, dy);
107  //return ::sqrt(dx * dx + dy * dy)/4;
108  //return 0;
109  return (::fabs(dx)+::fabs(dy))/2;
110  }
111  private:
112  Graph& m_g;
113  Vertex m_goal;
114 };
115 
116 
117 // Adds an edge to the graph.
118 // Edge id, cost, source and target ids and coordinates are copied also
119 template <class G, class E>
120 static void
121 graph_add_edge(G &graph, int id, int source, int target,
122  double cost, double s_x, double s_y, double t_x, double t_y)
123 {
124  E e;
125  bool inserted;
126 
127  if (cost < 0) // edges are not inserted in the graph if cost is negative
128  return;
129 
130  tie(e, inserted) = add_edge(source, target, graph);
131 
132  graph[e].cost = cost;
133  graph[e].id = id;
134 
135  typedef typename graph_traits<G>::vertex_descriptor Vertex;
136  Vertex s = vertex(source, graph);
137  Vertex t = vertex(target, graph);
138  graph[s].x=s_x;
139  graph[s].y=s_y;
140  graph[t].x=t_x;
141  graph[t].y=t_y;
142 }
143 
144 int
145 boost_astar(edge_astar_t *edges, size_t count,
146  int source_vertex_id, int target_vertex_id,
147  bool directed, bool has_reverse_cost,
148  path_element_t **path, size_t *path_count, char **err_msg)
149 {
150 try {
151 
152  // FIXME: use a template for the directedS parameters
153  typedef adjacency_list < listS, vecS, directedS, Vertex, Edge> graph_t;
154 
155  typedef graph_traits < graph_t >::vertex_descriptor vertex_descriptor;
156  typedef graph_traits < graph_t >::edge_descriptor edge_descriptor;
157 
158  // FIXME: compute this value
159  const auto num_nodes = ((directed && has_reverse_cost ? 2 : 1) *
160  count) + 100;
161 
162  graph_t graph(num_nodes);
163 
164  // interfers with APPLE build, can it just be commented out?
165  //property_map<graph_t, edge_weight_t>::type weightmap = get(edge_weight,
166  // graph);
167 
168  for (std::size_t j = 0; j < count; ++j)
169  {
170 
171  graph_add_edge<graph_t, edge_descriptor>(graph,
172  edges[j].id, edges[j].source,
173  edges[j].target, edges[j].cost,
174  edges[j].s_x, edges[j].s_y,
175  edges[j].t_x, edges[j].t_y);
176 
177  if (!directed || (directed && has_reverse_cost))
178  {
179  double cost;
180 
181  if (has_reverse_cost)
182  {
183  cost = edges[j].reverse_cost;
184  }
185  else
186  {
187  cost = edges[j].cost;
188  }
189 
190  graph_add_edge<graph_t, edge_descriptor>(graph,
191  edges[j].id,
192  edges[j].target,
193  edges[j].source,
194  cost,
195  edges[j].t_x,
196  edges[j].t_y,
197  edges[j].s_x,
198  edges[j].s_y);
199  }
200  }
201 
202  std::vector<vertex_descriptor> predecessors(num_vertices(graph));
203 
204  vertex_descriptor source_vertex = vertex(source_vertex_id, graph);
205 
206  if ((long)source_vertex < 0)
207  {
208  *err_msg = (char *) "Source vertex not found";
209  return -1;
210  }
211 
212  vertex_descriptor target_vertex = vertex(target_vertex_id, graph);
213  if ((long)target_vertex < 0)
214  {
215  *err_msg = (char *) "Target vertex not found";
216  return -1;
217  }
218 
219  std::vector<double> distances(num_vertices(graph));
220 
221  try {
222  // Call A* named parameter interface
223  astar_search
224  (graph, source_vertex,
225  distance_heuristic<graph_t, double>(graph, target_vertex),
226  predecessor_map(&predecessors[0]).
227  weight_map(get(&Edge::cost, graph)).
228  distance_map(&distances[0]).
229  visitor(astar_goal_visitor<vertex_descriptor>(target_vertex)));
230 
231  }
232  catch(found_goal& fg) {
233  // Target vertex found
234  vector<vertex_descriptor> path_vect;
235  int max = MAX_NODES;
236  path_vect.push_back(target_vertex);
237 
238  while (target_vertex != source_vertex)
239  {
240  if (target_vertex == predecessors[target_vertex])
241  {
242  *err_msg = (char *) "No path found";
243  return 0;
244 
245  }
246  target_vertex = predecessors[target_vertex];
247 
248  path_vect.push_back(target_vertex);
249  if (!max--)
250  {
251  *err_msg = (char *) "Overflow";
252  return -1;
253  }
254  }
255 
256  *path = (path_element_t *) malloc(sizeof(path_element_t) *
257  (path_vect.size() + 1));
258  *path_count = path_vect.size();
259 
260  for(int64_t i = static_cast<int64_t>(path_vect.size()) - 1, j = 0; i >= 0; i--, j++)
261  {
262  graph_traits < graph_t >::vertex_descriptor v_src;
263  graph_traits < graph_t >::vertex_descriptor v_targ;
264  graph_traits < graph_t >::edge_descriptor e;
265  graph_traits < graph_t >::out_edge_iterator out_i, out_end;
266 
267  (*path)[j].vertex_id = static_cast<int>(path_vect.at(i));
268 
269  (*path)[j].edge_id = -1;
270  (*path)[j].cost = distances[target_vertex];
271 
272  if (i == 0)
273  {
274  continue;
275  }
276 
277  v_src = path_vect.at(i);
278  v_targ = path_vect.at(i - 1);
279  double cost = 99999999.9;
280  int edge_id = 0;
281 
282  for (tie(out_i, out_end) = out_edges(v_src, graph);
283  out_i != out_end; ++out_i)
284  {
285  graph_traits < graph_t >::vertex_descriptor targ; // v set but not used
286  e = *out_i;
287  // v = source(e, graph);
288  targ = target(e, graph);
289 
290  if (targ == v_targ)
291  {
292  if (graph[*out_i].cost < cost)
293  {
294  edge_id = graph[*out_i].id;
295  cost = graph[*out_i].cost;
296  }
297  }
298  }
299  (*path)[j].edge_id = edge_id;
300  (*path)[j].cost = cost;
301  }
302 
303  return EXIT_SUCCESS;
304  }
305  }
306  catch(...) {
307  *err_msg = (char *) "Unknown exception caught!";
308  return -1;
309  }
310  *err_msg = (char *) "No path found";
311  return 0;
312 }
313 
Definition: alpha.h:33