GISCup 2015
 All Classes Files Functions Variables Typedefs Enumerations Enumerator Macros
astar.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 ASTAR_HPP_INC
17 #define ASTAR_HPP_INC
18 
19 
31 std::pair<bool, double> search_explicit(size_t s, size_t end)
32  {
33  lastStart = s; lastGoal = end;
35  h(locations,end);
36 
37  //@TODO: Move somewhere else
38  d.resize(num_vertices(g));
39  colors.resize(num_vertices(g));
40  p.resize(num_vertices(g));
41  f.resize(num_vertices(g)); // The up to date priorities
42 
44  mygraph_t::out_edge_iterator oe, oe_end;
45 
46  // Initialize all data structures
47  for (boost::tie(u, u_end) = vertices(g); u != u_end; ++u)
48  {
49  d[*u] = std::numeric_limits<double>::infinity();
50  colors[*u] = Color::white();
51  p[*u] = *u;
52  }
53  // Start Search
54 
55  queue_t Q;
56  colors[s] = Color::gray();
57  d[s] = 0;
58  f[s] = h(s);
59  Q.push(make_pair(f[s],s));
60  while(!Q.empty())
61  {
62  auto u = Q.top();
63  Q.pop();
64  /*
65  * Use the priority_queue and push element each time you
66  * would like to update it. Accept the fact that you will
67  * have useless entries in the queue. When popping the
68  * top value, check if it contains the up-to-date value.
69  * If not, ignore it and pop the next.
70  *
71  * This way you delay the removal of the updated element
72  * until it comes to the top.
73  * */
74 
75  if (u.first != f[u.second])
76  continue;
77 
78 
79 
80  if (u.second == end)
81  {
82  shortest_path.clear();
83  for(mygraph_t::vertex_descriptor v = end;; v = p[v]) {
84  shortest_path.push_front(v);
85  if(p[v] == v)
86  break;
87  }
88 #ifndef NO_STAT_OUTPUT
89  DEFAULT_STATS("Basic A*");
90 #endif
91 
92  return make_pair(true,d[end]);
93  }
94 
95  for (tie(oe, oe_end) = out_edges(u.second,g);
96  oe != oe_end;
97  ++oe)
98  {
99 
100  cost dst = weightmap[*oe]//dynamic_weightmap(*oe,g,weightmap, remove_map)
101  + d[u.second];
102  auto v = target(*oe,g);
103  STAT_EXAMINE();
104  if ( dst < d[v])
105  {
106  d[v] = dst;
107  p[v] = u.second;
108  f[v] = dst+h(v);
109  if (colors[v] == Color::white())
110  {
111  // discover
112  colors[v] = Color::gray();
113  Q.push(make_pair(f[v],v));
114  }else
115  if(colors[v] == Color::black())
116  {
117  // reopen
118  colors[v] = Color::gray();
119  Q.push(make_pair(f[v],v));
120  }else if(colors[v] == Color::gray())
121  {
122  // update in queue (we have found a shorter path)
123  Q.push(make_pair(f[v],v));
124 
125  }
126 
127  }
128  }// end for
129  colors[u.second] = Color::black();
130  }// while
131 #ifndef NO_STAT_OUTPUT
132 DEFAULT_STATS("Basic A*");
133 #endif
134 
135  return make_pair(false,std::numeric_limits<double>::infinity());
136  };
137 
138  #endif