From: Jiri Hubacek <hubacji1@fel.cvut.cz>
Date: Wed, 26 Sep 2018 16:16:37 +0000 (+0200)
Subject: Plot root/goal
X-Git-Tag: v0.2.0~16^2~3
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/466dc08743732c5acd176c5c2f363b18d7568a26

Plot root/goal
---

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index df76683..865eef4 100644
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -227,6 +227,13 @@ bool RRTBase::glplot()
                 glVertex2f(GLVERTEX(o.goal()));
         }
         glEnd();
+        // Plot root, goal
+        glPointSize(4);
+        glBegin(GL_POINTS);
+        glColor3f(1, 0, 0);
+        glVertex2f(GLVERTEX(this->root_));
+        glVertex2f(GLVERTEX(this->goal_));
+        glEnd();
         // Plot nodes
         std::vector<RRTNode *> s; // DFS stack
         std::vector<RRTNode *> r; // reset visited_