+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <jsoncpp/json/json.h>
-
-#include "psp.h"
-#include "rrtce.h"
-#ifndef EPP
- #define EPP RRTS
-#endif
-
-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;
-
- 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()
- );
-
- psp.fe();
-
- jvi["goal"][0] = psp.cc().x();
- jvi["goal"][1] = psp.cc().y();
- jvi["goal"][2] = psp.cc().h();
- unsigned int icnt = 0;
- for (auto &i: psp.possible_goals()) {
- jvi["goals"][icnt][0] = i.x();
- jvi["goals"][icnt][1] = i.y();
- jvi["goals"][icnt][2] = i.h();
- icnt++;
- }
- jvi["goals"][icnt][0] = psp.cc().x();
- jvi["goals"][icnt][1] = psp.cc().y();
- jvi["goals"][icnt][2] = psp.cc().h();
-
- epp.json(jvi);
- epp.init();
-
- while (epp.next()) {}
-
- jvo = epp.json();
- jvo["slot"] = jvi["slot"];
- std::cout << jvo << std::endl;
- return 0;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "psp.h"
-
-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;
-}
-
-int main()
-{
- Json::Value jvo; // JSON output
-
- double offset = -1.256994;
- double delta = 0.1;
- std::cerr << "#dist,fe cusps,fe time,fer cusps,fer time";
- std::cerr << std::endl;
- for (int i = 0; i < 14; i++) {
- jvo["goals"] = Json::nullValue;
- // setup
- PSPlanner psp;
- double ppsl = 0.01 + psp.cc().perfect_parking_slot_len();
- double psl = ppsl + offset + i * delta;
- psp.ps().border(
- 0, 0,
- 2.2, 0,
- 2.2, 0 + psl,
- 0, 0 + psl
- );
- // run fe planner
- TSTART();
- psp.fe(); // or fer()
- TEND();
- std::cerr << (psl - psp.cc().l() - 0.01);
- std::cerr << ",";
- std::cerr << psp.c();
- std::cerr << ",";
- std::cerr << TDIFF();
- // json output
- jvo["time"] = TDIFF();
- jvo["goal"][0] = psp.gc().x();
- jvo["goal"][1] = psp.gc().y();
- jvo["goal"][2] = psp.gc().h();
- unsigned int icnt = 0;
- for (auto &i: psp.cusps()) {
- jvo["goals"][icnt][0] = i.x();
- jvo["goals"][icnt][1] = i.y();
- jvo["goals"][icnt][2] = i.h();
- icnt++;
- }
- jvo["goals"][icnt][0] = psp.cc().x();
- jvo["goals"][icnt][1] = psp.cc().y();
- jvo["goals"][icnt][2] = psp.cc().h();
- jvo["slot"][0][0][0] = psp.ps().x1();
- jvo["slot"][0][0][1] = psp.ps().y1();
- jvo["slot"][0][1][0] = psp.ps().x2();
- jvo["slot"][0][1][1] = psp.ps().y2();
- jvo["slot"][0][2][0] = psp.ps().x3();
- jvo["slot"][0][2][1] = psp.ps().y3();
- jvo["slot"][0][3][0] = psp.ps().x4();
- jvo["slot"][0][3][1] = psp.ps().y4();
- jvo["cusp"] = psp.c();
- std::cout << jvo << std::endl;
- psp.ps().border(
- 0, 0,
- 2.2, 0,
- 2.2, 0 + psl,
- 0, 0 + psl
- );
- // run fer planner
- TSTART();
- psp.fer(); // or fer()
- TEND();
- std::cerr << ",";
- std::cerr << psp.c();
- std::cerr << ",";
- std::cerr << TDIFF();
- std::cerr << std::endl;
- // json output
- jvo["time"] = TDIFF();
- jvo["goal"][0] = psp.gc().x();
- jvo["goal"][1] = psp.gc().y();
- jvo["goal"][2] = psp.gc().h();
- icnt = 0;
- for (auto &i: psp.cusps()) {
- jvo["goals"][icnt][0] = i.x();
- jvo["goals"][icnt][1] = i.y();
- jvo["goals"][icnt][2] = i.h();
- icnt++;
- }
- jvo["goals"][icnt][0] = psp.cc().x();
- jvo["goals"][icnt][1] = psp.cc().y();
- jvo["goals"][icnt][2] = psp.cc().h();
- jvo["slot"][0][0][0] = psp.ps().x1();
- jvo["slot"][0][0][1] = psp.ps().y1();
- jvo["slot"][0][1][0] = psp.ps().x2();
- jvo["slot"][0][1][1] = psp.ps().y2();
- jvo["slot"][0][2][0] = psp.ps().x3();
- jvo["slot"][0][2][1] = psp.ps().y3();
- jvo["slot"][0][3][0] = psp.ps().x4();
- jvo["slot"][0][3][1] = psp.ps().y4();
- jvo["cusp"] = psp.c();
- std::cout << jvo << std::endl;
- }
- return 0;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <jsoncpp/json/json.h>
-#include <thread>
-
-#include "BrainTree.h"
-#include "rrtce.h"
-
-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;
-}
-
-RRTNode init_node(RRTNode *oi, double *oic)
-{
- RRTNode r(*oi);
- TEND();
- r.sp(oi->sp() * TDIFF());
- r.next();
- if (oi->h() == r.h())
- *oic = sqrt(pow(r.x() - oi->x(), 2) + pow(r.y() - oi->y(), 2));
- else
- *oic = std::min(
- std::abs(r.h() - oi->h())
- , 2 * M_PI - std::abs(r.h() - oi->h())
- ) * r.mtr();
- return r;
-}
-bool should_run_bt()
-{
- TEND();
- if (TDIFF() < 5)
- return true;
- return false;
-}
-
-#define EPP RRTCE7 // EPP stands for entry point planner.
-enum EppStat {
- RUNNING
- , FINISHED
-};
-EppStat epp_stat = FINISHED;
-std::vector<std::thread> epp_threads;
-void epp_proc(
- Json::Value jvi
- , std::vector<RRTNode> *fp
- , double *fc
- , double *oic
-)
-{
- epp_stat = RUNNING;
- EPP epp;
- epp.json(jvi);
- epp.init();
-
- while (epp.next()) {}
-
- if (epp.path().size() > 0) {
- *fc = cc(*epp.path().back()) + *oic;
- fp->clear();
- for (auto n: epp.path()) {
- fp->push_back(RRTNode());
- fp->back().x(n->x());
- fp->back().y(n->y());
- fp->back().h(n->h());
- }
- }
- epp_stat = FINISHED;
-}
-
-class EppFinished : public BrainTree::Node {
- public:
- Status update() override
- {
- if (epp_stat == FINISHED)
- return Status::Success;
- return Status::Failure;
- }
-};
-class EppGF : public BrainTree::Node {
- private:
- std::vector<RRTNode> *fp;
- public:
- EppGF(
- std::vector<RRTNode> *feasible_path
- )
- : fp(feasible_path)
- {
- }
- Status update() override
- {
- if (fp->size() == 0)
- return Status::Failure;
- unsigned int cu = 0;
- unsigned int co = 0;
- unsigned int pcnt = 0;
- for (auto n: *fp) {
- if (n.t(RRTNodeType::cusp))
- cu++;
- if (n.t(RRTNodeType::connected))
- co++;
- pcnt++;
- }
- if (cu > 5) // max cusps in feasible path
- return Status::Failure;
- return Status::Success;
- }
-};
-class EppRestart : public BrainTree::Node {
- private:
- Json::Value jvi;
- std::vector<RRTNode> *fp;
- double *fpc;
- RRTNode *oi;
- double *oic;
- public:
- EppRestart(
- Json::Value jvi
- , std::vector<RRTNode> *feasible_path
- , double *feasible_path_cost
- , RRTNode *orig_init
- , double *orig_init_cost
- )
- : jvi(jvi)
- , fp(feasible_path)
- , fpc(feasible_path_cost)
- , oi(orig_init)
- , oic(orig_init_cost)
- {
- }
- Status update() override
- {
- RRTNode in = init_node(oi, oic);
- jvi["init"][0] = in.x();
- jvi["init"][1] = in.y();
- jvi["init"][2] = in.h();
- epp_threads.push_back(std::thread(
- epp_proc
- , jvi
- , fp
- , fpc
- , oic
- ));
- return Status::Success;
- }
-};
-class ShouldRunBT : public BrainTree::Node {
- public:
- Status update() override
- {
- if (should_run_bt())
- return Status::Success;
- return Status::Failure;
- }
-};
-class UpdateFP : public BrainTree::Node {
- private:
- std::vector<RRTNode> *fi;
- double *fic;
- std::vector<RRTNode> *fe;
- double *fec;
- public:
- UpdateFP(
- std::vector<RRTNode> *final_path
- , double *final_path_cost
- , std::vector<RRTNode> *feasible_path
- , double *feasible_path_cost
- )
- : fi(final_path)
- , fic(final_path_cost)
- , fe(feasible_path)
- , fec(feasible_path_cost)
- {
- }
- Status update() override
- {
- if (*fec < *fic) {
- *fic = *fec;
- fi->clear();
- for (auto n: *fe) {
- fi->push_back(RRTNode());
- fi->back().x(n.x());
- fi->back().y(n.y());
- fi->back().h(n.h());
- }
- return Status::Success;
- }
- return Status::Failure;
- }
-};
-
-int main()
-{
- Json::Value jvi; // JSON input
- std::cin >> jvi;
-
- std::vector<RRTNode> final_path;
- double final_path_cost = 9999;
- std::vector<RRTNode> feasible_path;
- double feasible_path_cost = 9999;
- double orig_init_cost = 0;
- RRTNode orig_init;
- orig_init.x(jvi["init"][0].asDouble());
- orig_init.y(jvi["init"][1].asDouble());
- orig_init.h(jvi["init"][2].asDouble());
- orig_init.sp(2.7);
- //orig_init.st(M_PI / 32); // only for sc2_4
-
- struct timespec sleeping_time;
- sleeping_time.tv_sec = 0;
- sleeping_time.tv_nsec = 50 * 1000000;
-
- auto bt = BrainTree::Builder()
- .composite<BrainTree::Sequence>()
- .leaf<ShouldRunBT>()
- .composite<BrainTree::Selector>()
- .composite<BrainTree::Sequence>()
- .leaf<EppFinished>()
- .leaf<EppGF>(
- &feasible_path
- )
- .leaf<UpdateFP>(
- &final_path
- , &final_path_cost
- , &feasible_path
- , &feasible_path_cost
- )
- .leaf<EppRestart>(
- jvi
- , &feasible_path
- , &feasible_path_cost
- , &orig_init
- , &orig_init_cost
- )
- .end()
- .composite<BrainTree::Sequence>()
- .leaf<EppFinished>()
- .leaf<EppRestart>(
- jvi
- , &feasible_path
- , &feasible_path_cost
- , &orig_init
- , &orig_init_cost
- )
- .end()
- .decorator<BrainTree::Succeeder>()
- .leaf<ShouldRunBT>()
- .end()
- .end()
- .end()
- .build();
-
- TSTART();
- while (bt->update() != BrainTree::Node::Status::Failure) {
- nanosleep(&sleeping_time, (struct timespec *) NULL);
- }
- for (auto &t: epp_threads)
- t.join();
-
- Json::Value jvo = jvi; // JSON output
- {
- jvo["time"] = TDIFF();
- jvo["cost"] = final_path_cost;
- jvo["path"][0][0] = orig_init.x();
- jvo["path"][0][1] = orig_init.y();
- jvo["path"][0][2] = orig_init.h();
- unsigned int cu = 0;
- unsigned int co = 0;
- unsigned int pcnt = 1;
- for (auto n: final_path) {
- jvo["path"][pcnt][0] = n.x();
- jvo["path"][pcnt][1] = n.y();
- jvo["path"][pcnt][2] = n.h();
- if (n.t(RRTNodeType::cusp))
- cu++;
- if (n.t(RRTNodeType::connected))
- co++;
- pcnt++;
- }
- jvo["cusps-in-path"] = cu;
- jvo["connecteds-in-path"] = co;
- }
- std::cout << jvo << std::endl;
- return 0;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "rrtce.h"
-#define EPP RRTCE4
-
-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;
-}
-
-int main()
-{
- Json::Value jvi; // JSON input
- std::cin >> jvi;
-
- EPP epp;
- epp.json(jvi);
- epp.init();
-
- while (epp.next()) {}
-
- std::cout << epp.json() << std::endl;
- return 0;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "rrtce.h"
-#define EPP RRTCE6
-
-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;
-}
-
-int main()
-{
- Json::Value jvi; // JSON input
- std::cin >> jvi;
-
- EPP epp;
- epp.json(jvi);
- epp.init();
-
- while (epp.next()) {}
-
- std::cout << epp.json() << std::endl;
- return 0;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "rrtce.h"
-#define EPP RRTCE8
-
-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;
-}
-
-int main()
-{
- Json::Value jvi; // JSON input
- std::cin >> jvi;
-
- EPP epp;
- epp.json(jvi);
- epp.init();
-
- while (epp.next()) {}
-
- std::cout << epp.json() << std::endl;
- return 0;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "rrtce.h"
-#define EPP RRTCE9
-
-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;
-}
-
-int main()
-{
- Json::Value jvi; // JSON input
- std::cin >> jvi;
-
- EPP epp;
- epp.json(jvi);
- epp.init();
-
- while (epp.next()) {}
-
- std::cout << epp.json() << std::endl;
- return 0;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "rrtce.h"
-#define EPP RRTCE10
-
-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;
-}
-
-int main()
-{
- Json::Value jvi; // JSON input
- std::cin >> jvi;
-
- EPP epp;
- epp.json(jvi);
- epp.init();
-
- while (epp.next()) {}
-
- std::cout << epp.json() << std::endl;
- epp.deinit();
- return 0;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "rrtce.h"
-#define EPP RRTCE11
-
-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;
-}
-
-int main()
-{
- Json::Value jvi; // JSON input
- std::cin >> jvi;
-
- EPP epp;
- epp.json(jvi);
- epp.init();
-
- while (epp.next()) {}
-
- std::cout << epp.json() << std::endl;
- epp.deinit();
- return 0;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "rrtce.h"
-#define EPP RRTCE12
-
-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;
-}
-
-int main()
-{
- Json::Value jvi; // JSON input
- std::cin >> jvi;
-
- EPP epp;
- epp.json(jvi);
- epp.init();
-
- while (epp.next()) {}
-
- std::cout << epp.json() << std::endl;
- epp.deinit();
- return 0;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "rrtce.h"
-#define EPP RRTCE13
-
-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;
-}
-
-int main()
-{
- Json::Value jvi; // JSON input
- std::cin >> jvi;
-
- EPP epp;
- epp.json(jvi);
- epp.init();
-
- while (epp.next()) {}
-
- std::cout << epp.json() << std::endl;
- epp.deinit();
- return 0;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "rrtce.h"
-#define EPP RRTCE14
-
-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;
-}
-
-int main()
-{
- Json::Value jvi; // JSON input
- std::cin >> jvi;
-
- EPP epp;
- epp.json(jvi);
- epp.init();
-
- while (epp.next()) {}
-
- std::cout << epp.json() << std::endl;
- epp.deinit();
- return 0;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "rrts.h"
-
-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;
-}
-
-int main()
-{
- Json::Value jvi; // JSON input
- Json::Value jvo; // JSON output
- std::cin >> jvi;
-
- if (jvi["init"] == Json::nullValue) {
- std::cerr << "I need `init` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
-
- if (jvi["goal"] == Json::nullValue) {
- std::cerr << "I need `goal` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
-
- if (jvi["goals"] == Json::nullValue) {
- std::cerr << "I need `goals` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
-
- if (jvi["obst"] == Json::nullValue) {
- std::cerr << "I need `obst` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
-
- RRTS rrts;
- rrts.nodes().front().x(jvi["init"][0].asDouble());
- rrts.nodes().front().y(jvi["init"][1].asDouble());
- rrts.nodes().front().h(jvi["init"][2].asDouble());
- {
- RRTNode tmp_node;
- tmp_node.x(jvi["goal"][0].asDouble());
- tmp_node.y(jvi["goal"][1].asDouble());
- tmp_node.h(jvi["goal"][2].asDouble());
- rrts.goals().push_back(tmp_node);
- for (auto g: jvi["goals"]) {
- tmp_node.x(g[0].asDouble());
- tmp_node.y(g[1].asDouble());
- tmp_node.h(g[2].asDouble());
- rrts.goals().push_back(tmp_node);
- }
- }
- {
- Obstacle tmp_obstacle;
- for (auto o: jvi["obst"]) {
- tmp_obstacle.poly().clear();
- for (auto c: o) {
- double tmp_x = c[0].asDouble();
- double tmp_y = c[1].asDouble();
- auto tmp_tuple = std::make_tuple(tmp_x, tmp_y);
- tmp_obstacle.poly().push_back(tmp_tuple);
- }
- rrts.obstacles().push_back(tmp_obstacle);
- }
- }
-
- {
- double edist_init_goal = sqrt(
- pow(
- rrts.nodes().front().x()
- - rrts.goals().front().x(),
- 2
- )
- + pow(
- rrts.nodes().front().y()
- - rrts.goals().front().y(),
- 2
- )
- );
- rrts.set_sample(
- rrts.nodes().front().x(), edist_init_goal,
- rrts.nodes().front().y(), edist_init_goal,
- 0, 2 * M_PI
- );
- }
-
- TSTART();
- while (rrts.next()) {}
- TEND();
-
- {
- jvo["time"] = TDIFF();
- }
- {
- jvo["iterations"] = rrts.icnt();
- }
- {
- if (rrts.path().size() > 0) {
- jvo["cost"] = cc(*rrts.path().back());
- } else {
- jvo["cost"] = -1;
- }
- }
- {
- jvo["init"][0] = rrts.nodes().front().x();
- jvo["init"][1] = rrts.nodes().front().y();
- jvo["init"][2] = rrts.nodes().front().h();
- }
- {
- unsigned int ocnt = 0;
- for (auto o: rrts.obstacles()) {
- unsigned int ccnt = 0;
- for (auto c: o.poly()) {
- jvo["obst"][ocnt][ccnt][0] = std::get<0>(c);
- jvo["obst"][ocnt][ccnt][1] = std::get<1>(c);
- ccnt++;
- }
- ocnt++;
- }
- }
- {
- unsigned int gcnt = 0;
- for (auto g: rrts.goals()) {
- jvo["goals"][gcnt][0] = g.x();
- jvo["goals"][gcnt][1] = g.y();
- jvo["goals"][gcnt][2] = g.h();
- gcnt++;
- }
- }
- {
- if (rrts.path().size() > 0) {
- jvo["goal"][0] = rrts.path().back()->x();
- jvo["goal"][1] = rrts.path().back()->y();
- jvo["goal"][2] = rrts.path().back()->h();
- }
- unsigned int cu = 0;
- unsigned int co = 0;
- unsigned int pcnt = 0;
- for (auto n: rrts.path()) {
- jvo["path"][pcnt][0] = n->x();
- jvo["path"][pcnt][1] = n->y();
- jvo["path"][pcnt][2] = n->h();
- if (n->t(RRTNodeType::cusp))
- cu++;
- if (n->t(RRTNodeType::connected))
- co++;
- pcnt++;
- }
- jvo["cusps-in-path"] = cu;
- jvo["connecteds-in-path"] = co;
- }
- {
- jvo["nodes"] = (unsigned int) rrts.nodes().size();
- }
-
- std::cout << jvo << std::endl;
- return 0;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "rrtce.h"
-#define EPP RRTCE15
-
-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;
-}
-
-int main()
-{
- Json::Value jvi; // JSON input
- std::cin >> jvi;
-
- EPP epp;
- epp.json(jvi);
- epp.init();
-
- while (epp.next()) {}
-
- std::cout << epp.json() << std::endl;
- epp.deinit();
- return 0;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "rrtce.h"
-#define EPP RRTCE16
-
-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;
-}
-
-int main()
-{
- Json::Value jvi; // JSON input
- std::cin >> jvi;
-
- EPP epp;
- epp.json(jvi);
- epp.init();
-
- while (epp.next()) {}
-
- std::cout << epp.json() << std::endl;
- epp.deinit();
- return 0;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "psp.h"
-#include "rrtce.h"
-#define EPP RRTCE13
-
-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;
-
- 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()
- );
-
- psp.fe();
-
- jvi["goal"][0] = psp.cc().x();
- jvi["goal"][1] = psp.cc().y();
- jvi["goal"][2] = psp.cc().h();
- unsigned int icnt = 0;
- for (auto &i: psp.possible_goals()) {
- jvi["goals"][icnt][0] = i.x();
- jvi["goals"][icnt][1] = i.y();
- jvi["goals"][icnt][2] = i.h();
- icnt++;
- }
- jvi["goals"][icnt][0] = psp.cc().x();
- jvi["goals"][icnt][1] = psp.cc().y();
- jvi["goals"][icnt][2] = psp.cc().h();
-
- epp.json(jvi);
- epp.init();
- epp.sample_dist_type(1);
- 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;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "psp.h"
-#include "rrtce.h"
-#define EPP RRTCE14
-
-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;
-
- 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()
- );
-
- psp.fe();
-
- jvi["goal"][0] = psp.cc().x();
- jvi["goal"][1] = psp.cc().y();
- jvi["goal"][2] = psp.cc().h();
- unsigned int icnt = 0;
- for (auto &i: psp.possible_goals()) {
- jvi["goals"][icnt][0] = i.x();
- jvi["goals"][icnt][1] = i.y();
- jvi["goals"][icnt][2] = i.h();
- icnt++;
- }
- jvi["goals"][icnt][0] = psp.cc().x();
- jvi["goals"][icnt][1] = psp.cc().y();
- jvi["goals"][icnt][2] = psp.cc().h();
-
- epp.json(jvi);
- epp.init();
- epp.sample_dist_type(1);
- 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;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "psp.h"
-#include "rrtce.h"
-#define EPP RRTCE15
-
-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;
-
- 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()
- );
-
- psp.fe();
-
- jvi["goal"][0] = psp.cc().x();
- jvi["goal"][1] = psp.cc().y();
- jvi["goal"][2] = psp.cc().h();
- unsigned int icnt = 0;
- for (auto &i: psp.possible_goals()) {
- jvi["goals"][icnt][0] = i.x();
- jvi["goals"][icnt][1] = i.y();
- jvi["goals"][icnt][2] = i.h();
- icnt++;
- }
- jvi["goals"][icnt][0] = psp.cc().x();
- jvi["goals"][icnt][1] = psp.cc().y();
- jvi["goals"][icnt][2] = psp.cc().h();
-
- epp.json(jvi);
- epp.init();
- epp.sample_dist_type(1);
- 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;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "psp.h"
-#include "rrtce.h"
-#define EPP RRTCE16
-
-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;
-
- 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()
- );
-
- psp.fe();
-
- jvi["goal"][0] = psp.cc().x();
- jvi["goal"][1] = psp.cc().y();
- jvi["goal"][2] = psp.cc().h();
- unsigned int icnt = 0;
- for (auto &i: psp.possible_goals()) {
- jvi["goals"][icnt][0] = i.x();
- jvi["goals"][icnt][1] = i.y();
- jvi["goals"][icnt][2] = i.h();
- icnt++;
- }
- jvi["goals"][icnt][0] = psp.cc().x();
- jvi["goals"][icnt][1] = psp.cc().y();
- jvi["goals"][icnt][2] = psp.cc().h();
-
- epp.json(jvi);
- epp.init();
- epp.sample_dist_type(1);
- 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;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "psp.h"
-#include "rrtce.h"
-#define EPP RRTCE11
-
-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;
-
- 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()
- );
-
- psp.fe();
-
- jvi["goal"][0] = psp.cc().x();
- jvi["goal"][1] = psp.cc().y();
- jvi["goal"][2] = psp.cc().h();
- unsigned int icnt = 0;
- for (auto &i: psp.possible_goals()) {
- jvi["goals"][icnt][0] = i.x();
- jvi["goals"][icnt][1] = i.y();
- jvi["goals"][icnt][2] = i.h();
- icnt++;
- }
- jvi["goals"][icnt][0] = psp.cc().x();
- jvi["goals"][icnt][1] = psp.cc().y();
- jvi["goals"][icnt][2] = psp.cc().h();
-
- epp.json(jvi);
- epp.init();
- epp.sample_dist_type(1);
- 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;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "psp.h"
-#include "rrtce.h"
-#define EPP RRTCE7
-
-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;
-
- 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()
- );
-
- psp.fe();
-
- jvi["goal"][0] = psp.cc().x();
- jvi["goal"][1] = psp.cc().y();
- jvi["goal"][2] = psp.cc().h();
- unsigned int icnt = 0;
- for (auto &i: psp.possible_goals()) {
- jvi["goals"][icnt][0] = i.x();
- jvi["goals"][icnt][1] = i.y();
- jvi["goals"][icnt][2] = i.h();
- icnt++;
- }
- jvi["goals"][icnt][0] = psp.cc().x();
- jvi["goals"][icnt][1] = psp.cc().y();
- jvi["goals"][icnt][2] = psp.cc().h();
-
- epp.json(jvi);
- epp.init();
- epp.sample_dist_type(1);
- 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;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "psp.h"
-#include "rrtce.h"
-#define EPP RRTCE3
-
-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;
-
- 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()
- );
-
- psp.fe();
-
- jvi["goal"][0] = psp.cc().x();
- jvi["goal"][1] = psp.cc().y();
- jvi["goal"][2] = psp.cc().h();
- unsigned int icnt = 0;
- for (auto &i: psp.possible_goals()) {
- jvi["goals"][icnt][0] = i.x();
- jvi["goals"][icnt][1] = i.y();
- jvi["goals"][icnt][2] = i.h();
- icnt++;
- }
- jvi["goals"][icnt][0] = psp.cc().x();
- jvi["goals"][icnt][1] = psp.cc().y();
- jvi["goals"][icnt][2] = psp.cc().h();
-
- epp.json(jvi);
- epp.init();
- epp.sample_dist_type(1);
- 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;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "psp.h"
-#include "rrtce.h"
-#define EPP RRTCE2
-
-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;
-
- 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()
- );
-
- psp.fe();
-
- jvi["goal"][0] = psp.cc().x();
- jvi["goal"][1] = psp.cc().y();
- jvi["goal"][2] = psp.cc().h();
- unsigned int icnt = 0;
- for (auto &i: psp.possible_goals()) {
- jvi["goals"][icnt][0] = i.x();
- jvi["goals"][icnt][1] = i.y();
- jvi["goals"][icnt][2] = i.h();
- icnt++;
- }
- jvi["goals"][icnt][0] = psp.cc().x();
- jvi["goals"][icnt][1] = psp.cc().y();
- jvi["goals"][icnt][2] = psp.cc().h();
-
- epp.json(jvi);
- epp.init();
- epp.sample_dist_type(1);
- 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;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "rrtext.h"
-
-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;
-}
-
-int main()
-{
- Json::Value jvi; // JSON input
- Json::Value jvo; // JSON output
- std::cin >> jvi;
-
- if (jvi["init"] == Json::nullValue) {
- std::cerr << "I need `init` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
-
- if (jvi["goal"] == Json::nullValue) {
- std::cerr << "I need `goal` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
-
- if (jvi["goals"] == Json::nullValue) {
- std::cerr << "I need `goals` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
-
- if (jvi["obst"] == Json::nullValue) {
- std::cerr << "I need `obst` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
-
- RRTExt2 rrts;
- rrts.nodes().front().x(jvi["init"][0].asDouble());
- rrts.nodes().front().y(jvi["init"][1].asDouble());
- rrts.nodes().front().h(jvi["init"][2].asDouble());
- {
- RRTNode tmp_node;
- tmp_node.x(jvi["goal"][0].asDouble());
- tmp_node.y(jvi["goal"][1].asDouble());
- tmp_node.h(jvi["goal"][2].asDouble());
- rrts.goals().push_back(tmp_node);
- for (auto g: jvi["goals"]) {
- tmp_node.x(g[0].asDouble());
- tmp_node.y(g[1].asDouble());
- tmp_node.h(g[2].asDouble());
- rrts.goals().push_back(tmp_node);
- }
- }
- {
- Obstacle tmp_obstacle;
- for (auto o: jvi["obst"]) {
- tmp_obstacle.poly().clear();
- for (auto c: o) {
- double tmp_x = c[0].asDouble();
- double tmp_y = c[1].asDouble();
- auto tmp_tuple = std::make_tuple(tmp_x, tmp_y);
- tmp_obstacle.poly().push_back(tmp_tuple);
- }
- rrts.obstacles().push_back(tmp_obstacle);
- }
- }
-
- {
- double edist_init_goal = sqrt(
- pow(
- rrts.nodes().front().x()
- - rrts.goals().front().x(),
- 2
- )
- + pow(
- rrts.nodes().front().y()
- - rrts.goals().front().y(),
- 2
- )
- );
- rrts.set_sample(
- rrts.nodes().front().x(), edist_init_goal,
- rrts.nodes().front().y(), edist_init_goal,
- 0, 2 * M_PI
- );
- }
-
- rrts.init();
- TSTART();
- while (rrts.next()) {}
- TEND();
-
- {
- jvo["time"] = TDIFF();
- }
- {
- jvo["iterations"] = rrts.icnt();
- }
- {
- if (rrts.path().size() > 0) {
- jvo["cost"] = cc(*rrts.path().back());
- } else {
- jvo["cost"] = -1;
- }
- }
- {
- jvo["init"][0] = rrts.nodes().front().x();
- jvo["init"][1] = rrts.nodes().front().y();
- jvo["init"][2] = rrts.nodes().front().h();
- }
- {
- unsigned int ocnt = 0;
- for (auto o: rrts.obstacles()) {
- unsigned int ccnt = 0;
- for (auto c: o.poly()) {
- jvo["obst"][ocnt][ccnt][0] = std::get<0>(c);
- jvo["obst"][ocnt][ccnt][1] = std::get<1>(c);
- ccnt++;
- }
- ocnt++;
- }
- }
- //{
- // unsigned int ncnt = 0;
- // for (auto n: rrts.nodes()) {
- // jvo["nodes_x"][ncnt] = n.x();
- // jvo["nodes_y"][ncnt] = n.y();
- // //jvo["nodes_h"][ncnt] = n.h();
- // ncnt++;
- // }
- //}
- {
- jvo["nodes"] = (unsigned int) rrts.nodes().size();
- }
- {
- unsigned int gcnt = 0;
- for (auto g: rrts.goals()) {
- jvo["goals"][gcnt][0] = g.x();
- jvo["goals"][gcnt][1] = g.y();
- jvo["goals"][gcnt][2] = g.h();
- gcnt++;
- }
- }
- {
- if (rrts.path().size() > 0) {
- jvo["goal"][0] = rrts.path().back()->x();
- jvo["goal"][1] = rrts.path().back()->y();
- jvo["goal"][2] = rrts.path().back()->h();
- }
- unsigned int cu = 0;
- unsigned int co = 0;
- unsigned int pcnt = 0;
- for (auto n: rrts.path()) {
- jvo["path"][pcnt][0] = n->x();
- jvo["path"][pcnt][1] = n->y();
- jvo["path"][pcnt][2] = n->h();
- if (n->t(RRTNodeType::cusp))
- cu++;
- if (n->t(RRTNodeType::connected))
- co++;
- pcnt++;
- }
- jvo["cusps-in-path"] = cu;
- jvo["connecteds-in-path"] = co;
- }
-
- std::cout << jvo << std::endl;
- return 0;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "psp.h"
-#include "rrtce.h"
-#define EPP RRTCE1
-
-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;
-
- 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()
- );
-
- psp.fe();
-
- jvi["goal"][0] = psp.cc().x();
- jvi["goal"][1] = psp.cc().y();
- jvi["goal"][2] = psp.cc().h();
- unsigned int icnt = 0;
- for (auto &i: psp.possible_goals()) {
- jvi["goals"][icnt][0] = i.x();
- jvi["goals"][icnt][1] = i.y();
- jvi["goals"][icnt][2] = i.h();
- icnt++;
- }
- jvi["goals"][icnt][0] = psp.cc().x();
- jvi["goals"][icnt][1] = psp.cc().y();
- jvi["goals"][icnt][2] = psp.cc().h();
-
- epp.json(jvi);
- epp.init();
- epp.sample_dist_type(1);
- 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;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "psp.h"
-#include "rrtce.h"
-#define EPP RRTCE17
-
-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;
-
- 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()
- );
-
- psp.fe();
-
- jvi["goal"][0] = psp.cc().x();
- jvi["goal"][1] = psp.cc().y();
- jvi["goal"][2] = psp.cc().h();
- unsigned int icnt = 0;
- for (auto &i: psp.possible_goals()) {
- jvi["goals"][icnt][0] = i.x();
- jvi["goals"][icnt][1] = i.y();
- jvi["goals"][icnt][2] = i.h();
- icnt++;
- }
- jvi["goals"][icnt][0] = psp.cc().x();
- jvi["goals"][icnt][1] = psp.cc().y();
- jvi["goals"][icnt][2] = psp.cc().h();
-
- epp.json(jvi);
- epp.init();
- epp.sample_dist_type(1);
- 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;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "rrtce.h"
-
-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;
-}
-
-int main()
-{
- Json::Value jvi; // JSON input
- Json::Value jvo; // JSON output
- std::cin >> jvi;
-
- if (jvi["init"] == Json::nullValue) {
- std::cerr << "I need `init` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
-
- if (jvi["goal"] == Json::nullValue) {
- std::cerr << "I need `goal` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
-
- if (jvi["goals"] == Json::nullValue) {
- std::cerr << "I need `goals` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
-
- if (jvi["obst"] == Json::nullValue) {
- std::cerr << "I need `obst` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
-
- RRTCE1 rrts;
- rrts.nodes().front().x(jvi["init"][0].asDouble());
- rrts.nodes().front().y(jvi["init"][1].asDouble());
- rrts.nodes().front().h(jvi["init"][2].asDouble());
- {
- RRTNode tmp_node;
- tmp_node.x(jvi["goal"][0].asDouble());
- tmp_node.y(jvi["goal"][1].asDouble());
- tmp_node.h(jvi["goal"][2].asDouble());
- rrts.goals().push_back(tmp_node);
- for (auto g: jvi["goals"]) {
- tmp_node.x(g[0].asDouble());
- tmp_node.y(g[1].asDouble());
- tmp_node.h(g[2].asDouble());
- rrts.goals().push_back(tmp_node);
- }
- }
- {
- Obstacle tmp_obstacle;
- for (auto o: jvi["obst"]) {
- tmp_obstacle.poly().clear();
- for (auto c: o) {
- double tmp_x = c[0].asDouble();
- double tmp_y = c[1].asDouble();
- auto tmp_tuple = std::make_tuple(tmp_x, tmp_y);
- tmp_obstacle.poly().push_back(tmp_tuple);
- }
- rrts.obstacles().push_back(tmp_obstacle);
- }
- }
-
- {
- double edist_init_goal = sqrt(
- pow(
- rrts.nodes().front().x()
- - rrts.goals().front().x(),
- 2
- )
- + pow(
- rrts.nodes().front().y()
- - rrts.goals().front().y(),
- 2
- )
- );
- rrts.set_sample(
- rrts.nodes().front().x(), edist_init_goal,
- rrts.nodes().front().y(), edist_init_goal,
- 0, 2 * M_PI
- );
- }
-
- rrts.init();
- TSTART();
- while (rrts.next()) {}
- TEND();
-
- {
- jvo["time"] = TDIFF();
- }
- {
- jvo["iterations"] = rrts.icnt();
- }
- {
- if (rrts.path().size() > 0) {
- jvo["cost"] = cc(*rrts.path().back());
- } else {
- jvo["cost"] = -1;
- }
- }
- {
- jvo["init"][0] = rrts.nodes().front().x();
- jvo["init"][1] = rrts.nodes().front().y();
- jvo["init"][2] = rrts.nodes().front().h();
- }
- {
- unsigned int ocnt = 0;
- for (auto o: rrts.obstacles()) {
- unsigned int ccnt = 0;
- for (auto c: o.poly()) {
- jvo["obst"][ocnt][ccnt][0] = std::get<0>(c);
- jvo["obst"][ocnt][ccnt][1] = std::get<1>(c);
- ccnt++;
- }
- ocnt++;
- }
- }
- //{
- // unsigned int ncnt = 0;
- // for (auto n: rrts.nodes()) {
- // jvo["nodes_x"][ncnt] = n.x();
- // jvo["nodes_y"][ncnt] = n.y();
- // //jvo["nodes_h"][ncnt] = n.h();
- // ncnt++;
- // }
- //}
- {
- jvo["nodes"] = (unsigned int) rrts.nodes().size();
- }
- {
- unsigned int gcnt = 0;
- for (auto g: rrts.goals()) {
- jvo["goals"][gcnt][0] = g.x();
- jvo["goals"][gcnt][1] = g.y();
- jvo["goals"][gcnt][2] = g.h();
- gcnt++;
- }
- }
- {
- if (rrts.path().size() > 0) {
- jvo["goal"][0] = rrts.path().back()->x();
- jvo["goal"][1] = rrts.path().back()->y();
- jvo["goal"][2] = rrts.path().back()->h();
- }
- unsigned int cu = 0;
- unsigned int co = 0;
- unsigned int pcnt = 0;
- for (auto n: rrts.path()) {
- jvo["path"][pcnt][0] = n->x();
- jvo["path"][pcnt][1] = n->y();
- jvo["path"][pcnt][2] = n->h();
- if (n->t(RRTNodeType::cusp))
- cu++;
- if (n->t(RRTNodeType::connected))
- co++;
- pcnt++;
- }
- jvo["cusps-in-path"] = cu;
- jvo["connecteds-in-path"] = co;
- }
-
- std::cout << jvo << std::endl;
- return 0;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "rrtce.h"
-
-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;
-}
-
-int main()
-{
- Json::Value jvi; // JSON input
- Json::Value jvo; // JSON output
- std::cin >> jvi;
-
- if (jvi["init"] == Json::nullValue) {
- std::cerr << "I need `init` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
-
- if (jvi["goal"] == Json::nullValue) {
- std::cerr << "I need `goal` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
-
- if (jvi["goals"] == Json::nullValue) {
- std::cerr << "I need `goals` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
-
- if (jvi["obst"] == Json::nullValue) {
- std::cerr << "I need `obst` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
-
- RRTCE2 rrts;
- rrts.nodes().front().x(jvi["init"][0].asDouble());
- rrts.nodes().front().y(jvi["init"][1].asDouble());
- rrts.nodes().front().h(jvi["init"][2].asDouble());
- {
- RRTNode tmp_node;
- tmp_node.x(jvi["goal"][0].asDouble());
- tmp_node.y(jvi["goal"][1].asDouble());
- tmp_node.h(jvi["goal"][2].asDouble());
- rrts.goals().push_back(tmp_node);
- for (auto g: jvi["goals"]) {
- tmp_node.x(g[0].asDouble());
- tmp_node.y(g[1].asDouble());
- tmp_node.h(g[2].asDouble());
- rrts.goals().push_back(tmp_node);
- }
- }
- {
- Obstacle tmp_obstacle;
- for (auto o: jvi["obst"]) {
- tmp_obstacle.poly().clear();
- for (auto c: o) {
- double tmp_x = c[0].asDouble();
- double tmp_y = c[1].asDouble();
- auto tmp_tuple = std::make_tuple(tmp_x, tmp_y);
- tmp_obstacle.poly().push_back(tmp_tuple);
- }
- rrts.obstacles().push_back(tmp_obstacle);
- }
- }
-
- {
- double edist_init_goal = sqrt(
- pow(
- rrts.nodes().front().x()
- - rrts.goals().front().x(),
- 2
- )
- + pow(
- rrts.nodes().front().y()
- - rrts.goals().front().y(),
- 2
- )
- );
- rrts.set_sample(
- rrts.nodes().front().x(), edist_init_goal,
- rrts.nodes().front().y(), edist_init_goal,
- 0, 2 * M_PI
- );
- }
-
- rrts.init();
- TSTART();
- while (rrts.next()) {}
- TEND();
-
- {
- jvo["time"] = TDIFF();
- }
- {
- jvo["iterations"] = rrts.icnt();
- }
- {
- if (rrts.path().size() > 0) {
- jvo["cost"] = cc(*rrts.path().back());
- } else {
- jvo["cost"] = -1;
- }
- }
- {
- jvo["init"][0] = rrts.nodes().front().x();
- jvo["init"][1] = rrts.nodes().front().y();
- jvo["init"][2] = rrts.nodes().front().h();
- }
- {
- unsigned int ocnt = 0;
- for (auto o: rrts.obstacles()) {
- unsigned int ccnt = 0;
- for (auto c: o.poly()) {
- jvo["obst"][ocnt][ccnt][0] = std::get<0>(c);
- jvo["obst"][ocnt][ccnt][1] = std::get<1>(c);
- ccnt++;
- }
- ocnt++;
- }
- }
- //{
- // unsigned int ncnt = 0;
- // for (auto n: rrts.nodes()) {
- // jvo["nodes_x"][ncnt] = n.x();
- // jvo["nodes_y"][ncnt] = n.y();
- // //jvo["nodes_h"][ncnt] = n.h();
- // ncnt++;
- // }
- //}
- {
- jvo["nodes"] = (unsigned int) rrts.nodes().size();
- }
- {
- unsigned int gcnt = 0;
- for (auto g: rrts.goals()) {
- jvo["goals"][gcnt][0] = g.x();
- jvo["goals"][gcnt][1] = g.y();
- jvo["goals"][gcnt][2] = g.h();
- gcnt++;
- }
- }
- {
- if (rrts.path().size() > 0) {
- jvo["goal"][0] = rrts.path().back()->x();
- jvo["goal"][1] = rrts.path().back()->y();
- jvo["goal"][2] = rrts.path().back()->h();
- }
- unsigned int cu = 0;
- unsigned int co = 0;
- unsigned int pcnt = 0;
- for (auto n: rrts.path()) {
- jvo["path"][pcnt][0] = n->x();
- jvo["path"][pcnt][1] = n->y();
- jvo["path"][pcnt][2] = n->h();
- if (n->t(RRTNodeType::cusp))
- cu++;
- if (n->t(RRTNodeType::connected))
- co++;
- pcnt++;
- }
- jvo["cusps-in-path"] = cu;
- jvo["connecteds-in-path"] = co;
- }
-
- std::cout << jvo << std::endl;
- return 0;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "rrtce.h"
-
-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;
-}
-
-int main()
-{
- Json::Value jvi; // JSON input
- Json::Value jvo; // JSON output
- std::cin >> jvi;
-
- if (jvi["init"] == Json::nullValue) {
- std::cerr << "I need `init` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
-
- if (jvi["goal"] == Json::nullValue) {
- std::cerr << "I need `goal` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
-
- if (jvi["goals"] == Json::nullValue) {
- std::cerr << "I need `goals` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
-
- if (jvi["obst"] == Json::nullValue) {
- std::cerr << "I need `obst` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
-
- RRTCE3 rrts;
- rrts.nodes().front().x(jvi["init"][0].asDouble());
- rrts.nodes().front().y(jvi["init"][1].asDouble());
- rrts.nodes().front().h(jvi["init"][2].asDouble());
- {
- RRTNode tmp_node;
- tmp_node.x(jvi["goal"][0].asDouble());
- tmp_node.y(jvi["goal"][1].asDouble());
- tmp_node.h(jvi["goal"][2].asDouble());
- rrts.goals().push_back(tmp_node);
- for (auto g: jvi["goals"]) {
- tmp_node.x(g[0].asDouble());
- tmp_node.y(g[1].asDouble());
- tmp_node.h(g[2].asDouble());
- rrts.goals().push_back(tmp_node);
- }
- }
- {
- Obstacle tmp_obstacle;
- for (auto o: jvi["obst"]) {
- tmp_obstacle.poly().clear();
- for (auto c: o) {
- double tmp_x = c[0].asDouble();
- double tmp_y = c[1].asDouble();
- auto tmp_tuple = std::make_tuple(tmp_x, tmp_y);
- tmp_obstacle.poly().push_back(tmp_tuple);
- }
- rrts.obstacles().push_back(tmp_obstacle);
- }
- }
-
- {
- double edist_init_goal = sqrt(
- pow(
- rrts.nodes().front().x()
- - rrts.goals().front().x(),
- 2
- )
- + pow(
- rrts.nodes().front().y()
- - rrts.goals().front().y(),
- 2
- )
- );
- rrts.set_sample(
- rrts.nodes().front().x(), edist_init_goal,
- rrts.nodes().front().y(), edist_init_goal,
- 0, 2 * M_PI
- );
- }
-
- rrts.init();
- TSTART();
- while (rrts.next()) {}
- TEND();
-
- {
- jvo["time"] = TDIFF();
- }
- {
- jvo["iterations"] = rrts.icnt();
- }
- {
- if (rrts.path().size() > 0) {
- jvo["cost"] = cc(*rrts.path().back());
- } else {
- jvo["cost"] = -1;
- }
- }
- {
- jvo["init"][0] = rrts.nodes().front().x();
- jvo["init"][1] = rrts.nodes().front().y();
- jvo["init"][2] = rrts.nodes().front().h();
- }
- {
- unsigned int ocnt = 0;
- for (auto o: rrts.obstacles()) {
- unsigned int ccnt = 0;
- for (auto c: o.poly()) {
- jvo["obst"][ocnt][ccnt][0] = std::get<0>(c);
- jvo["obst"][ocnt][ccnt][1] = std::get<1>(c);
- ccnt++;
- }
- ocnt++;
- }
- }
- //{
- // unsigned int ncnt = 0;
- // for (auto n: rrts.nodes()) {
- // jvo["nodes_x"][ncnt] = n.x();
- // jvo["nodes_y"][ncnt] = n.y();
- // //jvo["nodes_h"][ncnt] = n.h();
- // ncnt++;
- // }
- //}
- {
- jvo["nodes"] = (unsigned int) rrts.nodes().size();
- }
- {
- unsigned int gcnt = 0;
- for (auto g: rrts.goals()) {
- jvo["goals"][gcnt][0] = g.x();
- jvo["goals"][gcnt][1] = g.y();
- jvo["goals"][gcnt][2] = g.h();
- gcnt++;
- }
- }
- {
- if (rrts.path().size() > 0) {
- jvo["goal"][0] = rrts.path().back()->x();
- jvo["goal"][1] = rrts.path().back()->y();
- jvo["goal"][2] = rrts.path().back()->h();
- }
- unsigned int cu = 0;
- unsigned int co = 0;
- unsigned int pcnt = 0;
- for (auto n: rrts.path()) {
- jvo["path"][pcnt][0] = n->x();
- jvo["path"][pcnt][1] = n->y();
- jvo["path"][pcnt][2] = n->h();
- if (n->t(RRTNodeType::cusp))
- cu++;
- if (n->t(RRTNodeType::connected))
- co++;
- pcnt++;
- }
- jvo["cusps-in-path"] = cu;
- jvo["connecteds-in-path"] = co;
- }
-
- std::cout << jvo << std::endl;
- return 0;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <jsoncpp/json/json.h>
-
-#include "rrtce.h"
-
-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;
-}
-
-int main()
-{
- Json::Value jvi; // JSON input
- Json::Value jvo; // JSON output
- std::cin >> jvi;
- if (jvi["init"] == Json::nullValue) {
- std::cerr << "I need `init` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
- if (jvi["goal"] == Json::nullValue) {
- std::cerr << "I need `goal` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
- if (jvi["goals"] == Json::nullValue) {
- std::cerr << "I need `goals` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
- if (jvi["obst"] == Json::nullValue) {
- std::cerr << "I need `obst` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
-
- std::vector<RRTNode> final_path;
- double final_path_cost = 9999;
- unsigned int final_path_steps = 0;
- double cost_from_orig_init = 0;
- unsigned int steps_from_orig_init = 0;
- RRTNode init_node;
- init_node.x(jvi["init"][0].asDouble());
- init_node.y(jvi["init"][1].asDouble());
- init_node.h(jvi["init"][2].asDouble());
- init_node.sp(2.7);
- init_node.st(M_PI / 32); // only for sc2_4
- RRTNode last_node(init_node);
- RRTNode orig_init(init_node);
- init_node.next();
- steps_from_orig_init++;
- {
- double angl_diff = std::abs(last_node.h() - init_node.h());
- if (angl_diff == 0)
- cost_from_orig_init += sqrt(
- pow(last_node.x() - init_node.x(), 2)
- + pow(last_node.y() - init_node.y(), 2)
- );
- else
- cost_from_orig_init += angl_diff * init_node.mtr();
- }
-
- RRTS rrts;
- rrts.nodes().front().x(init_node.x());
- rrts.nodes().front().y(init_node.y());
- rrts.nodes().front().h(init_node.h());
- {
- RRTNode tmp_node;
- tmp_node.x(jvi["goal"][0].asDouble());
- tmp_node.y(jvi["goal"][1].asDouble());
- tmp_node.h(jvi["goal"][2].asDouble());
- rrts.goals().push_back(tmp_node);
- for (auto g: jvi["goals"]) {
- tmp_node.x(g[0].asDouble());
- tmp_node.y(g[1].asDouble());
- tmp_node.h(g[2].asDouble());
- rrts.goals().push_back(tmp_node);
- }
- }
- {
- Obstacle tmp_obstacle;
- for (auto o: jvi["obst"]) {
- tmp_obstacle.poly().clear();
- for (auto c: o) {
- double tmp_x = c[0].asDouble();
- double tmp_y = c[1].asDouble();
- auto tmp_tuple = std::make_tuple(tmp_x, tmp_y);
- tmp_obstacle.poly().push_back(tmp_tuple);
- }
- rrts.obstacles().push_back(tmp_obstacle);
- }
- }
- {
- double edist_init_goal = sqrt(
- pow(
- rrts.nodes().front().x()
- - rrts.goals().front().x(),
- 2
- )
- + pow(
- rrts.nodes().front().y()
- - rrts.goals().front().y(),
- 2
- )
- );
- rrts.set_sample(
- rrts.nodes().front().x(), edist_init_goal,
- rrts.nodes().front().y(), edist_init_goal,
- 0, 2 * M_PI
- );
- }
-
- TSTART();
- while (rrts.next()) {}
- TEND();
- if (rrts.path().size() > 0) {
- if (
- cc(*rrts.path().back())
- + cost_from_orig_init
- < final_path_cost
- ) {
- final_path_cost =
- cc(*rrts.path().back())
- + cost_from_orig_init
- ;
- final_path_steps = steps_from_orig_init;
- final_path.clear();
- for (auto n: rrts.path()) {
- final_path.push_back(RRTNode());
- final_path.back().x(n->x());
- final_path.back().y(n->y());
- final_path.back().h(n->h());
- }
- }
- }
- int rcnt = 0;
- jvo[rcnt++] = rrts.json();
-for (int i = 1; i < 5; i++) {
- last_node = RRTNode(init_node);
- init_node.next();
- steps_from_orig_init++;
- {
- double angl_diff = std::abs(last_node.h() - init_node.h());
- if (angl_diff < 0.001)
- cost_from_orig_init += sqrt(
- pow(last_node.x() - init_node.x(), 2)
- + pow(last_node.y() - init_node.y(), 2)
- );
- else
- cost_from_orig_init += angl_diff * init_node.mtr();
- }
- RRTS rrts2;
- rrts2.goals() = rrts.goals();
- rrts2.obstacles() = rrts.obstacles();
- rrts2.nodes().front().x(init_node.x());
- rrts2.nodes().front().y(init_node.y());
- rrts2.nodes().front().h(init_node.h());
- {
- double edist_init_goal = sqrt(
- pow(
- rrts2.nodes().front().x()
- - rrts2.goals().front().x(),
- 2
- )
- + pow(
- rrts2.nodes().front().y()
- - rrts2.goals().front().y(),
- 2
- )
- );
- rrts2.set_sample(
- rrts2.nodes().front().x(), edist_init_goal,
- rrts2.nodes().front().y(), edist_init_goal,
- 0, 2 * M_PI
- );
- }
- TSTART();
- while (rrts2.next()) {}
- TEND();
- if (rrts2.path().size() > 0) {
- if (
- cc(*rrts2.path().back())
- + cost_from_orig_init
- < final_path_cost
- ) {
- final_path_cost =
- cc(*rrts2.path().back())
- + cost_from_orig_init
- ;
- final_path_steps = steps_from_orig_init;
- final_path.clear();
- for (auto n: rrts2.path()) {
- final_path.push_back(RRTNode());
- final_path.back().x(n->x());
- final_path.back().y(n->y());
- final_path.back().h(n->h());
- }
- }
- }
- jvo[rcnt++] = rrts2.json();
-}
- {
- jvo[rcnt]["obst"] = jvo[rcnt - 1]["obst"];
- jvo[rcnt]["cost"] = final_path_cost;
- unsigned int ncnt = 0;
- for (unsigned int i = 0; i < final_path_steps; i++) {
- jvo[rcnt]["path"][ncnt][0] = orig_init.x();
- jvo[rcnt]["path"][ncnt][1] = orig_init.y();
- jvo[rcnt]["path"][ncnt][2] = orig_init.h();
- orig_init.next();
- ncnt++;
- }
- for (auto n: final_path) {
- jvo[rcnt]["path"][ncnt][0] = n.x();
- jvo[rcnt]["path"][ncnt][1] = n.y();
- jvo[rcnt]["path"][ncnt][2] = n.h();
- ncnt++;
- }
- }
- std::cout << jvo << std::endl;
- return 0;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "rrtce.h"
-
-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;
-}
-
-int main()
-{
- Json::Value jvi; // JSON input
- Json::Value jvo; // JSON output
- std::cin >> jvi;
- if (jvi["init"] == Json::nullValue) {
- std::cerr << "I need `init` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
- if (jvi["goal"] == Json::nullValue) {
- std::cerr << "I need `goal` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
- if (jvi["goals"] == Json::nullValue) {
- std::cerr << "I need `goals` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
- if (jvi["obst"] == Json::nullValue) {
- std::cerr << "I need `obst` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
-
- RRTCE5 rrts;
- rrts.nodes().front().x(jvi["init"][0].asDouble());
- rrts.nodes().front().y(jvi["init"][1].asDouble());
- rrts.nodes().front().h(jvi["init"][2].asDouble());
- {
- RRTNode tmp_node;
- tmp_node.x(jvi["goal"][0].asDouble());
- tmp_node.y(jvi["goal"][1].asDouble());
- tmp_node.h(jvi["goal"][2].asDouble());
- rrts.goals().push_back(tmp_node);
- for (auto g: jvi["goals"]) {
- tmp_node.x(g[0].asDouble());
- tmp_node.y(g[1].asDouble());
- tmp_node.h(g[2].asDouble());
- rrts.goals().push_back(tmp_node);
- }
- }
- {
- Obstacle tmp_obstacle;
- for (auto o: jvi["obst"]) {
- tmp_obstacle.poly().clear();
- for (auto c: o) {
- double tmp_x = c[0].asDouble();
- double tmp_y = c[1].asDouble();
- auto tmp_tuple = std::make_tuple(tmp_x, tmp_y);
- tmp_obstacle.poly().push_back(tmp_tuple);
- }
- rrts.obstacles().push_back(tmp_obstacle);
- }
- }
- {
- double edist_init_goal = sqrt(
- pow(
- rrts.nodes().front().x()
- - rrts.goals().front().x(),
- 2
- )
- + pow(
- rrts.nodes().front().y()
- - rrts.goals().front().y(),
- 2
- )
- );
- rrts.set_sample(
- rrts.nodes().front().x(), edist_init_goal,
- rrts.nodes().front().y(), edist_init_goal,
- 0, 2 * M_PI
- );
- }
-
- rrts.init();
- TSTART();
- while (rrts.next()) {}
- TEND();
-
- std::cout << rrts.json() << std::endl;
- return 0;
-}
+++ /dev/null
-#include <chrono>
-#include <iostream>
-#include <json/json.h>
-
-#include "rrtce.h"
-
-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;
-}
-
-int main()
-{
- Json::Value jvi; // JSON input
- Json::Value jvo; // JSON output
- std::cin >> jvi;
- if (jvi["init"] == Json::nullValue) {
- std::cerr << "I need `init` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
- if (jvi["goal"] == Json::nullValue) {
- std::cerr << "I need `goal` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
- if (jvi["goals"] == Json::nullValue) {
- std::cerr << "I need `goals` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
- if (jvi["obst"] == Json::nullValue) {
- std::cerr << "I need `obst` in JSON input scenario";
- std::cerr << std::endl;
- return 1;
- }
-
- RRTCE7 rrts;
- rrts.nodes().front().x(jvi["init"][0].asDouble());
- rrts.nodes().front().y(jvi["init"][1].asDouble());
- rrts.nodes().front().h(jvi["init"][2].asDouble());
- {
- RRTNode tmp_node;
- tmp_node.x(jvi["goal"][0].asDouble());
- tmp_node.y(jvi["goal"][1].asDouble());
- tmp_node.h(jvi["goal"][2].asDouble());
- rrts.goals().push_back(tmp_node);
- for (auto g: jvi["goals"]) {
- tmp_node.x(g[0].asDouble());
- tmp_node.y(g[1].asDouble());
- tmp_node.h(g[2].asDouble());
- rrts.goals().push_back(tmp_node);
- }
- }
- {
- Obstacle tmp_obstacle;
- for (auto o: jvi["obst"]) {
- tmp_obstacle.poly().clear();
- for (auto c: o) {
- double tmp_x = c[0].asDouble();
- double tmp_y = c[1].asDouble();
- auto tmp_tuple = std::make_tuple(tmp_x, tmp_y);
- tmp_obstacle.poly().push_back(tmp_tuple);
- }
- rrts.obstacles().push_back(tmp_obstacle);
- }
- }
- {
- double edist_init_goal = sqrt(
- pow(
- rrts.nodes().front().x()
- - rrts.goals().front().x(),
- 2
- )
- + pow(
- rrts.nodes().front().y()
- - rrts.goals().front().y(),
- 2
- )
- );
- rrts.set_sample(
- rrts.nodes().front().x(), edist_init_goal,
- rrts.nodes().front().y(), edist_init_goal,
- 0, 2 * M_PI
- );
- }
-
- rrts.init();
- TSTART();
- while (rrts.next()) {}
- TEND();
-
- std::cout << rrts.json() << std::endl;
- return 0;
-}
projects / hubacji1 / iamcar2.git / commitdiff