From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Fri, 10 Sep 2021 14:43:57 +0000 (+0200)
Subject: Improve search cost
X-Git-Tag: v0.9.0~2^2~1
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/commitdiff_plain/97f895b89e60529ff4b9cef5ebdf3dcea17656d1

Improve search cost
---

diff --git a/src/rrtext10.cc b/src/rrtext10.cc
index 83fb6ed..f5d9b89 100644
--- a/src/rrtext10.cc
+++ b/src/rrtext10.cc
@@ -21,7 +21,12 @@ RRTExt10::cost_build(RRTNode const& f, RRTNode const& t) const
 double
 RRTExt10::cost_search(RRTNode const& f, RRTNode const& t) const
 {
-	return f.edist(t) + std::abs(t.h() - f.h()) + f.cusp() * 0.1;
+	double cost = f.edist(t);
+	double heur = std::min(std::abs(t.h() - f.h()),
+		2 * M_PI - std::abs(t.h() - f.h()));
+	heur *= this->bc_.mtr();
+	cost = std::max(cost, heur);
+	return cost + f.cusp() * 0.1;
 }
 
 } // namespace rrts