Add BFS algorithm.

alib2graph_algo/src/common/ReconstructPath.hpp

+// ReconstructPath.hpp
+//
+//     Created on: 19. 12. 2017
+//         Author: Jan Uhlik
+//    Modified by:
+//
+// Copyright (c) 2017 Czech Technical University in Prague | Faculty of Information Technology. All rights reserved.
+// Git repository: https://gitlab.fit.cvut.cz/algorithms-library-toolkit/automata-library
+
+#ifndef ALIB2_RECONSTRUCTPATH_HPP
+#define ALIB2_RECONSTRUCTPATH_HPP
+
+#include <alib/map>
+#include <alib/vector>
+#include <algorithm>
+
+namespace common {
+
+class ReconstructPath {
+// ---------------------------------------------------------------------------------------------------------------------
+ public:
+  template<typename TNode>
+  static ext::vector<TNode> reconstructPath(const ext::map<TNode, TNode> &p,
+                                            const TNode &start,
+                                            const TNode &goal);
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+  template<typename TNode>
+  static ext::vector<TNode> reconstructPath(const ext::map<TNode, TNode> &p_forward,
+                                            const ext::map<TNode, TNode> &p_backward,
+                                            const TNode &start,
+                                            const TNode &goal,
+                                            const TNode &intersection_node);
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+  template<typename TNode, typename TWeight>
+  static
+  ext::pair<ext::vector<TNode>, TWeight>
+  reconstructWeightedPath(const ext::map<TNode, TNode> &p,
+                          const ext::map<TNode, TWeight> &g,
+                          const TNode &start,
+                          const TNode &goal);
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+  template<typename TNode, typename TWeight>
+  static
+  ext::pair<ext::vector<TNode>, TWeight>
+  reconstructWeightedPath(const ext::map<TNode, TNode> &p_forward,
+                          const ext::map<TNode, TNode> &p_backward,
+                          const ext::map<TNode, TWeight> &g_forward,
+                          const ext::map<TNode, TWeight> &g_backward,
+                          const TNode &start,
+                          const TNode &goal,
+                          const TNode &intersection_node);
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+  template<typename TNode>
+  static
+  ext::vector<TNode>
+  joinPath(ext::vector<TNode> &forward_path, const ext::vector<TNode> &backward_path);
+
+// ---------------------------------------------------------------------------------------------------------------------
+};
+// =====================================================================================================================
+
+template<typename TNode>
+ext::vector<TNode> ReconstructPath::reconstructPath(const ext::map<TNode, TNode> &p,
+                                                    const TNode &start,
+                                                    const TNode &goal) {
+  ext::vector<TNode> path;
+
+  // Path not found -> return empty vector
+  if (p.find(goal) == p.end()) {
+    return path;
+  }
+
+  path.push_back(goal);
+  TNode current_vertex = goal;
+
+  while (current_vertex != start) {
+    current_vertex = p.at(current_vertex);
+    path.push_back(current_vertex);
+  }
+
+  // Path need to be reverse.
+  std::reverse(path.begin(), path.end());
+
+  return path;
+}
+
+template<typename TNode>
+ext::vector<TNode> ReconstructPath::reconstructPath(const ext::map<TNode, TNode> &p_forward,
+                                                    const ext::map<TNode, TNode> &p_backward,
+                                                    const TNode &start,
+                                                    const TNode &goal,
+                                                    const TNode &intersection_node) {
+  ext::vector<TNode> path;
+
+  // Path not found -> return empty vector
+  if (p_forward.find(intersection_node) == p_forward.end()
+      || p_backward.find(intersection_node) == p_backward.end()) {
+    return path;
+  }
+
+  // First part of the path. From start to intersectionVertex (include).
+  path.push_back(intersection_node);
+  TNode current_vertex = intersection_node;
+
+  while (current_vertex != start) {
+    current_vertex = p_forward.at(current_vertex);
+    path.push_back(current_vertex);
+  }
+  // This part of the path need to be reverse.
+  std::reverse(path.begin(), path.end());
+
+  // Second part of the path. From intersectionVertex (exclude) to goal.
+  current_vertex = intersection_node;
+  while (current_vertex != goal) {
+    current_vertex = p_backward.at(current_vertex);
+    path.push_back(current_vertex);
+  }
+  // No reverse at the end.
+
+  return path;
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+template<typename TNode, typename TWeight>
+ext::pair<ext::vector<TNode>, TWeight>
+ReconstructPath::reconstructWeightedPath(const ext::map<TNode, TNode> &p,
+                                         const ext::map<TNode, TWeight> &g,
+                                         const TNode &start,
+                                         const TNode &goal) {
+  if (g.find(goal) == g.end()) {
+    return ext::make_pair(ext::vector<TNode>(), std::numeric_limits<TWeight>::max());
+  }
+
+  return ext::make_pair(reconstructPath(p, start, goal), g.at(goal));
+}
+
+template<typename TNode, typename TWeight>
+ext::pair<ext::vector<TNode>, TWeight>
+ReconstructPath::reconstructWeightedPath(const ext::map<TNode, TNode> &p_forward,
+                                         const ext::map<TNode, TNode> &p_backward,
+                                         const ext::map<TNode, TWeight> &g_forward,
+                                         const ext::map<TNode, TWeight> &g_backward,
+                                         const TNode &start,
+                                         const TNode &goal,
+                                         const TNode &intersection_node) {
+  if (g_forward.find(intersection_node) == g_forward.end() || g_backward.find(intersection_node) == g_backward.end()) {
+    return ext::make_pair(ext::vector<TNode>(), std::numeric_limits<TWeight>::max());
+  }
+
+  return ext::make_pair(reconstructPath(p_forward, p_backward, start, goal, intersection_node),
+                        g_forward.at(intersection_node) + g_backward.at(intersection_node));
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+template<typename TNode>
+ext::vector<TNode> ReconstructPath::joinPath(ext::vector<TNode> &forward_path,
+                                             const ext::vector<TNode> &backward_path) {
+  // ++ because we want skip the insertion node -> already in forward_path
+  forward_path.insert(forward_path.end(), ++backward_path.rbegin(), backward_path.rend());
+  return forward_path;
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+} // namespace common
+
+#endif //ALIB2_RECONSTRUCTPATH_HPP

alib2graph_algo/src/traverse/BFS.cpp

+// BFS.cpp
+//
+// Created on: 08. 12. 2017
+// Author: Jan Uhlik
+// Modified by:
+//
+// Copyright (c) 2017 Czech Technical University in Prague | Faculty of Information Technology. All rights reserved.
+// Git repository: https://gitlab.fit.cvut.cz/algorithms-library-toolkit/automata-library
+
+#include "BFS.hpp"
+
+#include <registration/AlgoRegistration.hpp>
+#include <graph/GraphClasses.hpp>
+#include <grid/GridClasses.hpp>
+
+namespace traverse {
+class BFSBidirectional {};
+}
+
+namespace {
+
+// ---------------------------------------------------------------------------------------------------------------------
+// Normal Graph uni-directional
+
+auto BFS1 = registration::AbstractRegister<traverse::BFS,
+                                           ext::vector<DefaultNodeType>,
+                                           const graph::UndirectedGraph<> &,
+                                           const DefaultNodeType &,
+                                           const DefaultNodeType &>(traverse::BFS::findPathRegistration);
+
+auto BFS2 = registration::AbstractRegister<traverse::BFS,
+                                           ext::vector<DefaultNodeType>,
+                                           const graph::UndirectedMultiGraph<> &,
+                                           const DefaultNodeType &,
+                                           const DefaultNodeType &>(traverse::BFS::findPathRegistration);
+
+auto BFS3 = registration::AbstractRegister<traverse::BFS,
+                                           ext::vector<DefaultNodeType>,
+                                           const graph::DirectedGraph<> &,
+                                           const DefaultNodeType &,
+                                           const DefaultNodeType &>(traverse::BFS::findPathRegistration);
+
+auto BFS4 = registration::AbstractRegister<traverse::BFS,
+                                           ext::vector<DefaultNodeType>,
+                                           const graph::DirectedMultiGraph<> &,
+                                           const DefaultNodeType &,
+                                           const DefaultNodeType &>(traverse::BFS::findPathRegistration);
+
+auto BFS5 = registration::AbstractRegister<traverse::BFS,
+                                           ext::vector<DefaultNodeType>,
+                                           const graph::MixedGraph<> &,
+                                           const DefaultNodeType &,
+                                           const DefaultNodeType &>(traverse::BFS::findPathRegistration);
+
+auto BFS6 = registration::AbstractRegister<traverse::BFS,
+                                           ext::vector<DefaultNodeType>,
+                                           const graph::MixedMultiGraph<> &,
+                                           const DefaultNodeType &,
+                                           const DefaultNodeType &>(traverse::BFS::findPathRegistration);
+
+auto BFSGrid1 = registration::AbstractRegister<traverse::BFS,
+                                               ext::vector<DefaultSquareGridNodeType>,
+                                               const grid::SquareGrid4<> &,
+                                               const DefaultSquareGridNodeType &,
+                                               const DefaultSquareGridNodeType &>(traverse::BFS::findPathRegistration);
+
+auto BFSGrid2 = registration::AbstractRegister<traverse::BFS,
+                                               ext::vector<DefaultSquareGridNodeType>,
+                                               const grid::SquareGrid8<> &,
+                                               const DefaultSquareGridNodeType &,
+                                               const DefaultSquareGridNodeType &>(traverse::BFS::findPathRegistration);
+
+// ---------------------------------------------------------------------------------------------------------------------
+// Normal Graph bidirectional
+
+auto BFSBidirectional1 = registration::AbstractRegister<traverse::BFSBidirectional,
+                                                        ext::vector<DefaultNodeType>,
+                                                        const graph::UndirectedGraph<> &,
+                                                        const DefaultNodeType &,
+                                                        const DefaultNodeType &>(traverse::BFS::findPathBidirectionalRegistration);
+
+auto BFSBidirectional2 = registration::AbstractRegister<traverse::BFSBidirectional,
+                                                        ext::vector<DefaultNodeType>,
+                                                        const graph::UndirectedMultiGraph<> &,
+                                                        const DefaultNodeType &,
+                                                        const DefaultNodeType &>(traverse::BFS::findPathBidirectionalRegistration);
+
+auto BFSBidirectional3 = registration::AbstractRegister<traverse::BFSBidirectional,
+                                                        ext::vector<DefaultNodeType>,
+                                                        const graph::DirectedGraph<> &,
+                                                        const DefaultNodeType &,
+                                                        const DefaultNodeType &>(traverse::BFS::findPathBidirectionalRegistration);
+
+auto BFSBidirectional4 = registration::AbstractRegister<traverse::BFSBidirectional,
+                                                        ext::vector<DefaultNodeType>,
+                                                        const graph::DirectedMultiGraph<> &,
+                                                        const DefaultNodeType &,
+                                                        const DefaultNodeType &>(traverse::BFS::findPathBidirectionalRegistration);
+
+auto BFSBidirectional5 = registration::AbstractRegister<traverse::BFSBidirectional,
+                                                        ext::vector<DefaultNodeType>,
+                                                        const graph::MixedGraph<> &,
+                                                        const DefaultNodeType &,
+                                                        const DefaultNodeType &>(traverse::BFS::findPathBidirectionalRegistration);
+
+auto BFSBidirectional6 = registration::AbstractRegister<traverse::BFSBidirectional,
+                                                        ext::vector<DefaultNodeType>,
+                                                        const graph::MixedMultiGraph<> &,
+                                                        const DefaultNodeType &,
+                                                        const DefaultNodeType &>(traverse::BFS::findPathBidirectionalRegistration);
+
+auto BFSGridBidirectional1 = registration::AbstractRegister<traverse::BFSBidirectional,
+                                                            ext::vector<DefaultSquareGridNodeType>,
+                                                            const grid::SquareGrid4<> &,
+                                                            const DefaultSquareGridNodeType &,
+                                                            const DefaultSquareGridNodeType &>(traverse::BFS::findPathBidirectionalRegistration);
+
+auto BFSGridbidirectional2 = registration::AbstractRegister<traverse::BFSBidirectional,
+                                                            ext::vector<DefaultSquareGridNodeType>,
+                                                            const grid::SquareGrid8<> &,
+                                                            const DefaultSquareGridNodeType &,
+                                                            const DefaultSquareGridNodeType &>(traverse::BFS::findPathBidirectionalRegistration);
+
+// ---------------------------------------------------------------------------------------------------------------------
+// Weighted Graph uni-directional
+
+auto WBFS1 = registration::AbstractRegister<traverse::BFS,
+                                            ext::vector<DefaultNodeType>,
+                                            const graph::WeightedUndirectedGraph<> &,
+                                            const DefaultNodeType &,
+                                            const DefaultNodeType &>(traverse::BFS::findPathRegistration);
+
+auto WBFS2 = registration::AbstractRegister<traverse::BFS,
+                                            ext::vector<DefaultNodeType>,
+                                            const graph::WeightedUndirectedMultiGraph<> &,
+                                            const DefaultNodeType &,
+                                            const DefaultNodeType &>(traverse::BFS::findPathRegistration);
+
+auto WBFS3 = registration::AbstractRegister<traverse::BFS,
+                                            ext::vector<DefaultNodeType>,
+                                            const graph::WeightedDirectedGraph<> &,
+                                            const DefaultNodeType &,
+                                            const DefaultNodeType &>(traverse::BFS::findPathRegistration);
+
+auto WBFS4 = registration::AbstractRegister<traverse::BFS,
+                                            ext::vector<DefaultNodeType>,
+                                            const graph::WeightedDirectedMultiGraph<> &,
+                                            const DefaultNodeType &,
+                                            const DefaultNodeType &>(traverse::BFS::findPathRegistration);
+
+auto WBFS5 = registration::AbstractRegister<traverse::BFS,
+                                            ext::vector<DefaultNodeType>,
+                                            const graph::WeightedMixedGraph<> &,
+                                            const DefaultNodeType &,
+                                            const DefaultNodeType &>(traverse::BFS::findPathRegistration);
+
+auto WBFS6 = registration::AbstractRegister<traverse::BFS,
+                                            ext::vector<DefaultNodeType>,
+                                            const graph::WeightedMixedMultiGraph<> &,
+                                            const DefaultNodeType &,
+                                            const DefaultNodeType &>(traverse::BFS::findPathRegistration);
+
+auto WBFSGrid1 = registration::AbstractRegister<traverse::BFS,
+                                                ext::vector<DefaultSquareGridNodeType>,
+                                                const grid::WeightedSquareGrid4<> &,
+                                                const DefaultSquareGridNodeType &,
+                                                const DefaultSquareGridNodeType &>(traverse::BFS::findPathRegistration);
+
+auto WBFSGrid2 = registration::AbstractRegister<traverse::BFS,
+                                                ext::vector<DefaultSquareGridNodeType>,
+                                                const grid::WeightedSquareGrid8<> &,
+                                                const DefaultSquareGridNodeType &,
+                                                const DefaultSquareGridNodeType &>(traverse::BFS::findPathRegistration);
+
+// ---------------------------------------------------------------------------------------------------------------------
+// Weighted Graph bidirectional
+
+auto WBFSBidirectional1 = registration::AbstractRegister<traverse::BFSBidirectional,
+                                                         ext::vector<DefaultNodeType>,
+                                                         const graph::WeightedUndirectedGraph<> &,
+                                                         const DefaultNodeType &,
+                                                         const DefaultNodeType &>(traverse::BFS::findPathBidirectionalRegistration);
+
+auto WBFSBidirectional2 = registration::AbstractRegister<traverse::BFSBidirectional,
+                                                         ext::vector<DefaultNodeType>,
+                                                         const graph::WeightedUndirectedMultiGraph<> &,
+                                                         const DefaultNodeType &,
+                                                         const DefaultNodeType &>(traverse::BFS::findPathBidirectionalRegistration);
+
+auto WBFSBidirectional3 = registration::AbstractRegister<traverse::BFSBidirectional,
+                                                         ext::vector<DefaultNodeType>,
+                                                         const graph::WeightedDirectedGraph<> &,
+                                                         const DefaultNodeType &,
+                                                         const DefaultNodeType &>(traverse::BFS::findPathBidirectionalRegistration);
+
+auto WBFSBidirectional4 = registration::AbstractRegister<traverse::BFSBidirectional,
+                                                         ext::vector<DefaultNodeType>,
+                                                         const graph::WeightedDirectedMultiGraph<> &,
+                                                         const DefaultNodeType &,
+                                                         const DefaultNodeType &>(traverse::BFS::findPathBidirectionalRegistration);
+
+auto WBFSBidirectional5 = registration::AbstractRegister<traverse::BFSBidirectional,
+                                                         ext::vector<DefaultNodeType>,
+                                                         const graph::WeightedMixedGraph<> &,
+                                                         const DefaultNodeType &,
+                                                         const DefaultNodeType &>(traverse::BFS::findPathBidirectionalRegistration);
+
+auto WBFSBidirectional6 = registration::AbstractRegister<traverse::BFSBidirectional,
+                                                         ext::vector<DefaultNodeType>,
+                                                         const graph::WeightedMixedMultiGraph<> &,
+                                                         const DefaultNodeType &,
+                                                         const DefaultNodeType &>(traverse::BFS::findPathBidirectionalRegistration);
+
+auto WBFSGridBidirectional1 = registration::AbstractRegister<traverse::BFSBidirectional,
+                                                             ext::vector<DefaultSquareGridNodeType>,
+                                                             const grid::WeightedSquareGrid4<> &,
+                                                             const DefaultSquareGridNodeType &,
+                                                             const DefaultSquareGridNodeType &>(traverse::BFS::findPathBidirectionalRegistration);
+
+auto WBFSGridbidirectional2 = registration::AbstractRegister<traverse::BFSBidirectional,
+                                                             ext::vector<DefaultSquareGridNodeType>,
+                                                             const grid::WeightedSquareGrid8<> &,
+                                                             const DefaultSquareGridNodeType &,
+                                                             const DefaultSquareGridNodeType &>(traverse::BFS::findPathBidirectionalRegistration);
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+}

alib2graph_algo/src/traverse/BFS.hpp

+// BFS.hpp
+//
+//     Created on: 13. 11. 2017
+//         Author: Jan Uhlik
+//    Modified by:
+//
+// Copyright (c) 2017 Czech Technical University in Prague | Faculty of Information Technology. All rights reserved.
+// Git repository: https://gitlab.fit.cvut.cz/algorithms-library-toolkit/automata-library
+
+#ifndef ALIB2_BFS_HPP
+#define ALIB2_BFS_HPP
+
+#include <functional>
+#include <queue>
+#include <alib/map>
+#include <alib/set>
+#include <iostream>
+
+#include <common/ReconstructPath.hpp>
+
+namespace traverse {
+
+class BFS {
+// ---------------------------------------------------------------------------------------------------------------------
+
+ public:
+
+  /// Run BFS algorithm from the \p start node in the \p graph.
+  ///
+  /// Whenever node is opened, \p f_user is called with two parameters (the opened node and distance to this node).
+  /// If return of \p f_user is true, then the algorithm is stopped.
+  ///
+  /// \param graph to explore.
+  /// \param start initial node.
+  /// \param f_user function which is called for every opened node with value of currently shortest path.
+  ///
+  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; });
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+  /// Find the shortest path using BFS algorithm from the \p start node to the \p goal node in the \p graph.
+  ///
+  /// Whenever node is opened, \p f_user is called with two parameters (the opened node and distance to this node).
+  ///
+  /// \param graph to explore.
+  /// \param start initial node.
+  /// \param goal final node.
+  /// \param f_user function which is called for every opened node with value of currently shortest path.
+  ///
+  /// \returns nodes in shortest path, if there is no such path vector is empty.
+  ///
+  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);
+
+  }
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+  /// Find the shortest path using BFS algorithm from the \p start node to the \p goal node in the \p graph.
+  /// This algorithm is run in both direction, from \p start and also from \p goal.
+  ///
+  /// Whenever node is opened, \p f_user is called with two parameters (the opened node and distance to this node).
+  ///
+  /// \param graph to explore.
+  /// \param start initial node.
+  /// \param goal final node.
+  /// \param f_user function which is called for every opened node with value of currently shortest path.
+  ///
+  /// \returns nodes in shortest path, if there is no such path vector is empty.
+  ///
+  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, 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
+#endif // ALIB2_BFS_HPP