Add ext3 skeleton

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 74c03ee47e1c85d0109cdc8bf53564e4babf54bf..d9ca70847639646bd9aedee7fa911d8b0e575840 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/rrtext3.cc
         src/rrtext2.cc
         src/rrtext1.cc
         src/reeds_shepp.cpp

diff --git a/README.md b/README.md
index 96cd47163bc1564e2a8aa4bcf875f73e333d5a78..ca075904914e1edbc2c57febad6a9f157d27ed2a 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:
+- `rrtext3.cc`: Dijkstra algorithm for path optimization,
 - `rrtext2.cc`: [cute c2][] for collision detection,
 - `rrtext1.cc`: different cost for building and searching.
 

diff --git a/api/rrtext.h b/api/rrtext.h
index 170f8e8b6bbb3580b218b541bd6f04048375b26b..7f5a0927ed625c57d87a2aee39694f079259fd39 100644 (file)
--- a/api/rrtext.h
+++ b/api/rrtext.h
@@ -6,6 +6,19 @@
 // ext2
 #include "cute_c2.h"
 
+/*! \brief Use Dijkstra algorithm to find the shorter path.
+*/
+class RRTExt3 : public virtual RRTS {
+        private:
+                std::vector<RRTNode *> orig_path_;
+        public:
+                std::vector<RRTNode *> &orig_path()
+                {
+                        return this->orig_path_;
+                };
+                std::vector<RRTNode *> path();
+};
+
 /*! \brief Use cute_c2 for collision detection.
 
 \see https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h

diff --git a/api/rrts.h b/api/rrts.h
index f2f02f6773d6fe2d2efedbc68cc32ca2ff9dc75c..3ff74a5f5a669c7a84209058e78e6d6c3ebfacaf 100644 (file)
--- a/api/rrts.h
+++ b/api/rrts.h
@@ -127,7 +127,7 @@ class RRTS {
                 virtual void deinit();
                 /*! \brief Return path found by RRT*.
                 */
-                std::vector<RRTNode *> path();
+                virtual std::vector<RRTNode *> path();
                 /*! \brief Run next RRT* iteration.
                 */
                 bool next();

diff --git a/src/rrtext3.cc b/src/rrtext3.cc
new file mode 100644 (file)
index 0000000..e9a9a53
--- /dev/null
+++ b/src/rrtext3.cc
@@ -0,0 +1,10 @@
+#include "rrtext.h"
+
+std::vector<RRTNode *> RRTExt3::path()
+{
+        if (this->orig_path().size() == 0) {
+                this->orig_path_ = RRTS::path();
+        }
+        std::vector<RRTNode *> path;
+        return path;
+}