IDAStar.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/set>
#include <alib/pair>
#include <limits>
#include <algorithm>
#include <stdexcept>
 
#include <common/ReconstructPath.hpp>
#include <common/SupportFunction.hpp>
 
namespace graph {
 
namespace shortest_path {
 
class IDAStar {
// ---------------------------------------------------------------------------------------------------------------------
 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::vector<TNode> path; // current path
    ext::set<TNode> path_set; // current path set for quick search
    TWeight path_size; // size of found path
  };
 
// ---------------------------------------------------------------------------------------------------------------------
 
  template<typename TGraph, typename TNode, typename F1, typename F2>
  static
  ext::pair<bool, typename TGraph::edge_type::weight_type>
  search(const TGraph &graph,
         IDAStar::Data<TNode, typename TGraph::edge_type::weight_type> &data,
         const TNode &goal,
         typename TGraph::edge_type::weight_type gscore,
         typename TGraph::edge_type::weight_type bound,
         F1 f_heuristic,
         F2 f_user);
 
// ---------------------------------------------------------------------------------------------------------------------
 
};
 
// =====================================================================================================================
 
template<typename TGraph, typename TNode, typename F1, typename F2>
ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
IDAStar::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;
  data.path.push_back(start);
  data.path_set.insert(start);
 
  weight_type bound = f_heuristic(start);
  while ( true ) {
    bool found;
    weight_type t;
    ext::tie(found, t) = search(graph, data, goal, 0, bound, f_heuristic, f_user);
 
    if (found) {
      return ext::make_pair(data.path, data.path_size); // Return found path
    } else if (t == std::numeric_limits<weight_type>::max()) {
      return ext::make_pair(ext::vector<TNode>(), std::numeric_limits<weight_type>::max()); // No path exists
    }
 
    bound = t;
  }
}
 
// ---------------------------------------------------------------------------------------------------------------------
 
template<typename TGraph, typename TNode, typename F1, typename F2>
ext::pair<bool, typename TGraph::edge_type::weight_type>
IDAStar::search(const TGraph &graph,
                IDAStar::Data<TNode, typename TGraph::edge_type::weight_type> &data,
                const TNode &goal,
                typename TGraph::edge_type::weight_type gscore,
                typename TGraph::edge_type::weight_type bound,
                F1 f_heuristic,
                F2 f_user) {
  using weight_type = typename TGraph::edge_type::weight_type;
 
  TNode n = data.path.back();
  weight_type f = gscore + f_heuristic(n);
 
  if (f > bound) {
    return ext::make_pair(false, f);
  }
 
  // Run user function
  f_user(n, gscore);
 
  // Check for goal
  if (n == goal) {
    data.path_size = gscore;
    return ext::make_pair(true, f);
  }
 
  weight_type min = std::numeric_limits<weight_type>::max();
  for (const auto &s_edge: graph.successorEdges(n)) {
    const TNode &s = common::SupportFunction::other(s_edge, n); // successor
 
    // Check if node is not in path
    if (data.path_set.find(s) != data.path_set.end()) {
      continue;
    }
 
    // Check for negative edge
    if (s_edge.weight() < 0) {
      throw std::out_of_range("IDAStar: Detect negative weight on edge in graph.");
    }
 
    bool found;
    weight_type t;
 
    data.path.push_back(s); // Insert node to the path
    data.path_set.insert(s);
    ext::tie(found, t) =
        search(graph, data, goal, gscore + s_edge.weight(), bound, f_heuristic, f_user); // Run recursion
 
    if (found) {
      return ext::make_pair(true, t);
    } else if (t < min) {
      min = t; // Update min
    }
 
    data.path.pop_back(); // Pop node from the path
    data.path_set.erase(data.path_set.find(s));
  }
 
  return ext::make_pair(false, min);
}
 
// ---------------------------------------------------------------------------------------------------------------------
 
} // namespace shortest_path
 
} // namespace graph