GISCup 2015
 All Classes Files Functions Variables Typedefs Enumerations Enumerator Macros
dijkstra.hpp
Go to the documentation of this file.
1 /* Copyright 2015 Martin Werner - <martin.werner@ifi.lmu.de>
2  *
3  * This program is free software: you can redistribute it and/or modify
4  * it under the terms of the GNU General Public License as published by
5  * the Free Software Foundation, either version 3 of the License, or
6  * (at your option) any later version.
7  *
8  * This program is distributed in the hope that it will be useful,
9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  */
16 #ifndef DIJKSTRA_HPP_INC
17 #define DIJKSTRA_HPP_INC
18 
22  std::pair<bool, double> search_dijkstra(size_t s, size_t end)
23  {
24  lastStart = s; lastGoal = end;
25 
26  d.resize(num_vertices(g));
27  colors.resize(num_vertices(g));
28  p.resize(num_vertices(g));
29 
31  mygraph_t::out_edge_iterator oe, oe_end;
32 
33  // Initialize all data structures
34  for (boost::tie(u, u_end) = vertices(g); u != u_end; ++u)
35  {
36  d[*u] = std::numeric_limits<double>::infinity();
37  colors[*u] = Color::white();
38  p[*u] = *u;
39  }
40  // Start Search
41  queue_t Q;
42  colors[s] = Color::gray();
43  d[s] = 0;
44  Q.push(make_pair(d[s],s));
45 
46  while(!Q.empty())
47  {
48  auto u = Q.top();
49  Q.pop();
50  if (u.first != d[u.second])
51  continue;
52 
53  if (u.second == end)
54  {
55  // Here, we know the shortest path and construct it from the predecessor list p
56  shortest_path.clear();
57  for(mygraph_t::vertex_descriptor v = end;; v = p[v]) {
58  shortest_path.push_front(v);
59  if(p[v] == v)
60  break;
61  }
62 #ifndef NO_STAT_OUTPUT
63  DEFAULT_STATS("Basic Dijkstra");
64 #endif
65 
66  return make_pair(true,d[end]);
67  }
68 
69 
70  for (tie(oe, oe_end) = out_edges(u.second,g);
71  oe != oe_end;
72  ++oe)
73  {
74  STAT_EXAMINE();
75 
76  cost dst = weightmap[*oe]
77  + d[u.second];
78  auto v = target(*oe,g);
79  if ( dst < d[v])
80  {
81  d[v] = dst;
82  p[v] = u.second;
83 
84  if (colors[v] == Color::white())
85  {
86  // discover
87  colors[v] = Color::gray();
88  Q.push(make_pair(d[v],v));
89  }else if(colors[v] == Color::gray())
90  {
91  // update in queue (we have found a shorter path)
92  Q.push(make_pair(d[v],v));
93 
94  }
95  }
96  }// end for
97  colors[u.second] = Color::black();
98  }// while
99 #ifndef NO_STAT_OUTPUT
100 DEFAULT_STATS("Basic Dijkstra");
101 #endif
102 
103  return make_pair(false,std::numeric_limits<double>::infinity());
104  };
105 
106 
107 
108 #endif