Plot car frame with GL

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 8a44e6e572cbfff7a61fc79b92b579226c1e1cff..0234fd450c562caadb46bb7ee2cf853806ca1dc6 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -399,12 +399,52 @@ bool RRTBase::glplot()
                 glVertex2f(GLVERTEX(this->samples_.back()));
                 glEnd();
         }
-        // Plot nodes
+        // Plot nodes (position, orientation
         std::vector<RRTNode *> s; // DFS stack
         std::vector<RRTNode *> r; // reset visited_
         RRTNode *tmp;
         glBegin(GL_LINES);
         s.push_back(this->root_);
+        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);
+                                BicycleCar bc(tmp->x(), tmp->y(), tmp->h());
+                                glVertex2f(
+                                        bc.lfx() * GLPLWSCALE,
+                                        bc.lfy() * GLPLHSCALE
+                                );
+                                glVertex2f(
+                                        bc.lrx() * GLPLWSCALE,
+                                        bc.lry() * GLPLHSCALE
+                                );
+                                glVertex2f(
+                                        bc.lrx() * GLPLWSCALE,
+                                        bc.lry() * GLPLHSCALE
+                                );
+                                glVertex2f(
+                                        bc.rrx() * GLPLWSCALE,
+                                        bc.rry() * GLPLHSCALE
+                                );
+                                glVertex2f(
+                                        bc.rrx() * GLPLWSCALE,
+                                        bc.rry() * GLPLHSCALE
+                                );
+                                glVertex2f(
+                                        bc.rfx() * GLPLWSCALE,
+                                        bc.rfy() * GLPLHSCALE
+                                );
+                        }
+                }
+        }
+        glEnd();
+        // Plot nodes
+        glBegin(GL_LINES);
+        s.push_back(this->root_);
         while (s.size() > 0) {
                 tmp = s.back();
                 s.pop_back();