From 1764f183594273992753f11b41cb11cb18a5e6f1 Mon Sep 17 00:00:00 2001
From: Jiri Hubacek <hubacji1@fel.cvut.cz>
Date: Mon, 2 Jul 2018 00:25:11 +0200
Subject: [PATCH] Add edges to JSON output

---
 base/main.cc | 28 ++++++++++++++++++++++++++++
 1 file changed, 28 insertions(+)

diff --git a/base/main.cc b/base/main.cc
index 184b473..93af0af 100644
--- a/base/main.cc
+++ b/base/main.cc
@@ -71,6 +71,9 @@ int main()
         Json::Value jvo; // JSON output
         int i = 0;
         int j = 0;
+        // log traj
+        i = 0;
+        j = 0;
         for (auto traj: p.tlog()) {
                 i = 0;
                 for (auto n: traj) {
@@ -81,6 +84,31 @@ int main()
                 }
                 j++;
         }
+        // log edges
+        i = 0;
+        j = 0;
+        std::vector<RRTNode *> s; // DFS stack
+        std::vector<RRTNode *> r; // reset visited_
+        RRTNode *tmp;
+        s.push_back(p.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);
+                                jvo["edge"][j][0][0] = tmp->x();
+                                jvo["edge"][j][0][1] = tmp->y();
+                                jvo["edge"][j][1][0] = ch->x();
+                                jvo["edge"][j][1][1] = ch->y();
+                                j++;
+                        }
+                }
+        }
+        for (auto n: r)
+                n->visit(false);
+        // print output
         std::cout << jvo << std::endl;
 
         // free mem
-- 
2.39.2