From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Mon, 30 Aug 2021 11:50:48 +0000 (+0200)
Subject: Fix backward dijkstra optimization
X-Git-Tag: v0.9.0~5^2
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/commitdiff_plain/572f9913185151e8a2e1b2ce4b42726169c9a6af

Fix backward dijkstra optimization
---

diff --git a/incl/rrtext.hh b/incl/rrtext.hh
index 038677f..6842312 100644
--- a/incl/rrtext.hh
+++ b/incl/rrtext.hh
@@ -104,6 +104,7 @@ private:
 		RRTNode* node = nullptr;
 		unsigned int i = 0;
 		bool v = false;
+		double d = 0.0;
 		bool vi();
 		DijkstraNode(RRTNode* n);
 	};
@@ -111,6 +112,10 @@ private:
 	public:
 		int operator() (DijkstraNode const& n1, DijkstraNode const& n2);
 	};
+	class DijkstraNodeBackwardComparator {
+	public:
+		int operator() (DijkstraNode const& n1, DijkstraNode const& n2);
+	};
 	std::vector<RRTNode*> opath_;
 	double ogoal_cc_ = 0.0;
 	double otime_ = 0.0;
diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index dada6a3..f6dd774 100644
--- a/src/rrtext13.cc
+++ b/src/rrtext13.cc
@@ -25,6 +25,13 @@ RRTExt13::DijkstraNodeComparator::operator() (DijkstraNode const& n1,
 	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()
 {
@@ -77,8 +84,10 @@ RRTExt13::interesting_backward()
 	return;
 #endif
 #if 1 // cusp and 1st non-drivable path poses are interesting
+	double goal_cc = this->goal_.cc();
 	this->dn_.push_back(DijkstraNode(this->opath_.back()));
 	this->dn_.back().i = this->dn_.size() - 1;
+	this->dn_.back().d = goal_cc - this->opath_.back()->cc();
 	for (unsigned int i = this->opath_.size() - 1; i > 1; i--) {
 		RRTNode& ni = *this->opath_[i];
 		this->bc_.set_pose(ni);
@@ -88,17 +97,20 @@ RRTExt13::interesting_backward()
 			if (!this->bc_.drivable(nj)) {
 				this->dn_.push_back(DijkstraNode(&njl));
 				this->dn_.back().i = this->dn_.size() - 1;
+				this->dn_.back().d = goal_cc - njl.cc();
 				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_.back().d = goal_cc - njl.cc();
 			}
 		}
 	}
 	this->dn_.push_back(DijkstraNode(this->opath_.front()));
 	this->dn_.back().i = this->dn_.size() - 1;
+	this->dn_.back().d = goal_cc - this->opath_.front()->cc();
 #endif
 }
 
@@ -149,29 +161,38 @@ RRTExt13::dijkstra_backward()
 {
 	std::priority_queue<DijkstraNode, std::vector<DijkstraNode>,
 		DijkstraNodeComparator> pq;
-	this->dn_.back().vi();
-	pq.push(this->dn_.back());
+	this->dn_.front().vi();
+	pq.push(this->dn_.front());
 	while (!pq.empty()) {
 		DijkstraNode td = pq.top();
 		RRTNode& t = *td.node;
 		pq.pop();
-		for (unsigned int i = td.i - 1; i > 0; i--) {
+		this->bc_.set_pose(t);
+		for (unsigned int i = td.i; i > 0; i--) {
+			DijkstraNode& fd = this->dn_[i];
 			RRTNode& f = *this->dn_[i].node;
-			double cost = f.cc() + this->cost_build(f, t);
+			if (!this->bc_.drivable(f)) {
+				continue;
+			}
+			double cost = td.d + this->cost_build(f, t);
 			this->steer(f, t);
 			if (this->steered_.size() == 0) {
 				break;
 			}
-			if (this->collide_steered()) {
+			unsigned int ss = this->steered_.size();
+			if (this->collide_steered()
+					|| ss != this->steered_.size()) {
 				continue;
 			}
 			this->bc_.set_pose(this->steered_.back());
 			bool td = this->bc_.drivable(t);
-			bool tn = this->bc_.edist(t) < this->eta_;
-			if (cost < t.cc() && td && tn) {
+			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.c(this->cost_build(this->nodes_.back(), t));
+				this->recompute_path_cc();
 				if (!this->dn_[i].vi()) {
 					pq.push(this->dn_[i]);
 				}