Add SPFA algorithm.

alib2graph_algo/src/shortest_path/SPFA.cpp

+// SPFA.cpp
+//
+//     Created on: 05. 03. 2018
+//         Author: Jan Uhlik
+//    Modified by:
+//
+// Copyright (c) 2017 Czech Technical University in Prague | Faculty of Information Technology. All rights reserved.
+// Git repository: https://gitlab.fit.cvut.cz/algorithms-library-toolkit/automata-library
+
+#include "SPFA.hpp"
+
+#include <registration/AlgoRegistration.hpp>
+
+namespace {
+
+auto SPFA1 = registration::AbstractRegister<shortest_path::SPFA,
+                                            ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+                                            const graph::WeightedUndirectedGraph<> &,
+                                            const DefaultNodeType &,
+                                            const DefaultNodeType &>(shortest_path::SPFA::findPathRegistration);
+
+auto SPFA2 = registration::AbstractRegister<shortest_path::SPFA,
+                                            ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+                                            const graph::WeightedUndirectedMultiGraph<> &,
+                                            const DefaultNodeType &,
+                                            const DefaultNodeType &>(shortest_path::SPFA::findPathRegistration);
+
+auto SPFA3 = registration::AbstractRegister<shortest_path::SPFA,
+                                            ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+                                            const graph::WeightedDirectedGraph<> &,
+                                            const DefaultNodeType &,
+                                            const DefaultNodeType &>(shortest_path::SPFA::findPathRegistration);
+
+auto SPFA4 = registration::AbstractRegister<shortest_path::SPFA,
+                                            ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+                                            const graph::WeightedDirectedMultiGraph<> &,
+                                            const DefaultNodeType &,
+                                            const DefaultNodeType &>(shortest_path::SPFA::findPathRegistration);
+
+auto SPFA5 = registration::AbstractRegister<shortest_path::SPFA,
+                                            ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+                                            const graph::WeightedMixedGraph<> &,
+                                            const DefaultNodeType &,
+                                            const DefaultNodeType &>(shortest_path::SPFA::findPathRegistration);
+
+auto SPFA6 = registration::AbstractRegister<shortest_path::SPFA,
+                                            ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+                                            const graph::WeightedMixedMultiGraph<> &,
+                                            const DefaultNodeType &,
+                                            const DefaultNodeType &>(shortest_path::SPFA::findPathRegistration);
+
+auto SPFAGrid1 = registration::AbstractRegister<shortest_path::SPFA,
+                                                ext::pair<ext::vector<DefaultSquareGridNodeType>,
+                                                          DefaultWeightType>,
+                                                const grid::WeightedSquareGrid4<> &,
+                                                const DefaultSquareGridNodeType &,
+                                                const DefaultSquareGridNodeType &>(shortest_path::SPFA::findPathRegistration);
+
+auto SPFAGrid2 = registration::AbstractRegister<shortest_path::SPFA,
+                                                ext::pair<ext::vector<DefaultSquareGridNodeType>,
+                                                          DefaultWeightType>,
+                                                const grid::WeightedSquareGrid8<> &,
+                                                const DefaultSquareGridNodeType &,
+                                                const DefaultSquareGridNodeType &>(shortest_path::SPFA::findPathRegistration);
+
+}

alib2graph_algo/src/shortest_path/SPFA.hpp

+// SPFA.hpp
+//
+//     Created on: 05. 03. 2018
+//         Author: Jan Uhlik
+//    Modified by:
+//
+// Copyright (c) 2017 Czech Technical University in Prague | Faculty of Information Technology. All rights reserved.
+// Git repository: https://gitlab.fit.cvut.cz/algorithms-library-toolkit/automata-library
+
+#ifndef ALIB2_SPFA_HPP
+#define ALIB2_SPFA_HPP
+
+#include <alib/list>
+#include <alib/set>
+#include <alib/map>
+#include <alib/vector>
+#include <queue>
+#include <stdexcept>
+#include <functional>
+#include <algorithm>
+
+#include <common/ReconstructPath.hpp>
+#include <common/SupportFunction.hpp>
+
+namespace shortest_path {
+
+class SPFA {
+// ---------------------------------------------------------------------------------------------------------------------
+ public:
+
+  /// Run SPFA algorithm from the \p start node in the \p graph.
+  ///
+  /// Whenever node is opened, \p f_user is called with two parameters (the opened node and value of currently shortest path).
+  ///
+  /// \param graph to explore.
+  /// \param start initial node.
+  /// \param f_user function which is called for every opened node with value of currently shortest path.
+  ///
+  /// \note TEdge of \p graph must follow graph::edge::WeightedEdge interface.
+  /// \sa graph::edge_type::WeightedEdge.
+  ///
+  /// \throws std::out_of_range if \p graph contains negative cycle.
+  ///
+  template<
+      typename TGraph,
+      typename TNode,
+      typename F = std::function<void(const TNode &,
+                                      const typename TGraph::edge_type::weight_type &)> >
+  static
+  void
+  run(const TGraph &graph,
+      const TNode &start,
+      F f_user = [](const TNode &,
+                    const typename TGraph::edge_type::weight_type &) -> void {});
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+  /// Find the shortest path using SPFA algorithm from the \p start node to the \p goal node in the \p graph.
+  ///
+  /// Whenever node is opened, \p f_user is called with two parameters (the opened node and value of currently shortest path).
+  ///
+  /// \param graph to explore.
+  /// \param start initial node.
+  /// \param goal final node.
+  /// \param f_user function which is called for every open node with value of currently shortest path.
+  ///
+  /// \returns pair where first := shortest path := distance of path, if there is no such path vector is empty and distance std::numeric_limits<edge_type:weight_type>::max()
+  ///
+  /// \note TEdge of \p graph must follow graph::edge::WeightedEdge interface
+  /// \sa graph::edge_type::WeightedEdge
+  ///
+  /// \throws std::out_of_range if \p graph contains negative cycle
+  ///
+  template<typename TGraph, typename TNode, typename F = std::function<void(const TNode &,
+                                                                            const typename TGraph::edge_type::weight_type &)>>
+  static
+  ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
+  findPath(const TGraph &graph,
+           const TNode &start,
+           const TNode &goal,
+           F f_user = [](const TNode &,
+                         const typename TGraph::edge_type::weight_type &) {});
+
+  template<typename TGraph, typename TNode>
+  static
+  ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
+  findPathRegistration(const TGraph &graph,
+                       const TNode &start,
+                       const TNode &goal) {
+    return findPath(graph, start, goal);
+  }
+
+// =====================================================================================================================
+ private:
+
+  template<typename TNode, typename TWeight>
+  struct Data {
+    std::deque<TNode> q; // queue
+    ext::map<TNode, TWeight> g; // distance (aka G score)
+    ext::map<TNode, TNode> p; // parents
+    ext::map<TNode, size_t> v; // visits
+  };
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+  template<typename TGraph, typename TNode, typename F>
+  static
+  Data<TNode, typename TGraph::edge_type::weight_type>
+  impl(const TGraph &graph,
+       const TNode &start,
+       F f_user);
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+  template<typename TNode, typename TWeight>
+  inline static void init(SPFA::Data<TNode, TWeight> &data, const TNode &start);
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+};
+
+// =====================================================================================================================
+
+template<typename TGraph, typename TNode, typename F>
+void SPFA::run(const TGraph &graph, const TNode &start, F f_user) {
+  impl(graph, start, f_user);
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+template<typename TGraph, typename TNode, typename F>
+ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type> SPFA::findPath(const TGraph &graph,
+                                                                                      const TNode &start,
+                                                                                      const TNode &goal,
+                                                                                      F f_user) {
+  using weight_type = typename TGraph::edge_type::weight_type;
+
+  Data<TNode, weight_type> data = impl(graph, start, f_user);
+  return common::ReconstructPath::reconstructWeightedPath(data.p, data.g, start, goal);
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+template<typename TGraph, typename TNode, typename F>
+SPFA::Data<TNode, typename TGraph::edge_type::weight_type>
+SPFA::impl(const TGraph &graph, const TNode &start, F f_user) {
+  using weight_type = typename TGraph::edge_type::weight_type;
+
+  Data<TNode, weight_type> data;
+
+  auto nodes = graph.getNodes();
+  size_t nodes_cnt = nodes.size();
+
+  // Init data
+  init(data, start);
+
+  while (!data.q.empty()) {
+    TNode n = data.q.front();
+    data.q.pop_front();
+
+    ++data.v[n];
+
+    // Run user's function
+    f_user(n, data.g[n]);
+
+    if (data.v[n] >= nodes_cnt) {
+      throw std::out_of_range("SPFA: Detect negative cycle in graph.");
+    }
+
+    for (const auto &s_edge: graph.successorEdges(n)) {
+      const TNode &s = common::SupportFunction::other(s_edge, n); // successor
+
+      // Calculate new G score
+      weight_type gscore = data.g.at(n) + s_edge.weight();
+
+      // Search if the node s was already visited
+      auto search_d = data.g.find(s);
+
+      // If not or the distance can be improve do relaxation
+      if (search_d == data.g.end() || search_d->second > gscore) {
+
+        data.g[s] = gscore;
+        data.p.insert_or_assign(s, n);
+
+        // More effective than code below
+        data.q.push_back(s);
+        // Not effective at all
+//        auto search_q = std::find(data.q.begin(), data.q.end(), s);
+//        if (search_q == data.q.end()) {
+//          data.q.push_back(s);
+//        }
+      }
+    }
+  }
+
+  return data;
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+template<typename TNode, typename TWeight>
+void SPFA::init(SPFA::Data<TNode, TWeight> &data, const TNode &start) {
+  data.q.push_back(start);
+  data.v[start] = 1;
+  data.g[start] = 0;
+  data.p.insert_or_assign(start, start);
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+} // namespace shortest_path
+#endif //ALIB2_SPFA_HPP