Add plot nodes from goal

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index f3d031f00bb8eb67b00aa4a174a29cdeca6f190e..d71dc28c37b1b25ea13ef7bdccdc8f4d456ce2b1 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -288,6 +288,23 @@ bool RRTBase::glplot()
                 }
         }
         glEnd();
+        // Plot nodes (from goal)
+        glBegin(GL_LINES);
+        s.push_back(this->goal_);
+        while (s.size() > 0) {
+                tmp = s.back();
+                s.pop_back();
+                if (!tmp->visit()) {
+                        r.push_back(tmp);
+                        for (auto ch: tmp->children()) {
+                                s.push_back(ch);
+                                glColor3f(0.5, 0.5, 0.5);
+                                glVertex2f(GLVERTEX(tmp));
+                                glVertex2f(GLVERTEX(ch));
+                        }
+                }
+        }
+        glEnd();
         std::vector<RRTNode *> cusps;
         // Plot last trajectory
         if (this->tlog().size() > 0) {