rrts/src/rrtext6.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 RRTExt6::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 RRTExt6::cost_search(RRTNode const& f, RRTNode const& t) const
  23 {
  24         return this->cost_build(f, t);
  25 }
  26
  27 } // namespace rrts