2 * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
4 * SPDX-License-Identifier: GPL-3.0-only
7 #include "reeds_shepp.h"
13 cb_steer(double q[4], void *w)
15 std::vector<RRTNode>* st = (std::vector<RRTNode>*) w;
16 st->push_back(RRTNode());
25 RRTExt16::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()};
30 ReedsSheppStateSpace rsss(this->bc_.mtr());
31 rsss.sample(q0, q1, this->eta_, cb_steer, &this->steered_);