Improve search cost

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

diff --git a/src/rrtext10.cc b/src/rrtext10.cc
index c9b5562a9a82b6cb65bf6a0e23ad8842e2def5e1..f5d9b893773c3876f2a3b284ec6ba8261e7b5e39 100644 (file)
--- a/src/rrtext10.cc
+++ b/src/rrtext10.cc
@@ -1,3 +1,9 @@
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
 #include "rrtext.hh"
 #include "reeds_shepp.h"
 
@@ -15,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