src/rrtext16.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 "reeds_shepp.h"
   8 #include "rrtext.hh"
   9
  10 namespace rrts {
  11
  12 static int
  13 cb_steer(double q[4], void *w)
  14 {
  15         std::vector<RRTNode>* st = (std::vector<RRTNode>*) w;
  16         st->push_back(RRTNode());
  17         st->back().x(q[0]);
  18         st->back().y(q[1]);
  19         st->back().h(q[2]);
  20         st->back().sp(q[3]);
  21         st->back().segment_type = q[4];
  22         return 0;
  23 }
  24
  25 void
  26 RRTExt16::steer(RRTNode const& f, RRTNode const& t)
  27 {
  28         this->steered_.clear();
  29         double q0[] = {f.x(), f.y(), f.h()};
  30         double q1[] = {t.x(), t.y(), t.h()};
  31         ReedsSheppStateSpace rsss(this->bc_.mtr());
  32         rsss.sample(q0, q1, this->eta_, cb_steer, &this->steered_);
  33 }
  34
  35 } // namespace rrts