Improve search cost

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

diff --git a/src/rrtext10.cc b/src/rrtext10.cc
index c5b5602c8403897b69c6087d5b2f2147a659a45a..f5d9b893773c3876f2a3b284ec6ba8261e7b5e39 100644 (file)
--- a/src/rrtext10.cc
+++ b/src/rrtext10.cc
@@ -1,18 +1,32 @@
-#include "rrtext.h"
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include "rrtext.hh"
 #include "reeds_shepp.h"
 
-double RRTExt10::cost_build(RRTNode &f, RRTNode &t)
+namespace rrts {
+
+double
+RRTExt10::cost_build(RRTNode const& f, RRTNode const& t) const
 {
-        double q0[] = {f.x(), f.y(), f.h()};
-        double q1[] = {t.x(), t.y(), t.h()};
-        ReedsSheppStateSpace rsss(this->bc.mtr());
-        return rsss.distance(q0, q1);
+       double q0[] = {f.x(), f.y(), f.h()};
+       double q1[] = {t.x(), t.y(), t.h()};
+       ReedsSheppStateSpace rsss(this->bc_.mtr());
+       return rsss.distance(q0, q1);
 }
 
-double RRTExt10::cost_search(RRTNode &f, RRTNode &t)
+double
+RRTExt10::cost_search(RRTNode const& f, RRTNode const& t) const
 {
-        double cost = 0;
-        cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2));
-        cost += std::abs(t.h() - f.h());
-        return cost;
+       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