root/trunk/core/src/shooting_star_boost_wrapper.cpp

Revision 355, 10.6 KB (checked in by anton, 12 months ago)

Partial fix for #190

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