forked from boostorg/graph
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathastar-cities.cpp
More file actions
236 lines (213 loc) · 7.03 KB
/
astar-cities.cpp
File metadata and controls
236 lines (213 loc) · 7.03 KB
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
225
226
227
228
229
230
231
232
233
234
235
236
//
//=======================================================================
// 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
};
using cost = float;
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:
using Vertex = typename graph_traits< Graph >::vertex_descriptor;
distance_heuristic(LocMap l, Vertex goal) : m_location(l), m_goal(goal) {}
CostType operator()(Vertex u)
{
auto dx = m_location[m_goal].x - m_location[u].x;
auto 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
using mygraph_t = adjacency_list< listS, vecS, undirectedS, no_property,
property< edge_weight_t, cost > >;
using WeightMap = property_map< mygraph_t, edge_weight_t >::type;
using vertex = mygraph_t::vertex_descriptor;
using edge_descriptor = mygraph_t::edge_descriptor;
using edge = std::pair< int, int >;
// 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);
auto 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(time(0));
auto start = random_vertex(g, gen);
auto 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 (auto v = goal;; v = p[v])
{
shortest_path.push_front(v);
if (p[v] == v)
break;
}
cout << "Shortest path from " << name[start] << " to " << name[goal]
<< ": ";
auto 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;
}