diff --git a/alib2graph_algo/src/shortest_path/Dijkstra.cpp b/alib2graph_algo/src/shortest_path/Dijkstra.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b90df9d45183ae02431578393d1229b0b32f6689
--- /dev/null
+++ b/alib2graph_algo/src/shortest_path/Dijkstra.cpp
@@ -0,0 +1,128 @@
+// Dijkstra.cpp
+//
+// Created on: 18. 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 "Dijkstra.hpp"
+
+#include <registration/AlgoRegistration.hpp>
+
+namespace shortest_path {
+class DijkstraBidirectional {};
+}
+
+namespace {
+
+// ---------------------------------------------------------------------------------------------------------------------
+// uni-directional
+
+auto Dijkstra1 = registration::AbstractRegister<shortest_path::Dijkstra,
+ ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+ const graph::WeightedUndirectedGraph<> &,
+ const DefaultNodeType &,
+ const DefaultNodeType &>(shortest_path::Dijkstra::findPathRegistration);
+
+auto Dijkstra2 = registration::AbstractRegister<shortest_path::Dijkstra,
+ ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+ const graph::WeightedUndirectedMultiGraph<> &,
+ const DefaultNodeType &,
+ const DefaultNodeType &>(shortest_path::Dijkstra::findPathRegistration);
+
+auto Dijkstra3 = registration::AbstractRegister<shortest_path::Dijkstra,
+ ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+ const graph::WeightedDirectedGraph<> &,
+ const DefaultNodeType &,
+ const DefaultNodeType &>(shortest_path::Dijkstra::findPathRegistration);
+
+auto Dijkstra4 = registration::AbstractRegister<shortest_path::Dijkstra,
+ ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+ const graph::WeightedDirectedMultiGraph<> &,
+ const DefaultNodeType &,
+ const DefaultNodeType &>(shortest_path::Dijkstra::findPathRegistration);
+
+auto Dijkstra5 = registration::AbstractRegister<shortest_path::Dijkstra,
+ ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+ const graph::WeightedMixedGraph<> &,
+ const DefaultNodeType &,
+ const DefaultNodeType &>(shortest_path::Dijkstra::findPathRegistration);
+
+auto Dijkstra6 = registration::AbstractRegister<shortest_path::Dijkstra,
+ ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+ const graph::WeightedMixedMultiGraph<> &,
+ const DefaultNodeType &,
+ const DefaultNodeType &>(shortest_path::Dijkstra::findPathRegistration);
+
+auto DijkstraGrid1 = registration::AbstractRegister<shortest_path::Dijkstra,
+ ext::pair<ext::vector<DefaultSquareGridNodeType>,
+ DefaultWeightType>,
+ const grid::WeightedSquareGrid4<> &,
+ const DefaultSquareGridNodeType &,
+ const DefaultSquareGridNodeType &>(shortest_path::Dijkstra::findPathRegistration);
+
+auto DijkstraGrid2 = registration::AbstractRegister<shortest_path::Dijkstra,
+ ext::pair<ext::vector<DefaultSquareGridNodeType>,
+ DefaultWeightType>,
+ const grid::WeightedSquareGrid8<> &,
+ const DefaultSquareGridNodeType &,
+ const DefaultSquareGridNodeType &>(shortest_path::Dijkstra::findPathRegistration);
+
+// ---------------------------------------------------------------------------------------------------------------------
+// bidirectional
+
+
+auto DijkstraBidirectional1 = registration::AbstractRegister<shortest_path::DijkstraBidirectional,
+ ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+ const graph::WeightedUndirectedGraph<> &,
+ const DefaultNodeType &,
+ const DefaultNodeType &>(shortest_path::Dijkstra::findPathBidirectionalRegistration);
+
+auto DijkstraBidirectional2 = registration::AbstractRegister<shortest_path::DijkstraBidirectional,
+ ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+ const graph::WeightedUndirectedMultiGraph<> &,
+ const DefaultNodeType &,
+ const DefaultNodeType &>(shortest_path::Dijkstra::findPathBidirectionalRegistration);
+
+auto DijkstraBidirectional3 = registration::AbstractRegister<shortest_path::DijkstraBidirectional,
+ ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+ const graph::WeightedDirectedGraph<> &,
+ const DefaultNodeType &,
+ const DefaultNodeType &>(shortest_path::Dijkstra::findPathBidirectionalRegistration);
+
+auto DijkstraBidirectional4 = registration::AbstractRegister<shortest_path::DijkstraBidirectional,
+ ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+ const graph::WeightedDirectedMultiGraph<> &,
+ const DefaultNodeType &,
+ const DefaultNodeType &>(shortest_path::Dijkstra::findPathBidirectionalRegistration);
+
+auto DijkstraBidirectional5 = registration::AbstractRegister<shortest_path::DijkstraBidirectional,
+ ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+ const graph::WeightedMixedGraph<> &,
+ const DefaultNodeType &,
+ const DefaultNodeType &>(shortest_path::Dijkstra::findPathBidirectionalRegistration);
+
+auto DijkstraBidirectional6 = registration::AbstractRegister<shortest_path::DijkstraBidirectional,
+ ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+ const graph::WeightedMixedMultiGraph<> &,
+ const DefaultNodeType &,
+ const DefaultNodeType &>(shortest_path::Dijkstra::findPathBidirectionalRegistration);
+
+auto DijkstraGridBidirectional1 = registration::AbstractRegister<shortest_path::DijkstraBidirectional,
+ ext::pair<ext::vector<DefaultSquareGridNodeType>,
+ DefaultWeightType>,
+ const grid::WeightedSquareGrid4<> &,
+ const DefaultSquareGridNodeType &,
+ const DefaultSquareGridNodeType &>(shortest_path::Dijkstra::findPathBidirectionalRegistration);
+
+auto DijkstraGridbidirectional2 = registration::AbstractRegister<shortest_path::DijkstraBidirectional,
+ ext::pair<ext::vector<DefaultSquareGridNodeType>,
+ DefaultWeightType>,
+ const grid::WeightedSquareGrid8<> &,
+ const DefaultSquareGridNodeType &,
+ const DefaultSquareGridNodeType &>(shortest_path::Dijkstra::findPathBidirectionalRegistration);
+
+// ---------------------------------------------------------------------------------------------------------------------
+}
diff --git a/alib2graph_algo/src/shortest_path/Dijkstra.hpp b/alib2graph_algo/src/shortest_path/Dijkstra.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..964fb830957626743750a479c86e318468c49e9f
--- /dev/null
+++ b/alib2graph_algo/src/shortest_path/Dijkstra.hpp
@@ -0,0 +1,402 @@
+// Dijkstra.hpp
+//
+// Created on: 18. 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_DIJKSTRA_HPP
+#define ALIB2_DIJKSTRA_HPP
+
+#include <list>
+#include <alib/set>
+#include <alib/map>
+#include <queue>
+#include <stdexcept>
+#include <functional>
+
+#include <common/ReconstructPath.hpp>
+#include <common/SupportFunction.hpp>
+
+namespace shortest_path {
+
+class Dijkstra {
+// ---------------------------------------------------------------------------------------------------------------------
+
+ public:
+
+ /// Run Dijkstra 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 value of currently shortest path).
+ /// 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.
+ ///
+ /// \note TEdge of \p graph must follow graph::edge::WeightedEdge interface.
+ /// \sa graph::edge_type::WeightedEdge.
+ ///
+ /// \throws std::out_of_range if \p graph contains an edge with a negative weight.
+ ///
+ template<
+ typename TGraph,
+ typename TNode,
+ typename F = std::function<bool(const TNode &,
+ const typename TGraph::edge_type::weight_type &)>>
+ static
+ void
+ run(const TGraph &graph,
+ const TNode &start,
+ F f_user = [](const TNode &,
+ const typename TGraph::edge_type::weight_type &) -> bool { return false; });
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+ /// Find the shortest path using Dijkstra 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 value of currently shortest path).
+ ///
+ /// \param graph to explore.
+ /// \param start initial node.
+ /// \param goal final node.
+ /// \param f_user function which is called for every open node with value of currently shortest path.
+ ///
+ /// \returns pair where first := shortest path := distance of path, if there is no such path vector is empty and distance std::numeric_limits<edge_type:weight_type>::max().
+ ///
+ /// \note TEdge of \p graph must follow graph::edge::WeightedEdge interface.
+ /// \sa graph::edge_type::WeightedEdge.
+ ///
+ /// \throws std::out_of_range if \p graph contains an edge with a negative weight.
+ ///
+ template<
+ typename TGraph,
+ typename TNode,
+ typename F = 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,
+ F f_user = [](const TNode &,
+ const typename TGraph::edge_type::weight_type &) {});
+
+ template<typename TGraph, typename TNode>
+ static
+ ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
+ findPathRegistration(const TGraph &graph,
+ const TNode &start,
+ const TNode &goal) {
+ return findPath(graph, start, goal);
+ }
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+ /// Find the shortest path using Dijkstra 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 value of currently shortest path).
+ ///
+ /// \param graph to explore.
+ /// \param start initial node.
+ /// \param goal final node.
+ /// \param f_user function which is called for every open node with value of currently shortest path.
+ ///
+ /// \returns pair where first := shortest path := distance of path, if there is no such path vector is empty and distance std::numeric_limits<edge_type:weight_type>::max().
+ ///
+ /// \note TEdge of \p graph must follow graph::edge::WeightedEdge interface.
+ /// \sa graph::edge_type::WeightedEdge.
+ ///
+ /// \throws std::out_of_range if \p graph contains an edge with a negative weight.
+ ///
+ template<typename TGraph,
+ typename TNode,
+ typename F = 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,
+ F f_user = [](const TNode &,
+ const typename TGraph::edge_type::weight_type &) {});
+
+ template<typename TGraph, typename TNode>
+ static
+ ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
+ findPathBidirectionalRegistration(const TGraph &graph,
+ const TNode &start,
+ const TNode &goal) {
+ return findPathBidirectional(graph, start, goal);
+ }
+
+
+// =====================================================================================================================
+
+ private:
+
+ template<typename TNode, typename TWeight>
+ struct Data {
+ ext::set<ext::pair<TWeight, TNode>> queue; // priority queue
+ ext::map<TNode, TWeight> g; // distances (aka G score)
+ ext::map<TNode, TNode> p; // parents
+ };
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+ template<typename TGraph, typename TNode, typename F1, typename F2>
+ static
+ ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
+ impl(const TGraph &graph,
+ const TNode &start,
+ const TNode &goal,
+ F1 f_user,
+ F2 f_stop);
+
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+ 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_user,
+ F2 f_stop,
+ F3 f_update);
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+ template<typename TGraph, typename TNode, typename F1, typename F2>
+ static
+ ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
+ implBidirectional(const TGraph &graph,
+ const TNode &start,
+ const TNode &goal,
+ F1 f_user,
+ F2 f_stop);
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+ template<typename TNode, typename TWeight>
+ inline static void init(Dijkstra::Data<TNode, TWeight> &data, const TNode &start);
+
+
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+};
+
+// =====================================================================================================================
+
+template<typename TGraph, typename TNode, typename F>
+void Dijkstra::run(const TGraph &graph, const TNode &start, F f_user) {
+ // goal -> start: in order to avoid call constructor
+ impl(graph, start, start, f_user, [](const TNode &) -> bool { return false; });
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+template<typename TGraph, typename TNode, typename F>
+ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
+Dijkstra::findPath(const TGraph &graph,
+ const TNode &start,
+ const TNode &goal,
+ F f_user) {
+ // We need to change user function to return false for every node in order to have only one relaxation function
+ return impl(graph, start, goal,
+ [&](const TNode &n, const typename TGraph::edge_type::weight_type &w) -> bool {
+ f_user(n, w);
+ return false;
+ }, [&goal](const TNode &n) -> bool { return goal == n; });
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+template<typename TGraph, typename TNode, typename F>
+ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
+Dijkstra::findPathBidirectional(const TGraph &graph,
+ const TNode &start,
+ const TNode &goal,
+ F f_user) {
+ // We need to change user function to return false for every node in order to have only one relaxation function
+ return implBidirectional(graph, start, goal,
+ [&](const TNode &n, const typename TGraph::edge_type::weight_type &w) -> bool {
+ f_user(n, w);
+ return false;
+ },
+ [](const TNode &) -> bool { return false; });
+}
+
+// =====================================================================================================================
+
+template<typename TGraph, typename TNode, typename F1, typename F2>
+ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
+Dijkstra::impl(const TGraph &graph,
+ const TNode &start,
+ const TNode &goal,
+ F1 f_user,
+ F2 f_stop) {
+ using weight_type = typename TGraph::edge_type::weight_type;
+
+ Data<TNode, weight_type> data;
+
+ // Init search
+ init(data, start);
+
+ while (!data.queue.empty()) {
+ bool stop = relaxation([&](const auto &node) -> auto { return graph.successorEdges(node); },
+ data,
+ f_user,
+ f_stop,
+ [](const TNode &) -> void {});
+
+ 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 Dijkstra::relaxation(FEdges successor_edges,
+ Data<TNode, TWeight> &data,
+ F1 f_user,
+ F2 f_stop,
+ F3 f_update) {
+ TNode n = data.queue.begin()->second;
+ data.queue.erase(data.queue.begin());
+
+ // Run user's function
+ if (f_user(n, data.g[n])) {
+ return true;
+ }
+
+ // 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("Dijkstra: 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_d = data.g.find(s);
+
+ // If not or the distance can be improve do relaxation
+ if (search_d == data.g.end() || search_d->second > gscore) {
+ // Search if the node s is in OPEN
+ auto search_q = data.queue.find(ext::make_pair(search_d->second, s));
+ if (search_q != data.queue.end()) {
+ // Erase node from priority queue
+ data.queue.erase(search_q);
+ }
+
+ data.g[s] = gscore;
+ data.p.insert_or_assign(s, n);
+ data.queue.insert(ext::make_pair(data.g[s], s));
+
+ f_update(s); // Update currently best path
+ }
+ }
+
+ return false;
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+template<typename TGraph, typename TNode, typename F1, typename F2>
+ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
+Dijkstra::implBidirectional(const TGraph &graph,
+ const TNode &start,
+ const TNode &goal,
+ F1 f_user,
+ F2 f_stop) {
+ using weight_type = typename TGraph::edge_type::weight_type;
+
+ // Check for negative edge
+ 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, backward_data;
+
+ // Init forward search
+ init(forward_data, start);
+ auto f_forward_update = [&](const auto &s) -> void {
+ if (backward_data.g.find(s) != backward_data.g.end()) {
+ if (forward_data.g.at(s) + backward_data.g.at(s) < p) {
+ p = forward_data.g.at(s) + backward_data.g.at(s);
+ intersection_nodes.push_back(s);
+ }
+ }
+ };
+
+ // Init backward search
+ init(backward_data, goal);
+ auto f_backward_update = [&](const auto &s) -> void {
+ if (forward_data.g.find(s) != forward_data.g.end()) {
+ if (backward_data.g.at(s) + forward_data.g.at(s) < p) {
+ p = backward_data.g.at(s) + forward_data.g.at(s);
+ intersection_nodes.push_back(s);
+ }
+ }
+ };
+
+ while (!forward_data.queue.empty() && !backward_data.queue.empty()) {
+ if (forward_data.queue.begin()->first + backward_data.queue.begin()->first + eps >= p) {
+ return common::ReconstructPath::reconstructWeightedPath(forward_data.p,
+ backward_data.p,
+ forward_data.g,
+ backward_data.g,
+ start,
+ goal,
+ intersection_nodes.back());
+ }
+
+ // Expand the lower value
+ if (forward_data.queue.begin()->first < backward_data.queue.begin()->first) {
+ // Forward search relaxationBidirectional
+ relaxation([&](const auto &node) -> auto { return graph.successorEdges(node); },
+ forward_data,
+ f_user,
+ f_stop,
+ f_forward_update);
+ } else {
+ // Backward search relaxationBidirectional
+ relaxation([&](const auto &node) -> auto { return graph.predecessorEdges(node); },
+ backward_data,
+ f_user,
+ f_stop,
+ f_backward_update);
+ }
+ }
+
+ return ext::make_pair(ext::vector<TNode>(), p);
+
+}
+// ---------------------------------------------------------------------------------------------------------------------
+
+template<typename TNode, typename TWeight>
+void Dijkstra::init(Dijkstra::Data<TNode, TWeight> &data, const TNode &start) {
+ data.g[start] = 0;
+ data.p.insert_or_assign(start, start);
+ data.queue.insert(ext::make_pair(data.g[start], start));
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+} // namespace shortest_path
+#endif //ALIB2_DIJKSTRA_HPP