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
24using namespace boost;
25using namespace std;
26
27// auxiliary types
28struct location
29{
30 float y, x; // lat, long
31};
32struct my_float
33{
34 float v;
35 explicit my_float(float v = float()) : v(v) {}
36};
37typedef my_float cost;
38ostream& operator<<(ostream& o, my_float f) { return o << f.v; }
39my_float operator+(my_float a, my_float b) { return my_float(a.v + b.v); }
40bool operator==(my_float a, my_float b) { return a.v == b.v; }
41bool operator<(my_float a, my_float b) { return a.v < b.v; }
42
43template < class Name, class LocMap > class city_writer
44{
45public:
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
68private:
69 Name name;
70 LocMap loc;
71 float minx, maxx, miny, maxy;
72 unsigned int ptx, pty;
73};
74
75template < class WeightMap > class time_writer
76{
77public:
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
84private:
85 WeightMap wm;
86};
87
88// euclidean distance heuristic
89template < class Graph, class CostType, class LocMap >
90class distance_heuristic : public astar_heuristic< Graph, CostType >
91{
92public:
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
102private:
103 LocMap m_location;
104 Vertex m_goal;
105};
106
107struct found_goal
108{
109}; // exception for termination
110
111// visitor that terminates when we find the goal
112template < class Vertex >
113class astar_goal_visitor : public boost::default_astar_visitor
114{
115public:
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
123private:
124 Vertex m_goal;
125};
126
127int 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

source code of boost/libs/graph/test/astar_search_test.cpp