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