MM.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 <ext/algorithm>
 
#include <alib/vector>
#include <alib/map>
#include <alib/set>
#include <alib/tuple>
 
#include <common/ReconstructPath.hpp>
#include <common/SupportFunction.hpp>
 
namespace graph {
 
namespace shortest_path {
 
class MM {
// ---------------------------------------------------------------------------------------------------------------------
 
 public:
 
  template<
      typename TGraph,
      typename TNode,
      typename F1 = std::function<typename TGraph::edge_type::weight_type(const TNode &)>,
      typename F2 = std::function<typename TGraph::edge_type::weight_type(const TNode &)>,
      typename F3 = std::function<void(const TNode &, const typename TGraph::edge_type::weight_type &)>>
  static
  ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
  findPathBidirectional(const TGraph &graph,
                        const TNode &start,
                        const TNode &goal,
                        F1 f_heuristic_forward,
                        F2 f_heuristic_backward,
                        F3 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>
  findPathBidirectionalRegistration(const TGraph &graph,
                                    const TNode &start,
                                    const TNode &goal,
                                    F1 f_heuristic) {
    return findPathBidirectional(graph, start, goal,
                                 [&](const TNode &n) { return f_heuristic(goal, n); },
                                 [&](const TNode &n) { return f_heuristic(start, n); });
 
  }
 
 
// =====================================================================================================================
 
 private:
 
  template<typename TNode, typename TWeight>
  struct Data {
    ext::set<ext::tuple<TWeight, TWeight, TNode>> queue; // priority queue
 
    ext::set<ext::pair<TWeight, TNode>> f_set; // F score of currently OPEN nodes
    ext::map<TNode, TWeight> f_map; // F score
 
    ext::set<ext::pair<TWeight, TNode>> g_set; // G score of currently OPEN nodes
    ext::map<TNode, TWeight> g_map; // G score
 
    ext::map<TNode, TNode> p; // parents
  };
 
// ---------------------------------------------------------------------------------------------------------------------
 
  template<typename FSuccEdge, typename TNode, typename TWeight, typename F1, typename F2, typename F3>
  static void relaxation(FSuccEdge successor_edges,
                         Data<TNode, TWeight> &data,
                         F1 f_heuristic,
                         F2 f_user,
                         F3 f_update);
 
// ---------------------------------------------------------------------------------------------------------------------
 
  template<typename TNode, typename TWeight, typename F>
  inline static void init(MM::Data<TNode, TWeight> &data, const TNode &start, F f_heuristic);
 
 
 
// ---------------------------------------------------------------------------------------------------------------------
};
 
// =====================================================================================================================
 
template<typename TGraph, typename TNode, typename F1, typename F2, typename F3>
ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
MM::findPathBidirectional(const TGraph &graph,
                          const TNode &start,
                          const TNode &goal,
                          F1 f_heuristic_forward,
                          F2 f_heuristic_backward,
                          F3 f_user) {
 
  using weight_type = typename TGraph::edge_type::weight_type;
 
  weight_type eps = common::SupportFunction::getMinEdgeValue(graph); // Smallest value of the weight in graph
 
  weight_type p = std::numeric_limits<weight_type>::max(); // Currently best path weight
  ext::vector<TNode> intersection_nodes; // Last one is currently best intersection node
  Data<TNode, weight_type> forward_data;
  Data<TNode, weight_type> backward_data;
 
  // Init forward search
  init(forward_data, start, f_heuristic_forward);
  auto f_forward_update = [&](const auto &s) -> void {
    if (backward_data.g_map.find(s) != backward_data.g_map.end()) {
      if (forward_data.g_map.at(s) + backward_data.g_map.at(s) < p) {
        p = forward_data.g_map.at(s) + backward_data.g_map.at(s);
        intersection_nodes.push_back(s);
      }
    }
  };
 
  // Init backward search
  init(backward_data, goal, f_heuristic_backward);
  auto f_backward_update = [&](const auto &s) -> void {
    if (forward_data.g_map.find(s) != forward_data.g_map.end()) {
      if (backward_data.g_map.at(s) + forward_data.g_map.at(s) < p) {
        p = backward_data.g_map.at(s) + forward_data.g_map.at(s);
        intersection_nodes.push_back(s);
      }
    }
  };
 
  while (!forward_data.queue.empty() && !backward_data.queue.empty()) {
    // Check whether can stop searching
    if (ext::max(std::min(std::get<0>(*forward_data.queue.begin()),
                                                  std::get<0>(*backward_data.queue.begin())),
                                         forward_data.f_set.begin()->first,
                                         backward_data.f_set.begin()->first,
                                         forward_data.g_set.begin()->first + backward_data.g_set.begin()->first + eps)
        >= p) {
      return common::ReconstructPath::reconstructWeightedPath(forward_data.p,
                                                              backward_data.p,
                                                              forward_data.g_map,
                                                              backward_data.g_map,
                                                              start,
                                                              goal,
                                                              intersection_nodes.back());
    }
 
    // Expand the lower value
    if (std::get<0>(*forward_data.queue.begin()) < std::get<0>(*backward_data.queue.begin())) {
      // Forward search
      relaxation([&](const auto &node) -> auto { return graph.successorEdges(node); },
                 forward_data,
                 f_heuristic_forward,
                 f_user,
                 f_forward_update);
    } else {
      // Backward search
      relaxation([&](const auto &node) -> auto { return graph.predecessorEdges(node); },
                 backward_data,
                 f_heuristic_backward,
                 f_user,
                 f_backward_update);
    }
  }
 
  return ext::make_pair(ext::vector<TNode>(), p);
}
 
// ---------------------------------------------------------------------------------------------------------------------
 
template<typename FSuccEdge, typename TNode, typename TWeight, typename F1, typename F2, typename F3>
void MM::relaxation(FSuccEdge successor_edges,
                    MM::Data<TNode, TWeight> &data,
                    F1 f_heuristic,
                    F2 f_user,
                    F3 f_update) {
  TNode n = std::get<2>(*data.queue.begin());
  data.queue.erase(data.queue.begin()); // erase from priority queue
  data.f_set.erase(data.f_set.find(ext::make_pair(data.f_map[n], n))); // erase from f score ordered array
  data.g_set.erase(data.g_set.find(ext::make_pair(data.g_map[n], n))); // erase from g score ordered array
 
  // Run user's function
  f_user(n, data.g_map[n]);
 
  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("MM: Detect negative weight on edge in graph.");
    }
 
    // Calculate new G score and F score
    TWeight gscore = data.g_map.at(n) + s_edge.weight();
 
    // Search if the node s was already visited
    auto search_g = data.g_map.find(s);
 
    // If not or the G score can be improve do relaxation
    if (search_g == data.g_map.end() || data.g_map.at(s) > gscore) {
      // Search if the node s is in OPEN
      auto search_q = data.queue.find(ext::make_tuple(std::max(data.f_map[s], 2 * data.g_map[s]), data.g_map[s], s));
 
      if (search_q != data.queue.end()) {
        // Erase node from priority queue
        data.queue.erase(search_q);
        data.g_set.erase(data.g_set.find(ext::make_pair(data.g_map[s], s)));
        data.f_set.erase(data.f_set.find(ext::make_pair(data.f_map[s], s)));
      }
 
      data.g_map[s] = gscore;
      data.g_set.insert(ext::make_pair(data.g_map[s], s));
      data.f_map[s] = gscore + f_heuristic(s);
      data.f_set.insert(ext::make_pair(data.f_map[s], s));
      data.p.insert_or_assign(s, n);
      data.queue.insert(ext::make_tuple(std::max(data.f_map[s], 2 * data.g_map[s]), data.g_map[s], s));
 
      f_update(s); // Update currently best path
    }
  }
}
 
// ---------------------------------------------------------------------------------------------------------------------
 
template<typename TNode, typename TWeight, typename F>
void MM::init(MM::Data<TNode, TWeight> &data, const TNode &start, F f_heuristic) {
  data.g_map[start] = 0;
  data.g_set.insert(ext::make_pair(data.g_map[start], start));
  data.f_map[start] = data.g_map[start] + f_heuristic(start);
  data.f_set.insert(ext::make_pair(data.f_map[start], start));
  data.p.insert_or_assign(start, start);
  data.queue.insert(ext::make_tuple(std::max(data.f_map[start], 2 * data.g_map[start]),
                                    data.g_map[start],
                                    start));
}
 
// ---------------------------------------------------------------------------------------------------------------------
 
} // namespace shortest_path
 
} // namespace graph