Merge branch 'feature/refactor-small-fixes'

[hubacji1/iamcar2.git] / src / test7.cc

diff --git a/src/test7.cc b/src/test7.cc
index c544a356a4bc7e0521ffe9707d4ddd004e8532c1..3f7e3ff940e0769d0281af33724616ed57b6dd37 100644 (file)
--- a/src/test7.cc
+++ b/src/test7.cc
@@ -49,7 +49,9 @@ int main()
 
         std::vector<RRTNode> final_path;
         double final_path_cost = 9999;
+        unsigned int final_path_steps = 0;
         double cost_from_orig_init = 0;
+        unsigned int steps_from_orig_init = 0;
         RRTNode init_node;
         init_node.x(jvi["init"][0].asDouble());
         init_node.y(jvi["init"][1].asDouble());
@@ -57,7 +59,9 @@ int main()
         init_node.sp(2.7);
         init_node.st(M_PI / 32); // only for sc2_4
         RRTNode last_node(init_node);
+        RRTNode orig_init(init_node);
         init_node.next();
+        steps_from_orig_init++;
         {
                 double angl_diff = std::abs(last_node.h() - init_node.h());
                 if (angl_diff == 0)
@@ -132,6 +136,7 @@ int main()
                                 cc(*rrts.path().back())
                                 + cost_from_orig_init
                         ;
+                        final_path_steps = steps_from_orig_init;
                         final_path.clear();
                         for (auto n: rrts.path()) {
                                 final_path.push_back(RRTNode());
@@ -141,12 +146,12 @@ int main()
                         }
                 }
         }
-        std::cerr << "finished 0, cost is " << final_path_cost << std::endl;
         int rcnt = 0;
         jvo[rcnt++] = rrts.json();
 for (int i = 1; i < 5; i++) {
         last_node = RRTNode(init_node);
         init_node.next();
+        steps_from_orig_init++;
         {
                 double angl_diff = std::abs(last_node.h() - init_node.h());
                 if (angl_diff < 0.001)
@@ -195,6 +200,7 @@ for (int i = 1; i < 5; i++) {
                                 cc(*rrts2.path().back())
                                 + cost_from_orig_init
                         ;
+                        final_path_steps = steps_from_orig_init;
                         final_path.clear();
                         for (auto n: rrts2.path()) {
                                 final_path.push_back(RRTNode());
@@ -204,10 +210,26 @@ for (int i = 1; i < 5; i++) {
                         }
                 }
         }
-        std::cerr << "finished " << i << ", cost is " << final_path_cost;
-        std::cerr << std::endl;
         jvo[rcnt++] = rrts2.json();
 }
+        {
+                jvo[rcnt]["obst"] = jvo[rcnt - 1]["obst"];
+                jvo[rcnt]["cost"] = final_path_cost;
+                unsigned int ncnt = 0;
+                for (unsigned int i = 0; i < final_path_steps; i++) {
+                        jvo[rcnt]["path"][ncnt][0] = orig_init.x();
+                        jvo[rcnt]["path"][ncnt][1] = orig_init.y();
+                        jvo[rcnt]["path"][ncnt][2] = orig_init.h();
+                        orig_init.next();
+                        ncnt++;
+                }
+                for (auto n: final_path) {
+                        jvo[rcnt]["path"][ncnt][0] = n.x();
+                        jvo[rcnt]["path"][ncnt][1] = n.y();
+                        jvo[rcnt]["path"][ncnt][2] = n.h();
+                        ncnt++;
+                }
+        }
         std::cout << jvo << std::endl;
         return 0;
 }