1
2
3 //
4 //=======================================================================
5 // Copyright (c) 2004 Kristopher Beevers
6 //
7 // Distributed under the Boost Software License, Version 1.0. (See
8 // accompanying file LICENSE_1_0.txt or copy at
9 // http://www.boost.org/LICENSE_1_0.txt)
10 //=======================================================================
11 //
12
13 #include <boost/graph/astar_search.hpp>
14 #include <boost/graph/adjacency_list.hpp>
15 #include <boost/graph/random.hpp>
16 #include <boost/random.hpp>
17 #include <utility>
18 #include <vector>
19 #include <list>
20 #include <iostream>
21 #include <math.h> // for sqrt
22 #include <time.h>
23
24 using namespace boost;
25 using namespace std;
26
27 // auxiliary types
28 struct location
29 {
30 float y, x; // lat, long
31 };
32 struct my_float
33 {
34 float v;
my_floatmy_float35 explicit my_float(float v = float()) : v(v) {}
36 };
37 typedef my_float cost;
operator <<(ostream & o,my_float f)38 ostream& operator<<(ostream& o, my_float f) { return o << f.v; }
operator +(my_float a,my_float b)39 my_float operator+(my_float a, my_float b) { return my_float(a.v + b.v); }
operator ==(my_float a,my_float b)40 bool operator==(my_float a, my_float b) { return a.v == b.v; }
operator <(my_float a,my_float b)41 bool operator<(my_float a, my_float b) { return a.v < b.v; }
42
43 template < class Name, class LocMap > class city_writer
44 {
45 public:
city_writer(Name n,LocMap l,float _minx,float _maxx,float _miny,float _maxy,unsigned int _ptx,unsigned int _pty)46 city_writer(Name n, LocMap l, float _minx, float _maxx, float _miny,
47 float _maxy, unsigned int _ptx, unsigned int _pty)
48 : name(n)
49 , loc(l)
50 , minx(_minx)
51 , maxx(_maxx)
52 , miny(_miny)
53 , maxy(_maxy)
54 , ptx(_ptx)
55 , pty(_pty)
56 {
57 }
58 template < class Vertex >
operator ()(ostream & out,const Vertex & v) const59 void operator()(ostream& out, const Vertex& v) const
60 {
61 float px = 1 - (loc[v].x - minx) / (maxx - minx);
62 float py = (loc[v].y - miny) / (maxy - miny);
63 out << "[label=\"" << name[v] << "\", pos=\""
64 << static_cast< unsigned int >(ptx * px) << ","
65 << static_cast< unsigned int >(pty * py) << "\", fontsize=\"11\"]";
66 }
67
68 private:
69 Name name;
70 LocMap loc;
71 float minx, maxx, miny, maxy;
72 unsigned int ptx, pty;
73 };
74
75 template < class WeightMap > class time_writer
76 {
77 public:
time_writer(WeightMap w)78 time_writer(WeightMap w) : wm(w) {}
operator ()(ostream & out,const Edge & e) const79 template < class Edge > void operator()(ostream& out, const Edge& e) const
80 {
81 out << "[label=\"" << wm[e] << "\", fontsize=\"11\"]";
82 }
83
84 private:
85 WeightMap wm;
86 };
87
88 // euclidean distance heuristic
89 template < class Graph, class CostType, class LocMap >
90 class distance_heuristic : public astar_heuristic< Graph, CostType >
91 {
92 public:
93 typedef typename graph_traits< Graph >::vertex_descriptor Vertex;
distance_heuristic(LocMap l,Vertex goal)94 distance_heuristic(LocMap l, Vertex goal) : m_location(l), m_goal(goal) {}
operator ()(Vertex u)95 CostType operator()(Vertex u)
96 {
97 float dx = m_location[m_goal].x - m_location[u].x;
98 float dy = m_location[m_goal].y - m_location[u].y;
99 return CostType(::sqrt(dx * dx + dy * dy));
100 }
101
102 private:
103 LocMap m_location;
104 Vertex m_goal;
105 };
106
107 struct found_goal
108 {
109 }; // exception for termination
110
111 // visitor that terminates when we find the goal
112 template < class Vertex >
113 class astar_goal_visitor : public boost::default_astar_visitor
114 {
115 public:
astar_goal_visitor(Vertex goal)116 astar_goal_visitor(Vertex goal) : m_goal(goal) {}
examine_vertex(Vertex u,Graph &)117 template < class Graph > void examine_vertex(Vertex u, Graph&)
118 {
119 if (u == m_goal)
120 throw found_goal();
121 }
122
123 private:
124 Vertex m_goal;
125 };
126
main(int,char **)127 int main(int, char**)
128 {
129
130 // specify some types
131 typedef adjacency_list< listS, vecS, undirectedS, no_property,
132 property< edge_weight_t, cost > >
133 mygraph_t;
134 typedef property_map< mygraph_t, edge_weight_t >::type WeightMap;
135 typedef mygraph_t::vertex_descriptor vertex;
136 typedef mygraph_t::edge_descriptor edge_descriptor;
137 typedef std::pair< int, int > edge;
138
139 // specify data
140 enum nodes
141 {
142 Troy,
143 LakePlacid,
144 Plattsburgh,
145 Massena,
146 Watertown,
147 Utica,
148 Syracuse,
149 Rochester,
150 Buffalo,
151 Ithaca,
152 Binghamton,
153 Woodstock,
154 NewYork,
155 N
156 };
157 const char* name[] = { "Troy", "Lake Placid", "Plattsburgh", "Massena",
158 "Watertown", "Utica", "Syracuse", "Rochester", "Buffalo", "Ithaca",
159 "Binghamton", "Woodstock", "New York" };
160 location locations[] = { // lat/long
161 { 42.73, 73.68 }, { 44.28, 73.99 }, { 44.70, 73.46 }, { 44.93, 74.89 },
162 { 43.97, 75.91 }, { 43.10, 75.23 }, { 43.04, 76.14 }, { 43.17, 77.61 },
163 { 42.89, 78.86 }, { 42.44, 76.50 }, { 42.10, 75.91 }, { 42.04, 74.11 },
164 { 40.67, 73.94 }
165 };
166 edge edge_array[]
167 = { edge(Troy, Utica), edge(Troy, LakePlacid), edge(Troy, Plattsburgh),
168 edge(LakePlacid, Plattsburgh), edge(Plattsburgh, Massena),
169 edge(LakePlacid, Massena), edge(Massena, Watertown),
170 edge(Watertown, Utica), edge(Watertown, Syracuse),
171 edge(Utica, Syracuse), edge(Syracuse, Rochester),
172 edge(Rochester, Buffalo), edge(Syracuse, Ithaca),
173 edge(Ithaca, Binghamton), edge(Ithaca, Rochester),
174 edge(Binghamton, Troy), edge(Binghamton, Woodstock),
175 edge(Binghamton, NewYork), edge(Syracuse, Binghamton),
176 edge(Woodstock, Troy), edge(Woodstock, NewYork) };
177 unsigned int num_edges = sizeof(edge_array) / sizeof(edge);
178 cost weights[] = { // estimated travel time (mins)
179 my_float(96), my_float(134), my_float(143), my_float(65), my_float(115),
180 my_float(133), my_float(117), my_float(116), my_float(74), my_float(56),
181 my_float(84), my_float(73), my_float(69), my_float(70), my_float(116),
182 my_float(147), my_float(173), my_float(183), my_float(74), my_float(71),
183 my_float(124)
184 };
185
186 // create graph
187 mygraph_t g(N);
188 WeightMap weightmap = get(edge_weight, g);
189 for (std::size_t j = 0; j < num_edges; ++j)
190 {
191 edge_descriptor e;
192 bool inserted;
193 boost::tie(e, inserted)
194 = add_edge(edge_array[j].first, edge_array[j].second, g);
195 weightmap[e] = weights[j];
196 }
197
198 // pick random start/goal
199 boost::minstd_rand gen(time(0));
200 vertex start = gen() % num_vertices(g);
201 vertex goal = gen() % num_vertices(g);
202
203 cout << "Start vertex: " << name[start] << endl;
204 cout << "Goal vertex: " << name[goal] << endl;
205
206 vector< mygraph_t::vertex_descriptor > p(num_vertices(g));
207 vector< cost > d(num_vertices(g));
208
209 boost::property_map< mygraph_t, boost::vertex_index_t >::const_type idx
210 = get(boost::vertex_index, g);
211
212 try
213 {
214 // call astar named parameter interface
215 astar_search(g, start,
216 distance_heuristic< mygraph_t, cost, location* >(locations, goal),
217 predecessor_map(make_iterator_property_map(p.begin(), idx))
218 .distance_map(make_iterator_property_map(d.begin(), idx))
219 .visitor(astar_goal_visitor< vertex >(goal))
220 .distance_inf(my_float((std::numeric_limits< float >::max)())));
221 }
222 catch (found_goal fg)
223 { // found a path to the goal
224 list< vertex > shortest_path;
225 for (vertex v = goal;; v = p[v])
226 {
227 shortest_path.push_front(v);
228 if (p[v] == v)
229 break;
230 }
231 cout << "Shortest path from " << name[start] << " to " << name[goal]
232 << ": ";
233 list< vertex >::iterator spi = shortest_path.begin();
234 cout << name[start];
235 for (++spi; spi != shortest_path.end(); ++spi)
236 cout << " -> " << name[*spi];
237 cout << endl << "Total travel time: " << d[goal] << endl;
238 return 0;
239 }
240
241 cout << "Didn't find a path from " << name[start] << "to" << name[goal]
242 << "!" << endl;
243 return 0;
244 }
245