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; |
35 | explicit my_float(float v = float()) : v(v) {} |
36 | }; |
37 | typedef my_float cost; |
38 | ostream& operator<<(ostream& o, my_float f) { return o << f.v; } |
39 | my_float operator+(my_float a, my_float b) { return my_float(a.v + b.v); } |
40 | bool operator==(my_float a, my_float b) { return a.v == b.v; } |
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: |
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 > |
59 | 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: |
78 | time_writer(WeightMap w) : wm(w) {} |
79 | 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; |
94 | distance_heuristic(LocMap l, Vertex goal) : m_location(l), m_goal(goal) {} |
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(x: 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: |
116 | astar_goal_visitor(Vertex goal) : m_goal(goal) {} |
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 | |
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 | { .y: 42.73, .x: 73.68 }, { .y: 44.28, .x: 73.99 }, { .y: 44.70, .x: 73.46 }, { .y: 44.93, .x: 74.89 }, |
162 | { .y: 43.97, .x: 75.91 }, { .y: 43.10, .x: 75.23 }, { .y: 43.04, .x: 76.14 }, { .y: 43.17, .x: 77.61 }, |
163 | { .y: 42.89, .x: 78.86 }, { .y: 42.44, .x: 76.50 }, { .y: 42.10, .x: 75.91 }, { .y: 42.04, .x: 74.11 }, |
164 | { .y: 40.67, .x: 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(p: 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(t0&: e, t1&: inserted) |
194 | = add_edge(u: edge_array[j].first, v: edge_array[j].second, g_&: g); |
195 | weightmap[e] = weights[j]; |
196 | } |
197 | |
198 | // pick random start/goal |
199 | boost::minstd_rand gen(time(timer: 0)); |
200 | vertex start = gen() % num_vertices(g_: g); |
201 | vertex goal = gen() % num_vertices(g_: 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_: g)); |
207 | vector< cost > d(num_vertices(g_: g)); |
208 | |
209 | boost::property_map< mygraph_t, boost::vertex_index_t >::const_type idx |
210 | = get(p: boost::vertex_index, g); |
211 | |
212 | try |
213 | { |
214 | // call astar named parameter interface |
215 | astar_search(g, s: start, |
216 | h: distance_heuristic< mygraph_t, cost, location* >(locations, goal), |
217 | params: predecessor_map(p: make_iterator_property_map(iter: p.begin(), id: idx)) |
218 | .distance_map(p: make_iterator_property_map(iter: d.begin(), id: idx)) |
219 | .visitor(p: astar_goal_visitor< vertex >(goal)) |
220 | .distance_inf(p: 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(x: 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 | |