Fix final path cost computation

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

diff --git a/src/test7.cc b/src/test7.cc
index 158ee20de895c33be41bf6ff9b9335c3c6da37ce..c544a356a4bc7e0521ffe9707d4ddd004e8532c1 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,25 @@ int main()
 
         std::vector<RRTNode> final_path;
         double final_path_cost = 9999;
+        double cost_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);
         init_node.next();
+        {
+                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 +123,15 @@ 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.clear();
                         for (auto n: rrts.path()) {
                                 final_path.push_back(RRTNode());
@@ -122,8 +141,22 @@ 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();
+        {
+                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();
@@ -153,8 +186,15 @@ 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.clear();
                         for (auto n: rrts2.path()) {
                                 final_path.push_back(RRTNode());
@@ -164,77 +204,10 @@ 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["time"] = TDIFF();
-        }
-        {
-                jvo["iterations"] = rrts.icnt();
-        }
-        {
-                jvo["init"][0] = rrts.nodes().front().x();
-                jvo["init"][1] = rrts.nodes().front().y();
-                jvo["init"][2] = rrts.nodes().front().h();
-        }
-        {
-                jvo["cost"] = final_path_cost;
-        }
-        {
-                if (final_path.size() > 0) {
-                        jvo["goal"][0] = final_path.back().x();
-                        jvo["goal"][1] = final_path.back().y();
-                        jvo["goal"][2] = final_path.back().h();
-                }
-                unsigned int cu = 0;
-                unsigned int co = 0;
-                unsigned int pcnt = 0;
-                for (auto n: final_path) {
-                        jvo["path"][pcnt][0] = n.x();
-                        jvo["path"][pcnt][1] = n.y();
-                        jvo["path"][pcnt][2] = n.h();
-                        if (n.t(RRTNodeType::cusp))
-                                cu++;
-                        if (n.t(RRTNodeType::connected))
-                                co++;
-                        pcnt++;
-                }
-                jvo["cusps-in-path"] = cu;
-                jvo["connecteds-in-path"] = co;
-        }
-        {
-                unsigned int gcnt = 0;
-                for (auto g: rrts.goals()) {
-                        jvo["goals"][gcnt][0] = g.x();
-                        jvo["goals"][gcnt][1] = g.y();
-                        jvo["goals"][gcnt][2] = g.h();
-                        gcnt++;
-                }
-        }
-        {
-                unsigned int ocnt = 0;
-                for (auto o: rrts.obstacles()) {
-                        unsigned int ccnt = 0;
-                        for (auto c: o.poly()) {
-                                jvo["obst"][ocnt][ccnt][0] = std::get<0>(c);
-                                jvo["obst"][ocnt][ccnt][1] = std::get<1>(c);
-                                ccnt++;
-                        }
-                        ocnt++;
-                }
-        }
-        {
-                jvo["nodes"] = (unsigned int) rrts.nodes().size();
-        }
-        //{
-        //        unsigned int ncnt = 0;
-        //        for (auto n: rrts.nodes()) {
-        //                jvo["nodes_x"][ncnt] = n.x();
-        //                jvo["nodes_y"][ncnt] = n.y();
-        //                //jvo["nodes_h"][ncnt] = n.h();
-        //                ncnt++;
-        //        }
-        //}
         std::cout << jvo << std::endl;
         return 0;
 }