9 #ifndef MRPT_DIJKSTRA_H
10 #define MRPT_DIJKSTRA_H
36 template<
class TYPE_GRAPH,
class MAPS_IMPLEMENTATION = mrpt::utils::map_traits_stdmap >
44 inline TDistance() : dist(
std::numeric_limits<double>::max() ) { }
62 typedef typename MAPS_IMPLEMENTATION::template map<TNodeID,TPairNodeIDs>
id2pairIDs_map_t;
63 typedef typename MAPS_IMPLEMENTATION::template map<TNodeID,TDistance>
id2dist_map_t;
64 typedef typename MAPS_IMPLEMENTATION::template map<TNodeID,TPrevious>
id2id_map_t;
79 typedef typename graph_t::edge_t
edge_t;
99 double (*functor_edge_weight)(
const graph_t& graph,
const TNodeID id_from,
const TNodeID id_to,
const edge_t &edge) = NULL,
100 void (*functor_on_progress)(
const graph_t& graph,
size_t visitedCount) = NULL
102 : m_cached_graph(graph), m_source_node_ID(source_node_ID)
123 graph.getAllNodes( m_lstNode_IDs );
124 const size_t nNodes = m_lstNode_IDs.size();
126 if ( m_lstNode_IDs.find(source_node_ID)==m_lstNode_IDs.end() )
134 size_t visitedCount = 0;
135 m_distances [source_node_ID] = 0;
136 m_distances_non_visited[source_node_ID] = 0;
139 graph.getAdjacencyMatrix(m_allNeighbors);
145 double min_d = std::numeric_limits<double>::max();
152 if (itDist->second.dist < min_d)
155 min_d = itDist->second.dist;
161 m_distances[u]=m_distances_non_visited[u];
162 m_distances_non_visited.erase(u);
167 if (functor_on_progress) (*functor_on_progress)(graph,visitedCount);
170 const std::set<TNodeID> & neighborsOfU = m_allNeighbors[u];
178 bool edge_ui_reverse =
false;
179 bool edge_ui_found =
false;
182 double edge_ui_weight;
183 if (!functor_edge_weight)
187 edge_ui = graph.edges.find( std::make_pair(u,i) );
188 if ( edge_ui==graph.edges.end() )
190 edge_ui = graph.edges.find( std::make_pair(i,u));
191 edge_ui_reverse =
true;
193 ASSERT_(edge_ui!=graph.edges.end());
194 edge_ui_weight = (*functor_edge_weight)( graph, edge_ui->first.first,edge_ui->first.second, edge_ui->second );
195 edge_ui_found =
true;
198 if ( (min_d+edge_ui_weight) < m_distances[i].dist)
200 m_distances[i].dist = m_distances_non_visited[i].dist = min_d+edge_ui_weight;
201 m_prev_node[i].id = u;
205 edge_ui = graph.edges.find( std::make_pair(u,i) );
206 if ( edge_ui==graph.edges.end() ) {
207 edge_ui = graph.edges.find( std::make_pair(i,u));
208 edge_ui_reverse =
true;
210 ASSERT_(edge_ui!=graph.edges.end());
213 if ( !edge_ui_reverse )
214 m_prev_arc[i] = std::make_pair(u,i);
215 else m_prev_arc[i] = std::make_pair(i,u);
218 }
while ( visitedCount<nNodes );
231 if (it==m_distances.end())
THROW_EXCEPTION(
"Node was not found in the graph when running Dijkstra");
232 return it->second.dist;
253 edge_list_t &out_path
257 if (target_node_ID==m_source_node_ID)
return;
265 out_path.push_front( it->second );
266 nod = m_prev_node.find(nod)->second.id;
267 }
while (nod!=m_source_node_ID);
289 const TNodeID id = itArcs->first;
290 const TNodeID id_from = itArcs->second.first;
291 const TNodeID id_to = itArcs->second.second;
293 std::list<TreeEdgeInfo> &edges = out_tree.
edges_to_children[
id==id_from ? id_to : id_from];
294 TreeEdgeInfo newEdge(
id);
295 newEdge.reverse = (
id==id_from);
297 ASSERTMSG_(itEdgeData!=m_cached_graph.edges.end(),
format(
"Edge %u->%u is in Dijkstra paths but not in original graph!",static_cast<unsigned int>(id_from),static_cast<unsigned int>(id_to) ))
298 newEdge.data = & itEdgeData->second;
299 edges.push_back( newEdge );
TNodeID getRootNodeID() const
Return the node ID of the tree root, as passed in the constructor.
MAPS_IMPLEMENTATION::template map< TNodeID, TDistance > id2dist_map_t
Auxiliary struct for topological distances from root node.
TMapNode2ListEdges edges_to_children
The edges of each node.
< Make available this typedef in this namespace too
TDistance(const double D)
TYPE_GRAPH graph_t
The type of the graph, typically a mrpt::graphs::CDirectedGraph<> or any other derived class...
std::map< TNodeID, TDistance > m_distances_non_visited
void getTreeGraph(tree_graph_t &out_tree) const
Returns a tree representation of the graph, as determined by the Dijkstra shortest paths from the roo...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
#define THROW_EXCEPTION(msg)
const std::set< TNodeID > & getListOfAllNodes() const
Return the set of all known node IDs (actually, a const ref to the internal set object).
const Scalar * const_iterator
const TDistance & operator=(const double D)
CDijkstra(const graph_t &graph, const TNodeID source_node_ID, double(*functor_edge_weight)(const graph_t &graph, const TNodeID id_from, const TNodeID id_to, const edge_t &edge)=NULL, void(*functor_on_progress)(const graph_t &graph, size_t visitedCount)=NULL)
Constructor, which takes the input graph and executes the entire Dijkstra algorithm from the given ro...
id2dist_map_t m_distances
All the distances.
const list_all_neighbors_t & getCachedAdjacencyMatrix() const
Return the adjacency matrix of the input graph, which is cached at construction so if needed later ju...
MAPS_IMPLEMENTATION::template map< TNodeID, TPairNodeIDs > id2pairIDs_map_t
const TNodeID m_source_node_ID
void getShortestPathTo(const TNodeID target_node_ID, edge_list_t &out_path) const
Returns the shortest path between the source node passed in the constructor and the given target node...
id2pairIDs_map_t m_prev_arc
uint64_t TNodeID
The type for node IDs in graphs of different types.
TNodeID root
The root of the tree.
MAPS_IMPLEMENTATION::template map< TNodeID, std::set< TNodeID > > list_all_neighbors_t
A std::map (or a similar container according to MAPS_IMPLEMENTATION) with all the neighbors of every ...
double getNodeDistanceToRoot(const TNodeID id) const
Return the distance from the root node to any other node using the Dijkstra-generated tree On unknown...
MAPS_IMPLEMENTATION::template map< TNodeID, TPrevious > id2id_map_t
list_all_neighbors_t m_allNeighbors
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::list< TPairNodeIDs > edge_list_t
A list of edges used to describe a path on the graph.
CDirectedTree< const edge_t * > tree_graph_t
Type for graph returned by getTreeGraph: a graph like the original input graph, but with edge data be...
std::set< TNodeID > m_lstNode_IDs
The Dijkstra algorithm for finding the shortest path between a given source node in a (weighted) dire...
const TYPE_GRAPH & m_cached_graph
Auxiliary struct for backward paths.
void clear()
Empty all edge data and set "root" to INVALID_NODEID.
graph_t::edge_t edge_t
The type of edge data in graph_t.
#define ASSERTMSG_(f, __ERROR_MSG)
#define THROW_EXCEPTION_CUSTOM_MSG1(msg, param1)