Add test template with reset

diff --git a/src/template-with-reset.cc b/src/template-with-reset.cc
new file mode 100644 (file)
index 0000000..2266a3e
--- /dev/null
+++ b/src/template-with-reset.cc
@@ -0,0 +1,121 @@
+#include <cassert>
+#include <iostream>
+#include <json/json.h>
+#include <vector>
+#include "fiat_punto.h"
+#include "pslot.hh"
+#include "rrtsp.hh"
+
+#ifndef OSP
+       #define OSP P36
+#endif
+
+rrts::OSP p;
+
+int main()
+{
+       Json::Value jvi;
+       std::cin >> jvi;
+       std::cout << std::fixed;
+       std::cerr << std::fixed;
+       assert(jvi["slot"] != Json::nullValue);
+       bcar::ParkingSlot s(
+               jvi["slot"][0][0].asDouble(), jvi["slot"][0][1].asDouble(),
+               jvi["slot"][1][0].asDouble(), jvi["slot"][1][1].asDouble(),
+               jvi["slot"][2][0].asDouble(), jvi["slot"][2][1].asDouble(),
+               jvi["slot"][3][0].asDouble(), jvi["slot"][3][1].asDouble());
+       bcar::BicycleCar c;
+       bool swapped = false;
+       if (s.parallel() && !s.right()) {
+               s.swap_side();
+               swapped = true;
+       }
+       auto pr = s.fe(c);
+       if (swapped) {
+               s.swap_side();
+               pr.reflect(s.entry());
+       }
+       // The following uses precomputed entries and needs `"fiat_punto.h"` to
+       // be included.
+       //auto pr = get_fiat_punto_entry(s.len(), s.w());
+       //pr = s.recompute_entry(pr);
+       c.x(pr.x());
+       c.y(pr.y());
+       c.h(pr.h());
+       c.sp(-0.1);
+       c.st(0.0);
+       std::vector<bcar::Pose> ispath;
+       if (s.right()) {
+               while (!c.rf().on_right_side_of(s.entry())) {
+                       c.next();
+                       ispath.push_back(c);
+               }
+       } else {
+               while (c.lf().on_right_side_of(s.entry())) {
+                       c.next();
+                       ispath.push_back(c);
+               }
+       }
+       if (s.parallel()) {
+               c.set_max_steer();
+               if (!s.right()) {
+                       c.st(c.st() * -1.0);
+               }
+               for (auto p: s.steer_in_slot(c)) {
+                       c.next();
+                       ispath.push_back(c);
+               }
+       }
+       jvi["goal"][0] = pr.x();
+       jvi["goal"][1] = pr.y();
+       jvi["goal"][2] = pr.b();
+       jvi["goal"][3] = pr.e();
+       p.json(jvi);
+       unsigned int icnt = 0;
+       unsigned int rcnt = 0;
+       unsigned int bcnt = 0;
+       Json::Value best_path;
+       Json::Value pj;
+       double cost = 0.0;
+       while (icnt < 1000) {
+               p.icnt(icnt);
+               while (p.next()) {}
+               icnt = p.icnt();
+               pj = p.json();
+               if (pj["path"] != Json::nullValue) {
+                       double gc = pj["goal_cc"].asDouble();
+                       assert(gc > 0.0);
+                       if (cost == 0.0 || gc < cost) {
+                               best_path = pj["path"];
+                               cost = gc;
+                               bcnt += 1;
+                       }
+               }
+               p.reset();
+               rcnt += 1;
+       }
+       double elapsed = p.scnt();
+       auto jvo = p.json();
+       unsigned int i = 0;
+       for (auto p: ispath) {
+               jvo["ispath"][i][0] = p.x();
+               jvo["ispath"][i][1] = p.y();
+               jvo["ispath"][i][2] = p.h();
+               i += 1;
+       }
+       jvo["init"] = jvi["init"];
+       jvo["slot"] = jvi["slot"];
+       jvo["obst"] = jvi["obst"];
+       jvo["entry"] = jvi["goal"];
+       jvo["goal"][0] = ispath.back().x();
+       jvo["goal"][1] = ispath.back().y();
+       jvo["goal"][2] = ispath.back().h();
+       jvo["time"] = elapsed;
+       jvo["icnt"] = p.icnt();
+       jvo["rcnt"] = rcnt;
+       jvo["bcnt"] = bcnt;
+       jvo["path"] = best_path;
+       jvo["goal_cc"] = cost;
+       std::cout << jvo << std::endl;
+       return 0;
+}