QList<unsigned> ShortestPathComputer::getShortestPathVertices(const unsigned startVertexIndex, const unsigned endVertexIndex, float *pathLength/* = NULL*/)
{
QList<unsigned> shortestPathVertices;
VertexDescriptor startVertex = vertex(startVertexIndex, mGraph);
std::vector<VertexDescriptor> predecessorMap(num_vertices(mGraph));
std::vector<float> distanceMap(num_vertices(mGraph));
dijkstra_shortest_paths(mGraph, startVertex,
predecessor_map(boost::make_iterator_property_map(predecessorMap.begin(), get(boost::vertex_index, mGraph))).
distance_map(boost::make_iterator_property_map(distanceMap.begin(), get(boost::vertex_index, mGraph))));
if (pathLength)
{
*pathLength = distanceMap.at(endVertexIndex);
}
unsigned vertexIndex1 = endVertexIndex;
unsigned vertexIndex2 = predecessorMap.at(vertexIndex1);
while (vertexIndex2 != vertexIndex1)
{
shortestPathVertices.push_front(vertexIndex2);
vertexIndex1 = vertexIndex2;
vertexIndex2 = predecessorMap.at(vertexIndex1);
}
shortestPathVertices.pop_front(); //去掉起点
return shortestPathVertices;
}
int main(int, char *[])
{
typedef adjacency_list < listS, vecS, directedS,
no_property, property < edge_weight_t, int > > graph_t;
typedef graph_traits < graph_t >::vertex_descriptor vertex_descriptor;
typedef std::pair<int, int> Edge;
const int num_nodes = 5;
enum nodes { A, B, C, D, E };
char name[] = "ABCDE";
Edge edge_array[] = { Edge(A, C), Edge(B, B), Edge(B, D), Edge(B, E),
Edge(C, B), Edge(C, D), Edge(D, E), Edge(E, A), Edge(E, B)
};
int weights[] = { 1, 2, 1, 2, 7, 3, 1, 1, 1 };
int num_arcs = sizeof(edge_array) / sizeof(Edge);
graph_t g(edge_array, edge_array + num_arcs, weights, num_nodes);
property_map<graph_t, edge_weight_t>::type weightmap = get(edge_weight, g);
std::vector<vertex_descriptor> p(num_vertices(g));
std::vector<int> d(num_vertices(g));
vertex_descriptor s = vertex(A, g);
dijkstra_shortest_paths(g, s,
predecessor_map(boost::make_iterator_property_map(p.begin(), get(boost::vertex_index, g))).
distance_map(boost::make_iterator_property_map(d.begin(), get(boost::vertex_index, g))));
std::cout << "distances and parents:" << std::endl;
graph_traits < graph_t >::vertex_iterator vi, vend;
for (boost::tie(vi, vend) = vertices(g); vi != vend; ++vi) {
std::cout << "distance(" << name[*vi] << ") = " << d[*vi] << ", ";
std::cout << "parent(" << name[*vi] << ") = " << name[p[*vi]] << std::
endl;
}
std::cout << std::endl;
std::ofstream dot_file("results/dijkstra-eg.dot");
dot_file << "digraph D {\n"
<< " rankdir=LR\n"
<< " size=\"4,3\"\n"
<< " ratio=\"fill\"\n"
<< " edge[style=\"bold\"]\n" << " node[shape=\"circle\"]\n";
graph_traits < graph_t >::edge_iterator ei, ei_end;
for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) {
graph_traits < graph_t >::edge_descriptor e = *ei;
graph_traits < graph_t >::vertex_descriptor
u = source(e, g), v = target(e, g);
dot_file << name[u] << " -> " << name[v]
<< "[label=\"" << get(weightmap, e) << "\"";
if (p[v] == u)
dot_file << ", color=\"black\"";
else
dot_file << ", color=\"grey\"";
dot_file << "]";
}
dot_file << "}";
system("pause");
return EXIT_SUCCESS;
}
inline void prim_minimum_spanning_tree
(const VertexListGraph& g, PredecessorMap p_map)
{
detail::prim_mst_impl
(g, *vertices(g).first, predecessor_map(p_map).
weight_map(get(edge_weight, g)),
get(edge_weight, g));
}
// get the shortest path for a given starting client id and a given set of client id
void DijkstraShortPath::getShortPathClientIDSet(const ClientID &start_cid, const set<ClientID> &end_cid_set, ClientID &sel_end_cid,
DistanceType &shortest_distance, vector<ClientID> &shortest_cid_vec)
{
vector<vertex_descriptor> p(num_vertices(g));
vector<DistanceType> d(num_vertices(g));
dijkstra_shortest_paths(g, vertex_map[start_cid],
predecessor_map(boost::make_iterator_property_map(p.begin(), get(boost::vertex_index, g))).
distance_map(boost::make_iterator_property_map(d.begin(), get(boost::vertex_index, g))));
//std::cout << "distances and partents for one client id and a vector of client id: " << std::endl;
shortest_distance = DBL_MAX;
graph_traits<graph_t>::vertex_iterator vi, vend;
for (boost::tie(vi, vend) = vertices(g); vi != vend; vi++)
{
//std::cout << "distance(" << cid_map[*vi] << ")=" << d[*vi] << ",";
//std::cout << "parent(" << cid_map[*vi] << ")=" << cid_map[p[*vi]] << std::endl;
if (end_cid_set.count(cid_map[*vi])==1&&
d[*vi] < shortest_distance)
{
shortest_distance = d[*vi];
sel_end_cid = cid_map[*vi];
}
}
shortest_cid_vec.clear();
//cout << start_cid << " " << vertex_map[start_cid] << ", " << sel_end_cid << " " << vertex_map[sel_end_cid] << endl;
vertex_descriptor vd;
for (vd = vertex_map[sel_end_cid]; vd != vertex_map[start_cid]; vd = p[vd])
{
//cout << cid_map[vd] << " ";
shortest_cid_vec.push_back(cid_map[vd]);
}
//cout << cid_map[vd] << endl;
shortest_cid_vec.push_back(cid_map[vd]);
reverse(shortest_cid_vec.begin(), shortest_cid_vec.end());
/*for (vector<ClientID>::iterator iter = shortest_cid_vec.begin();
iter != shortest_cid_vec.end(); iter++)
cout << *iter << ",";*/
}
int main() {
/*
#Graph
The following class hierarchy exists:
BidirectionalGraph -------- Incience ---------+
|
Adjacency --------+
|
VertexAndEdgeList ----+---- VertexList -------+---- Graph
| |
+---- EdgeList ---------+
|
AdjacenyMatrix ---+
*/
{
/*
#properties
Properties are values associated to edges and vertices.
*/
{
/*
There are a few predefined properties which you should use whenever possible
as they are already used in many algorithms, but you can also define your own properties.
Predefined properties include:
- `edge_weight_t`. Used for most algorithms that have a single value associated to each
edge such as Dijikstra.
- `vertex_name_t`
*/
{
typedef boost::property<boost::vertex_name_t, std::string> VertexProperties;
typedef boost::property<boost::edge_weight_t, int> EdgeProperties;
}
/*
Multiple properties can be specified either by:
- using a custom class as the property type. TODO is there any limitation to this?
- chaining multile properties
*/
{
}
/*
The absense of a property is speficied by boost::no_property.
*/
{
typedef boost::no_property VertexProperties;
}
}
typedef boost::property<boost::vertex_name_t, std::string> VertexProperties;
typedef boost::property<boost::edge_weight_t, int> EdgeProperties;
typedef boost::adjacency_list<
// Data structure to represent the out edges for each vertex.
// Possibilities:
//
// #vecS selects std::vector.
// #listS selects std::list.
// #slistS selects std::slist.
// #setS selects std::set.
// #multisetS selects std::multiset.
// #hash_setS selects std::hash_set.
//
// `S` standas for Selector.
boost::vecS,
// Data structure to represent the vertex set.
boost::vecS,
// Directed type.
// #bidirectionalS: directed graph with access to in and out edges
// #directedS: directed graph with access only to out-edges
// #undirectedS: undirected graph
boost::bidirectionalS,
// Optional.
VertexProperties,
// Optional.
EdgeProperties
> Graph;
//typedef boost::graph_traits<Graph>::vertex_iterator VertexIter;
//typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
//typedef boost::property_map<Graph, boost::vertex_index_t>::type IndexMap;
// Fix number of vertices, and add one edge at a time.
int num_vertices = 3;
Graph g(num_vertices);
boost::add_edge(0, 1, g);
boost::add_edge(1, 2, g);
// Fix number of vertices, and add one edge array.
{
int num_vertices = 3;
typedef std::pair<int, int> Edge;
std::vector<Edge> edges{
{0, 1},
{1, 2},
};
Graph g(edges.data(), edges.data() + edges.size(), num_vertices);
}
// It is also possible to add vertices with #add_vertex.
//#vertices
{
// Number of vertices.
boost::graph_traits<Graph>::vertices_size_type num_vertices = boost::num_vertices(g);
assert(num_vertices == 3u);
//#vertices() Returns a begin() end() vertex iterator pair so we know where to stop.
{
typedef std::vector<boost::graph_traits<Graph>::vertex_descriptor> Vertices;
Vertices vertices;
vertices.reserve(num_vertices);
//IndexMap
auto index = boost::get(boost::vertex_index, g);
//std::pair<vertex_iter, vertex_iter> vp
for (auto vp = boost::vertices(g); vp.first != vp.second; ++vp.first) {
// Vertex
auto v = *vp.first;
vertices.push_back(index[v]);
}
assert((vertices == Vertices{0, 1, 2}));
}
// The iterator is a ranom access iterator.
{
auto index = boost::get(boost::vertex_index, g);
auto it = boost::vertices(g).first;
assert(index[it[2]] == 2);
assert(index[it[1]] == 1);
}
}
//#edges
{
// It seems that only AdjencyMatrix has a method to get an edge given two vertices:
//edge(u, v, g)
}
}
//#source is also a global function: <http://stackoverflow.com/questions/16114616/why-is-boost-graph-librarys-source-a-global-function>
//#dijikstra
std::cout << "#dijkstra" << std::endl;
{
typedef boost::adjacency_list<
boost::listS,
boost::vecS,
boost::directedS,
boost::no_property,
boost::property<boost::edge_weight_t, int>
> Graph;
typedef boost::graph_traits<Graph>::vertex_descriptor vertex_descriptor;
typedef boost::graph_traits<Graph>::edge_descriptor edge_descriptor;
typedef std::pair<int, int> Edge;
// Model inputs.
const int num_nodes = 5;
const int sorce = 0;
std::vector<Edge> edges{
{0, 2}, {1, 1}, {1, 3}, {1, 4}, {2, 1},
{2, 3}, {3, 4}, {4, 0}, {4, 1}
};
std::vector<int> weights{
1, 2, 1, 2, 7,
3, 1, 1, 1
};
// Solve.
Graph g(edges.data(), edges.data() + edges.size(), weights.data(), num_nodes);
std::vector<vertex_descriptor> p(num_vertices(g));
std::vector<int> d(num_vertices(g));
vertex_descriptor s = vertex(sorce, g);
dijkstra_shortest_paths(g, s,
predecessor_map(boost::make_iterator_property_map(
p.begin(),
boost::get(boost::vertex_index, g)
)).distance_map(boost::make_iterator_property_map(
d.begin(),
boost::get(boost::vertex_index, g)
))
);
// Print solution to stdout.
std::cout << "node | distance from source | parent" << std::endl;
boost::graph_traits<Graph>::vertex_iterator vi, vend;
for (boost::tie(vi, vend) = vertices(g); vi != vend; ++vi)
std::cout << *vi << " " << d[*vi] << " " << p[*vi] << std::endl;
std::cout <<std::endl;
// Generate a .dot graph file with shortest path highlighted.
// To PNG with: dot -Tpng -o outfile.png input.dot
boost::property_map<Graph, boost::edge_weight_t>::type weightmap = boost::get(boost::edge_weight, g);
std::ofstream dot_file("dijkstra.dot");
dot_file << "digraph D {\n" << " rankdir=LR\n" << " size=\"4,3\"\n"
<< " ratio=\"fill\"\n" << " edge[style=\"bold\"]\n" << " node[shape=\"circle\"]\n";
boost::graph_traits <Graph>::edge_iterator ei, ei_end;
for (std::tie(ei, ei_end) = boost::edges(g); ei != ei_end; ++ei) {
edge_descriptor e = *ei;
boost::graph_traits<Graph>::vertex_descriptor
u = boost::source(e, g), v = boost::target(e, g);
dot_file << u << " -> " << v << "[label=\"" << boost::get(weightmap, e) << "\"";
if (p[v] == u)
dot_file << ", color=\"black\"";
else
dot_file << ", color=\"grey\"";
dot_file << "]";
}
dot_file << "}";
// Construct forward path to a destination.
int dest = 4;
int cur = dest;
std::vector<int> path;
path.push_back(cur);
while(cur != sorce) {
cur = p[cur];
path.push_back(cur);
}
std::reverse(path.begin(), path.end());
// Print.
std::cout << "Path to node " << std::to_string(dest) << ":" << std::endl;
for(auto& node : path) {
std::cout << node << std::endl;
}
}
}
template <typename PointT> void
pcl::registration::ELCH<PointT>::loopOptimizerAlgorithm (LOAGraph &g, double *weights)
{
std::list<int> crossings, branches;
crossings.push_back (static_cast<int> (loop_start_));
crossings.push_back (static_cast<int> (loop_end_));
weights[loop_start_] = 0;
weights[loop_end_] = 1;
int *p = new int[num_vertices (g)];
int *p_min = new int[num_vertices (g)];
double *d = new double[num_vertices (g)];
double *d_min = new double[num_vertices (g)];
double dist;
bool do_swap = false;
std::list<int>::iterator crossings_it, end_it, start_min, end_min;
// process all junctions
while (!crossings.empty ())
{
dist = -1;
// find shortest crossing for all vertices on the loop
for (crossings_it = crossings.begin (); crossings_it != crossings.end (); )
{
dijkstra_shortest_paths (g, *crossings_it,
predecessor_map(boost::make_iterator_property_map(p, get(boost::vertex_index, g))).
distance_map(boost::make_iterator_property_map(d, get(boost::vertex_index, g))));
end_it = crossings_it;
end_it++;
// find shortest crossing for one vertex
for (; end_it != crossings.end (); end_it++)
{
if (*end_it != p[*end_it] && (dist < 0 || d[*end_it] < dist))
{
dist = d[*end_it];
start_min = crossings_it;
end_min = end_it;
do_swap = true;
}
}
if (do_swap)
{
std::swap (p, p_min);
std::swap (d, d_min);
do_swap = false;
}
// vertex starts a branch
if (dist < 0)
{
branches.push_back (static_cast<int> (*crossings_it));
crossings_it = crossings.erase (crossings_it);
}
else
crossings_it++;
}
if (dist > -1)
{
remove_edge (*end_min, p_min[*end_min], g);
for (int i = p_min[*end_min]; i != *start_min; i = p_min[i])
{
//even right with weights[*start_min] > weights[*end_min]! (math works)
weights[i] = weights[*start_min] + (weights[*end_min] - weights[*start_min]) * d_min[i] / d_min[*end_min];
remove_edge (i, p_min[i], g);
if (degree (i, g) > 0)
{
crossings.push_back (i);
}
}
if (degree (*start_min, g) == 0)
crossings.erase (start_min);
if (degree (*end_min, g) == 0)
crossings.erase (end_min);
}
}
delete[] p;
delete[] p_min;
delete[] d;
delete[] d_min;
boost::graph_traits<LOAGraph>::adjacency_iterator adjacent_it, adjacent_it_end;
int s;
// error propagation
while (!branches.empty ())
{
s = branches.front ();
branches.pop_front ();
for (boost::tuples::tie (adjacent_it, adjacent_it_end) = adjacent_vertices (s, g); adjacent_it != adjacent_it_end; ++adjacent_it)
{
weights[*adjacent_it] = weights[s];
if (degree (*adjacent_it, g) > 1)
branches.push_back (static_cast<int> (*adjacent_it));
}
clear_vertex (s, g);
}
}
bool myGraph::testShortestPath()
{
//shortest path from a start point
typedef boost::adjacency_list < boost::listS, boost::vecS, boost::directedS, boost::no_property, boost::property < boost::edge_weight_t, int > > graph_t;
typedef boost::graph_traits < graph_t >::vertex_descriptor vertex_descriptor;
typedef std::pair<int, int> Edge;
const int num_nodes = 5;
enum nodes { A, B, C, D, E };
char name[] = "ABCDE";
Edge edge_array[] = { Edge(A, C), Edge(B, B), Edge(B, E),
Edge(C, D), Edge(B, D),Edge(C, B),Edge(D, E), Edge(E, A), Edge(E, B)
};
//Edge(B, D),Edge(C, B),Edge(D, E), Edge(E, A), Edge(E, B)
int weights[] = { 1, 1, 1, 1, 1, 1, 1, 1, 1 };
int num_arcs = sizeof(edge_array) / sizeof(Edge);
graph_t g(edge_array, edge_array + num_arcs, weights, num_nodes);
boost:: property_map<graph_t, boost::edge_weight_t>::type weightmap = get( boost::edge_weight, g);
std::vector<vertex_descriptor> p(num_vertices(g));
std::vector<int> d(num_vertices(g));
vertex_descriptor s = vertex(A, g);
std::ofstream dot_file;//("figs/dijkstra-eg.dot");
dot_file.open ("dijkstra-eg.txt");
dijkstra_shortest_paths(g, s,
predecessor_map(boost::make_iterator_property_map(p.begin(), get(boost::vertex_index, g))).
distance_map(boost::make_iterator_property_map(d.begin(), get(boost::vertex_index, g))));
dot_file << "distances and parents:" << std::endl;
boost::graph_traits < graph_t >::vertex_iterator vi, vend;
for (boost::tie(vi, vend) = vertices(g); vi != vend; ++vi) {
dot_file << "distance(" << name[*vi] << ") = " << d[*vi] << ", ";
dot_file << "parent(" << name[*vi] << ") = " << name[p[*vi]] << std::
endl;
}
//std::cout << std::endl;
dot_file << "digraph D {\n"
<< " rankdir=LR\n"
<< " size=\"4,3\"\n"
<< " ratio=\"fill\"\n"
<< " edge[style=\"bold\"]\n" << " node[shape=\"circle\"]\n";
boost::graph_traits < graph_t >::edge_iterator ei, ei_end;
for (boost::tie(ei, ei_end) = boost::edges(g); ei != ei_end; ++ei) {
boost::graph_traits < graph_t >::edge_descriptor e = *ei;
boost::graph_traits < graph_t >::vertex_descriptor
u = source(e, g), v = target(e, g);
dot_file << name[u] << " -> " << name[v]
<< "[label=\"" << get(weightmap, e) << "\"";
if (p[v] == u)
dot_file << ", color=\"black\"";
else
dot_file << ", color=\"grey\"";
dot_file << "]";
}
dot_file << "}";
dot_file.close();
return true;
}
vector<vector<vector<int>>> myGraph::getShortestPath(int start, int &length, vector<vector<int>> &edge)
{
vector<vector<int>> path,path_b;
vector<vector<vector<int>>> pathes;
//shortest path from a start point
//typedef boost::adjacency_list < boost::listS, boost::vecS, boost::directedS, boost::no_property, boost::property < boost::edge_weight_t, int > > graph_t;
//typedef boost::graph_traits < graph_t >::vertex_descriptor vertex_descriptor;
//typedef std::pair<int, int> Edge;
typedef boost::property<boost::edge_weight_t, int> EdgeWeightProperty;
typedef boost::adjacency_list<boost::listS,boost:: vecS, boost::directedS, boost::no_property,
EdgeWeightProperty > Graph;
typedef boost::graph_traits < Graph >::vertex_descriptor vertex_descriptor;
Graph G;
//add_edge(0, 1, 18, G);
/*const int num_nodes = 5;//nodeSet.size(); //???
enum nodes { A, B, C, D, E };
char name[] = "ABCDE";
Edge edge_array[] = { Edge(A, C), Edge(B, B), Edge(B, E)};*/
for(int i=0; i<edge.size(); i++)
{
//edge_array.push_back
//add_edge(edge[*it][0], edge[*it][1], temp);
add_edge(edge[i][0], edge[i][1], edge[i][edge[i].size()-1], G);
}
//int weights[] = { 1, 2, 1, 2, 7, 3, 1, 1, 1 };
//int num_arcs = sizeof(edge_array) / sizeof(Edge);
//graph_t g(edge_array, edge_array + num_arcs, weights, num_nodes);
boost:: property_map<Graph, boost::edge_weight_t>::type weightmap = get( boost::edge_weight, G);
std::vector<vertex_descriptor> p(num_vertices(G));
std::vector<int> d(num_vertices(G));
if (start >= num_vertices(G) || start<0)
{
length=10000000;
return pathes;
}
vertex_descriptor s = vertex(start, G);
std::ofstream dot_file;//("figs/dijkstra-eg.dot");
dot_file.open ("dijkstra-eg.txt");
dijkstra_shortest_paths(G, s,
predecessor_map(boost::make_iterator_property_map(p.begin(), get(boost::vertex_index, G))).
distance_map(boost::make_iterator_property_map(d.begin(), get(boost::vertex_index, G))));
vector<int> testPath;
length=0;
boost::graph_traits < Graph >::edge_iterator ei, ei_end;
for (boost::tie(ei, ei_end) = boost::edges(G); ei != ei_end; ++ei)
{
boost::graph_traits < Graph >::edge_descriptor e = *ei;
boost::graph_traits < Graph >::vertex_descriptor
u = source(e, G), v = target(e, G);
if (p[v] == u)
//dot_file << ", color=\"black\"";// in path
{
vector<int> edge;
edge.push_back(u); edge.push_back(v); edge.push_back(d[v]);
path.push_back(edge);
length += get(weightmap, e);
}
else
{
vector<int> edge;
edge.push_back(u); edge.push_back(v);
path_b.push_back(edge);
}
}
pathes.push_back(path); pathes.push_back(path_b);
return pathes;
}
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;
}
