From 97f895b89e60529ff4b9cef5ebdf3dcea17656d1 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Fri, 10 Sep 2021 16:43:57 +0200
Subject: [PATCH] Improve search cost

---
 src/rrtext10.cc | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

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
-- 
2.39.2