2 * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
4 * SPDX-License-Identifier: GPL-3.0-only
13 this->orig_path().clear();
14 this->first_optimized_path().clear();
15 this->orig_path_cost_ = 9999.9;
16 this->first_optimized_path_cost_ = 9999.9;
19 void RRTExt3::first_path_optimization()
21 if (this->orig_path().size() == 0) {
22 this->orig_path_ = RRTS::path();
23 if (this->orig_path().size() == 0)
26 this->orig_path_cost(this->orig_path().back()->cc);
28 class DijkstraNode : public RRTNode {
30 DijkstraNode *s = nullptr;
42 DijkstraNode(const RRTNode& n) {
50 DijkstraNode *p_ = nullptr;
51 DijkstraNode *p() const { return this->p_; }
52 void p(DijkstraNode *p) { this->p_ = p; }
54 class DijkstraNodeComparator {
57 const DijkstraNode &n1,
58 const DijkstraNode &n2
64 std::vector<DijkstraNode> dn;
65 dn.reserve(this->orig_path().size());
66 unsigned int dncnt = 0;
67 for (auto n: this->orig_path()) {
69 n->t(RRTNodeType::cusp)
70 || n->t(RRTNodeType::connected)
72 dn.push_back(DijkstraNode(*n));
74 dn.back().s = &dn.back();
76 dn.back().i = dncnt++;
79 dn.push_back(DijkstraNode(*this->orig_path().back()));
80 dn.back().cc = this->orig_path().back()->cc;
81 dn.back().s = &dn.back();
82 dn.back().n = this->orig_path().back();
83 dn.back().i = dncnt++;
86 std::vector<DijkstraNode>,
87 DijkstraNodeComparator
92 DijkstraNode f = pq.top();
94 for (unsigned int i = f.i + 1; i < dncnt; i++) {
95 double cost = f.cc + this->cost_search(f, dn[i]);
96 this->steer(f, dn[i]);
97 if (this->steered().size() == 0)
98 break; // TODO why not continue?
99 if (std::get<0>(this->collide_steered_from(f)))
101 if (cost < dn[i].cc) {
109 DijkstraNode *ln = nullptr;
114 if (ln == nullptr || ln->p() == nullptr)
116 while (ln->p() != nullptr) {
118 RRTNode *f = ln->p()->n;
120 if (std::get<0>(this->collide_steered_from(*f)))
122 this->join_steered(f);
123 t->p(&this->nodes().back());
124 t->c(this->cost_build(this->nodes().back(), *t));
127 RRTS::compute_path();
130 void RRTExt3::second_path_optimization()
132 if (this->first_optimized_path().size() == 0) {
135 class DijkstraNode : public RRTNode {
137 DijkstraNode *s = nullptr;
138 RRTNode *n = nullptr;
149 DijkstraNode(const RRTNode& n) {
157 DijkstraNode *p_ = nullptr;
158 DijkstraNode *p() const { return this->p_; }
159 void p(DijkstraNode *p) { this->p_ = p; }
161 class DijkstraNodeComparator {
164 const DijkstraNode &n1,
165 const DijkstraNode &n2
168 return n1.cc > n2.cc;
171 std::vector<DijkstraNode> dn;
172 dn.reserve(this->orig_path().size());
173 unsigned int dncnt = 0;
174 for (auto n: this->orig_path()) {
176 n->t(RRTNodeType::cusp)
177 || n->t(RRTNodeType::connected)
179 dn.push_back(DijkstraNode(*n));
180 dn.back().cc = n->cc;
181 dn.back().s = &dn.back();
183 dn.back().i = dncnt++;
186 dn.push_back(DijkstraNode(*this->orig_path().back()));
187 dn.back().cc = this->orig_path().back()->cc;
188 dn.back().s = &dn.back();
189 dn.back().n = this->orig_path().back();
190 dn.back().i = dncnt++;
193 std::vector<DijkstraNode>,
194 DijkstraNodeComparator
199 while (!pq.empty()) {
200 DijkstraNode t = pq.top();
202 for (unsigned int i = t.i - 1; i > 0; i--) {
203 double cost = dn[i].cc + this->cost_search(dn[i], t);
204 this->steer(dn[i], t);
205 if (this->steered().size() == 0)
206 break; // TODO why not continue?
207 if (std::get<0>(this->collide_steered_from(dn[i])))
217 DijkstraNode *fn = nullptr;
224 if (fn == nullptr || fn->p() == nullptr)
226 DijkstraNode *ln = &dn.back();
227 while (ln->p() != fn) {
229 RRTNode *f = ln->p()->n;
231 if (std::get<0>(this->collide_steered_from(*f)))
233 this->join_steered(f);
234 t->p(&this->nodes().back());
235 t->c(this->cost_build(this->nodes().back(), *t));
238 RRTS::compute_path();
241 void RRTExt3::compute_path()
243 RRTS::compute_path();
244 auto tstart = std::chrono::high_resolution_clock::now();
245 this->first_path_optimization();
246 this->first_optimized_path_ = RRTS::path();
247 if (this->first_optimized_path_.size() > 0) {
248 this->first_optimized_path_cost(
249 this->first_optimized_path_.back()->cc
254 this->second_path_optimization();
255 auto tend = std::chrono::high_resolution_clock::now();
256 auto tdiff = std::chrono::duration_cast<std::chrono::duration<double>>(
258 this->log_opt_time_.push_back(tdiff.count());
261 Json::Value RRTExt3::json()
263 Json::Value jvo = RRTS::json();
264 jvo["orig_path_cost"] = this->orig_path_cost();
268 unsigned int pcnt = 0;
269 for (auto n: this->orig_path()) {
270 jvo["orig_path"][pcnt][0] = n->x();
271 jvo["orig_path"][pcnt][1] = n->y();
272 jvo["orig_path"][pcnt][2] = n->h();
273 if (n->t(RRTNodeType::cusp))
275 if (n->t(RRTNodeType::connected))
279 jvo["orig_cusps-in-path"] = cu;
280 jvo["orig_connecteds-in-path"] = co;
285 void RRTExt3::json(Json::Value jvi)
287 return RRTS::json(jvi);