From 96425d9c9f9ec948f5a64eca773f237101149563 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jan=20Uhl=C3=ADk?= <jan@uhlik.me>
Date: Thu, 29 Mar 2018 21:01:39 +0200
Subject: [PATCH] Add IDA* algorithm.
---
alib2graph_algo/src/shortest_path/IDAStar.cpp | 92 ++++++++
alib2graph_algo/src/shortest_path/IDAStar.hpp | 208 ++++++++++++++++++
2 files changed, 300 insertions(+)
create mode 100644 alib2graph_algo/src/shortest_path/IDAStar.cpp
create mode 100644 alib2graph_algo/src/shortest_path/IDAStar.hpp
diff --git a/alib2graph_algo/src/shortest_path/IDAStar.cpp b/alib2graph_algo/src/shortest_path/IDAStar.cpp
new file mode 100644
index 0000000000..cf52bd4593
--- /dev/null
+++ b/alib2graph_algo/src/shortest_path/IDAStar.cpp
@@ -0,0 +1,92 @@
+// IDAStar.cpp
+//
+// Created on: 08. 03. 2018
+// 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 "IDAStar.hpp"
+
+#include <registration/AlgoRegistration.hpp>
+
+namespace {
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+auto IDAStar1 = registration::AbstractRegister<shortest_path::IDAStar,
+ ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+ const graph::WeightedUndirectedGraph<> &,
+ const DefaultNodeType &,
+ const DefaultNodeType &,
+ std::function<DefaultWeightType(const DefaultNodeType &,
+ const DefaultNodeType &)> >(
+ shortest_path::IDAStar::findPathRegistration);
+
+auto IDAStar2 = registration::AbstractRegister<shortest_path::IDAStar,
+ ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+ const graph::WeightedUndirectedMultiGraph<> &,
+ const DefaultNodeType &,
+ const DefaultNodeType &,
+ std::function<DefaultWeightType(const DefaultNodeType &,
+ const DefaultNodeType &)> >(
+ shortest_path::IDAStar::findPathRegistration);
+
+auto IDAStar3 = registration::AbstractRegister<shortest_path::IDAStar,
+ ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+ const graph::WeightedDirectedGraph<> &,
+ const DefaultNodeType &,
+ const DefaultNodeType &,
+ std::function<DefaultWeightType(const DefaultNodeType &,
+ const DefaultNodeType &)> >(
+ shortest_path::IDAStar::findPathRegistration);
+
+auto IDAStar4 = registration::AbstractRegister<shortest_path::IDAStar,
+ ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+ const graph::WeightedDirectedMultiGraph<> &,
+ const DefaultNodeType &,
+ const DefaultNodeType &,
+ std::function<DefaultWeightType(const DefaultNodeType &,
+ const DefaultNodeType &)> >(
+ shortest_path::IDAStar::findPathRegistration);
+
+auto IDAStar5 = registration::AbstractRegister<shortest_path::IDAStar,
+ ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+ const graph::WeightedMixedGraph<> &,
+ const DefaultNodeType &,
+ const DefaultNodeType &,
+ std::function<DefaultWeightType(const DefaultNodeType &,
+ const DefaultNodeType &)> >(
+ shortest_path::IDAStar::findPathRegistration);
+
+auto IDAStar6 = registration::AbstractRegister<shortest_path::IDAStar,
+ ext::pair<ext::vector<DefaultNodeType>, DefaultWeightType>,
+ const graph::WeightedMixedMultiGraph<> &,
+ const DefaultNodeType &,
+ const DefaultNodeType &,
+ std::function<DefaultWeightType(const DefaultNodeType &,
+ const DefaultNodeType &)> >(
+ shortest_path::IDAStar::findPathRegistration);
+
+auto IDAStarGrid1 = registration::AbstractRegister<shortest_path::IDAStar,
+ ext::pair<ext::vector<DefaultSquareGridNodeType>, DefaultWeightType>,
+ const grid::WeightedSquareGrid4<> &,
+ const DefaultSquareGridNodeType &,
+ const DefaultSquareGridNodeType &,
+ std::function<DefaultWeightType(const DefaultSquareGridNodeType &,
+ const DefaultSquareGridNodeType &)> >(
+ shortest_path::IDAStar::findPathRegistration);
+
+auto IDAStarGrid2 = registration::AbstractRegister<shortest_path::IDAStar,
+ ext::pair<ext::vector<DefaultSquareGridNodeType>, DefaultWeightType>,
+ const grid::WeightedSquareGrid8<> &,
+ const DefaultSquareGridNodeType &,
+ const DefaultSquareGridNodeType &,
+ std::function<DefaultWeightType(const DefaultSquareGridNodeType &,
+ const DefaultSquareGridNodeType &)> >(
+ shortest_path::IDAStar::findPathRegistration);
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+}
diff --git a/alib2graph_algo/src/shortest_path/IDAStar.hpp b/alib2graph_algo/src/shortest_path/IDAStar.hpp
new file mode 100644
index 0000000000..68d1359891
--- /dev/null
+++ b/alib2graph_algo/src/shortest_path/IDAStar.hpp
@@ -0,0 +1,208 @@
+// IDAStar.hpp
+//
+// Created on: 08. 03. 2018
+// 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_IDASTAR_HPP
+#define ALIB2_IDASTAR_HPP
+
+#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>
+
+using namespace std;
+
+namespace shortest_path {
+
+class IDAStar {
+// ---------------------------------------------------------------------------------------------------------------------
+ public:
+
+ /// Find the shortest path using IDAStar algorithm from the \p start node to the \p goal node in the \p graph.
+ ///
+ /// The heuristic function must be admissible and monotone.
+ ///
+ /// The heuristic function must be admissible and monotone.
+ ///
+ /// \param graph to explore.
+ /// \param start initial node.
+ /// \param goal final node.
+ /// \param f_heuristic heuristic function which accept node and return edge_type::weight_type.
+ /// \param f_user function which is called for every opened 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 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 (1) {
+ bool found;
+ weight_type t;
+ std::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);
+ std::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
+#endif //ALIB2_IDASTAR_HPP
--
GitLab