Add Extend* procedure for Connect*

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index 72b682568032dfd9d7d6e00176c41540280dc071..5930891e0bd1c680fea84a067a8c080287dd1db8 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -821,6 +821,75 @@ int Klemm2015::extendstar1(RRTNode *rs, RRTNode **xn)
         return ret;
 }
 
+int Klemm2015::extendstarC(RRTNode *rs)
+{
+        int ret = 0; // 0 - advanced, 1 - reached, 2 - trapped
+        char tree = this->root()->tree();
+#if NNVERSION>2
+        RRTNode *nn = this->nn(this->iy_, rs, this->cost, tree);
+#elif NNVERSION>1
+        RRTNode *nn = this->nn(this->iy_, rs, this->cost);
+#else
+        RRTNode *nn = this->nn(this->nodes(), rs, this->cost);
+#endif
+        RRTNode *pn = nn;
+        std::vector<RRTNode *> nvs;
+        bool en_add = true;
+        for (auto ns: this->steer(nn, rs)) {
+                if (!en_add) {
+                        delete ns;
+                } else {
+#if NVVERSION>2
+                       nvs = this->nv(
+                                       this->iy_,
+                                       ns,
+                                       this->cost,
+                                       MIN(
+                                               GAMMA_RRTSTAR(
+                                                       this->nodes().size()),
+                                               0.2),
+                                       '0'); // TODO const
+#elif NVVERSION>1
+                        nvs = this->nv(
+                                        this->iy_,
+                                        ns,
+                                        this->cost,
+                                        MIN(
+                                                GAMMA_RRTSTAR(
+                                                        this->nodes().size()),
+                                                0.2)); // TODO const
+#else
+                        nvs = this->nv(
+                                        this->root(),
+                                        ns,
+                                        this->cost,
+                                        MIN(
+                                                GAMMA_RRTSTAR(
+                                                        this->nodes().size()),
+                                                0.2)); // TODO const
+#endif
+                        this->nodes().push_back(ns);
+                        this->add_iy(ns);
+                        // connect
+                        if (!this->connect(pn, ns, nvs)) {
+                                this->iy_[IYI(ns->y())].pop_back();
+                                en_add = false;
+                                ret = 2;
+                        } else {
+                                // rewire
+                                this->rewire(nvs, ns);
+                                pn = ns;
+                                if (this->goal_found(pn, this->cost)) {
+                                        this->tlog(this->findt());
+                                        en_add = false;
+                                        ret = 1;
+                                }
+                        }
+                }
+        }
+        return ret;
+}
+
 int Klemm2015::connectstar(RRTNode *x)
 {
         int ret = 0; // 0 - advanced, 1 - reached, 2 - trapped

diff --git a/incl/rrtplanner.h b/incl/rrtplanner.h
index fd7ee5b67d7db2dc862c4e427506e1d70030f6a3..87d617d293e8f19453ee14b122342bda13fbef8f 100644 (file)
--- a/incl/rrtplanner.h
+++ b/incl/rrtplanner.h
@@ -185,6 +185,7 @@ class Klemm2015: public Karaman2011 {
                 RRTNode *orig_goal_ = nullptr;
         protected:
                 int extendstar1(RRTNode *rs, RRTNode **xn);
+                int extendstarC(RRTNode *rs);
                 int connectstar(RRTNode *x);
                 void swap();
         public: