From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Wed, 16 Jan 2019 15:34:14 +0000 (+0100)
Subject: Update T2 with adding to `ixy_` data structure
X-Git-Tag: v0.5.0~3^2~6
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/241d52dcd1756631c9c61368071c7a064ea7315b

Update T2 with adding to `ixy_` data structure
---

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 59ef374..0cabc23 100644
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -90,6 +90,7 @@ RRTBase::RRTBase():
         this->nodes_.reserve(20000);
         this->nodes_.push_back(this->root_);
         this->add_iy(this->root_);
+        this->add_ixy(this->root_);
 }
 
 RRTBase::RRTBase(RRTNode *init, RRTNode *goal):
@@ -99,6 +100,7 @@ RRTBase::RRTBase(RRTNode *init, RRTNode *goal):
         this->nodes_.reserve(20000);
         this->nodes_.push_back(init);
         this->add_iy(init);
+        this->add_ixy(init);
 }
 
 // getter
@@ -768,6 +770,7 @@ bool RRTBase::opt_path()
                         } else {
                                 this->nodes().push_back(ns);
                                 this->add_iy(ns);
+                                this->add_ixy(ns);
                                 pn->add_child(ns, this->cost(pn, ns));
                                 pn = ns;
                         }
diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index c3b45bb..d5667fe 100644
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -442,6 +442,7 @@ bool T2::next()
                                 delete ns;
                                 en_add = false;
                         } else {
+                                this->add_ixy(ns);
                                 // rewire
                                 this->rewire(nvs, ns);
                                 pn = ns;
@@ -480,6 +481,7 @@ bool T2::next()
                                 delete ns;
                                 en_add = false;
                         } else {
+                                this->add_ixy(ns);
                                 // rewire
                                 this->rewire(nvs, ns);
                                 pn = ns;
@@ -518,6 +520,7 @@ bool T2::next()
                                         delete ns;
                                         en_add = false;
                                 } else {
+                                        this->add_ixy(ns);
                                         this->ocost(ns);
                                         pn = ns;
                                         if (this->goal_found(pn, CO)) {
@@ -550,6 +553,7 @@ bool T2::next()
                                         delete ns;
                                         en_add = false;
                                 } else {
+                                        this->add_ixy(ns);
                                         this->ocost(ns);
                                         pn = ns;
                                         if (this->goal_found(pn, CO)) {