Add and use #cusp

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

diff --git a/src/rrtext10.cc b/src/rrtext10.cc
index 63e2ade6596f449b21d72c932bc359d9e988e8bf..83fb6ed639681802058da73ff7c6d5a5961eb34d 100644 (file)
--- a/src/rrtext10.cc
+++ b/src/rrtext10.cc
@@ -21,7 +21,7 @@ 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());
+       return f.edist(t) + std::abs(t.h() - f.h()) + f.cusp() * 0.1;
 }
 
 } // namespace rrts