123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224 |
- //
- //=======================================================================
- // 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;
-
- }
|