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; }