Fix final path cost computation

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

diff --git a/src/test7.cc b/src/test7.cc
index dddf48d64d34425b0d78823fbc480405db1f812f..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());
@@ -126,7 +145,18 @@ int main()
         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();
@@ -156,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());