Add ext6 (RS for build and search)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 048d82a9df89ff9742a4bb4b2612c42da067d522..46906527c9513e86f864395af02ca47a6089405d 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -13,6 +13,7 @@ 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

diff --git a/README.md b/README.md
index 0a2b57fd6440b7d087fcfb612919871693d91320..4ff9eb6900f427c7b16fb6ddd17d49281056fb08 100644 (file)
--- a/README.md
+++ b/README.md
@@ -30,6 +30,7 @@ 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,

diff --git a/api/rrtext.h b/api/rrtext.h
index 172b220cea2818e3b7273110cf54bacd96f1e0d5..5c9721ae39f421c2e07d48c5432b5e11be054ebc 100644 (file)
--- a/api/rrtext.h
+++ b/api/rrtext.h
@@ -6,6 +6,18 @@
 // 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

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);
+}