3 #define ETA 1.0 // for steer, nv
5 __typeof__ (cV) _cV = (cV); \
6 pow(log(_cV) / _cV, 1.0 / 3.0); \
20 double RRTS::cost(RRTNode &f, RRTNode &t)
23 cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2));
29 double x = this->ndx_(this->gen_);
30 double y = this->ndy_(this->gen_);
31 double h = this->ndh_(this->gen_);
32 this->samples().push_back(RRTNode());
33 this->samples().back().x(x);
34 this->samples().back().y(y);
35 this->samples().back().h(h);
38 RRTNode *RRTS::nn(RRTNode &t)
40 RRTNode *nn = &this->nodes().front();
41 double cost = this->cost(*nn, t);
42 for (auto &f: this->nodes()) {
43 if (this->cost(f, t) < cost) {
45 cost = this->cost(f, t);
51 std::vector<RRTNode *> RRTS::nv(RRTNode &t)
53 std::vector<RRTNode *> nv;
54 double cost = std::min(GAMMA(this->nodes().size()), ETA);
55 for (auto &f: this->nodes())
56 if (this->cost(f, t) < cost)
61 void RRTS::steer(RRTNode &f, RRTNode &t)
63 double angl = atan2(t.y() - f.y(), t.x() - f.x());
64 this->steered().clear();
65 this->steered().push_back(RRTNode());
66 this->steered().back().x(f.x() + ETA * cos(angl));
67 this->steered().back().y(f.y() + ETA * sin(angl));
68 this->steered().back().h(angl);
75 RRTNode *t = &this->steered().front();
76 RRTNode *f = this->nn(this->samples().back());
77 double cost = this->cost(*f, *t);
78 for (auto n: this->nv(*t)) {
79 if (this->cost(*n, *t) < cost) {
81 cost = this->cost(*n, *t);
84 this->nodes().push_back(this->steered().front());
85 this->steered().erase(this->steered().begin());
86 t = &this->nodes().back();
88 t->c(f->c() + this->cost(*f, *t));
95 RRTNode *f = &this->nodes().back();
96 for (auto n: this->nv(*f)) {
97 if (f->c() + this->cost(*f, *n) < n->c())
103 std::vector<RRTNode *> RRTS::path()
105 std::vector<RRTNode *> path;
115 *this->nn(this->samples().back()),
116 this->samples().back()
120 for (auto &n: this->goals()) {
121 double cost = this->cost(this->nodes().back(), n);
126 || this->nodes().back().c() + cost < n.c()
128 n.p(&this->nodes().back());
129 n.c(this->nodes().back().c() + cost);
133 if (this->icnt_ > 999)
138 void RRTS::set_sample(
139 double mx, double dx,
140 double my, double dy,
144 this->ndx_ = std::normal_distribution<double>(mx, dx);
145 this->ndy_ = std::normal_distribution<double>(my, dy);
146 this->ndh_ = std::normal_distribution<double>(mh, dh);
150 : gen_(std::random_device{}())
152 this->nodes().push_back(RRTNode()); // root
155 double cc(RRTNode &t)
159 while (n != nullptr) {