Improve search cost

[hubacji1/rrts.git] / src / rrtext10.cc

diff --git a/src/rrtext10.cc b/src/rrtext10.cc
index 63e2ade6596f449b21d72c932bc359d9e988e8bf..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());
+       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