4 std::vector<RRTNode *> RRTExt3::first_path_optimization()
6 if (this->orig_path().size() == 0) {
7 this->orig_path_ = RRTS::path();
8 if (this->orig_path().size() == 0)
9 return this->orig_path();
11 this->orig_path_cost(cc(*this->orig_path().back()));
13 class DijkstraNode : public RRTNode {
15 DijkstraNode *s = nullptr;
27 using RRTNode::RRTNode;
29 DijkstraNode *p_ = nullptr;
30 DijkstraNode *p() const { return this->p_; }
31 void p(DijkstraNode *p) { this->p_ = p; }
33 class DijkstraNodeComparator {
36 const DijkstraNode &n1,
37 const DijkstraNode &n2
43 std::vector<DijkstraNode> dn;
44 dn.reserve(this->orig_path().size());
45 unsigned int dncnt = 0;
46 for (auto n: this->orig_path()) {
48 n->t(RRTNodeType::cusp)
49 || n->t(RRTNodeType::connected)
51 dn.push_back(DijkstraNode(*n));
52 dn.back().cc = cc(*n);
53 dn.back().s = &dn.back();
55 dn.back().i = dncnt++;
58 dn.push_back(DijkstraNode(*this->orig_path().back()));
59 dn.back().cc = cc(*this->orig_path().back());
60 dn.back().s = &dn.back();
61 dn.back().n = this->orig_path().back();
62 dn.back().i = dncnt++;
65 std::vector<DijkstraNode>,
66 DijkstraNodeComparator
71 DijkstraNode f = pq.top();
73 for (unsigned int i = f.i + 1; i < dncnt; i++) {
74 double cost = f.cc + this->cost_search(f, dn[i]);
75 this->steer(f, dn[i]);
76 if (this->steered().size() == 0)
77 break; // TODO why not continue?
78 if (std::get<0>(this->collide_steered_from(f)))
80 if (cost < dn[i].cc) {
88 DijkstraNode *ln = nullptr;
93 if (ln == nullptr || ln->p() == nullptr)
94 return this->orig_path();
95 while (ln->p() != nullptr) {
97 RRTNode *f = ln->p()->n;
99 if (std::get<0>(this->collide_steered_from(*f)))
100 return this->orig_path();
101 this->join_steered(f);
102 t->p(&this->nodes().back());
103 t->c(this->cost_build(this->nodes().back(), *t));
109 std::vector<RRTNode *> RRTExt3::path()
111 this->first_optimized_path_ = this->first_path_optimization();
112 if (this->first_optimized_path_.size() > 0)
113 this->first_optimized_path_cost(
114 cc(*this->first_optimized_path_.back())
116 return this->first_optimized_path_;
119 Json::Value RRTExt3::json()
121 Json::Value jvo = RRTS::json();
122 jvo["orig_path_cost"] = this->orig_path_cost();
126 unsigned int pcnt = 0;
127 for (auto n: this->path()) {
128 jvo["orig_path"][pcnt][0] = n->x();
129 jvo["orig_path"][pcnt][1] = n->y();
130 jvo["orig_path"][pcnt][2] = n->h();
131 if (n->t(RRTNodeType::cusp))
133 if (n->t(RRTNodeType::connected))
137 jvo["orig_cusps-in-path"] = cu;
138 jvo["orig_connecteds-in-path"] = co;
143 void RRTExt3::json(Json::Value jvi)
145 return RRTS::json(jvi);