X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar2.git/blobdiff_plain/40caf1bccefbc41c697f4dfc2249d17361659828..05fa1df054dc961b9c77edb82103de5630ab909f:/rrts/src/rrts.cc

diff --git a/rrts/src/rrts.cc b/rrts/src/rrts.cc
index d6f8f00..6bcf151 100644
--- a/rrts/src/rrts.cc
+++ b/rrts/src/rrts.cc
@@ -55,15 +55,25 @@ RRTNode::p() const
 }
 
 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
 {
@@ -541,7 +551,7 @@ RRTS::next()
 			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;
 			}