}
void
-RRTNode::p(RRTNode& p)
+RRTNode::p(RRTNode& p, bool connecting_goal)
{
- if (this != &p) {
- this->_p = &p;
+ assert(this != &p);
+ if (!connecting_goal) {
+ assert(!(std::abs(p.x() - this->x()) < 1e-3
+ && std::abs(p.y() - this->y()) < 1e-3
+ && std::abs(p.h() - this->h()) < 1e-3));
}
+ this->_p = &p;
+ this->p_is_cusp(this->would_be_cusp_if_parent(p));
+ this->cusp_cnt(p);
+}
+
+void
+RRTNode::p(RRTNode& p)
+{
+ return this->p(p, false);
}
unsigned int
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_cnt(*f);
this->_steered.erase(this->_steered.begin());
f = t;
}
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);
- // When f and t has different directions, the node (f == t) is cusp:
- // TODO
this->_steered.erase(this->_steered.begin());
t = &this->_steered.front();
#if USE_RRTS
t = &this->_nodes.back();
t->p(*f);
t->c(this->cost_build(*f, *t));
- t->cusp_cnt(*f);
this->_steered.erase(this->_steered.begin());
return true;
}
jvo["paths"][j][i][2] = n.h();
jvo["paths"][j][i][3] = n.sp();
jvo["paths"][j][i][4] = n.st();
+ jvo["paths"][j][i][5] = n.p_is_cusp();
i++;
}
jvo["costs"][j] = path.back().cc();
jvo["paths"][j][i][2] = n->h();
jvo["paths"][j][i][3] = n->sp();
jvo["paths"][j][i][4] = n->st();
+ jvo["paths"][j][i][5] = n->p_is_cusp();
jvo["path"][i][0] = n->x();
jvo["path"][i][1] = n->y();
jvo["path"][i][2] = n->h();
jvo["path"][i][3] = n->sp();
jvo["path"][i][4] = n->st();
+ jvo["path"][i][5] = n->p_is_cusp();
i++;
}
jvo["costs"][j] = this->_path.back()->cc();
double ncc = this->_nodes.back().cc() + nc;
if (this->_goal.p() == nullptr
|| ncc < this->_goal.cc()) {
- this->_goal.p(this->_nodes.back());
+ this->_goal.p(this->_nodes.back(), true);
this->_goal.c(nc);
gf = true;
}