X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar2.git/blobdiff_plain/ad8fcd4781d83e876e116be55a8a8c2e60de0a4a..f85a83aeda45d43008def1e4825c8f7a64fd1b5f:/rrts/src/rrtext13.cc

diff --git a/rrts/src/rrtext13.cc b/rrts/src/rrtext13.cc
index 758f4d9..7277bdb 100644
--- a/rrts/src/rrtext13.cc
+++ b/rrts/src/rrtext13.cc
@@ -10,34 +10,6 @@
 
 namespace rrts {
 
-RRTExt13::DijkstraNode::DijkstraNode(RRTNode* n) : node(n)
-{
-}
-
-bool
-RRTExt13::DijkstraNode::vi()
-{
-	if (this->v) {
-		return true;
-	}
-	this->v = true;
-	return false;
-}
-
-int
-RRTExt13::DijkstraNodeComparator::operator() (DijkstraNode const& n1,
-		DijkstraNode const& n2)
-{
-	return n1.node->cc() > n2.node->cc();
-}
-
-int
-RRTExt13::DijkstraNodeBackwardComparator::operator() (DijkstraNode const& n1,
-		DijkstraNode const& n2)
-{
-	return n1.d > n2.d;
-}
-
 void
 RRTExt13::interesting_forward()
 {
@@ -65,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;
 			}
@@ -107,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();
@@ -129,11 +101,11 @@ RRTExt13::dijkstra_forward()
 	pq.push(this->dn_.front());
 	while (!pq.empty()) {
 		DijkstraNode fd = pq.top();
-		RRTNode& f = *fd.node;
+		RRTNode& f = *fd.p_rrtnode;
 		pq.pop();
 		this->set_bc_pose_to(f);
 		for (unsigned int i = fd.i + 1; i < this->dn_.size(); i++) {
-			RRTNode& t = *this->dn_[i].node;
+			RRTNode& t = *this->dn_[i].p_rrtnode;
 			if (!this->_bc.drivable(t)) {
 				continue;
 			}
@@ -147,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]);
@@ -171,12 +145,12 @@ RRTExt13::dijkstra_backward()
 	pq.push(this->dn_.front());
 	while (!pq.empty()) {
 		DijkstraNode td = pq.top();
-		RRTNode& t = *td.node;
+		RRTNode& t = *td.p_rrtnode;
 		pq.pop();
 		this->set_bc_pose_to(t);
 		for (unsigned int i = td.i; i > 0; i--) {
 			DijkstraNode& fd = this->dn_[i];
-			RRTNode& f = *this->dn_[i].node;
+			RRTNode& f = *this->dn_[i].p_rrtnode;
 			if (!this->_bc.drivable(f)) {
 				continue;
 			}
@@ -190,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()) {
@@ -209,7 +185,7 @@ RRTExt13::dijkstra_backward()
 }
 
 void
-RRTExt13::compute_path()
+RRTExt13::compute_path(void)
 {
 	RRTS::compute_path();
 	if (this->_goal.cc() == 0.0 || this->_path.size() == 0) {
@@ -255,7 +231,7 @@ RRTExt13::RRTExt13()
 }
 
 Json::Value
-RRTExt13::json() const
+RRTExt13::json(void) const
 {
 	auto jvo = RRTS::json();
 	unsigned int i = 0;
@@ -272,7 +248,7 @@ RRTExt13::json() const
 }
 
 void
-RRTExt13::reset()
+RRTExt13::reset(void)
 {
 	RRTS::reset();
 	this->opath_.clear();