7 this->orig_path().clear();
8 this->first_optimized_path().clear();
9 this->orig_path_cost_ = 9999.9;
10 this->first_optimized_path_cost_ = 9999.9;
13 void RRTExt3::first_path_optimization()
15 if (this->orig_path().size() == 0) {
16 this->orig_path_ = RRTS::path();
17 if (this->orig_path().size() == 0)
20 this->orig_path_cost(this->orig_path().back()->cc);
22 class DijkstraNode : public RRTNode {
24 DijkstraNode *s = nullptr;
36 DijkstraNode(const RRTNode& n) {
44 DijkstraNode *p_ = nullptr;
45 DijkstraNode *p() const { return this->p_; }
46 void p(DijkstraNode *p) { this->p_ = p; }
48 class DijkstraNodeComparator {
51 const DijkstraNode &n1,
52 const DijkstraNode &n2
58 std::vector<DijkstraNode> dn;
59 dn.reserve(this->orig_path().size());
60 unsigned int dncnt = 0;
61 for (auto n: this->orig_path()) {
63 n->t(RRTNodeType::cusp)
64 || n->t(RRTNodeType::connected)
66 dn.push_back(DijkstraNode(*n));
68 dn.back().s = &dn.back();
70 dn.back().i = dncnt++;
73 dn.push_back(DijkstraNode(*this->orig_path().back()));
74 dn.back().cc = this->orig_path().back()->cc;
75 dn.back().s = &dn.back();
76 dn.back().n = this->orig_path().back();
77 dn.back().i = dncnt++;
80 std::vector<DijkstraNode>,
81 DijkstraNodeComparator
86 DijkstraNode f = pq.top();
88 for (unsigned int i = f.i + 1; i < dncnt; i++) {
89 double cost = f.cc + this->cost_search(f, dn[i]);
90 this->steer(f, dn[i]);
91 if (this->steered().size() == 0)
92 break; // TODO why not continue?
93 if (std::get<0>(this->collide_steered_from(f)))
95 if (cost < dn[i].cc) {
103 DijkstraNode *ln = nullptr;
108 if (ln == nullptr || ln->p() == nullptr)
110 while (ln->p() != nullptr) {
112 RRTNode *f = ln->p()->n;
114 if (std::get<0>(this->collide_steered_from(*f)))
116 this->join_steered(f);
117 t->p(&this->nodes().back());
118 t->c(this->cost_build(this->nodes().back(), *t));
121 RRTS::compute_path();
124 void RRTExt3::second_path_optimization()
126 if (this->first_optimized_path().size() == 0) {
129 class DijkstraNode : public RRTNode {
131 DijkstraNode *s = nullptr;
132 RRTNode *n = nullptr;
143 DijkstraNode(const RRTNode& n) {
151 DijkstraNode *p_ = nullptr;
152 DijkstraNode *p() const { return this->p_; }
153 void p(DijkstraNode *p) { this->p_ = p; }
155 class DijkstraNodeComparator {
158 const DijkstraNode &n1,
159 const DijkstraNode &n2
162 return n1.cc > n2.cc;
165 std::vector<DijkstraNode> dn;
166 dn.reserve(this->orig_path().size());
167 unsigned int dncnt = 0;
168 for (auto n: this->orig_path()) {
170 n->t(RRTNodeType::cusp)
171 || n->t(RRTNodeType::connected)
173 dn.push_back(DijkstraNode(*n));
174 dn.back().cc = n->cc;
175 dn.back().s = &dn.back();
177 dn.back().i = dncnt++;
180 dn.push_back(DijkstraNode(*this->orig_path().back()));
181 dn.back().cc = this->orig_path().back()->cc;
182 dn.back().s = &dn.back();
183 dn.back().n = this->orig_path().back();
184 dn.back().i = dncnt++;
187 std::vector<DijkstraNode>,
188 DijkstraNodeComparator
193 while (!pq.empty()) {
194 DijkstraNode t = pq.top();
196 for (unsigned int i = t.i - 1; i > 0; i--) {
197 double cost = dn[i].cc + this->cost_search(dn[i], t);
198 this->steer(dn[i], t);
199 if (this->steered().size() == 0)
200 break; // TODO why not continue?
201 if (std::get<0>(this->collide_steered_from(dn[i])))
211 DijkstraNode *fn = nullptr;
218 if (fn == nullptr || fn->p() == nullptr)
220 DijkstraNode *ln = &dn.back();
221 while (ln->p() != fn) {
223 RRTNode *f = ln->p()->n;
225 if (std::get<0>(this->collide_steered_from(*f)))
227 this->join_steered(f);
228 t->p(&this->nodes().back());
229 t->c(this->cost_build(this->nodes().back(), *t));
232 RRTS::compute_path();
235 void RRTExt3::compute_path()
237 RRTS::compute_path();
238 auto tstart = std::chrono::high_resolution_clock::now();
239 this->first_path_optimization();
240 this->first_optimized_path_ = RRTS::path();
241 if (this->first_optimized_path_.size() > 0) {
242 this->first_optimized_path_cost(
243 this->first_optimized_path_.back()->cc
248 this->second_path_optimization();
249 auto tend = std::chrono::high_resolution_clock::now();
250 auto tdiff = std::chrono::duration_cast<std::chrono::duration<double>>(
252 this->log_opt_time_.push_back(tdiff.count());
255 Json::Value RRTExt3::json()
257 Json::Value jvo = RRTS::json();
258 jvo["orig_path_cost"] = this->orig_path_cost();
262 unsigned int pcnt = 0;
263 for (auto n: this->orig_path()) {
264 jvo["orig_path"][pcnt][0] = n->x();
265 jvo["orig_path"][pcnt][1] = n->y();
266 jvo["orig_path"][pcnt][2] = n->h();
267 if (n->t(RRTNodeType::cusp))
269 if (n->t(RRTNodeType::connected))
273 jvo["orig_cusps-in-path"] = cu;
274 jvo["orig_connecteds-in-path"] = co;
279 void RRTExt3::json(Json::Value jvi)
281 return RRTS::json(jvi);