src/uniform-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         psp.fe();
  57
  58         auto lm = psp.last_maneuver();
  59         std::reverse(lm.begin(), lm.end());
  60         i = 0;
  61         for (auto g: lm) {
  62                 jvi["goals"][i][0] = g.x();
  63                 jvi["goals"][i][1] = g.y();
  64                 jvi["goals"][i][2] = g.h();
  65                 i++;
  66         }
  67
  68         epp.json(jvi);
  69         epp.init();
  70         epp.sample_dist_type(2);
  71         // TODO the following has no meaning for uniform circle sampling
  72         epp.set_sample(-50, 50, -50, 50, 0, 2 * M_PI);
  73
  74         while (epp.next()) {}
  75
  76         jvo = epp.json();
  77         jvo["slot"] = jvi["slot"];
  78         std::cout << jvo << std::endl;
  79         return 0;
  80 }