src/uniform-stored-sampling-template.cc

   1 #include <algorithm>
   2 #include <chrono>
   3 #include <iostream>
   4 #include <jsoncpp/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.8107096809;
  61                         jvi["entry"][1] = -1044324.0805424277;
  62                         jvi["entry"][2] = 0.407050423340469;
  63                         jvi["entry"][3] = 0.40715042334046897;
  64                 //} else { // sc1_0
  65                 //        jvi["entry"][0] = 6.688127114360318;
  66                 //        jvi["entry"][1] = 0.9001931028769716;
  67                 //        jvi["entry"][2] = 0.3416999999999787;
  68                 //        jvi["entry"][3] = 0.40749999999997144;
  69                 }
  70         }
  71         psp.fe();
  72
  73         auto lm = psp.last_maneuver();
  74         std::reverse(lm.begin(), lm.end());
  75         i = 0;
  76         for (auto g: lm) {
  77                 jvi["goals"][i][0] = g.x();
  78                 jvi["goals"][i][1] = g.y();
  79                 jvi["goals"][i][2] = g.h();
  80                 i++;
  81         }
  82
  83         epp.json(jvi);
  84         epp.init();
  85         epp.sample_dist_type(3);
  86         // TODO the following has no meaning for uniform circle sampling
  87         epp.set_sample(-50, 50, -50, 50, 0, 2 * M_PI);
  88
  89         while (epp.next()) {}
  90
  91         jvo = epp.json();
  92         jvo["slot"] = jvi["slot"];
  93         std::cout << jvo << std::endl;
  94         return 0;
  95 }