From 466dc08743732c5acd176c5c2f363b18d7568a26 Mon Sep 17 00:00:00 2001
From: Jiri Hubacek <hubacji1@fel.cvut.cz>
Date: Wed, 26 Sep 2018 18:16:37 +0200
Subject: [PATCH] Plot root/goal

---
 base/rrtbase.cc | 7 +++++++
 1 file changed, 7 insertions(+)

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_
-- 
2.39.2