2 * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
4 * SPDX-License-Identifier: GPL-3.0-only
8 #include "reeds_shepp.h"
10 double RRTExt1::cost_build(RRTNode &f, RRTNode &t)
12 double q0[] = {f.x(), f.y(), f.h()};
13 double q1[] = {t.x(), t.y(), t.h()};
14 ReedsSheppStateSpace rsss(this->bc.mtr());
15 return rsss.distance(q0, q1);
18 double RRTExt1::cost_search(RRTNode &f, RRTNode &t)
21 cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2));
22 double heur = std::min(
23 std::abs(t.h() - f.h()),
24 2 * M_PI - std::abs(t.h() - f.h())
26 heur *= this->bc.mtr();
27 cost = std::max(cost, heur);