X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/blobdiff_plain/89e722b20795ac72ef1696398622115bf0e30672..ee6a0a27d41ecbbc30dafbbf84bf480aed29e041:/src/rrtext13.cc

diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index 5545c95..1917ddf 100644
--- a/src/rrtext13.cc
+++ b/src/rrtext13.cc
@@ -1,3 +1,4 @@
+#include <algorithm>
 #include <queue>
 #include "rrtext.hh"
 
@@ -25,7 +26,7 @@ RRTExt13::DijkstraNodeComparator::operator() (DijkstraNode const& n1,
 }
 
 void
-RRTExt13::pick_interesting()
+RRTExt13::interesting_forward()
 {
 	this->dn_.clear();
 	this->dn_.reserve(this->path_.size());
@@ -62,6 +63,45 @@ RRTExt13::pick_interesting()
 #endif
 }
 
+void
+RRTExt13::interesting_backward()
+{
+	this->dn_.clear();
+	this->dn_.reserve(this->path_.size());
+#if 0 // all path poses are interesting
+	for (auto n: this->opath_) {
+		this->dn_.push_back(DijkstraNode(n));
+		this->dn_.back().i = this->dn_.size() - 1;
+	}
+	std::reverse(this->dn_.begin(), this->dn_.end());
+	return;
+#endif
+#if 1 // cusp and 1st non-drivable path poses are interesting
+	this->dn_.push_back(DijkstraNode(this->opath_.back()));
+	this->dn_.back().i = this->dn_.size() - 1;
+	for (unsigned int i = this->opath_.size() - 1; i > 1; i--) {
+		RRTNode& ni = *this->opath_[i];
+		this->bc_.set_pose(ni);
+		for (unsigned int j = i - 1; j > 0; j--) {
+			RRTNode& nj = *this->opath_[j];
+			RRTNode& njl = *this->opath_[j + 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_.front()));
+	this->dn_.back().i = this->dn_.size() - 1;
+#endif
+}
+
 void
 RRTExt13::dijkstra_forward()
 {