Add rebase method - rebase RRT to another node

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 7ac7e00ed69a80d9825996307e92ca3112f353dc..d1597ded713f3d8785f26c34f05ef64e52ecfebc 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
 - Nearest neighbour `nn3` procedure based on indexing over `y` axis.
 - Near vertices `nv2` procedure based on indexing over `y` axis.
 - OpenMP parallelization of nearest neighbour and near vertices procedures.
+- Rebase method that changes (rebases) RRT root to another RRT node.
 
 ### Changed
 - Build with Ninja.

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index d3df06e66484c8c936eca33ef1626e2346cfd60c..04a48dea3b9b968ab2a4b617fdfd8ac6eaf43c53 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -293,6 +293,53 @@ bool RRTBase::collide(RRTNode *init, RRTNode *goal)
         return false;
 }
 
+bool RRTBase::rebase(RRTNode *nr)
+{
+        std::vector<RRTNode *> s; // DFS stack
+        RRTNode *tmp;
+        unsigned int i = 0;
+        unsigned int to_del = 0;
+        int iy = 0;
+        s.push_back(this->root_);
+        while (s.size() > 0) {
+                tmp = s.back();
+                s.pop_back();
+                for (auto ch: tmp->children()) {
+                        if (ch != nr)
+                                s.push_back(ch);
+                }
+                to_del = this->nodes_.size();
+                #pragma omp parallel for reduction(min: to_del)
+                for (i = 0; i < this->nodes_.size(); i++) {
+                        if (this->nodes_[i] == tmp)
+                                to_del = i;
+                }
+                if (to_del < this->nodes_.size())
+                        this->nodes_.erase(this->nodes_.begin() + to_del);
+                to_del = this->samples_.size();
+                #pragma omp parallel for reduction(min: to_del)
+                for (i = 0; i < this->samples_.size(); i++) {
+                        if (this->samples_[i] == tmp)
+                                to_del = i;
+                }
+                if (to_del < this->samples_.size())
+                        this->samples_.erase(this->nodes_.begin() + to_del);
+                iy = floor(tmp->y() / IYSTEP) + floor(IYSIZE / 2);
+                to_del = this->iy_[iy].size();
+                #pragma omp parallel  for reduction(min: to_del)
+                for (i = 0; i < this->iy_[iy].size(); i++) {
+                        if (this->iy_[iy][i] == tmp)
+                                to_del = i;
+                }
+                if (to_del < this->iy_[iy].size())
+                        this->iy_[iy].erase(this->iy_[iy].begin() + to_del);
+                this->dnodes().push_back(tmp);
+        }
+        this->root_ = nr;
+        this->root_->parent(nullptr);
+        return true;
+}
+
 std::vector<RRTNode *> RRTBase::findt()
 {
         return this->findt(this->goal_);

diff --git a/incl/rrtbase.h b/incl/rrtbase.h
index ba8c0e1d263087aa98b818fb690ef5e827531f81..73475177e47e01ab1fbfe73eb1018cd1168cfae5 100644 (file)
--- a/incl/rrtbase.h
+++ b/incl/rrtbase.h
@@ -84,6 +84,7 @@ class RRTBase {
                                 RRTNode *node,
                                 float (*cost)(RRTNode *, RRTNode *));
                 bool collide(RRTNode *init, RRTNode *goal);
+                bool rebase(RRTNode *nr);
                 std::vector<RRTNode *> findt();
                 std::vector<RRTNode *> findt(RRTNode *n);