Add slot cusp to RRTBase

[hubacji1/iamcar.git] / base / rrtbase.cc

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 4709c8e71c446da727e60088fa2a3818d99d3cb1..125fe8f0007ff39cf9d4954cf25d647fe579aa86 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -169,6 +169,11 @@ std::vector<std::vector<RRTNode *>> &RRTBase::tlog()
         return this->tlog_;
 }
 
+std::vector<RRTNode *> &RRTBase::slot_cusp()
+{
+        return this->slot_cusp_;
+}
+
 bool RRTBase::goal_found()
 {
         return this->goal_found_;
@@ -297,6 +302,14 @@ bool RRTBase::goal_found(bool f)
         return f;
 }
 
+void RRTBase::slot_cusp(std::vector<RRTNode *> sc)
+{
+        for (unsigned int i = 0; i < sc.size() - 1; i++)
+                sc[i]->add_child(sc[i + 1], this->cost(sc[i], sc[i + 1]));
+        sc[0]->parent(this->goal());
+        this->slot_cusp_ = sc;
+}
+
 // other
 bool RRTBase::glplot()
 {