Improve search cost

diff --git a/src/rrtext10.cc b/src/rrtext10.cc
index 83fb6ed639681802058da73ff7c6d5a5961eb34d..f5d9b893773c3876f2a3b284ec6ba8261e7b5e39 100644 (file)
--- 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