if (this != &p) {
this->_p = &p;
}
+ this->p_is_cusp(this->would_be_cusp_if_parent(p));
+ this->cusp_cnt(p);
}
unsigned int
-RRTNode::cusp() const
+RRTNode::cusp_cnt() const
{
- return this->_cusp;
+ return this->_cusp_cnt;
}
void
-RRTNode::cusp(RRTNode const& p)
+RRTNode::cusp_cnt(RRTNode const& p)
{
- this->_cusp = p.cusp();
- if (this->sp() != p.sp() || this->sp() == 0.0) {
- this->_cusp++;
+ this->_cusp_cnt = p.cusp_cnt();
+ if (this->_p_is_cusp) {
+ this->_cusp_cnt++;
}
}
this->_segment_type = st;
}
+bool
+RRTNode::p_is_cusp(void) const
+{
+ return this->_p_is_cusp;
+}
+
+void
+RRTNode::p_is_cusp(bool isit)
+{
+ this->_p_is_cusp = isit;
+}
+
+bool
+RRTNode::would_be_cusp_if_parent(RRTNode const& p) const
+{
+ if (p.sp() == 0) {
+ assert(this->sp() != 0);
+ if (p.p()) {
+ if (sgn(p.p()->sp()) != sgn(this->sp())) {
+ return true;
+ } else {
+ return false;
+ }
+ } else {
+ return true; // only root has no parent and it is cusp
+ }
+ } else {
+ if (this->sp() == 0) {
+ return false; // this is cusp, not the parent
+ } else if (sgn(p.sp()) != sgn(this->sp())) {
+ return true;
+ } else {
+ return false;
+ }
+ }
+}
+
bool
RRTNode::operator==(RRTNode const& n)
{
RRTNode* t = &this->_nodes.back();
t->p(*f);
t->c(this->cost_build(*f, *t));
- t->cusp(*f);
this->_steered.erase(this->_steered.begin());
f = t;
}
{
RRTNode* f = this->_nn;
RRTNode* t = &this->_steered.front();
+ // Require the steer method to return first node equal to nn:
+ assert(std::abs(t->x() - f->x()) < 1e-3);
+ assert(std::abs(t->y() - f->x()) < 1e-3);
+ assert(std::abs(t->h() - f->x()) < 1e-3);
+ this->_steered.erase(this->_steered.begin());
+ t = &this->_steered.front();
#if USE_RRTS
double cost = f->cc() + this->cost_build(*f, *t);
for (auto n: this->nv_) {
t = &this->_nodes.back();
t->p(*f);
t->c(this->cost_build(*f, *t));
- t->cusp(*f);
this->_steered.erase(this->_steered.begin());
return true;
}
this->_bc.set_pose_to(p);
}
+void
+RRTS::set_bc_to_become(std::string what)
+{
+ this->_bc.become(what);
+}
+
RRTGoal const&
RRTS::goal(void) const
{