Add method find trajectory (backward) from node

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 83fa5f27d8055314418406fba4a6e25a87e33653..0dc1ac9b5c166822f864c66a5b1fca47ec2bf9bb 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -290,11 +290,16 @@ bool RRTBase::collide(RRTNode *init, RRTNode *goal)
 }
 
 std::vector<RRTNode *> RRTBase::findt()
+{
+        return this->findt(this->goal_);
+}
+
+std::vector<RRTNode *> RRTBase::findt(RRTNode *n)
 {
         std::vector<RRTNode *> nodes;
-        if (!this->goal_->parent())
+        if (!n->parent())
                 return nodes;
-        RRTNode *tmp = this->goal_;
+        RRTNode *tmp = n;
         while (tmp != this->root_) {
                 nodes.push_back(tmp);
                 if (!tmp->parent())

diff --git a/incl/rrtbase.h b/incl/rrtbase.h
index ae0f11a1cc536f8a208bdf6693f63ccb1321699b..cee16318dc48c0615ac16ce87f12433557e3c215 100644 (file)
--- a/incl/rrtbase.h
+++ b/incl/rrtbase.h
@@ -83,6 +83,7 @@ class RRTBase {
                                 float (*cost)(RRTNode *, RRTNode *));
                 bool collide(RRTNode *init, RRTNode *goal);
                 std::vector<RRTNode *> findt();
+                std::vector<RRTNode *> findt(RRTNode *n);
 
                 // virtuals - implemented by child classes
                 virtual bool next() = 0;