Make cost from orig init RRT independent, fix incl

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

diff --git a/src/test7.cc b/src/test7.cc
index e0544589218cb613e1484341583186078126bc7f..380c23c336d4ef7169bb67500aa4d1bca196df57 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_;
@@ -58,9 +58,18 @@ int main()
         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;
-        cost_from_orig_init += rrts.cost_build(last_node, init_node);
         rrts.nodes().front().x(init_node.x());
         rrts.nodes().front().y(init_node.y());
         rrts.nodes().front().h(init_node.h());
@@ -135,8 +144,17 @@ int main()
 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;
-        cost_from_orig_init += rrts2.cost_build(last_node, init_node);
         rrts2.goals() = rrts.goals();
         rrts2.obstacles() = rrts.obstacles();
         rrts2.nodes().front().x(init_node.x());