4 std::vector<RRTNode *> RRTExt3::path()
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));