rrts/src/rrtext1.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.h"
   8 #include "reeds_shepp.h"
   9
  10 double RRTExt1::cost_build(RRTNode &f, RRTNode &t)
  11 {
  12         double q0[] = {f.x(), f.y(), f.h()};
  13         double q1[] = {t.x(), t.y(), t.h()};
  14         ReedsSheppStateSpace rsss(this->bc.mtr());
  15         return rsss.distance(q0, q1);
  16 }
  17
  18 double RRTExt1::cost_search(RRTNode &f, RRTNode &t)
  19 {
  20         double cost = 0;
  21         cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2));
  22         double heur = std::min(
  23                 std::abs(t.h() - f.h()),
  24                 2 * M_PI - std::abs(t.h() - f.h())
  25         );
  26         heur *= this->bc.mtr();
  27         cost = std::max(cost, heur);
  28         return cost;
  29 }