X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar2.git/blobdiff_plain/ec79ce8bf46ebedeac36e7f38a7a735040670f53..82c63df514da47ffb441b8cb878a5f7a2a6ad860:/rrts/src/rrts.cc

diff --git a/rrts/src/rrts.cc b/rrts/src/rrts.cc
index 7cd4684..0c1a663 100644
--- a/rrts/src/rrts.cc
+++ b/rrts/src/rrts.cc
@@ -55,25 +55,37 @@ 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() 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++;
 	}
 }
 
@@ -89,6 +101,45 @@ RRTNode::st(int st)
 	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 (std::abs(p.sp()) < 1e-3) {
+		// It should not be possible to have two zero speed nodes next
+		// to each other. In practice, this sometimes happens.
+		//assert(std::abs(this->sp()) > 1e-3);
+		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 (std::abs(this->sp()) < 1e-3) {
+			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)
 {
@@ -140,7 +191,6 @@ RRTS::join_steered(RRTNode* f)
 		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;
 	}
@@ -151,6 +201,15 @@ RRTS::connect()
 {
 	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
+		&& std::abs(t->y() - f->y()) < 1e-3
+		&& std::abs(t->h() - f->h()) < 1e-3);
+	this->_steered.erase(this->_steered.begin());
+	t = &this->_steered.front();
+	assert(!(std::abs(t->x() - f->x()) < 1e-3
+		&& std::abs(t->y() - f->y()) < 1e-3
+		&& std::abs(t->h() - f->h()) < 1e-3));
 #if USE_RRTS
 	double cost = f->cc() + this->cost_build(*f, *t);
 	for (auto n: this->nv_) {
@@ -172,7 +231,6 @@ RRTS::connect()
 	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;
 }
@@ -392,6 +450,7 @@ RRTS::json(void) const
 			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();
@@ -404,11 +463,13 @@ RRTS::json(void) const
 		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();
@@ -478,13 +539,15 @@ RRTS::next()
 	this->join_steered(&this->_nodes.back());
 	RRTNode* just_added = &this->_nodes.back();
 	bool gf = false;
-	while (ss > 0 && just_added->p() != nullptr) {
+	while (ss > 0 && just_added != nullptr) {
 		this->steer(*just_added, this->_goal);
 		if (this->collide_steered()) {
 			ss--;
 			just_added = just_added->p();
 			continue;
 		}
+		// The first of steered is the same as just_added.
+		this->_steered.erase(this->_steered.begin());
 		this->join_steered(just_added);
 		bool gn = this->_goal.edist(this->_nodes.back()) < this->eta();
 		bool gd = this->goal_drivable_from(this->_nodes.back());
@@ -495,7 +558,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;
 			}