delete this->goal_;
}
-RRTBase::RRTBase():
- root_(new RRTNode()),
- goal_(new RRTNode()),
- gen_(std::random_device{}())
+RRTBase::RRTBase()
+ : root_(new RRTNode())
+ , goal_(new RRTNode())
+ , gen_(std::random_device{}())
+ , ndx_(HMAX - HMIN, (HMAX - HMIN) / 4)
+ , ndy_(VMAX - VMIN, (VMAX - VMIN) / 4)
+ , ndh_(0, M_PI * 2 / 4)
{
this->nodes_.reserve(NOFNODES);
this->nodes_.push_back(this->root_);
this->add_ixy(this->root_);
}
-RRTBase::RRTBase(RRTNode *init, RRTNode *goal):
- root_(init),
- goal_(goal),
- gen_(std::random_device{}())
+RRTBase::RRTBase(RRTNode *init, RRTNode *goal)
+ : root_(init)
+ , goal_(goal)
+ , gen_(std::random_device{}())
+ , ndx_(HMIN + (HMAX - HMIN) / 2, (HMAX - HMIN) / 4)
+ , ndy_(VMIN + (VMAX - VMIN) / 2, (VMAX - VMIN) / 4)
+ , ndh_(0, M_PI * 2 / 4)
{
this->nodes_.reserve(NOFNODES);
this->nodes_.push_back(init);
return this->dnodes_;
}
+std::queue<RRTNode *> &RRTBase::firsts()
+{
+ return this->firsts_;
+}
+
PolygonObstacle &RRTBase::frame()
{
return this->frame_;
h += this->samplingInfo_.h0;
return new RRTNode(x, y, h);
} else {
- return sa1();
+ float x = this->ndx_(this->gen_);
+ float y = this->ndy_(this->gen_);
+ float h = this->ndh_(this->gen_);
+ return new RRTNode(x, y, h);
}
}