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