Store computed path in class variable

[hubacji1/rrts.git] / src / rrtext3.cc

diff --git a/src/rrtext3.cc b/src/rrtext3.cc
index aff866c0325d093c41b74e2023285a85aedb2861..8077b7e8f390ff6aa05044467383f24de2fdd76a 100644 (file)
--- a/src/rrtext3.cc
+++ b/src/rrtext3.cc
@@ -1,12 +1,12 @@
 #include <queue>
 #include "rrtext.h"
 
-std::vector<RRTNode *> RRTExt3::first_path_optimization()
+void RRTExt3::first_path_optimization()
 {
         if (this->orig_path().size() == 0) {
                 this->orig_path_ = RRTS::path();
                 if (this->orig_path().size() == 0)
-                        return this->orig_path();
+                        return;
                 else
                         this->orig_path_cost(this->orig_path().back()->cc);
         }
@@ -97,25 +97,25 @@ std::vector<RRTNode *> RRTExt3::first_path_optimization()
                         ln = &n;
         }
         if (ln == nullptr || ln->p() == nullptr)
-                return this->orig_path();
+                return;
         while (ln->p() != nullptr) {
                 RRTNode *t = ln->n;
                 RRTNode *f = ln->p()->n;
                 this->steer(*f, *t);
                 if (std::get<0>(this->collide_steered_from(*f)))
-                        return this->orig_path();
+                        return;
                 this->join_steered(f);
                 t->p(&this->nodes().back());
                 t->c(this->cost_build(this->nodes().back(), *t));
                 ln = ln->p();
         }
-        return RRTS::path();
+        RRTS::compute_path();
 }
 
-std::vector<RRTNode *> RRTExt3::second_path_optimization()
+void RRTExt3::second_path_optimization()
 {
         if (this->first_optimized_path().size() == 0) {
-                return this->orig_path();
+                return;
         }
         class DijkstraNode : public RRTNode {
                 public:
@@ -207,38 +207,40 @@ std::vector<RRTNode *> RRTExt3::second_path_optimization()
                 }
         }
         if (fn == nullptr || fn->p() == nullptr)
-                return this->first_optimized_path();
+                return;
         DijkstraNode *ln = &dn.back();
         while (ln->p() != fn) {
                 RRTNode *t = ln->n;
                 RRTNode *f = ln->p()->n;
                 this->steer(*f, *t);
                 if (std::get<0>(this->collide_steered_from(*f)))
-                        return this->first_optimized_path();
+                        return;
                 this->join_steered(f);
                 t->p(&this->nodes().back());
                 t->c(this->cost_build(this->nodes().back(), *t));
                 ln = ln->p();
         }
-        return RRTS::path();
+        RRTS::compute_path();
 }
 
-std::vector<RRTNode *> RRTExt3::path()
+void RRTExt3::compute_path()
 {
+        RRTS::compute_path();
         auto tstart = std::chrono::high_resolution_clock::now();
-        this->first_optimized_path_ = this->first_path_optimization();
-        if (this->first_optimized_path_.size() > 0)
+        this->first_path_optimization();
+        this->first_optimized_path_ = RRTS::path();
+        if (this->first_optimized_path_.size() > 0) {
                 this->first_optimized_path_cost(
                         this->first_optimized_path_.back()->cc
                 );
-        else
-                return this->orig_path();
-        auto opt2 = this->second_path_optimization();
+        } else {
+                return;
+        }
+        this->second_path_optimization();
         auto tend = std::chrono::high_resolution_clock::now();
         auto tdiff = std::chrono::duration_cast<std::chrono::duration<double>>(
             tend - tstart);
         this->log_opt_time_.push_back(tdiff.count());
-        return opt2;
 }
 
 Json::Value RRTExt3::json()