}
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
RRTNode::cusp_cnt() const
{
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;
}