Merge branch 'feature/cost-heuristics'

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 0a6ad9683a4021571ac809f5390e241726e73dab..655826ef30145bc6afe13a576696806bbeaa7058 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -15,6 +15,7 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - Compound extensions.
 - Dijkstra algorithm for path optimization (`rrtext3.cc`).
 - Grid based `nn` and `nv` methods (`rrtext4.cc`).
+- Compound extensions.
 
 [cute c2]: https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
 

diff --git a/CMakeLists.txt b/CMakeLists.txt
index b4d533726c3781405cd3a3d2e7a9f507c9650411..46906527c9513e86f864395af02ca47a6089405d 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -13,6 +13,8 @@ include_directories(${CMAKE_CURRENT_SOURCE_DIR}/api)
 
 add_library(rrts SHARED
         src/rrts.cc
+        src/rrtext6.cc
+        src/rrtext5.cc
         src/rrtext4.cc
         src/rrtext3.cc
         src/rrtext2.cc

diff --git a/README.md b/README.md
index 5281101c229db9df4c69d1b031ef9ea2b3348939..2ae930608ed8ecd876ad9bfda851ff1cf7be88bd 100644 (file)
--- a/README.md
+++ b/README.md
@@ -30,16 +30,22 @@ and upgrades to RRT, *extensions* are declared in `rrtext.h` and implemented in
 
 ## Implemented extensions
 There is a list of implemented extensions and what they include:
+- `rrtext6.cc`: Reeds and Shepp for both -- building and search costs,
+- `rrtext5.cc`: different cost for building (Reeds and Shepp) and searching
+  (Euclidean distance),
 - `rrtext4.cc`: store RRT nodes to grid,
 - `rrtext3.cc`: Dijkstra algorithm for path optimization,
 - `rrtext2.cc`: [cute c2][] for collision detection,
-- `rrtext1.cc`: different cost for building and searching.
+- `rrtext1.cc`: different cost for building (Reeds and Shepp) and searching
+  (Matej's heuristics).
 
 [cute c2]: https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
 
 ## Compound extensions
 There is a list of classes with reference to extensions used:
 - `RRTCE1`: 1, 2.
+- `RRTCE2`: 2, 5.
+- `RRTCE3`: 2, 6.
 
 # Contribute
 Use [OneFlow][3] branching model and keep the [changelog][4].

diff --git a/api/rrtce.h b/api/rrtce.h
index dea0cd88156fb0bfdaa7729d3bba10c48f6cc6d5..43c5caf1067d8b4aba36ff51a37c486d3b6f60b3 100644 (file)
--- a/api/rrtce.h
+++ b/api/rrtce.h
@@ -12,5 +12,7 @@ Compound extensions have no implementation.
 #include "rrtext.h"
 
 class RRTCE1 : public RRTExt1, public RRTExt2 {};
+class RRTCE2 : public RRTExt2, public RRTExt5 {};
+class RRTCE3 : public RRTExt2, public RRTExt6 {};
 
 #endif /* RRTCE_H */

diff --git a/api/rrtext.h b/api/rrtext.h
index cfb8a428c4c3468f2623c800f6cede5df5e4e507..5c9721ae39f421c2e07d48c5432b5e11be054ebc 100644 (file)
--- a/api/rrtext.h
+++ b/api/rrtext.h
@@ -6,6 +6,33 @@
 // ext2
 #include "cute_c2.h"
 
+/*! \brief Reeds and Shepp cost for building and search.
+*/
+class RRTExt6 : public virtual RRTS {
+        public:
+                /*! \brief Reeds and Shepp path length.
+                */
+                double cost_build(RRTNode &f, RRTNode &t);
+                /*! \brief Reeds and Shepp path length.
+                */
+                double cost_search(RRTNode &f, RRTNode &t);
+};
+
+/*! \brief Different costs extension.
+
+Use different cost for bulding tree data structure and searching in the
+structure. This one is complementary to `rrtext1.cc`.
+*/
+class RRTExt5 : public virtual RRTS {
+        public:
+                /*! \brief Reeds and Shepp path length.
+                */
+                double cost_build(RRTNode &f, RRTNode &t);
+                /*! \brief Euclidean distance.
+                */
+                double cost_search(RRTNode &f, RRTNode &t);
+};
+
 /*! \brief Use grid data structure to store RRT nodes.
 
 This approach speeds up the search process for the nearest neighbor and

diff --git a/src/rrtext5.cc b/src/rrtext5.cc
new file mode 100644 (file)
index 0000000..e0fbeea
--- /dev/null
+++ b/src/rrtext5.cc
@@ -0,0 +1,17 @@
+#include "rrtext.h"
+#include "reeds_shepp.h"
+
+double RRTExt5::cost_build(RRTNode &f, RRTNode &t)
+{
+        double q0[] = {f.x(), f.y(), f.h()};
+        double q1[] = {t.x(), t.y(), t.h()};
+        ReedsSheppStateSpace rsss(f.mtr());
+        return rsss.distance(q0, q1);
+}
+
+double RRTExt5::cost_search(RRTNode &f, RRTNode &t)
+{
+        double cost = 0;
+        cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2));
+        return cost;
+}

diff --git a/src/rrtext6.cc b/src/rrtext6.cc
new file mode 100644 (file)
index 0000000..02d9de9
--- /dev/null
+++ b/src/rrtext6.cc
@@ -0,0 +1,18 @@
+#include "rrtext.h"
+#include "reeds_shepp.h"
+
+double RRTExt6::cost_build(RRTNode &f, RRTNode &t)
+{
+        double q0[] = {f.x(), f.y(), f.h()};
+        double q1[] = {t.x(), t.y(), t.h()};
+        ReedsSheppStateSpace rsss(f.mtr());
+        return rsss.distance(q0, q1);
+}
+
+double RRTExt6::cost_search(RRTNode &f, RRTNode &t)
+{
+        double q0[] = {f.x(), f.y(), f.h()};
+        double q1[] = {t.x(), t.y(), t.h()};
+        ReedsSheppStateSpace rsss(f.mtr());
+        return rsss.distance(q0, q1);
+}