00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef MRPT_DIJKSTRA_H
00029 #define MRPT_DIJKSTRA_H
00030
00031 #include <mrpt/math/graphs.h>
00032
00033 #include <list>
00034
00035 namespace mrpt
00036 {
00037 namespace math
00038 {
00039 using namespace std;
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050 template<typename TYPE_EDGES>
00051 class CDijkstra
00052 {
00053 public:
00054 typedef size_t TNodeID;
00055 typedef std::list<std::pair<TNodeID,TNodeID> > TListEdges;
00056
00057 static const TNodeID INVALID_TNODEID = static_cast<TNodeID>(-1);
00058
00059 protected:
00060
00061
00062 struct TDistance
00063 {
00064 unsigned dist;
00065
00066 TDistance() : dist( std::numeric_limits<unsigned>::max() ) { }
00067 TDistance(const unsigned &D) : dist(D) { }
00068 const TDistance & operator =(const unsigned &D) { dist = D; return *this;}
00069 };
00070
00071 struct TPrevious
00072 {
00073 TPrevious() : id( INVALID_TNODEID ) { }
00074 TNodeID id;
00075 };
00076
00077
00078 std::map<TNodeID,TDistance> d;
00079 std::map<TNodeID,TPrevious> previous;
00080 std::map<TNodeID, std::pair<TNodeID,TNodeID> > previous_arcs;
00081 std::map<TNodeID, bool> visited;
00082 TNodeID m_source_node_ID;
00083
00084 public:
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099 CDijkstra(
00100 const mrpt::math::CDirectedGraph<TYPE_EDGES> &graph,
00101 const TNodeID &source_node_ID,
00102 double (*functor_edge_weight)(const TYPE_EDGES &edge) = NULL
00103 )
00104 : m_source_node_ID(source_node_ID)
00105 {
00106 MRPT_TRY_START;
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126 std::set<TNodeID> lstNode_IDs;
00127 graph.getAllNodes( lstNode_IDs );
00128 const size_t nNodes = lstNode_IDs.size();
00129
00130 if ( lstNode_IDs.find(source_node_ID)==lstNode_IDs.end() )
00131 THROW_EXCEPTION_CUSTOM_MSG1("Cannot find the source node_ID=%u in the graph",static_cast<unsigned int>(source_node_ID));
00132
00133
00134
00135
00136
00137
00138 size_t visitedCount = 0;
00139 d[source_node_ID] = 0;
00140
00141 std::set<TNodeID>::const_iterator u;
00142
00143 do
00144 {
00145
00146 unsigned min_d = std::numeric_limits<unsigned>::max();
00147 u = lstNode_IDs.end();
00148
00149 for (std::set<TNodeID>::const_iterator i=lstNode_IDs.begin();i!=lstNode_IDs.end();++i)
00150 {
00151 if (d[*i].dist < min_d && !visited[*i])
00152 {
00153 u = i;
00154 min_d = d[*u].dist;
00155 }
00156 }
00157
00158 ASSERT_(u!=lstNode_IDs.end());
00159
00160 visited[*u] = true;
00161 visitedCount++;
00162
00163
00164 std::set<TNodeID> neighborsOfU;
00165 graph.getNeighborsOf(*u,neighborsOfU);
00166 for (std::set<TNodeID>::const_iterator i=neighborsOfU.begin();i!=neighborsOfU.end();++i)
00167 {
00168 if ( (min_d+1) < d[*i].dist)
00169 {
00170 d[*i].dist = min_d+1;
00171 previous[*i].id = *u;
00172 typename mrpt::math::CDirectedGraph<TYPE_EDGES>::const_iterator the_edge =
00173 graph.edges.find( make_pair(*u,*i) );
00174 if ( the_edge!=graph.end() )
00175 {
00176 previous_arcs[*i] = make_pair(*u,*i);
00177 }
00178 else
00179 {
00180 the_edge = graph.edges.find( make_pair<TNodeID,TNodeID>(*i,*u));
00181 ASSERT_(the_edge!=graph.end());
00182 previous_arcs[*i] = make_pair(*i,*u);
00183 }
00184 }
00185 }
00186
00187 } while ( visitedCount<nNodes );
00188
00189 MRPT_TRY_END;
00190 }
00191
00192
00193
00194
00195
00196
00197
00198 void getShortestPathTo(
00199 const TNodeID target_node_ID,
00200 TListEdges &out_path
00201 ) const
00202 {
00203 out_path.clear();
00204 if (target_node_ID==m_source_node_ID) return;
00205
00206 TNodeID nod = target_node_ID;
00207 do
00208 {
00209 std::map<TNodeID, std::pair<TNodeID,TNodeID> >::const_iterator it = previous_arcs.find(nod);
00210 ASSERT_(it!=previous_arcs.end())
00211
00212 out_path.push_front( it->second );
00213 nod = previous.find(nod)->second.id;
00214 } while (nod!=m_source_node_ID);
00215
00216 }
00217
00218 };
00219
00220 }
00221 }
00222 #endif