BFS.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 <queue>
#include <alib/map>
#include <alib/set>
#include <iostream>
 
#include <common/ReconstructPath.hpp>
 
namespace graph {
 
namespace traverse {
 
class BFS {
// ---------------------------------------------------------------------------------------------------------------------
 
 public:
 
  template<
      typename TGraph,
      typename TNode,
      typename F = std::function<void(const TNode &, const size_t &)>>
  static
  void
  run(const TGraph &graph,
      const TNode &start,
      F f_user = [](const TNode &, const size_t &) -> bool { return false; });
 
// ---------------------------------------------------------------------------------------------------------------------
 
  template<
      typename TGraph,
      typename TNode,
      typename F = std::function<void(const TNode &, const size_t &)>>
  static
  ext::vector<TNode>
  findPath(const TGraph &graph,
           const TNode &start,
           const TNode &goal,
           F f_user = [](const TNode &, const size_t &) {});
 
  template<typename TGraph, typename TNode>
  static
  ext::vector<TNode>
  findPathRegistration(const TGraph &graph,
                       const TNode &start,
                       const TNode &goal) {
    return findPath(graph, start, goal);
 
  }
 
// ---------------------------------------------------------------------------------------------------------------------
 
  template<
      typename TGraph,
      typename TNode,
      typename F = std::function<void(const TNode &, const size_t &)>>
  static
  ext::vector<TNode>
  findPathBidirectional(const TGraph &graph,
                        const TNode &start,
                        const TNode &goal,
                        F f_user = [](const TNode &, const size_t &) {});
 
  template<typename TGraph, typename TNode>
  static
  ext::vector<TNode>
  findPathBidirectionalRegistration(const TGraph &graph,
                                    const TNode &start,
                                    const TNode &goal) {
    return findPath(graph, start, goal);
  }
 
// =====================================================================================================================
 
 private:
 
// ---------------------------------------------------------------------------------------------------------------------
 
  template<typename TNode>
  struct Data {
    std::queue<TNode> queue;
    ext::set<TNode> v;
    ext::map<TNode, TNode> p;
    ext::map<TNode, size_t> d;
  };
 
// ---------------------------------------------------------------------------------------------------------------------
 
  template<typename TGraph, typename TNode, typename F1, typename F2>
  static
  ext::vector<TNode>
  implNormal(const TGraph &graph,
             const TNode &start,
             const TNode &goal,
             F1 f_user,
             F2 f_stop);
 
// ---------------------------------------------------------------------------------------------------------------------
 
  template<typename FSucc, typename TNode, typename F1, typename F2>
  static bool expansion(FSucc successors,
                        Data<TNode> &data,
                        F1 f_user,
                        F2 f_stop);
 
// ---------------------------------------------------------------------------------------------------------------------
 
  template<typename TGraph, typename TNode, typename F1>
  static
  ext::vector<TNode>
  implBidirectional(const TGraph &graph,
                    const TNode &start,
                    const TNode &goal,
                    F1 f_user);
 
// ---------------------------------------------------------------------------------------------------------------------
 
  template<typename TNode>
  inline static void init(BFS::Data<TNode> &data, const TNode &start);
 
// ---------------------------------------------------------------------------------------------------------------------
};
 
// =====================================================================================================================
 
template<typename TGraph, typename TNode, typename F>
void BFS::run(const TGraph &graph, const TNode &start, F f_user) {
  // goal -> start: in order to avoid call constructor
  implNormal(graph, start, start, f_user, [](const TNode &) -> bool { return false; });
}
 
// ---------------------------------------------------------------------------------------------------------------------
 
template<typename TGraph, typename TNode, typename F>
ext::vector<TNode>
BFS::findPath(const TGraph &graph,
              const TNode &start,
              const TNode &goal,
              F f_user) {
  return implNormal(graph, start, goal,
                    [&](const TNode &n, const size_t &d) -> bool {
                      f_user(n, d);
                      return false;
                    },
                    [&goal](const TNode &n) -> bool { return goal == n; });
}
 
// ---------------------------------------------------------------------------------------------------------------------
 
template<typename TGraph, typename TNode, typename F>
ext::vector<TNode>
BFS::findPathBidirectional(const TGraph &graph,
                           const TNode &start,
                           const TNode &goal,
                           F f_user) {
  return implBidirectional(graph, start, goal,
                           [&](const TNode &n, const size_t &d) -> bool {
                             f_user(n, d);
                             return false;
                           });
}
 
// =====================================================================================================================
 
// ---------------------------------------------------------------------------------------------------------------------
 
template<typename TGraph, typename TNode, typename F1, typename F2>
ext::vector<TNode>
BFS::implNormal(const TGraph &graph,
                const TNode &start,
                const TNode &goal,
                F1 f_user,
                F2 f_stop) {
  Data<TNode> data;
 
  // Init search
  init(data, start);
 
  while (!data.queue.empty()) {
    bool stop = expansion([&](const TNode &node) { return graph.successors(node); }, data, f_user, f_stop);
 
    if (stop) {
      break;
    }
 
  }
 
  return common::ReconstructPath::reconstructPath(data.p, start, goal);
}
 
// ---------------------------------------------------------------------------------------------------------------------
 
template<typename FSucc, typename TNode, typename F1, typename F2>
bool BFS::expansion(FSucc successors,
                    Data<TNode> &data,
                    F1 f_user,
                    F2 f_stop) {
  TNode n = data.queue.front();
  data.queue.pop();
 
  // Run user's function
  if (f_user(n, data.d[n])) {
    return true;
  }
 
  // Stop if reach the goal
  if (f_stop(n)) {
    return true;
  }
 
  for (const auto &s : successors(n)) {
    if (data.v.count(s) == 0) {
      data.v.insert(s);
      data.p.insert_or_assign(s, n);
      data.queue.push(s);
    }
  }
 
  return false;
}
 
// ---------------------------------------------------------------------------------------------------------------------
 
template<typename TGraph, typename TNode, typename F1>
ext::vector<TNode>
BFS::implBidirectional(const TGraph &graph,
                       const TNode &start,
                       const TNode &goal,
                       F1 f_user) {
  Data<TNode> forward_data;
  Data<TNode> backward_data;
  ext::vector<TNode> intersection;
 
  // Init forward search
  init(forward_data, start);
 
  // Init backward search
  init(backward_data, goal);
 
  while (!forward_data.queue.empty() && !backward_data.queue.empty()) {
    // Forward search relaxation
    bool stop = expansion([&](const TNode &node) { return graph.successors(node); }, forward_data, f_user,
                          [&](const TNode &n) {
                            if (backward_data.v.find(n) != backward_data.v.end()) {
                              intersection.push_back(n);
                              return true;
                            }
                            return false;
                          });
 
 
    // If there is a intersection, then we can reconstruct path
    if (stop) {
      return common::ReconstructPath::reconstructPath(forward_data.p,
                                                      backward_data.p,
                                                      start,
                                                      goal,
                                                      *intersection.begin());
    }
 
    // Backward search relaxation
    stop = expansion([&](const TNode &node) { return graph.predecessors(node); }, backward_data, f_user,
                     [&](const TNode &n) {
                       if (forward_data.v.find(n) != forward_data.v.end()) {
                         intersection.push_back(n);
                         return true;
                       }
                       return false;
                     });
 
    // If there is a intersection, then we can reconstruct path
    if (stop) {
      return common::ReconstructPath::reconstructPath(forward_data.p,
                                                      backward_data.p,
                                                      start,
                                                      goal,
                                                      *intersection.begin());
    }
  }
 
  // Path not found -> return empty vector
  return ext::vector<TNode>();
}
 
// ---------------------------------------------------------------------------------------------------------------------
 
template<typename TNode>
void BFS::init(BFS::Data<TNode> &data, const TNode &start) {
  data.queue.push(start);
  data.v.insert(start);
  data.p.insert_or_assign(start, start);
  data.d[start] = 0;
}
 
// ---------------------------------------------------------------------------------------------------------------------
 
} // namespace traverse
 
} // namespace graph