pgRouting  2.2
pgRouting extends the PostGIS / PostgreSQL geospatial database to provide geospatial routing functionality.
 All Classes Functions Variables Pages
shooting_star_boost_wrapper.cpp
1 /*PGR-GNU*****************************************************************
2 
3 Copyright (c) 2015 pgRouting developers
4 Mail: project@pgrouting.org
5 
6 ------
7 
8 This program is free software; you can redistribute it and/or modify
9 it under the terms of the GNU General Public License as published by
10 the Free Software Foundation; either version 2 of the License, or
11 (at your option) any later version.
12 
13 This program is distributed in the hope that it will be useful,
14 but WITHOUT ANY WARRANTY; without even the implied warranty of
15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 GNU General Public License for more details.
17 
18 You should have received a copy of the GNU General Public License
19 along with this program; if not, write to the Free Software
20 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
21 
22 ********************************************************************PGR-GNU*/
23 /*
24  * Shooting* Shortest path algorithm for PostgreSQL
25  *
26  * Copyright (c) 2007 Anton A. Patrushev, Orkney, Inc.
27  *
28  * This program is free software; you can redistribute it and/or modify
29  * it under the terms of the GNU General Public License as published by
30  * the Free Software Foundation; either version 2 of the License, or
31  * (at your option) any later version.
32  *
33  * This program is distributed in the hope that it will be useful,
34  * but WITHOUT ANY WARRANTY; without even the implied warranty of
35  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
36  * GNU General Public License for more details.
37  *
38  * You should have received a copy of the GNU General Public License
39  * along with this program; if not, write to the Free Software
40  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
41  *
42  */
43 
44 // Include C header first for windows build issue
45 #include "shooting_star.h"
46 
47 #include <boost/config.hpp>
48 #include <boost/version.hpp>
49 
50 #include <boost/graph/graph_traits.hpp>
51 #include <boost/graph/adjacency_list.hpp>
52 #if BOOST_VERSION > 103900
53 #include <boost/property_map/vector_property_map.hpp>
54 #else
55 #include <boost/vector_property_map.hpp>
56 #endif
57 #include <shooting_star_search.hpp>
58 
59 #include <cmath> // for sqrt
60 
61 using namespace std;
62 using namespace boost;
63 
64 struct Edge
65 {
66  int id;
67  int source;
68  int target;
69  float8 cost;
70  float distance;
71  float rank;
72  std::map< int, vector< std::pair<float, std::vector<int> > >, std::less<int> > adjacent_edges;
73  default_color_type color;
74 
75  std::size_t index;
76 };
77 
78 struct Vertex
79 {
80  int id;
81  float8 x;
82  float8 y;
83 };
84 
85 // exception for termination
86 template <class Edge>
87 class found_goal
88 {
89  public:
90  found_goal() {}
91  found_goal(Edge target) : target_edge(target) {}
92  found_goal(const found_goal &fg) {}
93  ~found_goal() {}
94  Edge get_target()
95  {
96  return target_edge;
97  }
98  private:
99  Edge target_edge;
100 };
101 
102 
103 // visitor that terminates when we find the goal
104 template <class Edge>
106 {
107 public:
108  shooting_star_goal_visitor(Edge goal, int max_id) : m_goal(goal){}
109  shooting_star_goal_visitor(const shooting_star_goal_visitor &gv) : m_goal(gv.m_goal){}
111 
112  template <class Graph>
113  void examine_edge(Edge e, Graph& g, int e_max_id)
114  {
115  if( g[e].id == g[m_goal].id || g[e].id == g[m_goal].id + e_max_id )
116  {
117  throw found_goal<Edge>(e);
118  }
119  }
120  template <class Graph>
121  void finish_edge(Edge e, Graph& g) {}
122 private:
123  Edge m_goal;
124  int e_max_id;
125 };
126 
127 // Heuristic function
128 template <class Graph, class CostType>
129 class distance_heuristic
130 {
131 public:
132  typedef typename graph_traits<Graph>::edge_descriptor Edge;
133  distance_heuristic(Graph& g, Edge goal):m_g(g), m_goal(goal){}
134  CostType operator()(Edge e)
135  {
136  CostType dx = m_g[source(m_goal, m_g)].x - m_g[source(e, m_g)].x;
137  CostType dxt = m_g[target(m_goal, m_g)].x - m_g[target(e, m_g)].x;
138  CostType dy = m_g[source(m_goal, m_g)].y - m_g[source(e, m_g)].y;
139  CostType dyt = m_g[target(m_goal, m_g)].y - m_g[target(e, m_g)].y;
140  //You can choose any heuristical function from below
141  //return ::max(dx, dy);
142  //return ::sqrt(dx * dx + dy * dy)/2;
143  //return 0;
144  //return (min(::fabs(dx),::fabs(dxt))+min(::fabs(dy),::fabs(dyt)))/2;
145  return sqrt(pow(min(::fabs(dx),::fabs(dxt)),2)+pow(min(::fabs(dy),::fabs(dyt)),2))/2;
146  }
147 private:
148  Graph& m_g;
149  Edge m_goal;
150 };
151 
152 
153 // Adds an edge to the graph
154 // Also copies all attributes and adjacent edges
155 template <class G, class E>
156 static void
157 graph_add_edge(G &graph, int index,
158  int id, int source, int target,
159  float8 cost, float8 s_x, float8 s_y, float8 t_x, float8 t_y,
160  std::map< int, vector< std::pair<float, vector<int> > >, std::less<int> > adjacent_edges)
161 {
162 
163  E e;
164  bool inserted;
165 
166  if (cost < 0) // edges are inserted as unpassable if cost is negative
167  cost = MAX_COST;
168 
169  tie(e, inserted) = add_edge(source, target, graph);
170 
171  graph[e].cost = cost;
172  graph[e].id = id;
173 
174  graph[e].source = source;
175  graph[e].target = target;
176 
177  graph[e].adjacent_edges = adjacent_edges;
178 
179  graph[e].rank = 0;
180  graph[e].distance = 0;
181 
182  graph[e].index = index;
183 
184 
185  typedef typename graph_traits<G>::vertex_descriptor Vertex;
186  Vertex s = vertex(source, graph);
187  Vertex t = vertex(target, graph);
188 
189  graph[s].id=source;
190  graph[t].id=target;
191 
192  graph[s].x=s_x;
193  graph[s].y=s_y;
194  graph[t].x=t_x;
195  graph[t].y=t_y;
196 
197 }
198 
199 // Main Shooting* function.
200 // It renumbers vertices, fills the graph with edges,
201 // calculates a route and return a result.
202 int
203 boost_shooting_star(edge_shooting_star_t *edges_array, unsigned int count,
204  int source_edge_id, int target_edge_id,
205  bool directed, bool has_reverse_cost,
206  path_element_t **path, int *path_count, char **err_msg, int e_max_id)
207 {
208 
209  typedef adjacency_list<vecS, vecS, directedS, Vertex, Edge> graph_t;
210 
211  typedef graph_traits < graph_t >::vertex_descriptor vertex_descriptor;
212  typedef graph_traits < graph_t >::edge_descriptor edge_descriptor;
213 
214  const unsigned int num_nodes = count*2;
215 
216  int z;
217  int src, trg, offset, rule_num;
218 
219  graph_t graph(num_nodes);
220 
221  std::map< int, vector< std::pair<float, vector<int> > >, std::less<int> > adjacent_edges;
222 
223  std::map< int, int, std::less<int> > vertices;
224 
225  vector<int> rule;
226 
227  offset = 1;
228  rule_num = 0;
229 
230  for (std::size_t j = 0; j < count; ++j)
231  {
232  //Vertex ids renumbering moved here
233  src = edges_array[j].source;
234  trg = edges_array[j].target;
235 
236  if(vertices[src]==0)
237  {
238  vertices[src]=j+offset;
239  }
240  edges_array[j].source=vertices[src];
241 
242  if(vertices[trg]==0)
243  {
244  offset++;
245  vertices[trg]=j+offset;
246  }
247  edges_array[j].target=vertices[trg];
248 
249  for(z=0; z<MAX_RULE_LENGTH;++z)
250  {
251  if(edges_array[j].rule[z] > 0)
252  {
253  rule.push_back(edges_array[j].rule[z]);
254  }
255  }
256 
257  if(edges_array[j].to_cost > 0)
258  {
259  adjacent_edges[edges_array[j].id].push_back(std::pair<float8, vector<int> > (edges_array[j].to_cost, rule) );
260  rule.clear();
261  }
262 
263  if((j < count-1 && edges_array[j].id != edges_array[j+1].id)||(j==count-1))
264  {
265  graph_add_edge<graph_t, edge_descriptor>(graph, j,
266  edges_array[j].id, edges_array[j].source,
267  edges_array[j].target, edges_array[j].cost,
268  edges_array[j].s_x, edges_array[j].s_y,
269  edges_array[j].t_x, edges_array[j].t_y, adjacent_edges);
270 
271  // if the edge is not directed or if it is directed and has reverse cost
272  if (!directed || (directed && has_reverse_cost))
273  {
274  float8 cost, reverse_cost;
275 
276  if (has_reverse_cost)
277  {
278  cost = edges_array[j].cost;
279  reverse_cost = edges_array[j].reverse_cost;
280 
281  //If chosen source/target edge's cost is high, take the edge for opposite direction
282  if(cost > reverse_cost)
283  {
284  if(edges_array[j].id == source_edge_id)
285  source_edge_id += e_max_id;
286  else if(edges_array[j].id == target_edge_id)
287  target_edge_id += e_max_id;
288  }
289  }
290  else
291  {
292  cost = edges_array[j].cost;
293  }
294 
295 
296  if(adjacent_edges[edges_array[j].id].size() > 0)
297  {
298  adjacent_edges[edges_array[j].id+e_max_id].assign( adjacent_edges[edges_array[j].id].begin(), adjacent_edges[edges_array[j].id].end() );
299  adjacent_edges.erase(edges_array[j].id);
300  }
301 
302 
303  graph_add_edge<graph_t, edge_descriptor>(graph, j,
304  edges_array[j].id+e_max_id, edges_array[j].target,
305  edges_array[j].source, cost,
306  edges_array[j].s_x, edges_array[j].s_y,
307  edges_array[j].t_x, edges_array[j].t_y, adjacent_edges);
308  }
309 
310  adjacent_edges.clear();
311  rule_num = 0;
312  }
313  else
314  {
315  rule_num++;
316  }
317  }
318 
319 
320  edge_descriptor source_edge;
321  edge_descriptor target_edge;
322 
323  bool source_found = false, target_found = false;
324 
325  graph_traits<graph_t>::edge_iterator ei, ei_end;
326 
327  for(tie(ei, ei_end) = edges(graph); ei != ei_end; ++ei)
328  {
329  if(graph[*ei].id == source_edge_id || graph[*ei].id == source_edge_id - e_max_id)
330  {
331  source_edge = *ei;
332  source_found = true;
333  break;
334  }
335 
336  }
337 
338  if (!source_found)
339  {
340  *err_msg = (char *) "Source edge not found";
341  return -2;
342  }
343 
344 
345  for(tie(ei, ei_end) = edges(graph); ei != ei_end; ++ei)
346  {
347  if(graph[*ei].id == target_edge_id || graph[*ei].id == target_edge_id - e_max_id)
348  {
349  target_edge = *ei;
350  target_found = true;
351  break;
352  }
353  }
354 
355 
356  if (!target_found)
357  {
358  *err_msg = (char *) "Target edge not found";
359  return -3;
360  }
361 
362  property_map<graph_t, std::size_t Edge::*>::type edge_index = get(&Edge::index, graph);
363 
364  std::map< int, edge_descriptor, std::less<int> > predecessors;
365 
366  property_map<graph_t, float Edge::*>::type rank = get(&Edge::rank, graph);
367  property_map<graph_t, float Edge::*>::type distance = get(&Edge::distance, graph);
368 
369  try
370  {
371  //calling Shooting* search
372  shooting_star_search
373  (graph, source_edge,
374  distance_heuristic<graph_t, float>(graph, target_edge),
375  weight_map(get(&Edge::cost, graph)).
376  weight_map2(get(&Edge::adjacent_edges, graph)).
377  edge_color_map(get(&Edge::color, graph)).
378  visitor(shooting_star_goal_visitor<edge_descriptor>(target_edge, e_max_id)),
379  edge_index,
380  distance, rank,
381  predecessors, e_max_id
382  );
383 
384  }
385  catch(found_goal<edge_descriptor> &fg)
386  {
387 
388  vector<edge_descriptor> path_vect;
389  int max = MAX_NODES;
390 
391  target_edge = fg.get_target();
392 
393  path_vect.push_back(target_edge);
394 
395  while (target_edge != source_edge)
396  {
397 
398  if ((target_edge == predecessors[graph[target_edge].id]) && (predecessors[graph[target_edge].id] != source_edge))
399  {
400  *err_msg = (char *) "No path found";
401  return -1;
402 
403  }
404 
405  target_edge = predecessors[graph[target_edge].id];
406 
407  //Check if we have u-turns within same edge at the beginning
408  if( !(abs(graph[predecessors[graph[target_edge].id]].id - graph[target_edge].id) == e_max_id && (target_edge != source_edge || predecessors[graph[target_edge].id] != source_edge)) )
409  {
410  path_vect.push_back(target_edge);
411  }
412 
413  // This check was made to be sure that we can
414  // restore the path from the target edge within
415  // MAX_NODE iterations.
416  // Sometimes it doesn't work properly and search exits here
417  // even if the target edge was reached.
418 
419  if (!max--)
420  {
421  *err_msg = (char *) "No path found";
422  return -1;
423  }
424  }
425 
426  *path = (path_element_t *) malloc(sizeof(path_element_t) *
427  (path_vect.size() + 1));
428  *path_count = path_vect.size();
429 
430  int start_from = path_vect.size() - 1;
431 
432  for(int i = start_from, j = 0; i >= 0; i--, j++)
433  {
434  float cost;
435  graph_traits < graph_t >::edge_descriptor e;
436 
437  e = path_vect.at(i);
438 
439  if(graph[e].id > e_max_id)
440  {
441  graph[e].id -= e_max_id;
442  }
443 
444  (*path)[j].edge_id = graph[e].id;
445  (*path)[j].cost = graph[e].cost;
446  (*path)[j].vertex_id = source(e, graph);
447  }
448 
449  return EXIT_SUCCESS;
450  }
451 
452  *err_msg = (char *) "Target was not reached";
453  return 0;
454 
455 }
456 
Definition: alpha.h:33