+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
#include <queue>
#include "rrtext.h"
-std::vector<RRTNode *> RRTExt3::first_path_optimization()
+void RRTExt3::reset()
+{
+ RRTS::reset();
+ this->orig_path().clear();
+ this->first_optimized_path().clear();
+ this->orig_path_cost_ = 9999.9;
+ this->first_optimized_path_cost_ = 9999.9;
+}
+
+void RRTExt3::first_path_optimization()
{
- if (this->orig_path().size() == 0) {
- this->orig_path_ = RRTS::path();
- if (this->orig_path().size() == 0)
- return this->orig_path();
- else
- this->orig_path_cost(cc(*this->orig_path().back()));
- }
- class DijkstraNode : public RRTNode {
- public:
- DijkstraNode *s = nullptr;
- RRTNode *n = nullptr;
- unsigned int i = 0;
- double cc = 9999;
- bool v = false;
- bool vi()
- {
- if (this->v)
- return true;
- this->v = true;
- return false;
- }
- using RRTNode::RRTNode;
- // override
- DijkstraNode *p_ = nullptr;
- DijkstraNode *p() const { return this->p_; }
- void p(DijkstraNode *p) { this->p_ = p; }
- };
- class DijkstraNodeComparator {
- public:
- int operator() (
- const DijkstraNode &n1,
- const DijkstraNode &n2
- )
- {
- return n1.cc > n2.cc;
- }
- };
- std::vector<DijkstraNode> dn;
- dn.reserve(this->orig_path().size());
- unsigned int dncnt = 0;
- for (auto n: this->orig_path()) {
- if (
- n->t(RRTNodeType::cusp)
- || n->t(RRTNodeType::connected)
- ) {
- dn.push_back(DijkstraNode(*n));
- dn.back().cc = cc(*n);
- dn.back().s = &dn.back();
- dn.back().n = n;
- dn.back().i = dncnt++;
- }
- }
- dn.push_back(DijkstraNode(*this->orig_path().back()));
- dn.back().cc = cc(*this->orig_path().back());
- dn.back().s = &dn.back();
- dn.back().n = this->orig_path().back();
- dn.back().i = dncnt++;
- std::priority_queue<
- DijkstraNode,
- std::vector<DijkstraNode>,
- DijkstraNodeComparator
- > pq;
- dn.front().vi();
- pq.push(dn.front());
- while (!pq.empty()) {
- DijkstraNode f = pq.top();
- pq.pop();
- for (unsigned int i = f.i + 1; i < dncnt; i++) {
- double cost = f.cc + this->cost_search(f, dn[i]);
- this->steer(f, dn[i]);
- if (this->steered().size() == 0)
- break; // TODO why not continue?
- if (std::get<0>(this->collide_steered_from(f)))
- continue;
- if (cost < dn[i].cc) {
- dn[i].cc = cost;
- dn[i].p(f.s);
- if (!dn[i].vi())
- pq.push(dn[i]);
- }
- }
- }
- DijkstraNode *ln = nullptr;
- for (auto &n: dn) {
- if (n.v)
- ln = &n;
- }
- if (ln == nullptr || ln->p() == nullptr)
- return this->orig_path();
- while (ln->p() != nullptr) {
- RRTNode *t = ln->n;
- RRTNode *f = ln->p()->n;
- this->steer(*f, *t);
- if (std::get<0>(this->collide_steered_from(*f)))
- return this->orig_path();
- this->join_steered(f);
- t->p(&this->nodes().back());
- t->c(this->cost_build(this->nodes().back(), *t));
- ln = ln->p();
- }
- return RRTS::path();
+ if (this->orig_path().size() == 0) {
+ this->orig_path_ = RRTS::path();
+ if (this->orig_path().size() == 0)
+ return;
+ else
+ this->orig_path_cost(this->orig_path().back()->cc);
+ }
+ class DijkstraNode : public RRTNode {
+ public:
+ DijkstraNode *s = nullptr;
+ RRTNode *n = nullptr;
+ unsigned int i = 0;
+ double cc = 9999;
+ bool v = false;
+ bool vi()
+ {
+ if (this->v)
+ return true;
+ this->v = true;
+ return false;
+ }
+ DijkstraNode(const RRTNode& n) {
+ this->x(n.x());
+ this->y(n.y());
+ this->h(n.h());
+ this->sp(n.sp());
+ this->st(n.st());
+ }
+ // override
+ DijkstraNode *p_ = nullptr;
+ DijkstraNode *p() const { return this->p_; }
+ void p(DijkstraNode *p) { this->p_ = p; }
+ };
+ class DijkstraNodeComparator {
+ public:
+ int operator() (
+ const DijkstraNode &n1,
+ const DijkstraNode &n2
+ )
+ {
+ return n1.cc > n2.cc;
+ }
+ };
+ std::vector<DijkstraNode> dn;
+ dn.reserve(this->orig_path().size());
+ unsigned int dncnt = 0;
+ for (auto n: this->orig_path()) {
+ if (
+ n->t(RRTNodeType::cusp)
+ || n->t(RRTNodeType::connected)
+ ) {
+ dn.push_back(DijkstraNode(*n));
+ dn.back().cc = n->cc;
+ dn.back().s = &dn.back();
+ dn.back().n = n;
+ dn.back().i = dncnt++;
+ }
+ }
+ dn.push_back(DijkstraNode(*this->orig_path().back()));
+ dn.back().cc = this->orig_path().back()->cc;
+ dn.back().s = &dn.back();
+ dn.back().n = this->orig_path().back();
+ dn.back().i = dncnt++;
+ std::priority_queue<
+ DijkstraNode,
+ std::vector<DijkstraNode>,
+ DijkstraNodeComparator
+ > pq;
+ dn.front().vi();
+ pq.push(dn.front());
+ while (!pq.empty()) {
+ DijkstraNode f = pq.top();
+ pq.pop();
+ for (unsigned int i = f.i + 1; i < dncnt; i++) {
+ double cost = f.cc + this->cost_search(f, dn[i]);
+ this->steer(f, dn[i]);
+ if (this->steered().size() == 0)
+ break; // TODO why not continue?
+ if (std::get<0>(this->collide_steered_from(f)))
+ continue;
+ if (cost < dn[i].cc) {
+ dn[i].cc = cost;
+ dn[i].p(f.s);
+ if (!dn[i].vi())
+ pq.push(dn[i]);
+ }
+ }
+ }
+ DijkstraNode *ln = nullptr;
+ for (auto &n: dn) {
+ if (n.v)
+ ln = &n;
+ }
+ if (ln == nullptr || ln->p() == nullptr)
+ return;
+ while (ln->p() != nullptr) {
+ RRTNode *t = ln->n;
+ RRTNode *f = ln->p()->n;
+ this->steer(*f, *t);
+ if (std::get<0>(this->collide_steered_from(*f)))
+ return;
+ this->join_steered(f);
+ t->p(&this->nodes().back());
+ t->c(this->cost_build(this->nodes().back(), *t));
+ ln = ln->p();
+ }
+ RRTS::compute_path();
}
-std::vector<RRTNode *> RRTExt3::second_path_optimization()
+void RRTExt3::second_path_optimization()
{
- if (this->first_optimized_path().size() == 0) {
- return this->orig_path();
- }
- class DijkstraNode : public RRTNode {
- public:
- DijkstraNode *s = nullptr;
- RRTNode *n = nullptr;
- unsigned int i = 0;
- double cc = 9999;
- bool v = false;
- bool vi()
- {
- if (this->v)
- return true;
- this->v = true;
- return false;
- }
- using RRTNode::RRTNode;
- // override
- DijkstraNode *p_ = nullptr;
- DijkstraNode *p() const { return this->p_; }
- void p(DijkstraNode *p) { this->p_ = p; }
- };
- class DijkstraNodeComparator {
- public:
- int operator() (
- const DijkstraNode &n1,
- const DijkstraNode &n2
- )
- {
- return n1.cc > n2.cc;
- }
- };
- std::vector<DijkstraNode> dn;
- dn.reserve(this->orig_path().size());
- unsigned int dncnt = 0;
- for (auto n: this->orig_path()) {
- if (
- n->t(RRTNodeType::cusp)
- || n->t(RRTNodeType::connected)
- ) {
- dn.push_back(DijkstraNode(*n));
- dn.back().cc = cc(*n);
- dn.back().s = &dn.back();
- dn.back().n = n;
- dn.back().i = dncnt++;
- }
- }
- dn.push_back(DijkstraNode(*this->orig_path().back()));
- dn.back().cc = cc(*this->orig_path().back());
- dn.back().s = &dn.back();
- dn.back().n = this->orig_path().back();
- dn.back().i = dncnt++;
- std::priority_queue<
- DijkstraNode,
- std::vector<DijkstraNode>,
- DijkstraNodeComparator
- > pq;
- // TODO rewrite
- dn.back().vi();
- pq.push(dn.back());
- while (!pq.empty()) {
- DijkstraNode t = pq.top();
- pq.pop();
- for (unsigned int i = t.i - 1; i > 0; i--) {
- double cost = dn[i].cc + this->cost_search(dn[i], t);
- this->steer(dn[i], t);
- if (this->steered().size() == 0)
- break; // TODO why not continue?
- if (std::get<0>(this->collide_steered_from(dn[i])))
- continue;
- if (cost < t.cc) {
- t.cc = cost;
- t.p(dn[i].s);
- if (!dn[i].vi())
- pq.push(dn[i]);
- }
- }
- }
- DijkstraNode *fn = nullptr;
- for (auto &n: dn) {
- if (n.v) {
- fn = &n;
- break;
- }
- }
- if (fn == nullptr || fn->p() == nullptr)
- return this->first_optimized_path();
- DijkstraNode *ln = &dn.back();
- while (ln->p() != fn) {
- RRTNode *t = ln->n;
- RRTNode *f = ln->p()->n;
- this->steer(*f, *t);
- if (std::get<0>(this->collide_steered_from(*f)))
- return this->first_optimized_path();
- this->join_steered(f);
- t->p(&this->nodes().back());
- t->c(this->cost_build(this->nodes().back(), *t));
- ln = ln->p();
- }
- return RRTS::path();
+ if (this->first_optimized_path().size() == 0) {
+ return;
+ }
+ class DijkstraNode : public RRTNode {
+ public:
+ DijkstraNode *s = nullptr;
+ RRTNode *n = nullptr;
+ unsigned int i = 0;
+ double cc = 9999;
+ bool v = false;
+ bool vi()
+ {
+ if (this->v)
+ return true;
+ this->v = true;
+ return false;
+ }
+ DijkstraNode(const RRTNode& n) {
+ this->x(n.x());
+ this->y(n.y());
+ this->h(n.h());
+ this->sp(n.sp());
+ this->st(n.st());
+ }
+ // override
+ DijkstraNode *p_ = nullptr;
+ DijkstraNode *p() const { return this->p_; }
+ void p(DijkstraNode *p) { this->p_ = p; }
+ };
+ class DijkstraNodeComparator {
+ public:
+ int operator() (
+ const DijkstraNode &n1,
+ const DijkstraNode &n2
+ )
+ {
+ return n1.cc > n2.cc;
+ }
+ };
+ std::vector<DijkstraNode> dn;
+ dn.reserve(this->orig_path().size());
+ unsigned int dncnt = 0;
+ for (auto n: this->orig_path()) {
+ if (
+ n->t(RRTNodeType::cusp)
+ || n->t(RRTNodeType::connected)
+ ) {
+ dn.push_back(DijkstraNode(*n));
+ dn.back().cc = n->cc;
+ dn.back().s = &dn.back();
+ dn.back().n = n;
+ dn.back().i = dncnt++;
+ }
+ }
+ dn.push_back(DijkstraNode(*this->orig_path().back()));
+ dn.back().cc = this->orig_path().back()->cc;
+ dn.back().s = &dn.back();
+ dn.back().n = this->orig_path().back();
+ dn.back().i = dncnt++;
+ std::priority_queue<
+ DijkstraNode,
+ std::vector<DijkstraNode>,
+ DijkstraNodeComparator
+ > pq;
+ // TODO rewrite
+ dn.back().vi();
+ pq.push(dn.back());
+ while (!pq.empty()) {
+ DijkstraNode t = pq.top();
+ pq.pop();
+ for (unsigned int i = t.i - 1; i > 0; i--) {
+ double cost = dn[i].cc + this->cost_search(dn[i], t);
+ this->steer(dn[i], t);
+ if (this->steered().size() == 0)
+ break; // TODO why not continue?
+ if (std::get<0>(this->collide_steered_from(dn[i])))
+ continue;
+ if (cost < t.cc) {
+ t.cc = cost;
+ t.p(dn[i].s);
+ if (!dn[i].vi())
+ pq.push(dn[i]);
+ }
+ }
+ }
+ DijkstraNode *fn = nullptr;
+ for (auto &n: dn) {
+ if (n.v) {
+ fn = &n;
+ break;
+ }
+ }
+ if (fn == nullptr || fn->p() == nullptr)
+ return;
+ DijkstraNode *ln = &dn.back();
+ while (ln->p() != fn) {
+ RRTNode *t = ln->n;
+ RRTNode *f = ln->p()->n;
+ this->steer(*f, *t);
+ if (std::get<0>(this->collide_steered_from(*f)))
+ return;
+ this->join_steered(f);
+ t->p(&this->nodes().back());
+ t->c(this->cost_build(this->nodes().back(), *t));
+ ln = ln->p();
+ }
+ RRTS::compute_path();
}
-std::vector<RRTNode *> RRTExt3::path()
+void RRTExt3::compute_path()
{
- this->first_optimized_path_ = this->first_path_optimization();
- if (this->first_optimized_path_.size() > 0)
- this->first_optimized_path_cost(
- cc(*this->first_optimized_path_.back())
- );
- else
- return this->orig_path();
- return this->second_path_optimization();
+ RRTS::compute_path();
+ auto tstart = std::chrono::high_resolution_clock::now();
+ this->first_path_optimization();
+ this->first_optimized_path_ = RRTS::path();
+ if (this->first_optimized_path_.size() > 0) {
+ this->first_optimized_path_cost(
+ this->first_optimized_path_.back()->cc
+ );
+ } else {
+ return;
+ }
+ this->second_path_optimization();
+ auto tend = std::chrono::high_resolution_clock::now();
+ auto tdiff = std::chrono::duration_cast<std::chrono::duration<double>>(
+ tend - tstart);
+ this->log_opt_time_.push_back(tdiff.count());
}
Json::Value RRTExt3::json()
{
- Json::Value jvo = RRTS::json();
- jvo["orig_path_cost"] = this->orig_path_cost();
- {
- unsigned int cu = 0;
- unsigned int co = 0;
- unsigned int pcnt = 0;
- for (auto n: this->path()) {
- jvo["orig_path"][pcnt][0] = n->x();
- jvo["orig_path"][pcnt][1] = n->y();
- jvo["orig_path"][pcnt][2] = n->h();
- if (n->t(RRTNodeType::cusp))
- cu++;
- if (n->t(RRTNodeType::connected))
- co++;
- pcnt++;
- }
- jvo["orig_cusps-in-path"] = cu;
- jvo["orig_connecteds-in-path"] = co;
- }
- return jvo;
+ Json::Value jvo = RRTS::json();
+ jvo["orig_path_cost"] = this->orig_path_cost();
+ {
+ unsigned int cu = 0;
+ unsigned int co = 0;
+ unsigned int pcnt = 0;
+ for (auto n: this->orig_path()) {
+ jvo["orig_path"][pcnt][0] = n->x();
+ jvo["orig_path"][pcnt][1] = n->y();
+ jvo["orig_path"][pcnt][2] = n->h();
+ if (n->t(RRTNodeType::cusp))
+ cu++;
+ if (n->t(RRTNodeType::connected))
+ co++;
+ pcnt++;
+ }
+ jvo["orig_cusps-in-path"] = cu;
+ jvo["orig_connecteds-in-path"] = co;
+ }
+ return jvo;
}
void RRTExt3::json(Json::Value jvi)
{
- return RRTS::json(jvi);
+ return RRTS::json(jvi);
}
projects / hubacji1 / rrts.git / blobdiff