From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Mon, 30 Aug 2021 10:54:02 +0000 (+0200)
Subject: Improve pick interesting method
X-Git-Tag: v0.9.0~5^2~5
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/commitdiff_plain/89e722b20795ac72ef1696398622115bf0e30672?ds=sidebyside

Improve pick interesting method
---

diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index 6027886..5545c95 100644
--- a/src/rrtext13.cc
+++ b/src/rrtext13.cc
@@ -37,24 +37,24 @@ RRTExt13::pick_interesting()
 	return;
 #endif
 #if 1 // cusp and 1st non-drivable path poses are interesting
+	this->dn_.push_back(DijkstraNode(this->opath_.front()));
+	this->dn_.back().i = this->dn_.size() - 1;
 	for (unsigned int i = 0; i < this->opath_.size() - 1; i++) {
-	//unsigned int i = 0; { // just for cusp, comment out drivable section
 		RRTNode& ni = *this->opath_[i];
 		this->bc_.set_pose(ni);
 		for (unsigned int j = i + 1; j < this->opath_.size(); j++) {
 			RRTNode& nj = *this->opath_[j];
 			RRTNode& njl = *this->opath_[j - 1];
-			if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())
-					|| njl.edist(nj) < this->eta_) {
-				this->dn_.push_back(DijkstraNode(&njl));
-				this->dn_.back().i = this->dn_.size() - 1;
-			}
 			if (!this->bc_.drivable(nj)) {
 				this->dn_.push_back(DijkstraNode(&njl));
 				this->dn_.back().i = this->dn_.size() - 1;
 				i = j;
 				break;
 			}
+			if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())) {
+				this->dn_.push_back(DijkstraNode(&njl));
+				this->dn_.back().i = this->dn_.size() - 1;
+			}
 		}
 	}
 	this->dn_.push_back(DijkstraNode(this->opath_.back()));