2 * SPDX-FileCopyrightText: 2022 Jiri Vlasak
4 * SPDX-License-Identifier: GPL-3.0-only
13 cb_steer(double q[3], double t, void *w)
15 std::vector<RRTNode>* st = (std::vector<RRTNode>*) w;
16 st->push_back(RRTNode());
25 RRTExt19::steer(RRTNode const &f, RRTNode const &t)
27 this->steered_.clear();
28 double q0[] = {f.x(), f.y(), f.h()};
29 double q1[] = {t.x(), t.y(), t.h()};
31 int r = dubins_shortest_path(&path, q0, q1, this->bc_.mtr());
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);
41 } /* namespace rrts */