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) {
}
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