238 lines
7.1 KiB
C++
238 lines
7.1 KiB
C++
|
|
|
|
//
|
|
//=======================================================================
|
|
// 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;
|
|
}
|