From adcb165e445eb666eab2b2ee7038ed87d27aa7ec Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 17 Dec 2018 10:24:14 +0100
Subject: [PATCH] Split opt procedure from tips nodes gathering

---
 base/rrtbase.cc | 70 ++++++++++++++++++++++++-------------------------
 incl/rrtbase.h  |  4 ++-
 2 files changed, 38 insertions(+), 36 deletions(-)

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 3b7cc8f..fbbc565 100644
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -521,30 +521,10 @@ class RRTNodeDijkstraComparator {
                 }
 };
 
-bool RRTBase::optp_dijkstra()
+bool RRTBase::optp_dijkstra(
+                std::vector<RRTNode *> &cusps,
+                std::vector<int> &npi)
 {
-        if (this->tlog().size() == 0)
-                return false;
-        float oc = this->tlog().back().front()->ccost();
-        std::vector<RRTNode *> tmp_cusps;
-        for (auto n: this->tlog().back()) {
-                if (sgn(n->s()) == 0) {
-                        tmp_cusps.push_back(n);
-                } else if (n->parent() &&
-                                sgn(n->s()) != sgn(n->parent()->s())) {
-                        tmp_cusps.push_back(n);
-                        tmp_cusps.push_back(n->parent());
-                }
-        }
-        if (tmp_cusps.size() < 2)
-                return false;
-        std::vector<RRTNode *> cusps;
-        for (unsigned int i = 0; i < tmp_cusps.size(); i++) {
-                if (tmp_cusps[i] != tmp_cusps[i + 1])
-                        cusps.push_back(tmp_cusps[i]);
-        }
-        std::reverse(cusps.begin(), cusps.end());
-        // Begin of Dijkstra
         std::vector<RRTNodeDijkstra> dnodes;
         for (unsigned int i = 0; i < cusps.size(); i++)
                 if (i > 0)
@@ -573,11 +553,7 @@ bool RRTBase::optp_dijkstra()
                                 this->cost(cusps[tmp.ni], cusps[i]);
                         steered = this->steer(cusps[tmp.ni], cusps[i]);
                         for (unsigned int j = 0; j < steered.size() - 1; j++)
-                                steered[j]->add_child(
-                                                steered[j + 1],
-                                                this->cost(
-                                                        steered[j],
-                                                        steered[j + 1]));
+                                steered[j]->add_child(steered[j + 1], 1);
                         if (this->collide(
                                         steered[0],
                                         steered[steered.size() - 1]))
@@ -588,9 +564,10 @@ bool RRTBase::optp_dijkstra()
                                 if (!dnodes[i].vi())
                                         pq.push(dnodes[i]);
                         }
+                        for (auto n: steered)
+                                delete n;
                 }
         }
-        std::vector<int> npi; // new path indexes
         unsigned int tmpi = 0;
         for (auto n: dnodes) {
                 if (n.v && n.ni > tmpi)
@@ -602,6 +579,35 @@ bool RRTBase::optp_dijkstra()
         }
         npi.push_back(tmpi);
         std::reverse(npi.begin(), npi.end());
+        return true;
+}
+
+bool RRTBase::opt_path()
+{
+        if (this->tlog().size() == 0)
+                return false;
+        float oc = this->tlog().back().front()->ccost();
+        std::vector<RRTNode *> tmp_cusps;
+        for (auto n: this->tlog().back()) {
+                if (sgn(n->s()) == 0) {
+                        tmp_cusps.push_back(n);
+                } else if (n->parent() &&
+                                sgn(n->s()) != sgn(n->parent()->s())) {
+                        tmp_cusps.push_back(n);
+                        tmp_cusps.push_back(n->parent());
+                }
+        }
+        if (tmp_cusps.size() < 2)
+                return false;
+        std::vector<RRTNode *> cusps;
+        for (unsigned int i = 0; i < tmp_cusps.size(); i++) {
+                if (tmp_cusps[i] != tmp_cusps[i + 1])
+                        cusps.push_back(tmp_cusps[i]);
+        }
+        std::reverse(cusps.begin(), cusps.end());
+        std::vector<int> npi; // new path indexes
+        if (!this->optp_dijkstra(cusps, npi))
+                return false;
         RRTNode *pn = cusps[npi[0]];
         for (unsigned int i = 0; i < npi.size() - 1; i++) {
                 for (auto ns: this->steer(cusps[npi[i]], cusps[npi[i + 1]])) {
@@ -622,18 +628,12 @@ bool RRTBase::optp_dijkstra()
                         pn = ns;
                 }
         }
-        // End of Dijkstra
         this->root()->update_ccost();
         if (this->tlog().back().front()->ccost() < oc)
                 return true;
         return false;
 }
 
-bool RRTBase::opt_path()
-{
-        return this->optp_dijkstra();
-}
-
 bool RRTBase::rebase(RRTNode *nr)
 {
         if (!nr || this->goal_ == nr || this->root_ == nr)
diff --git a/incl/rrtbase.h b/incl/rrtbase.h
index 5c713b4..e7af3d5 100644
--- a/incl/rrtbase.h
+++ b/incl/rrtbase.h
@@ -95,7 +95,9 @@ class RRTBase {
                                 RRTNode *node,
                                 float (*cost)(RRTNode *, RRTNode *));
                 bool collide(RRTNode *init, RRTNode *goal);
-                bool optp_dijkstra();
+                bool optp_dijkstra(
+                                std::vector<RRTNode *> &cusps,
+                                std::vector<int> &npi);
                 bool opt_path();
                 bool rebase(RRTNode *nr);
                 std::vector<RRTNode *> findt();
-- 
2.39.2