Add method to set dimensions of more cars

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

diff --git a/src/test2.cc b/src/test2.cc
index b92ced8316c57a6f6c99c40cc3acec2c7440abe5..6f4f3793200f7f9228ea64c15c1e9d937f73c6ec 100644 (file)
--- a/src/test2.cc
+++ b/src/test2.cc
@@ -1,6 +1,6 @@
 #include <chrono>
 #include <iostream>
-#include <jsoncpp/json/json.h>
+#include <json/json.h>
 
 #include "rrts.h"
 
@@ -82,9 +82,43 @@ int main()
                 }
         }
 
+        {
+                double edist_init_goal = sqrt(
+                        pow(
+                                rrts.nodes().front().x()
+                                - rrts.goals().front().x(),
+                                2
+                        )
+                        + pow(
+                                rrts.nodes().front().y()
+                                - rrts.goals().front().y(),
+                                2
+                        )
+                );
+                rrts.set_sample(
+                        rrts.nodes().front().x(), edist_init_goal,
+                        rrts.nodes().front().y(), edist_init_goal,
+                        0, 2 * M_PI
+                );
+        }
+
+        TSTART();
+        while (rrts.next()) {}
+        TEND();
+
         {
                 jvo["time"] = TDIFF();
         }
+        {
+                jvo["iterations"] = rrts.icnt();
+        }
+        {
+                if (rrts.path().size() > 0) {
+                        jvo["cost"] = cc(*rrts.path().back());
+                } else {
+                        jvo["cost"] = -1;
+                }
+        }
         {
                 jvo["init"][0] = rrts.nodes().front().x();
                 jvo["init"][1] = rrts.nodes().front().y();
@@ -117,13 +151,24 @@ int main()
                         jvo["goal"][1] = rrts.path().back()->y();
                         jvo["goal"][2] = rrts.path().back()->h();
                 }
+                unsigned int cu = 0;
+                unsigned int co = 0;
                 unsigned int pcnt = 0;
                 for (auto n: rrts.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;
+        }
+        {
+                jvo["nodes"] = (unsigned int) rrts.nodes().size();
         }
 
         std::cout << jvo << std::endl;