X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar2.git/blobdiff_plain/4b31eb4c045adc3ac8e53076e3e5173b566ee1c2..43552001c5058717dd65e5ab64a9da2eedb025ea:/rrts/src/rrtext13.cc

diff --git a/rrts/src/rrtext13.cc b/rrts/src/rrtext13.cc
index 1619d7b..7277bdb 100644
--- a/rrts/src/rrtext13.cc
+++ b/rrts/src/rrtext13.cc
@@ -37,7 +37,7 @@ RRTExt13::interesting_forward()
 				i = j;
 				break;
 			}
-			if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())) {
+			if (nj.p_is_cusp()) {
 				this->dn_.push_back(DijkstraNode(&njl));
 				this->dn_.back().i = this->dn_.size() - 1;
 			}
@@ -79,7 +79,7 @@ RRTExt13::interesting_backward()
 				i = j;
 				break;
 			}
-			if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())) {
+			if (nj.p_is_cusp()) {
 				this->dn_.push_back(DijkstraNode(&njl));
 				this->dn_.back().i = this->dn_.size() - 1;
 				this->dn_.back().d = goal_cc - njl.cc();
@@ -119,12 +119,14 @@ RRTExt13::dijkstra_forward()
 					|| ss != this->_steered.size()) {
 				continue;
 			}
+			// The first steered is the same as f.
+			this->_steered.erase(this->_steered.begin());
 			this->set_bc_pose_to(this->_steered.back());
 			bool td = this->_bc.drivable(t);
 			bool tn = this->_bc.edist(t) < 2.0 * this->_eta;
 			if (cost < t.cc() && td && tn) {
 				this->join_steered(&f);
-				t.p(this->_nodes.back());
+				t.p(this->_nodes.back(), true);
 				t.c(this->cost_build(this->_nodes.back(), t));
 				if (!this->dn_[i].vi()) {
 					pq.push(this->dn_[i]);
@@ -162,13 +164,15 @@ RRTExt13::dijkstra_backward()
 					|| ss != this->_steered.size()) {
 				continue;
 			}
+			// The first steered is the same as f.
+			this->_steered.erase(this->_steered.begin());
 			this->set_bc_pose_to(this->_steered.back());
 			bool td = this->_bc.drivable(t);
 			bool tn = this->_bc.edist(t) < 2.0 * this->_eta;
 			if (cost < fd.d && td && tn) {
 				fd.d = cost;
 				this->join_steered(&f);
-				t.p(this->_nodes.back());
+				t.p(this->_nodes.back(), true);
 				t.c(this->cost_build(this->_nodes.back(), t));
 				this->recompute_cc_for_predecessors_and(&t);
 				if (!this->dn_[i].vi()) {