inline void
astar_dispatch2
(VertexListGraph& g,
typename graph_traits<VertexListGraph>::vertex_descriptor s,
AStarHeuristic h, CostMap cost, DistanceMap distance,
WeightMap weight, IndexMap index_map, ColorMap color,
const Params& params)
{
dummy_property_map p_map;
typedef typename property_traits<CostMap>::value_type C;
astar_search
(g, s, h,
choose_param(get_param(params, graph_visitor),
make_astar_visitor(null_visitor())),
choose_param(get_param(params, vertex_predecessor), p_map),
cost, distance, weight, index_map, color,
choose_param(get_param(params, distance_compare_t()),
std::less<C>()),
choose_param(get_param(params, distance_combine_t()),
closed_plus<C>()),
choose_param(get_param(params, distance_inf_t()),
std::numeric_limits<C>::max BOOST_PREVENT_MACRO_SUBSTITUTION ()),
choose_param(get_param(params, distance_zero_t()),
C()));
}
route road_net::get_route (road_net::node_descriptor start_node,
road_net::node_descriptor end_node) const
{
std::vector<road_net::node_descriptor> p (num_vertices(net_));
try
{
astar_search (net_, start_node,
heuristic(start_node),
visitor(goal_visitor(end_node)).
weight_map(speed_weight_map(net_)).
predecessor_map(&p[0])
);
}
catch (found_goal const& exc)
{
return p;
}
throw no_route_found();
}
// Solve the maze using A-star search. Return true if a solution was found.
bool maze::solve() {
boost::static_property_map<distance> weight(1);
// The predecessor map is a vertex-to-vertex mapping.
typedef boost::unordered_map<vertex_descriptor,
vertex_descriptor,
vertex_hash> pred_map;
pred_map predecessor;
boost::associative_property_map<pred_map> pred_pmap(predecessor);
// The distance map is a vertex-to-distance mapping.
typedef boost::unordered_map<vertex_descriptor,
distance,
vertex_hash> dist_map;
dist_map distance;
boost::associative_property_map<dist_map> dist_pmap(distance);
vertex_descriptor s = source();
vertex_descriptor g = goal();
euclidean_heuristic heuristic(g);
astar_goal_visitor visitor(g);
try {
astar_search(m_barrier_grid, s, heuristic,
boost::weight_map(weight).
predecessor_map(pred_pmap).
distance_map(dist_pmap).
visitor(visitor) );
} catch(found_goal fg) {
// Walk backwards from the goal through the predecessor chain adding
// vertices to the solution path.
for (vertex_descriptor u = g; u != s; u = predecessor[u])
m_solution.insert(u);
m_solution.insert(s);
m_solution_length = distance[g];
return true;
}
return false;
}
bool TMap::findPath( int from, int to )
{
if( mMapGraphNeedsUpdate )
{
initGraph();
}
//vertex start = from;//mRoomId;
//vertex goal = to;//mTargetID;
TRoom * pFrom = mpRoomDB->getRoom( from );
TRoom * pTo = mpRoomDB->getRoom( to );
if( !pFrom || !pTo )
{
return false;
}
vertex start = roomidToIndex[from];
vertex goal = roomidToIndex[to];
vector<mygraph_t::vertex_descriptor> p(num_vertices(g));
vector<cost> d(num_vertices(g));
QTime t;
t.start();
try
{
astar_search( g,
start,
distance_heuristic<mygraph_t, cost, std::vector<location> >(locations, goal),
predecessor_map(&p[0]).distance_map(&d[0]).
visitor(astar_goal_visitor<vertex>(goal)) );
}
catch( found_goal fg )
{
qDebug("TMap::findPath(%i,%i) time elapsed in astar:%imSec", from, to, t.elapsed() );
t.restart();
list<vertex> shortest_path;
for(vertex v = goal; ; v = p[v])
{
//cout << "assembling path: v="<<v<<endl;
qDebug("TMap::findPath(...) assembling path: v=%i", v);
int nextRoom = indexToRoomid[v];
if( ! mpRoomDB->getRoom( nextRoom ) )
{
qDebug("TMap::findPath(%i,%i) ERROR path assembly: path room not in map!", from, to);
return false;
}
shortest_path.push_front(nextRoom);
if(p[v] == v)
break;
}
TRoom * pRD1 = mpRoomDB->getRoom(from);
TRoom * pRD2 = mpRoomDB->getRoom(to);
if( !pRD1 || !pRD2 )
return false;
qDebug("Shortest path from %i to %i:", pRD1->getId(), pRD2->getId());
list<vertex>::iterator spi = shortest_path.begin();
qDebug() << pRD1->getId();
mPathList.clear();
mDirList.clear();
int curRoom = from;
for( ++spi; spi != shortest_path.end(); ++spi )
{
TRoom * pRcurRoom = mpRoomDB->getRoom( curRoom );
TRoom * pRPath = mpRoomDB->getRoom( *spi );
if( !pRcurRoom || !pRPath )
{
// cout << "ERROR: path not possible. curRoom not in map!" << endl;
qDebug("TMap::findPath(%i,%i) ERROR path not possible. curRoom not in map!", from, to);
mPathList.clear();
mDirList.clear();
return false;
}
// cout <<" spi:"<<*spi<<" curRoom:"<< curRoom << endl;//" -> ";
qDebug(" spi:%i curRoom:%i", *spi, curRoom);
mPathList.push_back( *spi );
if( pRcurRoom->getNorth() == pRPath->getId() )
{
mDirList.push_back("n");
}
else if( pRcurRoom->getNortheast() == pRPath->getId() )
{
mDirList.push_back("ne");
}
else if( pRcurRoom->getNorthwest() == pRPath->getId() )
{
mDirList.push_back("nw");
}
else if( pRcurRoom->getSoutheast() == pRPath->getId() )
{
mDirList.push_back("se");
}
else if( pRcurRoom->getSouthwest() == pRPath->getId() )
{
mDirList.push_back("sw");
}
else if( pRcurRoom->getSouth() == pRPath->getId() )
{
mDirList.push_back("s");
}
else if( pRcurRoom->getEast() == pRPath->getId() )
{
mDirList.push_back("e");
}
else if( pRcurRoom->getWest() == pRPath->getId() )
{
mDirList.push_back("w");
}
else if( pRcurRoom->getUp() == pRPath->getId() )
{
mDirList.push_back("up");
}
else if( pRcurRoom->getDown() == pRPath->getId() )
{
mDirList.push_back("down");
}
else if( pRcurRoom->getIn() == pRPath->getId() )
{
mDirList.push_back("in");
}
else if( pRcurRoom->getOut() == pRPath->getId() )
{
mDirList.push_back("out");
}
else if( pRcurRoom->getOtherMap().size() > 0 )
{
QMapIterator<int, QString> it( pRcurRoom->getOtherMap() );
while( it.hasNext() )
{
it.next();
if( it.key() == pRPath->getId() )
{
QString _cmd = it.value();
if( _cmd.size() > 0 )
{
if(_cmd.startsWith('0'))
{
_cmd = _cmd.mid(1);
mDirList.push_back( _cmd );
qDebug(" adding special exit: roomID:%i OPEN special exit:%s", pRcurRoom->getId(), qPrintable(_cmd) );
}
else if( _cmd.startsWith('1'))
{
_cmd = _cmd.mid(1);
qDebug(" NOT adding roomID:%i LOCKED special exit:%s", pRcurRoom->getId(), qPrintable(_cmd));
}
else
{
qWarning("ERROR adding roomID:%i UNPATCHED special exit found:%s", pRcurRoom->getId(), qPrintable(_cmd));
}
}
else
qWarning("ERROR adding roomID:%i NULL command special exit found.", pRcurRoom->getId());
}
}
}
qDebug(" added to DirList:%s", qPrintable( mDirList.back() ) );
curRoom = *spi;
}
qDebug("TMap::findPath(%i,%i) time elapsed building path:%imSec", from, to, t.elapsed() );
return true;
}
return false;
}
bool TMap::findPath( int from, int to )
{
if( mMapGraphNeedsUpdate )
{
initGraph();
}
vertex start = from;//mRoomId;
vertex goal = to;//mTargetID;
if( ! rooms.contains( start ) || ! rooms.contains( goal ) )
{
return false;
}
vector<mygraph_t::vertex_descriptor> p(num_vertices(g));
vector<cost> d(num_vertices(g));
try
{
astar_search( g,
start,
distance_heuristic<mygraph_t, cost, std::vector<location> >(locations, goal),
predecessor_map(&p[0]).distance_map(&d[0]).
visitor(astar_goal_visitor<vertex>(goal)) );
}
catch( found_goal fg )
{
list<vertex> shortest_path;
for(vertex v = goal; ; v = p[v])
{
cout << "assembling path: v="<<v<<endl;
if( ! rooms.contains( v ) )
{
cout<<"ERROR path assembly: path room not in map!"<<endl;
return false;
}
shortest_path.push_front(v);
if(p[v] == v) break;
}
cout << "Shortest path from " << rooms[start]->id << " to "
<< rooms[goal]->id << ": ";
list<vertex>::iterator spi = shortest_path.begin();
cout << rooms[start]->id;
mPathList.clear();
mDirList.clear();
int curRoom = start;
for( ++spi; spi != shortest_path.end(); ++spi )
{
if( ! rooms.contains( curRoom ) )
{
cout << "ERROR: path not possible. curRoom not in map!" << endl;
return false;
}
mPathList.push_back( *spi );
if( rooms[curRoom]->north == rooms[*spi]->id )
{
mDirList.push_back("n");
}
else if( rooms[curRoom]->northeast == rooms[*spi]->id )
{
mDirList.push_back("ne");
}
else if( rooms[curRoom]->northwest == rooms[*spi]->id )
{
mDirList.push_back("nw");
}
else if( rooms[curRoom]->southeast == rooms[*spi]->id )
{
mDirList.push_back("se");
}
else if( rooms[curRoom]->southwest == rooms[*spi]->id )
{
mDirList.push_back("sw");
}
else if( rooms[curRoom]->south == rooms[*spi]->id )
{
mDirList.push_back("s");
}
else if( rooms[curRoom]->east == rooms[*spi]->id )
{
mDirList.push_back("e");
}
else if( rooms[curRoom]->west == rooms[*spi]->id )
{
mDirList.push_back("w");
}
else if( rooms[curRoom]->up == rooms[*spi]->id )
{
mDirList.push_back("up");
}
else if( rooms[curRoom]->down == rooms[*spi]->id )
{
mDirList.push_back("down");
}
else if( rooms[curRoom]->in == rooms[*spi]->id )
{
mDirList.push_back("in");
}
else if( rooms[curRoom]->out == rooms[*spi]->id )
{
mDirList.push_back("out");
}
else if( rooms[curRoom]->other.size() > 0 )
{
QMapIterator<int, QString> it( rooms[curRoom]->other );
while( it.hasNext() )
{
it.next();
if( it.key() == rooms[*spi]->id )
{
mDirList.push_back( it.value() );
}
}
}
curRoom = *spi;
}
return true;
}
return false;
}
int CPathProductor::AStarFind(void)
{
m_arrayPath.clear();
size_t num_edges =m_arrayEdge.size();
if (num_edges <= 0)
{
return 0;
}
size_t szCount = m_arrayBarCode.size() + 1;
// create graph
mygraph_t g(szCount);
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(m_arrayEdge[j].first,
m_arrayEdge[j].second, g);
weightmap[e] = m_arrayWeights[j];
}
lane_vertex start = -1;
lane_vertex goal = -1;
// from will be the first element
int nIndexFrom = 0;
for (auto it = m_arrayBarCode.cbegin();
it != m_arrayBarCode.cend(); ++it, ++nIndexFrom)
{
if (*it == m_nFrom)
{
start = nIndexFrom;
break;
}
}
// to will be the last element
int nIndexTo = m_arrayBarCode.size();
for (auto it = m_arrayBarCode.crbegin();
it != m_arrayBarCode.crend(); ++it, --nIndexTo)
{
if (*it == m_nTo)
{
--nIndexTo;
goal = nIndexTo;
break;
}
}
if (start < 0 || goal < 0)
{
cout<< "Can not find Vetex for path." << endl;
return 0;
}
//cout << "Start vertex: " << m_arrayBarCode[start] << endl;
//cout << "Goal vertex: " << m_arrayBarCode[goal] << endl;
vector<mygraph_t::vertex_descriptor> p(num_vertices(g));
vector<cost> d(num_vertices(g));
try
{
// call astar named parameter interface
astar_search
(g, start,
distance_heuristic<mygraph_t, cost, vec_location >
(m_arrayLocation, goal),
predecessor_map(&p[0]).distance_map(&d[0]).
visitor(astar_goal_visitor<lane_vertex>(goal)));
}
catch(found_goal /*fg*/)
{
// found a path to the goal
list<lane_vertex> shortest_path;
for(auto v = goal;; v = p[v])
{
shortest_path.push_front(v);
if(p[v] == v)
break;
}
for(auto spi = shortest_path.begin(); spi != shortest_path.end(); ++spi)
{
int nBarCode = m_arrayBarCode[*spi];
m_arrayPath.push_back(nBarCode);
}
int nTotalTravel = (int)d[goal];
cout << endl << "Total travel time: " << nTotalTravel << endl;
return nTotalTravel;
}
return 0;
}
