Add Matej's heuristics cost

diff --git a/api/rrts.h b/api/rrts.h
index 73a3be8ddbc1ccbbce5f49e6f7d62467bf9e056e..11ff44e00e551a287898a00c8e3f46d4f1f5533a 100644 (file)
--- a/api/rrts.h
+++ b/api/rrts.h
@@ -68,6 +68,7 @@ class RRTS {
                 bool collide_two_nodes(RRTNode &f, RRTNode &t);
                 double cost(RRTNode &f, RRTNode &t);
                 double cost_build(RRTNode &f, RRTNode &t);
+                double cost_search(RRTNode &f, RRTNode &t);
                 void sample();
                         std::default_random_engine gen_;
                         std::normal_distribution<double> ndx_;

diff --git a/src/rrts.cc b/src/rrts.cc
index dfc5539d8f86ac35c00d5cc518adf80e09023f67..d275736fae9c7fec10a1ac2be04bae7238f83bff 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -62,6 +62,19 @@ double RRTS::cost_build(RRTNode &f, RRTNode &t)
         return rsss.distance(q0, q1);
 }
 
+double RRTS::cost_search(RRTNode &f, RRTNode &t)
+{
+        double cost = 0;
+        cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2));
+        double heur = std::min(
+                std::abs(t.h() - f.h()),
+                2 * M_PI - std::abs(t.h() - f.h())
+        );
+        heur *= f.mtr();
+        cost = std::max(cost, heur);
+        return cost;
+}
+
 void RRTS::sample()
 {
         double x = this->ndx_(this->gen_);