Remove costs heuristics

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 611d7861cfa664180ab6ce2905e7fd52e7008e64..0dcaa51e8c349759f6897450ef6628ee1de3a8ea 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -10,7 +10,6 @@ The format is based on [Keep a Changelog][] and this project adheres to
 ## Unreleased
 ### Added
 - Bicycle car constructor for RRTNode.
-- Cost heuristics.
 
 ### Changed
 - Extend next with steering from just added nodes.

diff --git a/src/rrts.cc b/src/rrts.cc
index 34e15ee008e225aa3cb504fcc6808e140337291d..c486742f920b24bc17ffee5c023e27c738a16581 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -75,27 +75,6 @@ double RRTS::cost(RRTNode &f, RRTNode &t)
         return cost;
 }
 
-double RRTS::cost_build(RRTNode &f, RRTNode &t)
-{
-        double q0[] = {f.x(), f.y(), f.h()};
-        double q1[] = {t.x(), t.y(), t.h()};
-        ReedsSheppStateSpace rsss(f.mtr());
-        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_);