From ee6a0a27d41ecbbc30dafbbf84bf480aed29e041 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Mon, 30 Aug 2021 12:55:01 +0200
Subject: [PATCH] Use interesting forward/backward methods

---
 incl/rrtext.hh  |  3 ++-
 src/rrtext13.cc | 42 +++++++++++++++++++++++++++++++++++++++++-
 2 files changed, 43 insertions(+), 2 deletions(-)

diff --git a/incl/rrtext.hh b/incl/rrtext.hh
index 69f9175..038677f 100644
--- a/incl/rrtext.hh
+++ b/incl/rrtext.hh
@@ -115,7 +115,8 @@ private:
 	double ogoal_cc_ = 0.0;
 	double otime_ = 0.0;
 	std::vector<DijkstraNode> dn_;
-	void pick_interesting();
+	void interesting_forward();
+	void interesting_backward();
 	void dijkstra_forward();
 	void dijkstra_backward();
 	void compute_path();
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()
 {
-- 
2.39.2