Add edges to JSON output

diff --git a/base/main.cc b/base/main.cc
index 184b473fd759d9a01230b949b3df729055b3e259..93af0afe96853b12a0292e8dc9e5256aaa017ca2 100644 (file)
--- 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