From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 29 Mar 2021 12:13:39 +0000 (+0200)
Subject: Add missing cpp template for test48
X-Git-Tag: v0.4.0~4
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar2.git/commitdiff_plain/97135d0459d43715c49b5f65a0b1f37975e0b3ff

Add missing cpp template for test48
---

diff --git a/src/uniform-stored-sampling-template.cc b/src/uniform-stored-sampling-template.cc
new file mode 100644
index 0000000..597b392
--- /dev/null
+++ b/src/uniform-stored-sampling-template.cc
@@ -0,0 +1,95 @@
+#include <algorithm>
+#include <chrono>
+#include <iostream>
+#include <jsoncpp/json/json.h>
+
+#include "psp.h"
+#include "rrtce.h"
+#ifndef EPP
+        #define EPP RRTS
+#endif
+
+double edist(double x1, double y1, double x2, double y2)
+{
+        return sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
+}
+
+std::chrono::high_resolution_clock::time_point TSTART_;
+std::chrono::high_resolution_clock::time_point TEND_;
+inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); }
+inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); }
+inline double TDIFF()
+{
+        std::chrono::duration<double> DT_;
+        DT_ = std::chrono::duration_cast<std::chrono::duration<double>>(
+                TEND_ - TSTART_
+        );
+        return DT_.count();
+}
+inline void TPRINT(const char *what)
+{
+        std::cerr << what << ": " << TDIFF() << std::endl;
+}
+
+PSPlanner psp;
+EPP epp;
+int main()
+{
+        Json::Value jvi; // JSON input
+        Json::Value jvo; // JSON output
+        std::cin >> jvi;
+        unsigned int i = 0;
+
+        psp.ps().border(
+                jvi["slot"][0][0][0].asDouble(),
+                jvi["slot"][0][0][1].asDouble(),
+
+                jvi["slot"][0][1][0].asDouble(),
+                jvi["slot"][0][1][1].asDouble(),
+
+                jvi["slot"][0][2][0].asDouble(),
+                jvi["slot"][0][2][1].asDouble(),
+
+                jvi["slot"][0][3][0].asDouble(),
+                jvi["slot"][0][3][1].asDouble()
+        );
+        if (psp.ps().parallel()) {
+                auto s = psp.ps();
+                double len1 = edist(s.x1(), s.y1(), s.x4(), s.y4());
+                if (len1 < 7.0) { // sc4_1
+                        jvi["entry"][0] = -744205.8107096809;
+                        jvi["entry"][1] = -1044324.0805424277;
+                        jvi["entry"][2] = 0.407050423340469;
+                        jvi["entry"][3] = 0.40715042334046897;
+                //} else { // sc1_0
+                //        jvi["entry"][0] = 6.688127114360318;
+                //        jvi["entry"][1] = 0.9001931028769716;
+                //        jvi["entry"][2] = 0.3416999999999787;
+                //        jvi["entry"][3] = 0.40749999999997144;
+                }
+        }
+        psp.fe();
+
+        auto lm = psp.last_maneuver();
+        std::reverse(lm.begin(), lm.end());
+        i = 0;
+        for (auto g: lm) {
+                jvi["goals"][i][0] = g.x();
+                jvi["goals"][i][1] = g.y();
+                jvi["goals"][i][2] = g.h();
+                i++;
+        }
+
+        epp.json(jvi);
+        epp.init();
+        epp.sample_dist_type(3);
+        // TODO the following has no meaning for uniform circle sampling
+        epp.set_sample(-50, 50, -50, 50, 0, 2 * M_PI);
+
+        while (epp.next()) {}
+
+        jvo = epp.json();
+        jvo["slot"] = jvi["slot"];
+        std::cout << jvo << std::endl;
+        return 0;
+}