From 895aac50e6544137596280e3248e80484665ad1a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jan=20Uhl=C3=ADk?= <jan@uhlik.me>
Date: Thu, 29 Mar 2018 20:59:20 +0200
Subject: [PATCH] Add basic sqaure grid heuristic functions.
---
.../src/heuristic/ChebyshevDistance.cpp | 26 ++++++++
.../src/heuristic/ChebyshevDistance.hpp | 57 ++++++++++++++++++
.../src/heuristic/DiagonalDistance.cpp | 26 ++++++++
.../src/heuristic/DiagonalDistance.hpp | 59 +++++++++++++++++++
.../src/heuristic/EuclideanDistance.cpp | 26 ++++++++
.../src/heuristic/EuclideanDistance.hpp | 59 +++++++++++++++++++
.../src/heuristic/EuclideanDistanceNoSQRT.cpp | 26 ++++++++
.../src/heuristic/EuclideanDistanceNoSQRT.hpp | 59 +++++++++++++++++++
.../src/heuristic/ManhattenDistance.cpp | 26 ++++++++
.../src/heuristic/ManhattenDistance.hpp | 59 +++++++++++++++++++
.../src/heuristic/SquareGridHeuristics.hpp | 19 ++++++
11 files changed, 442 insertions(+)
create mode 100644 alib2graph_algo/src/heuristic/ChebyshevDistance.cpp
create mode 100644 alib2graph_algo/src/heuristic/ChebyshevDistance.hpp
create mode 100644 alib2graph_algo/src/heuristic/DiagonalDistance.cpp
create mode 100644 alib2graph_algo/src/heuristic/DiagonalDistance.hpp
create mode 100644 alib2graph_algo/src/heuristic/EuclideanDistance.cpp
create mode 100644 alib2graph_algo/src/heuristic/EuclideanDistance.hpp
create mode 100644 alib2graph_algo/src/heuristic/EuclideanDistanceNoSQRT.cpp
create mode 100644 alib2graph_algo/src/heuristic/EuclideanDistanceNoSQRT.hpp
create mode 100644 alib2graph_algo/src/heuristic/ManhattenDistance.cpp
create mode 100644 alib2graph_algo/src/heuristic/ManhattenDistance.hpp
create mode 100644 alib2graph_algo/src/heuristic/SquareGridHeuristics.hpp
diff --git a/alib2graph_algo/src/heuristic/ChebyshevDistance.cpp b/alib2graph_algo/src/heuristic/ChebyshevDistance.cpp
new file mode 100644
index 0000000000..9bf78f5bc1
--- /dev/null
+++ b/alib2graph_algo/src/heuristic/ChebyshevDistance.cpp
@@ -0,0 +1,26 @@
+// ChebyshevDistance.cpp
+//
+// Created on: 15. 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 "ChebyshevDistance.hpp"
+
+#include <registration/AlgoRegistration.hpp>
+#include <common/DefaultTypes.hpp>
+
+namespace {
+
+auto ChebyshevDistance = registration::AbstractRegister<heuristic::ChebyshevDistance,
+ std::function<DefaultCoordinateType(const ext::pair<
+ DefaultCoordinateType,
+ DefaultCoordinateType> &,
+ const ext::pair<
+ DefaultCoordinateType,
+ DefaultCoordinateType> &)> >(
+ heuristic::ChebyshevDistance::chebyshevDistanceFunction);
+
+}
diff --git a/alib2graph_algo/src/heuristic/ChebyshevDistance.hpp b/alib2graph_algo/src/heuristic/ChebyshevDistance.hpp
new file mode 100644
index 0000000000..32d2a11bdd
--- /dev/null
+++ b/alib2graph_algo/src/heuristic/ChebyshevDistance.hpp
@@ -0,0 +1,57 @@
+// ChebyshevDistance.hpp
+//
+// Created on: 15. 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_CHEBYSHEVDISTANCE_HPP
+#define ALIB2_CHEBYSHEVDISTANCE_HPP
+
+#include <cmath>
+#include <alib/pair>
+#include <functional>
+
+namespace heuristic {
+
+class ChebyshevDistance {
+// ---------------------------------------------------------------------------------------------------------------------
+
+ public:
+ template<typename TCoordinate>
+ static TCoordinate chebyshevDistance(const ext::pair<TCoordinate, TCoordinate> &goal,
+ const ext::pair<TCoordinate, TCoordinate> &node);
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+ template<typename TCoordinate>
+ static std::function<TCoordinate(const ext::pair<TCoordinate, TCoordinate> &,
+ const ext::pair<TCoordinate, TCoordinate> &)> chebyshevDistanceFunction();
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+};
+
+// =====================================================================================================================
+
+template<typename TCoordinate>
+TCoordinate ChebyshevDistance::chebyshevDistance(const ext::pair<TCoordinate, TCoordinate> &goal,
+ const ext::pair<TCoordinate, TCoordinate> &node) {
+ return std::max(node.first - goal.first, node.second - goal.second);
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+template<typename TCoordinate>
+std::function<TCoordinate(const ext::pair<TCoordinate, TCoordinate> &,
+ const ext::pair<TCoordinate, TCoordinate> &)> ChebyshevDistance::chebyshevDistanceFunction() {
+ return (TCoordinate (*)(const ext::pair<TCoordinate, TCoordinate> &,
+ const ext::pair<TCoordinate, TCoordinate> &)) chebyshevDistance;
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+} // namespace heuristic
+#endif //ALIB2_CHEBYSHEVDISTANCE_HPP
diff --git a/alib2graph_algo/src/heuristic/DiagonalDistance.cpp b/alib2graph_algo/src/heuristic/DiagonalDistance.cpp
new file mode 100644
index 0000000000..d45afd6348
--- /dev/null
+++ b/alib2graph_algo/src/heuristic/DiagonalDistance.cpp
@@ -0,0 +1,26 @@
+// DiagonalDistance.cpp
+//
+// Created on: 15. 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 "DiagonalDistance.hpp"
+
+#include <registration/AlgoRegistration.hpp>
+#include <common/DefaultTypes.hpp>
+
+namespace {
+
+auto DiagonalDistance = registration::AbstractRegister<heuristic::DiagonalDistance,
+ std::function<DefaultWeightType(const ext::pair<
+ DefaultCoordinateType,
+ DefaultCoordinateType> &,
+ const ext::pair<
+ DefaultCoordinateType,
+ DefaultCoordinateType> &)> >(
+ heuristic::DiagonalDistance::diagonalDistanceFunction);
+
+}
diff --git a/alib2graph_algo/src/heuristic/DiagonalDistance.hpp b/alib2graph_algo/src/heuristic/DiagonalDistance.hpp
new file mode 100644
index 0000000000..f30028086d
--- /dev/null
+++ b/alib2graph_algo/src/heuristic/DiagonalDistance.hpp
@@ -0,0 +1,59 @@
+// DiagonalDistance.hpp
+//
+// Created on: 15. 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_DIAGONALDISTANCE_HPP
+#define ALIB2_DIAGONALDISTANCE_HPP
+
+#include <cmath>
+#include <alib/pair>
+#include <functional>
+
+namespace heuristic {
+
+class DiagonalDistance {
+// ---------------------------------------------------------------------------------------------------------------------
+ public:
+ template<typename TCoordinate>
+ static double diagonalDistance(const ext::pair<TCoordinate, TCoordinate> &goal,
+ const ext::pair<TCoordinate, TCoordinate> &node);
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+ template<typename TCoordinate>
+ static std::function<double(const ext::pair<TCoordinate, TCoordinate> &,
+ const ext::pair<TCoordinate, TCoordinate> &)> diagonalDistanceFunction();
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+};
+
+// =====================================================================================================================
+
+template<typename TCoordinate>
+double DiagonalDistance::diagonalDistance(const ext::pair<TCoordinate, TCoordinate> &goal,
+ const ext::pair<TCoordinate, TCoordinate> &node) {
+ TCoordinate d_max = std::max(std::abs(node.first - goal.first), std::abs(node.second - goal.second));
+ TCoordinate d_min = std::min(std::abs(node.first - goal.first), std::abs(node.second - goal.second));
+
+ return M_SQRT2 * d_min + (d_max - d_min);
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+template<typename TCoordinate>
+std::function<double(const ext::pair<TCoordinate, TCoordinate> &,
+ const ext::pair<TCoordinate, TCoordinate> &)> DiagonalDistance::diagonalDistanceFunction() {
+ return (double (*)(const ext::pair<TCoordinate, TCoordinate> &,
+ const ext::pair<TCoordinate, TCoordinate> &)) diagonalDistance;
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+} // namespace heuristic
+#endif //ALIB2_DIAGONALDISTANCE_HPP
diff --git a/alib2graph_algo/src/heuristic/EuclideanDistance.cpp b/alib2graph_algo/src/heuristic/EuclideanDistance.cpp
new file mode 100644
index 0000000000..99c1237460
--- /dev/null
+++ b/alib2graph_algo/src/heuristic/EuclideanDistance.cpp
@@ -0,0 +1,26 @@
+// EuclideanDistance.cpp
+//
+// Created on: 15. 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 "EuclideanDistance.hpp"
+
+#include <registration/AlgoRegistration.hpp>
+#include <common/DefaultTypes.hpp>
+
+namespace {
+
+auto EuclideanDistance = registration::AbstractRegister<heuristic::EuclideanDistance,
+ std::function<DefaultWeightType(const ext::pair<
+ DefaultCoordinateType,
+ DefaultCoordinateType> &,
+ const ext::pair<
+ DefaultCoordinateType,
+ DefaultCoordinateType> &)> >(
+ heuristic::EuclideanDistance::euclideanDistanceFunction);
+
+}
diff --git a/alib2graph_algo/src/heuristic/EuclideanDistance.hpp b/alib2graph_algo/src/heuristic/EuclideanDistance.hpp
new file mode 100644
index 0000000000..7424b52a90
--- /dev/null
+++ b/alib2graph_algo/src/heuristic/EuclideanDistance.hpp
@@ -0,0 +1,59 @@
+// EuclideanDistance.hpp
+//
+// Created on: 15. 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_EUCLIDEANDISTANCE_HPP
+#define ALIB2_EUCLIDEANDISTANCE_HPP
+
+#include <cmath>
+#include <alib/pair>
+#include <functional>
+
+namespace heuristic {
+
+class EuclideanDistance {
+// ---------------------------------------------------------------------------------------------------------------------
+
+ public:
+ template<typename TCoordinate>
+ static double euclideanDistance(const ext::pair<TCoordinate, TCoordinate> &goal,
+ const ext::pair<TCoordinate, TCoordinate> &node);
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+ template<typename TCoordinate>
+ static std::function<double(const ext::pair<TCoordinate, TCoordinate> &,
+ const ext::pair<TCoordinate, TCoordinate> &)> euclideanDistanceFunction();
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+};
+
+// =====================================================================================================================
+
+template<typename TCoordinate>
+double EuclideanDistance::euclideanDistance(const ext::pair<TCoordinate, TCoordinate> &goal,
+ const ext::pair<TCoordinate, TCoordinate> &node) {
+ return std::sqrt(
+ (node.first - goal.first) * (node.first - goal.first) +
+ (node.second - goal.second) * (node.second - goal.second));
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+template<typename TCoordinate>
+std::function<double(const ext::pair<TCoordinate, TCoordinate> &,
+ const ext::pair<TCoordinate, TCoordinate> &)> EuclideanDistance::euclideanDistanceFunction() {
+ return (double (*)(const ext::pair<TCoordinate, TCoordinate> &,
+ const ext::pair<TCoordinate, TCoordinate> &)) euclideanDistance;
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+} // namespace heuristic
+#endif //ALIB2_EUCLIDEANDISTANCE_HPP
diff --git a/alib2graph_algo/src/heuristic/EuclideanDistanceNoSQRT.cpp b/alib2graph_algo/src/heuristic/EuclideanDistanceNoSQRT.cpp
new file mode 100644
index 0000000000..b9c53060d6
--- /dev/null
+++ b/alib2graph_algo/src/heuristic/EuclideanDistanceNoSQRT.cpp
@@ -0,0 +1,26 @@
+// EuclideanDistanceNoSQRT.cpp
+//
+// Created on: 15. 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 "EuclideanDistanceNoSQRT.hpp"
+
+#include <registration/AlgoRegistration.hpp>
+#include <common/DefaultTypes.hpp>
+
+namespace {
+
+auto EuclideanDistanceNoSQRT = registration::AbstractRegister<heuristic::EuclideanDistanceNoSQRT,
+ std::function<DefaultWeightType(const ext::pair<
+ DefaultCoordinateType,
+ DefaultCoordinateType> &,
+ const ext::pair<
+ DefaultCoordinateType,
+ DefaultCoordinateType> &)> >(
+ heuristic::EuclideanDistanceNoSQRT::euclideanDistanceNoSQRTFunction);
+
+}
diff --git a/alib2graph_algo/src/heuristic/EuclideanDistanceNoSQRT.hpp b/alib2graph_algo/src/heuristic/EuclideanDistanceNoSQRT.hpp
new file mode 100644
index 0000000000..ee834f9ac5
--- /dev/null
+++ b/alib2graph_algo/src/heuristic/EuclideanDistanceNoSQRT.hpp
@@ -0,0 +1,59 @@
+// EuclideanDistanceNoSQRT.hpp
+//
+// Created on: 15. 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_EUCLIDEANDISTANCENOSQRT_HPP
+#define ALIB2_EUCLIDEANDISTANCENOSQRT_HPP
+
+#include <alib/pair>
+#include <functional>
+
+namespace heuristic {
+
+class EuclideanDistanceNoSQRT {
+// ---------------------------------------------------------------------------------------------------------------------
+
+ public:
+
+ template<typename TCoordinate>
+ static double euclideanDistanceNoSQRT(const ext::pair<TCoordinate, TCoordinate> &goal,
+ const ext::pair<TCoordinate, TCoordinate> &node);
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+ template<typename TCoordinate>
+ static std::function<double(const ext::pair<TCoordinate, TCoordinate> &,
+ const ext::pair<TCoordinate, TCoordinate> &)> euclideanDistanceNoSQRTFunction();
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+};
+
+// =====================================================================================================================
+
+template<typename TCoordinate>
+double EuclideanDistanceNoSQRT::euclideanDistanceNoSQRT(const ext::pair<TCoordinate, TCoordinate> &goal,
+ const ext::pair<TCoordinate, TCoordinate> &node) {
+ return (node.first - goal.first) * (node.first - goal.first) +
+ (node.second - goal.second) * (node.second - goal.second);
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+template<typename TCoordinate>
+std::function<double(const ext::pair<TCoordinate, TCoordinate> &,
+ const ext::pair<TCoordinate,
+ TCoordinate> &)> EuclideanDistanceNoSQRT::euclideanDistanceNoSQRTFunction() {
+ return (double (*)(const ext::pair<TCoordinate, TCoordinate> &,
+ const ext::pair<TCoordinate, TCoordinate> &)) euclideanDistanceNoSQRT;
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+} // namespace heuristic
+#endif //ALIB2_EUCLIDEANDISTANCENOSQRT_HPP
diff --git a/alib2graph_algo/src/heuristic/ManhattenDistance.cpp b/alib2graph_algo/src/heuristic/ManhattenDistance.cpp
new file mode 100644
index 0000000000..b4b10e8def
--- /dev/null
+++ b/alib2graph_algo/src/heuristic/ManhattenDistance.cpp
@@ -0,0 +1,26 @@
+// ManhattenDistance.cpp
+//
+// Created on: 15. 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 "ManhattenDistance.hpp"
+
+#include <registration/AlgoRegistration.hpp>
+#include <common/DefaultTypes.hpp>
+
+namespace {
+
+auto ManhattenDistance = registration::AbstractRegister<heuristic::ManhattenDistance,
+ std::function<DefaultCoordinateType(const ext::pair<
+ DefaultCoordinateType,
+ DefaultCoordinateType> &,
+ const ext::pair<
+ DefaultCoordinateType,
+ DefaultCoordinateType> &)> >(
+ heuristic::ManhattenDistance::manhattenDistanceFunction);
+
+}
\ No newline at end of file
diff --git a/alib2graph_algo/src/heuristic/ManhattenDistance.hpp b/alib2graph_algo/src/heuristic/ManhattenDistance.hpp
new file mode 100644
index 0000000000..fac3fcd232
--- /dev/null
+++ b/alib2graph_algo/src/heuristic/ManhattenDistance.hpp
@@ -0,0 +1,59 @@
+// ManhattenDistance.hpp
+//
+// Created on: 15. 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_MANHATTENDISTANCE_HPP
+#define ALIB2_MANHATTENDISTANCE_HPP
+
+#include <cmath>
+#include <alib/pair>
+#include <functional>
+
+namespace heuristic {
+
+class ManhattenDistance {
+// ---------------------------------------------------------------------------------------------------------------------
+
+ public:
+
+ template<typename TCoordinate>
+ static TCoordinate manhattenDistance(const ext::pair<TCoordinate, TCoordinate> &goal,
+ const ext::pair<TCoordinate, TCoordinate> &node);
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+ template<typename TCoordinate>
+ static
+ std::function<TCoordinate(const ext::pair<TCoordinate, TCoordinate> &,
+ const ext::pair<TCoordinate, TCoordinate> &)> manhattenDistanceFunction();
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+};
+
+// =====================================================================================================================
+
+template<typename TCoordinate>
+TCoordinate ManhattenDistance::manhattenDistance(const ext::pair<TCoordinate, TCoordinate> &goal,
+ const ext::pair<TCoordinate, TCoordinate> &node) {
+ return std::abs(node.first - goal.first) + std::abs(node.second - goal.second);
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+template<typename TCoordinate>
+std::function<TCoordinate(const ext::pair<TCoordinate, TCoordinate> &,
+ const ext::pair<TCoordinate, TCoordinate> &)> ManhattenDistance::manhattenDistanceFunction() {
+ return (TCoordinate (*)(const ext::pair<TCoordinate, TCoordinate> &,
+ const ext::pair<TCoordinate, TCoordinate> &)) manhattenDistance;
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+} // namespace heuristic
+#endif //ALIB2_MANHATTENDISTANCE_HPP
diff --git a/alib2graph_algo/src/heuristic/SquareGridHeuristics.hpp b/alib2graph_algo/src/heuristic/SquareGridHeuristics.hpp
new file mode 100644
index 0000000000..88c367c166
--- /dev/null
+++ b/alib2graph_algo/src/heuristic/SquareGridHeuristics.hpp
@@ -0,0 +1,19 @@
+// SquareGridHeuristics.hpp
+//
+// Created on: 15. 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_SQUAREGRIDHEURISTICS_HPP
+#define ALIB2_SQUAREGRIDHEURISTICS_HPP
+
+#include "ChebyshevDistance.hpp"
+#include "DiagonalDistance.hpp"
+#include "EuclideanDistance.hpp"
+#include "EuclideanDistanceNoSQRT.hpp"
+#include "ManhattenDistance.hpp"
+
+#endif //ALIB2_SQUAREGRIDHEURISTICS_HPP
--
GitLab