src/uniform-stored-sampling-template.cc

   1 #include <algorithm>
   2 #include <chrono>
   3 #include <iostream>
   4 #include <json/json.h>
   5
   6 #include "psp.h"
   7 #include "rrtce.h"
   8 #ifndef EPP
   9         #define EPP RRTS
  10 #endif
  11
  12 double edist(double x1, double y1, double x2, double y2)
  13 {
  14         return sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
  15 }
  16
  17 std::chrono::high_resolution_clock::time_point TSTART_;
  18 std::chrono::high_resolution_clock::time_point TEND_;
  19 inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); }
  20 inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); }
  21 inline double TDIFF()
  22 {
  23         std::chrono::duration<double> DT_;
  24         DT_ = std::chrono::duration_cast<std::chrono::duration<double>>(
  25                 TEND_ - TSTART_
  26         );
  27         return DT_.count();
  28 }
  29 inline void TPRINT(const char *what)
  30 {
  31         std::cerr << what << ": " << TDIFF() << std::endl;
  32 }
  33
  34 PSPlanner psp;
  35 EPP epp;
  36 int main()
  37 {
  38         Json::Value jvi; // JSON input
  39         Json::Value jvo; // JSON output
  40         std::cin >> jvi;
  41         unsigned int i = 0;
  42
  43         psp.ps().border(
  44                 jvi["slot"][0][0][0].asDouble(),
  45                 jvi["slot"][0][0][1].asDouble(),
  46
  47                 jvi["slot"][0][1][0].asDouble(),
  48                 jvi["slot"][0][1][1].asDouble(),
  49
  50                 jvi["slot"][0][2][0].asDouble(),
  51                 jvi["slot"][0][2][1].asDouble(),
  52
  53                 jvi["slot"][0][3][0].asDouble(),
  54                 jvi["slot"][0][3][1].asDouble()
  55         );
  56         if (psp.ps().parallel()) {
  57                 auto s = psp.ps();
  58                 double len1 = edist(s.x1(), s.y1(), s.x4(), s.y4());
  59                 if (len1 < 7.0) { // sc4_1
  60                         jvi["entry"][0] = -744205.7720085467;
  61                         jvi["entry"][1] = -1044324.2341447787;
  62                         jvi["entry"][2] = 0.40625042334046907;
  63                         jvi["entry"][3] = 0.4748504233404615;
  64                         jvi["entries"][0][0] = -744208.558833301;
  65                         jvi["entries"][0][1] = -1044325.6668039856;
  66                         jvi["entries"][0][2] = 0.4748504233404615;
  67                         jvi["entries"][1][0] = -744208.5992336513;
  68                         jvi["entries"][1][1] = -1044325.4503620268;
  69                         jvi["entries"][1][2] = 0.40625042334046907;
  70                 //} else { // sc1_0
  71                 //        jvi["entry"][0] = 6.688127114360318;
  72                 //        jvi["entry"][1] = 0.9001931028769716;
  73                 //        jvi["entry"][2] = 0.3416999999999787;
  74                 //        jvi["entry"][3] = 0.40749999999997144;
  75                 }
  76         }
  77         psp.fe();
  78
  79         auto lm = psp.last_maneuver();
  80         std::reverse(lm.begin(), lm.end());
  81         i = 0;
  82         for (auto g: lm) {
  83                 jvi["goals"][i][0] = g.x();
  84                 jvi["goals"][i][1] = g.y();
  85                 jvi["goals"][i][2] = g.h();
  86                 i++;
  87         }
  88
  89         epp.json(jvi);
  90         epp.init();
  91         epp.sample_dist_type(3);
  92         // TODO the following has no meaning for uniform circle sampling
  93         epp.set_sample(-50, 50, -50, 50, 0, 2 * M_PI);
  94
  95         while (epp.next()) {}
  96
  97         jvo = epp.json();
  98         jvo["slot"] = jvi["slot"];
  99         std::cout << jvo << std::endl;
 100         return 0;
 101 }