Merge branch 'feature/refactor-small-fixes'

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

diff --git a/src/test7.cc b/src/test7.cc
index 62a0d675717221dab6eabae07373f0d32954790f..3f7e3ff940e0769d0281af33724616ed57b6dd37 100644 (file)
--- a/src/test7.cc
+++ b/src/test7.cc
@@ -2,7 +2,7 @@
 #include <iostream>
 #include <jsoncpp/json/json.h>
 
-#include "rrts.h"
+#include "rrtce.h"
 
 std::chrono::high_resolution_clock::time_point TSTART_;
 std::chrono::high_resolution_clock::time_point TEND_;
@@ -49,13 +49,29 @@ 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());
         init_node.h(jvi["init"][2].asDouble());
         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)
+                        cost_from_orig_init += sqrt(
+                                pow(last_node.x() - init_node.x(), 2)
+                                + pow(last_node.y() - init_node.y(), 2)
+                        );
+                else
+                        cost_from_orig_init += angl_diff * init_node.mtr();
+        }
 
         RRTS rrts;
         rrts.nodes().front().x(init_node.x());
@@ -111,8 +127,16 @@ int main()
         while (rrts.next()) {}
         TEND();
         if (rrts.path().size() > 0) {
-                if (cc(*rrts.path().back()) < final_path_cost) {
-                        final_path_cost = cc(*rrts.path().back());
+                if (
+                        cc(*rrts.path().back())
+                        + cost_from_orig_init
+                        < final_path_cost
+                ) {
+                        final_path_cost =
+                                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());
@@ -122,10 +146,22 @@ int main()
                         }
                 }
         }
-        std::cerr << "finished 0, cost is " << final_path_cost << std::endl;
-        std::cout << rrts.json() << 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)
+                        cost_from_orig_init += sqrt(
+                                pow(last_node.x() - init_node.x(), 2)
+                                + pow(last_node.y() - init_node.y(), 2)
+                        );
+                else
+                        cost_from_orig_init += angl_diff * init_node.mtr();
+        }
         RRTS rrts2;
         rrts2.goals() = rrts.goals();
         rrts2.obstacles() = rrts.obstacles();
@@ -155,8 +191,16 @@ for (int i = 1; i < 5; i++) {
         while (rrts2.next()) {}
         TEND();
         if (rrts2.path().size() > 0) {
-                if (cc(*rrts2.path().back()) < final_path_cost) {
-                        final_path_cost = cc(*rrts2.path().back());
+                if (
+                        cc(*rrts2.path().back())
+                        + cost_from_orig_init
+                        < final_path_cost
+                ) {
+                        final_path_cost =
+                                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());
@@ -166,9 +210,26 @@ for (int i = 1; i < 5; i++) {
                         }
                 }
         }
-        std::cerr << "finished " << i << ", cost is " << final_path_cost;
-        std::cerr << std::endl;
-        std::cout << rrts2.json() << 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;
 }