]>
Commit | Line | Data |
---|---|---|
7c673cae FG |
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 | ||
7c673cae FG |
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> | |
f67539c2 | 21 | #include <math.h> // for sqrt |
7c673cae FG |
22 | #include <time.h> |
23 | ||
24 | using namespace boost; | |
25 | using namespace std; | |
26 | ||
7c673cae FG |
27 | // auxiliary types |
28 | struct location | |
29 | { | |
f67539c2 TL |
30 | float y, x; // lat, long |
31 | }; | |
32 | struct my_float | |
33 | { | |
34 | float v; | |
35 | explicit my_float(float v = float()) : v(v) {} | |
7c673cae | 36 | }; |
7c673cae | 37 | typedef my_float cost; |
f67539c2 TL |
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; } | |
7c673cae | 42 | |
f67539c2 TL |
43 | template < class Name, class LocMap > class city_writer |
44 | { | |
7c673cae | 45 | public: |
f67539c2 TL |
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 | ||
7c673cae | 68 | private: |
f67539c2 TL |
69 | Name name; |
70 | LocMap loc; | |
71 | float minx, maxx, miny, maxy; | |
72 | unsigned int ptx, pty; | |
7c673cae FG |
73 | }; |
74 | ||
f67539c2 TL |
75 | template < class WeightMap > class time_writer |
76 | { | |
7c673cae | 77 | public: |
f67539c2 TL |
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 | ||
7c673cae | 84 | private: |
f67539c2 | 85 | WeightMap wm; |
7c673cae FG |
86 | }; |
87 | ||
7c673cae | 88 | // euclidean distance heuristic |
f67539c2 TL |
89 | template < class Graph, class CostType, class LocMap > |
90 | class distance_heuristic : public astar_heuristic< Graph, CostType > | |
7c673cae FG |
91 | { |
92 | public: | |
f67539c2 TL |
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(dx * dx + dy * dy)); | |
100 | } | |
101 | ||
7c673cae | 102 | private: |
f67539c2 TL |
103 | LocMap m_location; |
104 | Vertex m_goal; | |
7c673cae FG |
105 | }; |
106 | ||
f67539c2 TL |
107 | struct found_goal |
108 | { | |
109 | }; // exception for termination | |
7c673cae FG |
110 | |
111 | // visitor that terminates when we find the goal | |
f67539c2 | 112 | template < class Vertex > |
7c673cae FG |
113 | class astar_goal_visitor : public boost::default_astar_visitor |
114 | { | |
115 | public: | |
f67539c2 TL |
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 | ||
7c673cae | 123 | private: |
f67539c2 | 124 | Vertex m_goal; |
7c673cae FG |
125 | }; |
126 | ||
f67539c2 | 127 | int main(int, char**) |
7c673cae | 128 | { |
f67539c2 TL |
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; | |
7c673cae | 239 | } |
f67539c2 TL |
240 | |
241 | cout << "Didn't find a path from " << name[start] << "to" << name[goal] | |
242 | << "!" << endl; | |
7c673cae | 243 | return 0; |
7c673cae | 244 | } |