2 * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
4 * SPDX-License-Identifier: GPL-3.0-only
9 void RRTExt9::Cell::nn(RRTNode *t, RRTNode **nn, RRTS *p)
11 double cost = p->cost_search(**nn, *t);
12 for (auto f: this->nodes()) {
13 if (p->cost_search(*f, *t) < cost) {
15 cost = p->cost_search(*f, *t);
20 void RRTExt9::Cell::store_node(RRTNode *n)
22 this->nodes_.push_back(n);
23 this->changed_ = true;
30 unsigned int RRTExt9::xi(RRTNode n)
32 if (n.x() >= this->x_max_)
33 return GRID_MAX_XI - 1;
34 if (n.x() <= this->x_min_)
36 return (unsigned int) (floor(n.x() - this->x_min_) / GRID);
39 unsigned int RRTExt9::yi(RRTNode n)
41 if (n.y() > this->y_max_)
42 return GRID_MAX_YI - 1;
43 if (n.y() <= this->y_min_)
45 return (unsigned int) (floor(n.y() - this->y_min_) / GRID);
48 unsigned int RRTExt9::hi(RRTNode n)
50 if (n.h() > this->h_max_)
51 return GRID_MAX_HI - 1;
52 if (n.h() <= this->h_min_)
54 return (unsigned int) (n.h() * GRID_MAX_HI / (2 * M_PI));
60 this->x_min_ = this->nodes().back().x() - GRID_WIDTH / 2;
61 this->x_max_ = this->nodes().back().x() + GRID_WIDTH / 2;
62 this->y_min_ = this->nodes().back().y() - GRID_HEIGHT / 2;
63 this->y_max_ = this->nodes().back().y() + GRID_HEIGHT / 2;
66 void RRTExt9::deinit()
68 for (unsigned int i = 0; i < GRID_MAX_XI; i++)
69 for (unsigned int j = 0; j < GRID_MAX_YI; j++)
70 for (unsigned int k = 0; k < GRID_MAX_HI; k++)
71 this->grid_[i][j][k].nodes().clear();
74 void RRTExt9::store_node(RRTNode n)
77 RRTNode *sn = &this->nodes().back();
78 this->grid_[this->xi(n)][this->yi(n)][this->hi(n)].store_node(sn);
81 RRTNode *RRTExt9::nn(RRTNode &t)
83 RRTNode *nn = &this->nodes().front();
84 unsigned int txi = this->xi(t);
85 unsigned int tyi = this->yi(t);
86 unsigned int thi = this->hi(t);
88 while (this->cost_search(*nn, t) > l * GRID) {
93 if (xi_max > GRID_MAX_XI - 1)
94 xi_max = GRID_MAX_XI - 1;
99 if (yi_max > GRID_MAX_YI - 1)
100 yi_max = GRID_MAX_YI - 1;
101 int hi_min = thi - l; // TODO respect kinematic constraints
104 int hi_max = thi + l;
105 if (hi_max > GRID_MAX_HI - 1)
106 hi_max = GRID_MAX_HI - 1;
107 for (int hi = hi_min; hi <= hi_max; hi++) {
108 for (int xi = xi_min; xi <= xi_max; xi++) {
109 this->grid_[xi][yi_max][hi].nn(&t, &nn, this);
111 for (int xi = xi_min; xi <= xi_max; xi++) {
112 this->grid_[xi][yi_min][hi].nn(&t, &nn, this);
114 for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) {
115 this->grid_[xi_min][yi][hi].nn(&t, &nn, this);
117 for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) {
118 this->grid_[xi_max][yi][hi].nn(&t, &nn, this);
126 std::vector<RRTNode *> RRTExt9::nv(RRTNode &t)
128 std::vector<RRTNode *> nv;
129 unsigned int txi = this->xi(t);
130 unsigned int tyi = this->yi(t);
131 unsigned int thi = this->hi(t);
133 double cost = std::min(GAMMA(this->nodes().size()), ETA);
134 for (auto f: this->grid_[txi][tyi][thi].nodes())
135 if (this->cost_search(*f, t) < cost)