+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
#include "rrtext.h"
void RRTExt9::Cell::nn(RRTNode *t, RRTNode **nn, RRTS *p)
{
- double cost = p->cost_search(**nn, *t);
- for (auto f: this->nodes()) {
- if (p->cost_search(*f, *t) < cost) {
- *nn = f;
- cost = p->cost_search(*f, *t);
- }
- }
+ double cost = p->cost_search(**nn, *t);
+ for (auto f: this->nodes()) {
+ if (p->cost_search(*f, *t) < cost) {
+ *nn = f;
+ cost = p->cost_search(*f, *t);
+ }
+ }
}
void RRTExt9::Cell::store_node(RRTNode *n)
{
- this->nodes_.push_back(n);
- this->changed_ = true;
+ this->nodes_.push_back(n);
+ this->changed_ = true;
}
RRTExt9::Cell::Cell()
unsigned int RRTExt9::xi(RRTNode n)
{
- if (n.x() >= this->x_max_)
- return GRID_MAX_XI - 1;
- if (n.x() <= this->x_min_)
- return 0;
- return (unsigned int) (floor(n.x() - this->x_min_) / GRID);
+ if (n.x() >= this->x_max_)
+ return GRID_MAX_XI - 1;
+ if (n.x() <= this->x_min_)
+ return 0;
+ return (unsigned int) (floor(n.x() - this->x_min_) / GRID);
}
unsigned int RRTExt9::yi(RRTNode n)
{
- if (n.y() > this->y_max_)
- return GRID_MAX_YI - 1;
- if (n.y() <= this->y_min_)
- return 0;
- return (unsigned int) (floor(n.y() - this->y_min_) / GRID);
+ if (n.y() > this->y_max_)
+ return GRID_MAX_YI - 1;
+ if (n.y() <= this->y_min_)
+ return 0;
+ return (unsigned int) (floor(n.y() - this->y_min_) / GRID);
}
unsigned int RRTExt9::hi(RRTNode n)
{
- if (n.h() > this->h_max_)
- return GRID_MAX_HI - 1;
- if (n.h() <= this->h_min_)
- return 0;
- return (unsigned int) (n.h() * GRID_MAX_HI / (2 * M_PI));
+ if (n.h() > this->h_max_)
+ return GRID_MAX_HI - 1;
+ if (n.h() <= this->h_min_)
+ return 0;
+ return (unsigned int) (n.h() * GRID_MAX_HI / (2 * M_PI));
}
// API
void RRTExt9::init()
{
- this->x_min_ = this->nodes().back().x() - GRID_WIDTH / 2;
- this->x_max_ = this->nodes().back().x() + GRID_WIDTH / 2;
- this->y_min_ = this->nodes().back().y() - GRID_HEIGHT / 2;
- this->y_max_ = this->nodes().back().y() + GRID_HEIGHT / 2;
+ this->x_min_ = this->nodes().back().x() - GRID_WIDTH / 2;
+ this->x_max_ = this->nodes().back().x() + GRID_WIDTH / 2;
+ this->y_min_ = this->nodes().back().y() - GRID_HEIGHT / 2;
+ this->y_max_ = this->nodes().back().y() + GRID_HEIGHT / 2;
}
void RRTExt9::deinit()
{
- for (unsigned int i = 0; i < GRID_MAX_XI; i++)
- for (unsigned int j = 0; j < GRID_MAX_YI; j++)
- for (unsigned int k = 0; k < GRID_MAX_HI; k++)
- this->grid_[i][j][k].nodes().clear();
+ for (unsigned int i = 0; i < GRID_MAX_XI; i++)
+ for (unsigned int j = 0; j < GRID_MAX_YI; j++)
+ for (unsigned int k = 0; k < GRID_MAX_HI; k++)
+ this->grid_[i][j][k].nodes().clear();
}
void RRTExt9::store_node(RRTNode n)
{
- RRTS::store_node(n);
- RRTNode *sn = &this->nodes().back();
- this->grid_[this->xi(n)][this->yi(n)][this->hi(n)].store_node(sn);
+ RRTS::store_node(n);
+ RRTNode *sn = &this->nodes().back();
+ this->grid_[this->xi(n)][this->yi(n)][this->hi(n)].store_node(sn);
}
RRTNode *RRTExt9::nn(RRTNode &t)
{
- RRTNode *nn = &this->nodes().front();
- unsigned int txi = this->xi(t);
- unsigned int tyi = this->yi(t);
- unsigned int thi = this->hi(t);
- unsigned int l = 0;
- while (this->cost_search(*nn, t) > l * GRID) {
- int xi_min = txi - l;
- if (xi_min < 0)
- xi_min = 0;
- int xi_max = txi + l;
- if (xi_max > GRID_MAX_XI - 1)
- xi_max = GRID_MAX_XI - 1;
- int yi_min = tyi - l;
- if (yi_min < 0)
- yi_min = 0;
- int yi_max = tyi + l;
- if (yi_max > GRID_MAX_YI - 1)
- yi_max = GRID_MAX_YI - 1;
- int hi_min = thi - l; // TODO respect kinematic constraints
- if (hi_min < 0)
- hi_min = 0;
- int hi_max = thi + l;
- if (hi_max > GRID_MAX_HI - 1)
- hi_max = GRID_MAX_HI - 1;
- for (int hi = hi_min; hi <= hi_max; hi++) {
- for (int xi = xi_min; xi <= xi_max; xi++) {
- this->grid_[xi][yi_max][hi].nn(&t, &nn, this);
- }
- for (int xi = xi_min; xi <= xi_max; xi++) {
- this->grid_[xi][yi_min][hi].nn(&t, &nn, this);
- }
- for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) {
- this->grid_[xi_min][yi][hi].nn(&t, &nn, this);
- }
- for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) {
- this->grid_[xi_max][yi][hi].nn(&t, &nn, this);
- }
- }
- l++;
- }
- return nn;
+ RRTNode *nn = &this->nodes().front();
+ unsigned int txi = this->xi(t);
+ unsigned int tyi = this->yi(t);
+ unsigned int thi = this->hi(t);
+ unsigned int l = 0;
+ while (this->cost_search(*nn, t) > l * GRID) {
+ int xi_min = txi - l;
+ if (xi_min < 0)
+ xi_min = 0;
+ int xi_max = txi + l;
+ if (xi_max > GRID_MAX_XI - 1)
+ xi_max = GRID_MAX_XI - 1;
+ int yi_min = tyi - l;
+ if (yi_min < 0)
+ yi_min = 0;
+ int yi_max = tyi + l;
+ if (yi_max > GRID_MAX_YI - 1)
+ yi_max = GRID_MAX_YI - 1;
+ int hi_min = thi - l; // TODO respect kinematic constraints
+ if (hi_min < 0)
+ hi_min = 0;
+ int hi_max = thi + l;
+ if (hi_max > GRID_MAX_HI - 1)
+ hi_max = GRID_MAX_HI - 1;
+ for (int hi = hi_min; hi <= hi_max; hi++) {
+ for (int xi = xi_min; xi <= xi_max; xi++) {
+ this->grid_[xi][yi_max][hi].nn(&t, &nn, this);
+ }
+ for (int xi = xi_min; xi <= xi_max; xi++) {
+ this->grid_[xi][yi_min][hi].nn(&t, &nn, this);
+ }
+ for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) {
+ this->grid_[xi_min][yi][hi].nn(&t, &nn, this);
+ }
+ for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) {
+ this->grid_[xi_max][yi][hi].nn(&t, &nn, this);
+ }
+ }
+ l++;
+ }
+ return nn;
}
std::vector<RRTNode *> RRTExt9::nv(RRTNode &t)
{
- std::vector<RRTNode *> nv;
- unsigned int txi = this->xi(t);
- unsigned int tyi = this->yi(t);
- unsigned int thi = this->hi(t);
- unsigned int l = 0;
- double cost = std::min(GAMMA(this->nodes().size()), ETA);
- for (auto f: this->grid_[txi][tyi][thi].nodes())
- if (this->cost_search(*f, t) < cost)
- nv.push_back(f);
- return nv;
+ std::vector<RRTNode *> nv;
+ unsigned int txi = this->xi(t);
+ unsigned int tyi = this->yi(t);
+ unsigned int thi = this->hi(t);
+ unsigned int l = 0;
+ double cost = std::min(GAMMA(this->nodes().size()), ETA);
+ for (auto f: this->grid_[txi][tyi][thi].nodes())
+ if (this->cost_search(*f, t) < cost)
+ nv.push_back(f);
+ return nv;
}
projects / hubacji1 / rrts.git / blobdiff