rrts/src/rrtext19.cc

   1 /*
   2  * SPDX-FileCopyrightText: 2022 Jiri Vlasak
   3  *
   4  * SPDX-License-Identifier: GPL-3.0-only
   5  */
   6
   7 #include "rrtext.hh"
   8 #include "dubins.h"
   9
  10 namespace rrts {
  11
  12 static int
  13 cb_steer(double q[3], double t, 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(1.0);
  21         return 0;
  22 }
  23
  24 void
  25 RRTExt19::steer(RRTNode const &f, RRTNode const &t)
  26 {
  27         this->steered_.clear();
  28         double q0[] = {f.x(), f.y(), f.h()};
  29         double q1[] = {t.x(), t.y(), t.h()};
  30         DubinsPath path;
  31         int r = dubins_shortest_path(&path, q0, q1, this->bc_.mtr());
  32         if (r != 0) {
  33                 return;
  34         }
  35         dubins_path_sample_many(&path, this->eta_, cb_steer, &this->steered_);
  36         if (this->steered_.size() > 0) {
  37                 this->steered_.front().sp(0.0);
  38         }
  39 }
  40
  41 } /* namespace rrts */