GreedyBestFS.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 <functional>
#include <alib/vector>
#include <alib/pair>
#include <alib/set>
#include <alib/map>
 
#include <common/ReconstructPath.hpp>
#include <common/SupportFunction.hpp>
 
namespace graph {
 
namespace shortest_path {
 
class GreedyBestFS {
// ---------------------------------------------------------------------------------------------------------------------
 
 public:
 
  template<
      typename TGraph,
      typename TNode,
      typename F1 = std::function<typename TGraph::edge_type::weight_type(const TNode &)>,
      typename F2 = 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,
           F1 f_heuristic,
           F2 f_user = [](const TNode &,
                          const typename TGraph::edge_type::weight_type &) {});
 
  template<
      typename TGraph,
      typename TNode,
      typename F1 = std::function<typename TGraph::edge_type::weight_type(const TNode &, const TNode &)>
  >
  static
  ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
  findPathRegistration(const TGraph &graph,
                       const TNode &start,
                       const TNode &goal,
                       F1 f_heuristic) {
    return findPath(graph, start, goal, [&](const TNode &n) { return f_heuristic(goal, n); });
  }
 
// =====================================================================================================================
 
 private:
 
  template<typename TNode, typename TWeight>
  struct Data {
    ext::set<ext::pair<TWeight, TNode>> queue; // priority queue
    ext::map<TNode, TWeight> g; // G score
    ext::map<TNode, TNode> p; // parents
  };
 
// ---------------------------------------------------------------------------------------------------------------------
 
  template<typename FEdges, typename TNode, typename TWeight, typename F1, typename F2, typename F3>
  static bool relaxation(FEdges successor_edges,
                         Data<TNode, TWeight> &data,
                         F1 f_heuristic,
                         F2 f_user,
                         F3 f_stop);
 
// ---------------------------------------------------------------------------------------------------------------------
 
  template<typename TNode, typename TWeight, typename F>
  inline static void init(GreedyBestFS::Data<TNode, TWeight> &data, const TNode &start, F f_heuristic);
 
// ---------------------------------------------------------------------------------------------------------------------
 
};
 
// =====================================================================================================================
 
template<typename TGraph, typename TNode, typename F1, typename F2>
ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
GreedyBestFS::findPath(const TGraph &graph,
                       const TNode &start,
                       const TNode &goal,
                       F1 f_heuristic,
                       F2 f_user) {
  using weight_type = typename TGraph::edge_type::weight_type;
 
  Data<TNode, weight_type> data;
 
  // Init search
  init(data, start, f_heuristic);
 
  while (!data.queue.empty()) {
    bool stop = relaxation([&](const auto &node) -> auto { return graph.successorEdges(node); },
                           data,
                           f_heuristic,
                           f_user,
                           [&goal](const TNode &n) -> bool { return goal == n; });
 
    if (stop) {
      break;
    }
  }
 
  return common::ReconstructPath::reconstructWeightedPath(data.p, data.g, start, goal);
}
 
// ---------------------------------------------------------------------------------------------------------------------
 
template<typename FEdges, typename TNode, typename TWeight, typename F1, typename F2, typename F3>
bool
GreedyBestFS::relaxation(FEdges successor_edges,
                         GreedyBestFS::Data<TNode, TWeight> &data,
                         F1 f_heuristic,
                         F2 f_user,
                         F3 f_stop) {
  TNode n = data.queue.begin()->second;
  data.queue.erase(data.queue.begin());
 
  // Run user's function
  f_user(n, data.g[n]);
 
  // Stop if reach the goal
  if (f_stop(n)) {
    return true;
  }
 
  for (const auto &s_edge: successor_edges(n)) {
    const TNode &s = common::SupportFunction::other(s_edge, n); // successor
 
    // Check for negative edge
    if (s_edge.weight() < 0) {
      throw std::out_of_range("GreedyBestFS: Detect negative weight on edge in graph.");
    }
 
    // Calculate new G score
    TWeight gscore = data.g.at(n) + s_edge.weight();
 
    // Search if the node s was already visited
    auto search_g = data.g.find(s);
 
    // If not or the G score can be improve do relaxation
    if (search_g == data.g.end() || data.g.at(s) > gscore) {
      data.g[s] = gscore;
      data.p.insert_or_assign(s, n);
      if (search_g == data.g.end()) {
        data.queue.insert(ext::make_pair(f_heuristic(s), s));
      }
    }
  }
 
  return false;
}
 
// ---------------------------------------------------------------------------------------------------------------------
 
template<typename TNode, typename TWeight, typename F>
void GreedyBestFS::init(GreedyBestFS::Data<TNode, TWeight> &data, const TNode &start, F f_heuristic) {
  data.g[start] = 0;
  data.p.insert_or_assign(start, start);
  data.queue.insert(ext::make_pair(f_heuristic(start), start));
}
 
// ---------------------------------------------------------------------------------------------------------------------
 
} // namespace shortest_path
 
} // namespace graph