Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include <queue>
00018 #include <vector>
00019 #include <assert.h>
00020 #include "dijkstra.h"
00021
00022 namespace AISNavigation{
00023
00024 using namespace std;
00025
00026 Dijkstra::AdjacencyMapEntry::AdjacencyMapEntry(Graph::Vertex* child_,
00027 Graph::Vertex* parent_,
00028 Graph::Edge* edge_,
00029 double distance_){
00030 _child=child_;
00031 _parent=parent_;
00032 _edge=edge_;
00033 _distance=distance_;
00034 std::set<AdjacencyMapEntry*> choldren;
00035 }
00036
00037 Dijkstra::Dijkstra(Graph* g): _graph(g){
00038 for (Graph::VertexIDMap::const_iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); it++){
00039 AdjacencyMapEntry entry(it->second, 0,0,std::numeric_limits< double >::max());
00040 _adjacencyMap.insert(make_pair(entry.child(), entry));
00041 }
00042 }
00043
00044 void Dijkstra::reset(){
00045 for (Graph::VertexSet::iterator it=_visited.begin(); it!=_visited.end(); it++){
00046 AdjacencyMap::iterator at=_adjacencyMap.find(*it);
00047 assert(at!=_adjacencyMap.end());
00048 at->second=AdjacencyMapEntry(at->first,0,0,std::numeric_limits< double >::max());
00049 }
00050 _visited.clear();
00051 }
00052
00053
00054 bool operator<(const Dijkstra::AdjacencyMapEntry& a, const Dijkstra::AdjacencyMapEntry& b){
00055 return a.distance()>b.distance();
00056 }
00057
00058
00059 void Dijkstra::shortestPaths(Graph::Vertex* v,
00060 Dijkstra::CostFunction* cost,
00061 double maxDistance,
00062 double comparisonConditioner,
00063 bool directed){
00064 reset();
00065 std::priority_queue< AdjacencyMapEntry > frontier;
00066 AdjacencyMap::iterator it=_adjacencyMap.find(v);
00067 assert(it!=_adjacencyMap.end());
00068 it->second._distance=0.;
00069 frontier.push(it->second);
00070
00071 while(! frontier.empty()){
00072 AdjacencyMapEntry entry=frontier.top();
00073 frontier.pop();
00074 Graph::Vertex* u=entry.child();
00075 AdjacencyMap::iterator ut=_adjacencyMap.find(u);
00076 assert(ut!=_adjacencyMap.end());
00077 double uDistance=ut->second.distance();
00078
00079 std::pair< Graph::VertexSet::iterator, bool> insertResult=_visited.insert(u);
00080 Graph::EdgeSet::iterator et=u->edges().begin();
00081 while(et!=u->edges().end()){
00082 Graph::Edge* edge=*et;
00083 et++;
00084
00085 Graph::Vertex* z=0;
00086 if (edge->from()==u)
00087 z=edge->to();
00088 else if(edge->to()==u)
00089 z=edge->from();
00090 assert(z);
00091
00092 if (directed && edge->from()!=u)
00093 continue;
00094
00095 double edgeDistance=(*cost)(edge, u, z);
00096 if (edgeDistance==std::numeric_limits< double >::max())
00097 continue;
00098 double zDistance=uDistance+edgeDistance;
00099 AdjacencyMap::iterator ot=_adjacencyMap.find(z);
00100 assert(ot!=_adjacencyMap.end());
00101
00102 if (zDistance+comparisonConditioner<ot->second.distance() && zDistance<maxDistance){
00103 ot->second._distance=zDistance;
00104 ot->second._parent=u;
00105 ot->second._edge=edge;
00106 frontier.push(ot->second);
00107 }
00108 }
00109 }
00110 }
00111
00112 void Dijkstra::computeTree(Graph::Vertex* v __attribute__((unused)), AdjacencyMap& amap){
00113 for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); it++){
00114 AdjacencyMapEntry& entry(it->second);
00115 entry._children.clear();
00116 }
00117 for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); it++){
00118 AdjacencyMapEntry& entry(it->second);
00119 Graph::Vertex* parent=entry.parent();
00120 if (!parent){
00121 continue;
00122 }
00123 Graph::Vertex* v=entry.child();
00124 assert (v==it->first);
00125
00126 AdjacencyMap::iterator pt=amap.find(parent);
00127 assert(pt!=amap.end());
00128 pt->second._children.insert(v);
00129 }
00130 }
00131
00132
00133 void Dijkstra::visitAdjacencyMap(Graph::Vertex* v, AdjacencyMap& amap, TreeAction* action){
00134 typedef std::deque<Graph::Vertex*> Deque;
00135 Deque q;
00136 action->perform(v,0,0);
00137 q.push_back(v);
00138 int count=0;
00139 while (! q.empty()){
00140 Graph::Vertex* parent=q.front();
00141 q.pop_front();
00142 count++;
00143 AdjacencyMap::iterator parentIt=amap.find(parent);
00144 if (parentIt==amap.end())
00145 continue;
00146 Graph::VertexSet& childs(parentIt->second.children());
00147 for (Graph::VertexSet::iterator childsIt=childs.begin(); childsIt!=childs.end(); childsIt++){
00148 Graph::Vertex* child=*childsIt;
00149 AdjacencyMap::iterator adjacencyIt=amap.find(child);
00150 assert (adjacencyIt!=amap.end());
00151 Graph::Edge* edge=adjacencyIt->second.edge();
00152
00153 assert(adjacencyIt->first==child);
00154 assert(adjacencyIt->second.child()==child);
00155 assert(adjacencyIt->second.parent()==parent);
00156 action->perform(child, parent, edge);
00157 q.push_back(child);
00158 }
00159 }
00160
00161 }
00162
00163 void connectedSubset(Graph::VertexSet& connected, Graph::VertexSet& visited,
00164 Graph::VertexSet& startingSet,
00165 Graph* g, Graph::Vertex* v,
00166 Dijkstra::CostFunction* cost, double distance, double comparisonConditioner){
00167 typedef std::queue<Graph::Vertex*> VertexDeque;
00168 visited.clear();
00169 connected.clear();
00170 VertexDeque frontier;
00171 Dijkstra dv(g);
00172 connected.insert(v);
00173 frontier.push(v);
00174 while (! frontier.empty()){
00175 Graph::Vertex* v0=frontier.front();
00176 frontier.pop();
00177 dv.shortestPaths(v0,cost,distance,comparisonConditioner);
00178 for (Graph::VertexSet::iterator it=dv.visited().begin(); it!=dv.visited().end(); it++){
00179 visited.insert(*it);
00180 if (startingSet.find(*it)==startingSet.end())
00181 continue;
00182 std::pair<Graph::VertexSet::iterator, bool> insertOutcome=connected.insert(*it);
00183 if (insertOutcome.second){
00184 frontier.push(dynamic_cast<Graph::Vertex*>(*it));
00185 }
00186 }
00187 }
00188 }
00189
00190 double UniformCostFunction::operator () (Graph::Edge* edge __attribute__((unused)),
00191 Graph::Vertex* from __attribute__((unused)), Graph::Vertex* to __attribute__((unused))) {
00192 return 1.;
00193 }
00194
00195
00196
00197 };