src/rrtext10.cc

   1 /*
   2  * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
   3  *
   4  * SPDX-License-Identifier: GPL-3.0-only
   5  */
   6
   7 #include "rrtext.hh"
   8 #include "reeds_shepp.h"
   9
  10 namespace rrts {
  11
  12 double
  13 RRTExt10::cost_build(RRTNode const& f, RRTNode const& t) const
  14 {
  15         double q0[] = {f.x(), f.y(), f.h()};
  16         double q1[] = {t.x(), t.y(), t.h()};
  17         ReedsSheppStateSpace rsss(this->bc_.mtr());
  18         return rsss.distance(q0, q1);
  19 }
  20
  21 double
  22 RRTExt10::cost_search(RRTNode const& f, RRTNode const& t) const
  23 {
  24         double cost = f.edist(t);
  25         double heur = std::min(std::abs(t.h() - f.h()),
  26                 2 * M_PI - std::abs(t.h() - f.h()));
  27         heur *= this->bc_.mtr();
  28         cost = std::max(cost, heur);
  29         return cost + f.cusp() * 0.1;
  30 }
  31
  32 } // namespace rrts