SPFA.hpp

Go to the documentation of this file.

 
// Copyright (c) 2017 Czech Technical University in Prague | Faculty of Information Technology. All rights reserved.
 
#pragma once
 
#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 graph {
 
namespace shortest_path {
 
class SPFA {
// ---------------------------------------------------------------------------------------------------------------------
 public:
 
  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 {});
 
// ---------------------------------------------------------------------------------------------------------------------
 
  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
 
} // namespace graph