Make `nv` procedure part of RRTBase

diff --git a/CHANGELOG.md b/CHANGELOG.md
index cdea3605fce1aa82052e840592036237dffc09d5..775ee3ac39e0109bfb3cf5fd403d27019daa8ec9 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -16,6 +16,7 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - Make `sample` procedure part of RRTBase.
 - Make `cost` function part of RRTBase.
 - Make `nn` procedure part of RRTBase.
+- Make `nv` procedure part of RRTBase.
 
 ## 0.3.0 - 2018-12-03
 ### Added

diff --git a/CMakeLists.txt b/CMakeLists.txt
index fc164d7e09b43fc2297b840518fcd90c994dc7c3..aa86fb12572ed1285bf9d9c583f07ec27235ac8b 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -7,17 +7,10 @@ ENDIF(PLANNER)
 IF(TMAX)
         ADD_DEFINITIONS(-DTMAX=${TMAX})
 ENDIF(TMAX)
-IF(NV)
-        ADD_DEFINITIONS(-DNV=${NV})
-ENDIF(NV)
 IF(ST)
         ADD_DEFINITIONS(-DST=${ST})
 ENDIF(ST)
 
-IF(NVVERSION)
-        ADD_DEFINITIONS(-DNVVERSION=${NVVERSION})
-ENDIF(NVVERSION)
-
 find_package(OpenMP)
 if (OPENMP_FOUND)
     set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")

diff --git a/README.md b/README.md
index 0c76203e30956b6422164f43d82ae45c807034f5..aad8cf6b30985f82a83156234364ff42213c4764 100644 (file)
--- a/README.md
+++ b/README.md
@@ -33,8 +33,6 @@ The list of available macros with values:
         - `Kuwata2008` - RRT with changing cost and steering to goal.
         - `Karaman2011` - RRT* framework.
 - `TMAX` - Specify the upper time bound in seconds.
-- `NV`
-        - `nv1` - Near vertices basic DFS procedure.
 - `ST`
         - `st1` - Steer directly to goal.
         - `st2` - Steer with maximum turning radius and direction in mind.

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 4725655e405c4077524b755f67a88fcba79b6c78..47465c8fafec72bc2e3648df3755ae96cfbad6d2 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -30,6 +30,7 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #include "cost.h"
 #include "steer.h"
 #include "nn.h"
+#include "nv.h"
 
 extern SDL_Window* gw;
 extern SDL_GLContext gc;
@@ -691,3 +692,28 @@ RRTNode *RRTBase::nn(RRTNode *rs)
 {
         return nn4(this->iy_, rs, nullptr);
 }
+
+std::vector<RRTNode *> RRTBase::nv(RRTNode *node, float dist)
+{
+        std::vector<RRTNode *> nvs;
+        unsigned int iy = IYI(node->y());
+        unsigned int iy_dist = floor(dist / IYSTEP) + 1;
+        unsigned int i = 0; // vector index
+        unsigned int j = 0; // array index
+        unsigned int jmin = 0; // minimal j index
+        unsigned int jmax = 0; // maximal j index
+        jmin = iy - iy_dist;
+        jmin = (jmin > 0) ? jmin : 0;
+        jmax = iy + iy_dist + 1;
+        jmax = (jmax < IYSIZE) ? jmax : IYSIZE;
+        #pragma omp parallel for reduction(merge: nvs)
+        for (j = jmin; j < jmax; j++) {
+                #pragma omp parallel for reduction(merge: nvs)
+                for (i = 0; i < this->iy_[j].size(); i++) {
+                        if (this->cost(this->iy_[j][i], node) < dist) {
+                                nvs.push_back(this->iy_[j][i]);
+                        }
+                }
+        }
+        return nvs;
+}

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index b88199ccf00e46a4d8ce390b18327c44b69ab3c1..1f8d63def1b17d475383015e7df106a04b491eaf 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -167,7 +167,6 @@ bool Kuwata2008::next()
 
 Karaman2011::Karaman2011(RRTNode *init, RRTNode *goal):
         RRTBase(init, goal),
-        nv(NV),
         steer(ST)
 {
         srand(static_cast<unsigned>(time(0)));
@@ -193,35 +192,10 @@ bool Karaman2011::next()
                 if (!en_add) {
                         delete ns;
                 } else {
-#if NVVERSION>2
-        nvs = this->nv(
-                        this->iy_,
-                        ns,
-                        CO,
-                        MIN(
-                                GAMMA_RRTSTAR(
-                                        this->nodes().size()),
-                                0.2),
-                        '0'); // TODO const
-#elif NVVERSION>1
-                        nvs = this->nv(
-                                        this->iy_,
-                                        ns,
-                                        CO,
-                                        MIN(
-                                                GAMMA_RRTSTAR(
-                                                        this->nodes().size()),
-                                                0.2)); // TODO const
-#else
-                        nvs = this->nv(
-                                        this->root(),
-                                        ns,
-                                        CO,
-                                        MIN(
-                                                GAMMA_RRTSTAR(
+                        nvs = this->nv(ns,
+                                        MIN(GAMMA_RRTSTAR(
                                                         this->nodes().size()),
-                                                0.2)); // TODO const
-#endif
+                                                        0.2));
                         this->nodes().push_back(ns);
                         this->add_iy(ns);
                         // connect
@@ -311,7 +285,6 @@ bool Karaman2011::rewire(std::vector<RRTNode *> nvs, RRTNode *ns)
 
 T1::T1(RRTNode *init, RRTNode *goal):
         RRTBase(init, goal),
-        nv(NV),
         steer(ST)
 {
         srand(static_cast<unsigned>(time(0)));
@@ -336,25 +309,9 @@ bool T1::next()
         // RRT* for first node
         RRTNode *ns = steered[0];
         {
-#if NVVERSION>2
-        nvs = this->nv(
-                        this->iy_,
-                        ns,
-                        CO,
-                        MIN(
-                                GAMMA_RRTSTAR(
-                                        this->nodes().size()),
-                                0.2),
-                        '0'); // TODO const
-#elif NVVERSION>1
-                nvs = this->nv(this->iy_, ns, CO, MIN(
-                                        GAMMA_RRTSTAR(this->nodes().size()),
-                                        0.2)); // TODO const
-#else
-                nvs = this->nv(this->root(), ns, CO, MIN(
-                                        GAMMA_RRTSTAR(this->nodes().size()),
-                                        0.2)); // TODO const
-#endif
+                nvs = this->nv(ns, MIN(
+                                GAMMA_RRTSTAR(this->nodes().size()),
+                                0.2)); // TODO const
                 this->nodes().push_back(ns);
                 this->add_iy(ns);
                 connected = false;
@@ -457,35 +414,12 @@ bool T2::next()
                                 cusps++;
                         if (cusps > 4)
                                 en_add = false;
-#if NVVERSION>2
-        nvs = this->nv(
-                        this->iy_,
-                        ns,
-                        CO,
-                        MIN(
-                                GAMMA_RRTSTAR(
-                                        this->nodes().size()),
-                                0.2),
-                        '0'); // TODO const
-#elif NVVERSION>1
-                        nvs = this->nv(
-                                        this->iy_,
-                                        ns,
-                                        CO,
-                                        MIN(
-                                                GAMMA_RRTSTAR(
-                                                        this->nodes().size()),
-                                                0.2)); // TODO const
-#else
                         nvs = this->nv(
-                                        this->root(),
                                         ns,
-                                        CO,
                                         MIN(
                                                 GAMMA_RRTSTAR(
                                                         this->nodes().size()),
                                                 0.2)); // TODO const
-#endif
                         this->nodes().push_back(ns);
                         this->add_iy(ns);
                         // connect
@@ -547,29 +481,7 @@ bool T2::next()
 float T2::goal_cost()
 {
         std::vector<RRTNode *> nvs;
-#if NVVERSION>2
-        nvs = this->nv(
-                        this->iy_,
-                        this->goal(),
-                        CO,
-                        MIN(
-                                GAMMA_RRTSTAR(
-                                        this->nodes().size()),
-                                0.2),
-                        '0'); // TODO const
-#elif NVVERSION>1
-        nvs = this->nv(
-                        this->iy_,
-                        this->goal(),
-                        CO,
-                        0.2);
-#else
-        nvs = this->nv(
-                        this->root(),
-                        this->goal(),
-                        CO,
-                        0.2);
-#endif
+        nvs = this->nv(this->goal(), 0.2);
         for (auto nv: nvs) {
                 if (std::abs(this->goal()->h() - nv->h()) >=
                                 this->GOAL_FOUND_ANGLE)
@@ -671,7 +583,6 @@ bool T3::overlaptrees(RRTNode **ron, RRTNode **gon)
 
 Klemm2015::Klemm2015(RRTNode *init, RRTNode *goal):
         Karaman2011(init, goal),
-        nv(NV),
         steer(ST),
         orig_root_(init),
         orig_goal_(goal)
@@ -750,35 +661,12 @@ int Klemm2015::extendstar1(RRTNode *rs, RRTNode **xn)
         std::vector<RRTNode *> steered = this->steer(nn, rs);
         RRTNode *ns = steered[1];
         ns->tree(tree);
-#if NVVERSION>2
-       nvs = this->nv(
-               this->iy_,
-               ns,
-               CO,
-               MIN(
-                       GAMMA_RRTSTAR(
-                               this->nodes().size()),
-                       0.2),
-               '0'); // TODO const
-#elif NVVERSION>1
-       nvs = this->nv(
-               this->iy_,
-               ns,
-               CO,
-               MIN(
-                       GAMMA_RRTSTAR(
-                               this->nodes().size()),
-                       0.2)); // TODO const
-#else
        nvs = this->nv(
-               this->root(),
-               ns,
-               CO,
-               MIN(
-                       GAMMA_RRTSTAR(
-                               this->nodes().size()),
-                       0.2)); // TODO const
-#endif
+                        ns,
+                        MIN(
+                                GAMMA_RRTSTAR(
+                                        this->nodes().size()),
+                                0.2)); // TODO const
        this->nodes().push_back(ns);
        this->add_iy(ns);
        // connect
@@ -814,35 +702,12 @@ int Klemm2015::extendstarC(RRTNode *rs)
                 if (!en_add) {
                         delete ns;
                 } else {
-#if NVVERSION>2
-                       nvs = this->nv(
-                                       this->iy_,
-                                       ns,
-                                       CO,
-                                       MIN(
-                                               GAMMA_RRTSTAR(
-                                                       this->nodes().size()),
-                                               0.2),
-                                       '0'); // TODO const
-#elif NVVERSION>1
-                        nvs = this->nv(
-                                        this->iy_,
-                                        ns,
-                                        CO,
-                                        MIN(
-                                                GAMMA_RRTSTAR(
-                                                        this->nodes().size()),
-                                                0.2)); // TODO const
-#else
                         nvs = this->nv(
-                                        this->root(),
                                         ns,
-                                        CO,
                                         MIN(
                                                 GAMMA_RRTSTAR(
                                                         this->nodes().size()),
                                                 0.2)); // TODO const
-#endif
                         this->nodes().push_back(ns);
                         this->add_iy(ns);
                         // connect

diff --git a/incl/compile.h b/incl/compile.h
index 75444b9f344fb16866aa441285ca3c81ded31ea6..9843a1cf2f217f111941789eb080b04a734e33b0 100644 (file)
--- a/incl/compile.h
+++ b/incl/compile.h
@@ -26,10 +26,6 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #define TMAX 10
 #endif
 
-#ifndef NV
-#define NV nv1
-#endif
-
 #ifndef ST
 #define ST st1
 #endif

diff --git a/incl/rrtbase.h b/incl/rrtbase.h
index 7d82a58ec0d12ab40e85d05d103f75534026c726..b183c59bf394482a4f031b2ce5372889e959a1bc 100644 (file)
--- a/incl/rrtbase.h
+++ b/incl/rrtbase.h
@@ -104,6 +104,7 @@ class RRTBase {
                 RRTNode *sample();
                 float cost(RRTNode *init, RRTNode *goal);
                 RRTNode *nn(RRTNode *rs);
+                std::vector<RRTNode *> nv(RRTNode *node, float dist);
 
                 // virtuals - implemented by child classes
                 virtual bool next() = 0;

diff --git a/incl/rrtplanner.h b/incl/rrtplanner.h
index 25854e4f53034d15ac2296260b3a802dd317a16a..63e8b33eedd8eb33cdd528989be795e1c4bcbbae 100644 (file)
--- a/incl/rrtplanner.h
+++ b/incl/rrtplanner.h
@@ -58,19 +58,6 @@ class Karaman2011: public RRTBase {
                 Karaman2011(RRTNode *init, RRTNode *goal);
 
                 // RRT framework
-                std::vector<RRTNode *> (*nv)(
-#if NVVERSION>1
-                                std::vector<RRTNode *> (&nodes)[IYSIZE],
-#else
-                                RRTNode *root,
-#endif
-                                RRTNode *node,
-                                float (*cost)(RRTNode *, RRTNode *),
-                                float dist
-#if NVVERSION>2
-                                , char tree
-#endif
-                                );
                 std::vector<RRTNode *> (*steer)(
                                 RRTNode *init,
                                 RRTNode *goal);
@@ -82,19 +69,6 @@ class T1: public RRTBase {
                 T1(RRTNode *init, RRTNode *goal);
 
                 // RRT framework
-                std::vector<RRTNode *> (*nv)(
-#if NVVERSION>1
-                                std::vector<RRTNode *> (&nodes)[IYSIZE],
-#else
-                                RRTNode *root,
-#endif
-                                RRTNode *node,
-                                float (*cost)(RRTNode *, RRTNode *),
-                                float dist
-#if NVVERSION>2
-                                , char tree
-#endif
-                                );
                 std::vector<RRTNode *> (*steer)(
                                 RRTNode *init,
                                 RRTNode *goal);
@@ -136,19 +110,6 @@ class Klemm2015: public Karaman2011 {
                 Klemm2015(RRTNode *init, RRTNode *goal);
 
                 // RRT framework
-                std::vector<RRTNode *> (*nv)(
-#if NVVERSION>1
-                                std::vector<RRTNode *> (&nodes)[IYSIZE],
-#else
-                                RRTNode *root,
-#endif
-                                RRTNode *node,
-                                float (*cost)(RRTNode *, RRTNode *),
-                                float dist
-#if NVVERSION>2
-                                , char tree
-#endif
-                                );
                 std::vector<RRTNode *> (*steer)(
                                 RRTNode *init,
                                 RRTNode *goal);