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