#include "rrtext.h"
RRTExt8::KdNode::KdNode(RRTNode *n)
- : node_(n)
- , left_(nullptr)
- , right_(nullptr)
+ : node_(n)
+ , left_(nullptr)
+ , right_(nullptr)
{
}
void RRTExt8::delete_kd_nodes(KdNode *n)
{
- if (n == nullptr)
- return;
- this->delete_kd_nodes(n->left());
- this->delete_kd_nodes(n->right());
- delete n;
+ if (n == nullptr)
+ return;
+ this->delete_kd_nodes(n->left());
+ this->delete_kd_nodes(n->right());
+ delete n;
}
void RRTExt8::store_node(RRTNode *n, KdNode *&r, int l)
{
- if (r == nullptr)
- r = new KdNode(n);
- else if (l % 3 == 0 && n->x() < r->node()->x())
- store_node(n, r->left(), l + 1);
- else if (l % 3 == 0)
- store_node(n, r->right(), l + 1);
- else if (l % 3 == 1 && n->y() < r->node()->y())
- store_node(n, r->left(), l + 1);
- else if (l % 3 == 1)
- store_node(n, r->right(), l + 1);
- else if (l % 3 == 2 && n->h() < r->node()->h())
- store_node(n, r->left(), l + 1);
- else
- store_node(n, r->right(), l + 1);
+ if (r == nullptr)
+ r = new KdNode(n);
+ else if (l % 3 == 0 && n->x() < r->node()->x())
+ store_node(n, r->left(), l + 1);
+ else if (l % 3 == 0)
+ store_node(n, r->right(), l + 1);
+ else if (l % 3 == 1 && n->y() < r->node()->y())
+ store_node(n, r->left(), l + 1);
+ else if (l % 3 == 1)
+ store_node(n, r->right(), l + 1);
+ else if (l % 3 == 2 && n->h() < r->node()->h())
+ store_node(n, r->left(), l + 1);
+ else
+ store_node(n, r->right(), l + 1);
}
void RRTExt8::nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d)
{
- if (r == nullptr)
- return;
- if (this->cost_search(*r->node(), t) < d) {
- n = r->node();
- d = this->cost_search(*r->node(), t);
- }
- if (l % 3 == 0 && t.x() < r->node()->x()) {
- nn(n, t, r->left(), l + 1, d);
- if (r->node()->x() - t.x() < d)
- nn(n, t, r->right(), l + 1, d);
- } else if (l % 3 == 0) {
- nn(n, t, r->right(), l + 1, d);
- if (t.x() - r->node()->x() < d)
- nn(n, t, r->left(), l + 1, d);
- } else if (l % 3 == 1 && t.y() < r->node()->y()) {
- nn(n, t, r->left(), l + 1, d);
- if (r->node()->y() - t.y() < d)
- nn(n, t, r->right(), l + 1, d);
- } else if (l % 3 == 1) {
- nn(n, t, r->right(), l + 1, d);
- if (t.y() - r->node()->y() < d)
- nn(n, t, r->left(), l + 1, d);
- } else if (l % 3 == 2 && t.h() < r->node()->h()) {
- nn(n, t, r->left(), l + 1, d);
- if (r->node()->h() - t.h() < d)
- nn(n, t, r->right(), l + 1, d);
- } else {
- nn(n, t, r->right(), l + 1, d);
- if (t.h() - r->node()->h() < d)
- nn(n, t, r->left(), l + 1, d);
- }
+ if (r == nullptr)
+ return;
+ if (this->cost_search(*r->node(), t) < d) {
+ n = r->node();
+ d = this->cost_search(*r->node(), t);
+ }
+ if (l % 3 == 0 && t.x() < r->node()->x()) {
+ nn(n, t, r->left(), l + 1, d);
+ if (r->node()->x() - t.x() < d)
+ nn(n, t, r->right(), l + 1, d);
+ } else if (l % 3 == 0) {
+ nn(n, t, r->right(), l + 1, d);
+ if (t.x() - r->node()->x() < d)
+ nn(n, t, r->left(), l + 1, d);
+ } else if (l % 3 == 1 && t.y() < r->node()->y()) {
+ nn(n, t, r->left(), l + 1, d);
+ if (r->node()->y() - t.y() < d)
+ nn(n, t, r->right(), l + 1, d);
+ } else if (l % 3 == 1) {
+ nn(n, t, r->right(), l + 1, d);
+ if (t.y() - r->node()->y() < d)
+ nn(n, t, r->left(), l + 1, d);
+ } else if (l % 3 == 2 && t.h() < r->node()->h()) {
+ nn(n, t, r->left(), l + 1, d);
+ if (r->node()->h() - t.h() < d)
+ nn(n, t, r->right(), l + 1, d);
+ } else {
+ nn(n, t, r->right(), l + 1, d);
+ if (t.h() - r->node()->h() < d)
+ nn(n, t, r->left(), l + 1, d);
+ }
}
void RRTExt8::nv(
- std::vector<RRTNode*>& n,
- RRTNode &t,
- KdNode *r,
- int l,
- double const& d
+ std::vector<RRTNode*>& n,
+ RRTNode &t,
+ KdNode *r,
+ int l,
+ double const& d
)
{
- if (r == nullptr)
- return;
- if (this->cost_search(*r->node(), t) < d) {
- n.push_back(r->node());
- }
- if (l % 3 == 0 && t.x() < r->node()->x()) {
- this->nv(n, t, r->left(), l + 1, d);
- if (r->node()->x() - t.x() < d)
- this->nv(n, t, r->right(), l + 1, d);
- } else if (l % 3 == 0) {
- this->nv(n, t, r->right(), l + 1, d);
- if (t.x() - r->node()->x() < d)
- this->nv(n, t, r->left(), l + 1, d);
- } else if (l % 3 == 1 && t.y() < r->node()->y()) {
- this->nv(n, t, r->left(), l + 1, d);
- if (r->node()->y() - t.y() < d)
- this->nv(n, t, r->right(), l + 1, d);
- } else if (l % 3 == 1) {
- this->nv(n, t, r->right(), l + 1, d);
- if (t.y() - r->node()->y() < d)
- this->nv(n, t, r->left(), l + 1, d);
- } else if (l % 3 == 2 && t.h() < r->node()->h()) {
- this->nv(n, t, r->left(), l + 1, d);
- if (r->node()->h() - t.h() < d)
- this->nv(n, t, r->right(), l + 1, d);
- } else {
- this->nv(n, t, r->right(), l + 1, d);
- if (t.h() - r->node()->h() < d)
- this->nv(n, t, r->left(), l + 1, d);
- }
+ if (r == nullptr)
+ return;
+ if (this->cost_search(*r->node(), t) < d) {
+ n.push_back(r->node());
+ }
+ if (l % 3 == 0 && t.x() < r->node()->x()) {
+ this->nv(n, t, r->left(), l + 1, d);
+ if (r->node()->x() - t.x() < d)
+ this->nv(n, t, r->right(), l + 1, d);
+ } else if (l % 3 == 0) {
+ this->nv(n, t, r->right(), l + 1, d);
+ if (t.x() - r->node()->x() < d)
+ this->nv(n, t, r->left(), l + 1, d);
+ } else if (l % 3 == 1 && t.y() < r->node()->y()) {
+ this->nv(n, t, r->left(), l + 1, d);
+ if (r->node()->y() - t.y() < d)
+ this->nv(n, t, r->right(), l + 1, d);
+ } else if (l % 3 == 1) {
+ this->nv(n, t, r->right(), l + 1, d);
+ if (t.y() - r->node()->y() < d)
+ this->nv(n, t, r->left(), l + 1, d);
+ } else if (l % 3 == 2 && t.h() < r->node()->h()) {
+ this->nv(n, t, r->left(), l + 1, d);
+ if (r->node()->h() - t.h() < d)
+ this->nv(n, t, r->right(), l + 1, d);
+ } else {
+ this->nv(n, t, r->right(), l + 1, d);
+ if (t.h() - r->node()->h() < d)
+ this->nv(n, t, r->left(), l + 1, d);
+ }
}
// API
void RRTExt8::reset()
{
- RRTS::reset();
- this->delete_kd_nodes();
+ RRTS::reset();
+ this->delete_kd_nodes();
}
void RRTExt8::deinit()
{
- this->delete_kd_nodes(this->kdroot_);
+ this->delete_kd_nodes(this->kdroot_);
}
void RRTExt8::store_node(RRTNode n)
{
- RRTS::store_node(n);
- RRTNode *sn = &this->nodes().back();
- this->store_node(sn, this->kdroot_, 0);
+ RRTS::store_node(n);
+ RRTNode *sn = &this->nodes().back();
+ this->store_node(sn, this->kdroot_, 0);
}
RRTNode *RRTExt8::nn(RRTNode &t)
{
- RRTNode *n = &this->nodes().front();
- double d = 9999;
- this->nn(n, t, this->kdroot_, 0, d);
- return n;
+ RRTNode *n = &this->nodes().front();
+ double d = 9999;
+ this->nn(n, t, this->kdroot_, 0, d);
+ return n;
}
std::vector<RRTNode *> RRTExt8::nv(RRTNode &t)
{
- double cost = std::min(GAMMA(this->nodes().size()), 0.5);
- std::vector<RRTNode *> n;
- this->nv(n, t, this->kdroot_, 0, cost);
- return n;
+ double cost = std::min(GAMMA(this->nodes().size()), 0.5);
+ std::vector<RRTNode *> n;
+ this->nv(n, t, this->kdroot_, 0, cost);
+ return n;
}
projects / hubacji1 / rrts.git / blobdiff